#include "mbed.h"               // include the mbed libraries
 
	// Setup LEDs, we'll use them as activity indicators
	DigitalOut light1(LED1);
	DigitalOut light2(LED2);
	DigitalOut light3(LED3);
	DigitalOut light4(LED4);
 
	// Give me a high output because its useful for monkeying about with in Real Life
	DigitalOut throw_away(32);
 
	// Analog-to-Digital interface. Samples input when read.
	AnalogIn analog_in(1);	
 
	// Interrupt-trigger pins
	DigitalIn int_pin_wheel(2);	// Rotation Complete (connected to wheel sensor)
	DigitalIn int_pin_clock(18);	// Reference clock (connected to ref_clock PWM)
 
	// Pulse-Width-Module
	PwmOut ref_clock(1);	// create a PWMOut, connect to PWM output 1 (DIP16)
	PwmOut pwm_out(2);		// create a PWMOut, connect to PWM output 2 (DIP15)	
	float pwm_period;		// Seconds between rising edges
	float report_period;	// Seconds between printing status messages
	float current_reading = analog_in;	// Normalised value (between 0.0 and 1.0), with 1.0 meaning 3.3V
	float last_reading = current_reading;
 
//	float tick_duration = 0.025;	// 40Hz
	float tick_duration = 1;
	unsigned int current_tick = 0;
	unsigned int transitions = 0;	// Count number of voltage transitions
	float peak_reading = 0;			// Highest (normalised) voltage reading
	float trough_reading = 0;		// Lowest (normalised) voltage reading
	bool already_warned = 0;		// Have displayed "Imminent saturation" warning
 
	float epsilon = 0.1;	// Error margin for analog readings (normalised 0.0 - 1.0)
	bool rising_edge = 0;	// Holds whether signal edge is rising or falling. Used to determine transition points
 
	bool interrupt_fired = 0;	// Fired on each complete wheel rotation
 
void handleIntWheel(void)
{
	interrupt_fired=1;
}
 
void handleIntClock(void)
{
	current_tick++;
	printf(".");
}
struct listener_entry
{
    unsigned int tick_frequency;
    unsigned int trigger_tick;
    void (*callback_func)(void);
} entries[10];
 
bool add_tick_func(int tick_frequency, void (*callback_func)(void))
{	
	// Check if function already in list
	for(int i=0; i<10; i++)
	{
		if(entries[i].callback_func == callback_func)
		{
			entries[i].tick_frequency = tick_frequency;
			//printf("Func entry updated (slot %d)\r\n", i);
			return true;
		}
	}
 
	// Add function to table if free slot available
	for(int i=0; i<10; i++)
	{
		if(entries[i].tick_frequency == 0)
		{
			entries[i].tick_frequency = tick_frequency;
			entries[i].callback_func = callback_func;
			//printf("Func entry added (slot %d)\r\n", i);
			return true;
		}
	}
 
	printf("Error: Func table full\r\n");
	return false;
}
 
void tick(void)
{
	for(int i=0; i<10; i++)
	{
		if( entries[i].tick_frequency > 0 && entries[i].trigger_tick <= current_tick )
		{
			// Schedule next trigger
			entries[i].trigger_tick = current_tick + entries[i].tick_frequency;
			(*entries[i].callback_func)();
		}
	}
}
 
void callback_blink_light(void)
{
	static float current_brightness = 0.0;
	current_brightness += 0.005;
	current_brightness = (current_brightness > 1.0 )? 0.0 : current_brightness ;
	current_brightness = (current_brightness < 0.0 )? 1.0 : current_brightness ;
	pwm_out = pwm_period * current_brightness;     // set high time		
}
 
void callback_print_status(void)
{
	printf("%fHz %.1fV - %.1fV\r\n", (transitions/report_period/2.0), (trough_reading*3.3), (peak_reading*3.3));
	// Reset state
	transitions=peak_reading=trough_reading=already_warned=0;
}
 
void callback_update_leds(void)
{
	light4 = (~transitions >> 0) & 0x1;
	light3 = (~transitions >> 1) & 0x1;
	light2 = (~transitions >> 5) & 0x1;
	light1 = (~transitions >> 6) & 0x1;
}
 
void callback_print_interrupt_status(void)
{
	static unsigned int tick_int_last_fired_at = 0;
	if ( interrupt_fired )
	{
		interrupt_fired = 0;
		printf("\r\nINTERRUPT! (Last was %d ticks ago)\r\n", (current_tick - tick_int_last_fired_at));
		tick_int_last_fired_at = current_tick;
	}
}
 
int main() {
	light1 = light2 = light3 = light4 = 1; // OFF (LEDs are pulled-low)
	throw_away = 1;
 
	// Setup a reference clock
	ref_clock.period(tick_duration);
	ref_clock = tick_duration * 0.5;	// High time = 50%
	int_pin_clock.attach(DigitalIn::RisingEdge, &handleIntClock);
 
	int_pin_wheel.attach(DigitalIn::RisingEdge, &handleIntWheel);
 
	printf("\r\n\r\n");
	printf("RobM's Makeshift Oscillascope v1.0\r\n");
 
	add_tick_func((int)(0.02/tick_duration), callback_blink_light);	
	add_tick_func((int)(0.125/tick_duration), callback_update_leds);	
	add_tick_func((int)(0.125/tick_duration), callback_print_interrupt_status);	
 
	//printf("Enter PWM period: ");
	//scanf("%f", &pwm_period);
	//printf("\r\n");
	pwm_period = 0.001;
 
	pwm_out.period(pwm_period);
	pwm_out = pwm_period * 0.5;     // set high time
 
 
	//printf("Enter report period: ");
	//scanf("%f", &report_period);
	report_period = 5;
	//add_tick_func( (int)(report_period/tick_duration), callback_print_status);
	//printf("\r\n");	
 
	while(1)
	{			
		current_reading = analog_in;
		if ( rising_edge == false && current_reading > last_reading + epsilon )
		{
			rising_edge = true;
			transitions++;
		}
		else if( /* rising_edge == true && */ current_reading < last_reading - epsilon )
		{
			rising_edge = false;
			transitions++;
		}
 
		if( !already_warned && (transitions >> 28) & 0xF )
		{
			printf("Warning: Transition counter ~90%% to saturation, roll-over may occur\r\n");
			already_warned=1;
		}
 
		if ( current_reading > peak_reading ) 
			peak_reading = current_reading;
 
		if ( current_reading < trough_reading ) 
			trough_reading = current_reading;
 
		last_reading = current_reading;
 
		tick();
		current_tick++; // FIXME: Should be done by interrupt only!
	}
}
 
// vim: set ft=c ts=4 noet autoindent:

 /*
// Example for interrupt behaviour
 
void handleInt(void)
{
	// Remember that this function will be running in supervisor mode in an interrupt state
}
DigitalIn x(2);
x.attach(DigitalIn::RisingEdge, &handleInt);
x.attach(DigitalIn::FallingEdge, &handleInt);
// */

// HelloWorld example program
//  - flashes LED1 twice a second
 
#include "mbed.h"              
 
int main() {
    while(1) {
    	DigitalIn in_pin(33);
    	if(in_pin)
    	{
	        led1 = ~led1;   // toggle led1 by setting it to it's current value inverted
	        led2 = ~led2;   // toggle led1 by setting it to it's current value inverted
			wait(0.25);     // wait a quarter of a second    		
    	}
    }
}