//DEMO #0, using two Motes to talk to each other wirelessly
//set one Mote to RX and the other to TX, by the direction of the on/off/on switch
//in this example the RX is echoing data out the USB
void demo_0(void)
	{
	//left mode is transmitter of data
	if(MODE==LEFT)
		{
		//now go to PLL mode
		rprintf("\nentering transmit mode\n");
		wireless_TX_MODE();
		
		while(1)
			{
			delay_ms(1000);
	
			//fill up the buffer with goodies
			//wireless_BUFFER_byte('a');//print a single char
			//wireless_BUFFER_byte(3);//print a single uint8
			//wireless_BUFFER_byte(your_variable_here);//print a single uint8
			wireless_BUFFER_string("hello world!");// %c",'a');//send an entire string

			//go into transmit mode to xmit the goodies
			wireless_TX_TRANSMIT();
			}
		}

	//right mode is reciever of data
	else if(MODE==RIGHT)
		{
		//now go to RX_on mode
		rprintf("\ngoing into RX mode\n");
		wireless_RX_MODE();

		while(1)
			{
			delay_ms(2000);

			//this is how to read from and print out data from the buffer
			if(wireless_crc_valid)//if good data is recieved
				{
				wireless_RX_PROTECT(1);//prevent data overwrite
				for(uint8_t i=0; i<wireless_recieved_frame[1]-2 ; i++)//read through entire frame
					rprintf("%c",wireless_recieved_frame[i]);
				wireless_BUFFER_CLEAN();//data has been read, so unflag it to remove from buffer
				wireless_RX_PROTECT(0);
				}

			//example output of useful connectivity info
			rprintf("\nRSSI:%d ED:%d RND:%d\n",wireless_RSSI_val,wireless_ED_measurement,wireless_random_number);

			//flash LEDs if there is a signal
			if(wireless_ED_measurement>0)
				LED_on(&Green_LED);
			else
				LED_off(&Green_LED);

			if(wireless_crc_valid)
				LED_on(&Red_LED);
			else
				LED_off(&Red_LED);

			//reset values
			wireless_crc_valid=false;
			wireless_ED_measurement=0;
			wireless_RSSI_val=0;
			}
		}
	}


//DEMO #1: Get sensor and timer data and print it out, or store to uSD card, when you push the button
void demo_1(void)
	{
	TICK_COUNT start_time = clockGetus();//get starting time

	while(1)
		{
		//if button is pushed
		if(SWITCH_pressed(&button) && MODE==LEFT)
			{
			LED_on(&Green_LED);

			// Read the Analogue Input and store results
			uint16_t adc1_Sharpval = a2dConvert10bit(adc1_Sharp);

			//determine time elapsed since Axon Mote turned on
			TICK_COUNT time_ms = (clockGetus()-start_time)/1000;// Get current time in ms

			// Dump out ADC value and time passed
			rprintf("adc1_Sharp: %d, time (ms): %d\n",adc1_Sharpval,time_ms);

			//record data to microSD card
			//IMPORTANT: see FAT.h in WebbotLib manual for instructions, and you must format card first!


			//debounce
			delay_ms(200);

			LED_off(&Green_LED);
			}
	

		//Get compass information, print data out, and store it on a microSD card
		//if button is pushed
		else if(SWITCH_pressed(&button) && MODE==RIGHT)
			{
			LED_on(&Red_LED);
	/*
			// -------- Start HMC6343-------
			// Read the HMC6343 and store results
			compassRead(compass);
			// The bearing is stored in compass.compass.bearingDegrees;
			// This can be printed using 
			rprintf("compass bearing=%d\n",compass.compass.bearingDegrees);
	
			// The HMC6343 also returns a roll and pitch and these are
			// stored in compass.compass.rollDegrees
			// and compass.pitchDegrees respectively
			// All three values can be dumped using
			rprintf("compass: ");
			compassDump(compass);
			rprintfCRLF();
			// -------- End   HMC6343-------
	*/

			#ifdef USE_uSD
				// -------- SDcard Code -------
				//if file doesn't exist, say so
				if(fileExists(&disk_sdCard,"/TEST.TXT")==0)
					rprintf("File doesn't already exist, creating /TEST.TXT\n"); // => PC

				//open file, and create if doesn't exist
				int8_t fileOpen_value;
				fileOpen_value = fileOpen(&disk_sdCard,&f,"/TEST.TXT",'a');

				//write to file
				if(fileOpen_value==0)
					{
					rprintf("Writing to /TEST.TXT\n");

					// Redirect output to the file
					Writer old = rprintfInit(fileGetWriter(&f));

					for(int line = 1; line < 100; line++)
						{
						rprintf("%d\n",line);
						//rprintf("degrees and pitch: %d, %d\n",compass.compass.bearingDegrees,compass.compass.pitchDegrees); // => file
						}

					//close and save file
					fileClose(&f);

					// Restore rprintf to go to its original location
					rprintfInit(old);
					rprintf("Done\n\n"); // => PC
					}
				else
					fileOpen_errors(fileOpen_value); //report error

				// -------- End SDcard Code -------
			#endif

			//debounce
			delay_ms(200);
			
			LED_off(&Red_LED);
			}
		}
	}


//DEMO #2: Make a servo wiggle around
void demo_2(void)
	{
	act_setSpeed(&servo1,DRIVE_SPEED_MIN);
	delay_ms(1000);
	act_setSpeed(&servo1,DRIVE_SPEED_MAX);
	delay_ms(1000);
	}

//DEMO #3: send data by USB to the Axon Mote to command it to control a servo
//just type in numbers into hyperterminal, ie 1,2,3 ...
//see demo_0 to do this wirelessly
void demo_3(void)
	{
	////////////GET COMMAND/////////////
	int8_t cByte=uartGetByte(USB);

	if(cByte!=-1)//has new data
		{
		if (cByte == '1')//servo reverse
			{
			act_setSpeed(&servo1,DRIVE_SPEED_MIN);
			LED_on(&Red_LED);
			}
		if (cByte == '2')//servo forward
			{
			act_setSpeed(&servo1,DRIVE_SPEED_MAX);
			LED_on(&Green_LED);
			}
		if (cByte == '3')//stop servo
			{
			act_setSpeed(&servo1,0);
			LED_off(&Red_LED);
			LED_off(&Green_LED);
			}
		}
	}


//DEMO #4: how to wirelessly reset an Axon Mote
//see this page for more info: http://www.nongnu.org/avr-libc/user-manual/group__avr__watchdog.html
//use must call disable_watchdog() if you want to disable it
//it will be automatically disabled upon RESET, as default
void demo_4(void)
	{
	
	//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\npush button to reset remote unit\n");
		wireless_TX_MODE();
		
		while(1)
			{
			delay_ms(150);//button debounce
	
			//if user pushes button
			if(SWITCH_pressed(&button))
				{
				wireless_BUFFER_byte(2);//send a reset command
				rprintf("\nreset command sent wirelessly");
				
				//go into transmit mode to xmit the command
				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)
			{
			//this is how to read from and print out data from the buffer
			wireless_RX_PROTECT(1);//prevent data overwrite
			//for(uint8_t i=0; i<wireless_recieved_frame[1]-2 ; i++)

			//print data if received
			rprintf("\n%d",wireless_recieved_frame[0]);

			//if special command recieved, initiate reset
			if(wireless_recieved_frame[0]==2)
				{
				rprintf("\nRESETTING");
				wdt_enable(WDTO_250MS);
				}

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

			//example output of useful connectivity info
			rprintf("\nED:%d\n",wireless_ED_measurement);

			delay_ms(2000);
			}
		}
	}

//how to write to two seperate uSD files
void demo_5(void)
	{
	if(SWITCH_pressed(&button) && MODE==RIGHT)
		{
		LED_on(&Red_LED);

		#ifdef USE_uSD
			// -------- SDcard Code -------
			//if file doesn't exist, say so
			if(fileExists(&disk_sdCard,"/TEST.TXT")==0)
				rprintf("File doesn't already exist, creating /TEST.TXT\n"); // => PC

			//open file, and create if doesn't exist
			int8_t fileOpen_value;
			fileOpen_value = fileOpen(&disk_sdCard,&f,"/TEST.TXT",'a');

			//write to file
			if(fileOpen_value==0)
				{
				rprintf("Writing to /TEST.TXT\n");

				// Redirect output to the file
				Writer old = rprintfInit(fileGetWriter(&f));

				for(int line = 1; line < 100; line++)
					{
					rprintf("%d\n",line);
					//rprintf("degrees and pitch: %d, %d\n",compass.compass.bearingDegrees,compass.compass.pitchDegrees); // => file
					}

				//close and save file
				fileClose(&f);

				// Restore rprintf to go to its original location
				rprintfInit(old);
				rprintf("Done\n\n"); // => PC
				}
			else
				fileOpen_errors(fileOpen_value); //report error

			// -------- End SDcard Code -------
		
			// -------- SDcard Code -------
			//if file doesn't exist, say so
			if(fileExists(&disk_sdCard,"/TEST2.TXT")==0)
				rprintf("File doesn't already exist, creating /TEST2.TXT\n"); // => PC

			//open file, and create if doesn't exist
			//int8_t fileOpen_value;
			fileOpen_value = fileOpen(&disk_sdCard,&g,"/TEST2.TXT",'a');

			//write to file
			if(fileOpen_value==0)
				{
				rprintf("Writing to /TEST2.TXT\n");

				// Redirect output to the file
				Writer old = rprintfInit(fileGetWriter(&g));

				for(int line = 1; line < 100; line++)
					{
					rprintf("%d\n",line);
					//rprintf("degrees and pitch: %d, %d\n",compass.compass.bearingDegrees,compass.compass.pitchDegrees); // => file
					}

				//close and save file
				fileClose(&g);

				// Restore rprintf to go to its original location
				rprintfInit(old);
				rprintf("Done\n\n"); // => PC
				}
			else
				fileOpen_errors(fileOpen_value); //report error

			// -------- End SDcard Code -------
		#endif

		//debounce
		delay_ms(200);
		
		LED_off(&Red_LED);
		}
	}


//example on how to make a remote controlled robot
void demo_6(void)
	{
	//right mode is transmitter of data, connects to computer
	if(MODE==LEFT)
		{
		LED_on(&Green_LED);
		LED_off(&Red_LED);

		//now go to PLL mode
		rprintf("\nentering transmit mode\npush button to reset remote unit\n");
		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, goes on robot
	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%u",wireless_recieved_frame[0]);
				}

			//if special number command recieved, do something
			if(wireless_recieved_frame[0]==1)
				{
				rprintf("\nturning right");
				//turn right
				act_setSpeed(&servo_left,DRIVE_SPEED_MAX);
				act_setSpeed(&servo_right,DRIVE_SPEED_MIN);
				}

			//if special letter command recieved, do something else
			if(wireless_recieved_frame[0]+'0'=='a')
				{
				rprintf("\nturning left");
				//turn left
				act_setSpeed(&servo_left,DRIVE_SPEED_MIN);
				act_setSpeed(&servo_right,DRIVE_SPEED_MAX);
				}

			//if got data, clear it for next command
			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)
			}
		}
	}