#include "hardware.h"

// Initialise the hardware
void appInitHardware(void) {
	initHardware();
}
// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
	return 0;
}
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {

	// -------- Start Switch/Button-------
	// Switch/Button - see switch.h
	
	// To test if it is pressed then
	if(button.pressed()){
		// pressed
	}
	
	// To test if it is released then
	if(button.released()){
		// released
	}
	// -------- End   Switch/Button-------

	// -------- Start Marquee-------
	{		
		// A marquee is a 'Stream' so can use the print calls - 
		// but need to end with '\n' to start scrolling
	
		// If the endDelay is non-zero then the marquee will scroll
		// forever or until you call: marquee.stop();
		
		// If the endDelay is zero then the marquee will stop once
		// the entire line has been shown ('one-shot' mode)
		
		// In 'one-shot' mode then you may want to make sure that
		// a previous line has finished before you display a second line.
		// This can be done as follows:-
		marquee.setEndDelay(0); // Make sure we are in one-shot mode
		if(marquee.isActive()==FALSE){
		     if(loopCount==1){
		     	marquee.print_P(PSTR("ABCDEFGHIJKLMNOPQRSTUVWXYZ\n"));
		     }else{
				marquee.print_P(PSTR("Loop="));
				marquee.print(loopCount);
				marquee.println();
		     }
		}
			
	}
	// -------- End   Marquee-------

	// -------- Start LED-------
	// The LED can be manipulated using the calls in led.h
	
	// To turn the LED on:-
	led.on();
	
	// To turn the LED off:-
	led.off();
	// -------- End   LED-------

	// -------- Start Text to Speech-------
	// Say an English phrase
	speech.print("Death to all humans");
	// -------- End   Text to Speech-------

	// -------- Start PING sonar-------
	{
		// Read the PING sonar and store the results
		sonar.read();
		
		// Retrieve the value and print it out
		DISTANCE_TYPE cm = sonar.getDistance();
		PRINTF(stdout,"Distance=%u\n",cm);
			
		// or dump using:
		sonar.dump();
	}
	// -------- End   PING sonar-------

	// -------- Start Actuators -------
	// This example will move the motors back and forth using the loopStart time:
	TICK_COUNT ms = loopStart / 1000;		// Get current time in ms
	int16_t now = ms % (TICK_COUNT)10000; 	// 10 sec for a full swing
	if(now >= (int16_t)5000){				// Goes from 0ms...5000ms
		now = (int16_t)10000 - now;			// then 5000ms...0ms
	}
	// Map it into DRIVE_SPEED range
	DRIVE_SPEED speed = interpolate(now, 0, 5000, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);
	// Set speed for all motors/servos
	motor_left.setSpeed(speed);
	motor_right.setSpeed(speed);
	// -------- End   Actuators -------

	return 0;
}
