#include "hardware.h"
#include "Axon_Mote_utils.h"

boolean scanning=0;

// Initialise the hardware
void appInitHardware(void) {
	initHardware();
}
// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
	
	delay_ms(1000);

	//determine selected mode
	if(SWITCH_pressed(&Switch_Left))
		{
		LED_on(&Green_LED);//do something
		MODE=LEFT;
		}
	if(SWITCH_pressed(&Switch_Right))
		{
		LED_on(&Red_LED);//do something
		MODE=RIGHT;
		}
	
	//initiate wireless
	wireless_reset();

	//set channel, default is 17
	wireless_SetChannel(CHANNEL);

	//declare Axon Mote initiated
	rprintf("\n\nAxon Mote initiated.\nTransmit on Channel %d.\n\n",wireless_ReadChannel());

	return 0;
}


void turn_left(void)
	{
	act_setSpeed(&servo_left,-126);
	act_setSpeed(&servo_right,126);
	}
void turn_right(void)
	{
	act_setSpeed(&servo_left,126);
	act_setSpeed(&servo_right,-126);
	}
void go_straight(void)
	{
	act_setSpeed(&servo_left,126);
	act_setSpeed(&servo_right,126);
	}
void reverse(void)
	{
	act_setSpeed(&servo_left,-126);
	act_setSpeed(&servo_right,-126);
	}
void stop(void)
	{
	act_setSpeed(&servo_left,0);
	act_setSpeed(&servo_right,0);
	}

// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {
	
	//right mode is transmitter of data, push button to reset wireless Mote
	if(MODE==LEFT)
		{
		LED_on(&Green_LED);
		LED_off(&Red_LED);

		//now go to PLL mode
		rprintf("\nentering transmit mode\ntype to send commands");
		wireless_TX_MODE();
		
		while(1)
			{			
			uint8_t cByte=uartGetByte(USB)-'0';

			if(cByte!=207)//has new data
				{
				//buffer data
				wireless_BUFFER_byte(cByte);
				//rprintf("\n%u",cByte);
				delay_ms(10);//give it time to buffer data
				//transmit data
				wireless_TX_TRANSMIT();
				}
			}
		}

	//right mode is reciever of data, it gets reset
	else if(MODE==RIGHT)
		{
		LED_on(&Red_LED);
		LED_off(&Green_LED);

		//go to RX_on mode
		rprintf("\ngoing into RX mode");
		wireless_RX_MODE();

		while(1)
			{
			//prevent data overwrite, if recieved data
			if(wireless_recieved_frame[0] > 0)
				{
				wireless_RX_PROTECT(1);

				//print data if received
				rprintf("\n%d or %c",wireless_recieved_frame[0],wireless_recieved_frame[0]+'0');
				}

			//go left
			if(wireless_recieved_frame[0]+'0'=='a')
				{
				rprintf("\nturning left");

				turn_left();
				}

			//go right
			if(wireless_recieved_frame[0]+'0'=='d')
				{
				rprintf("\nturning right");

				turn_right();
				}

			//go straight
			if(wireless_recieved_frame[0]+'0'=='w')
				{
				rprintf("\ngoing straight");
				
				go_straight();
				}

			//reverse
			if(wireless_recieved_frame[0]+'0'=='x')
				{
				rprintf("\nreversing");
				
				reverse();
				}

			//stop
			if(wireless_recieved_frame[0]+'0'=='s')
				{
				rprintf("\nstop");
				
				stop();
				}				

			//if got data
			if(wireless_recieved_frame[0] > 0)
				{
				wireless_BUFFER_CLEAN();//data has been read, so unflag it to remove from buffer
				wireless_RX_PROTECT(0);//remove data protection
				wireless_recieved_frame[0]=0;//erase previous data
				}

			delay_ms(10);//give it time to clear flags (doesn't always work without a delay here)
			}
		}

return 0;
}