/* 
 * ****************************************************************************
 * RP6 ROBOT SYSTEM - ROBOT BASE EXAMPLES
 * ****************************************************************************
 * Example: SELFTEST PROGRAM
 * Author(s): Dominik S. Herwald
 * ****************************************************************************
 * Description:
 *
 * Test routines for all RP6 components. 
 *
 * ----------------------------------------------------------------------
 * UPDATE 25.09.2007 - The input routines have been changed to work with
 * new RP6 UART Library functions. Now the User input for the advanced
 * speed tests are much easier and there are some additional features. 
 * Please look at the text instructions you see before the tests start. 
 * ----------------------------------------------------------------------
 * 
 * You can compile this in two different versions. The FACTORY Version is
 * more restrictive with the voltage sensor and tests if voltage is about 
 * 7.20V. 
 * You can not guarantee this with standard accumulators of course and thus
 * you would need a laboratory power supply - which most of the home users
 * will not have. 
 *
 * --------
 *
 * Yes I know - this program has not the most beautyful code on earth... 
 * To be honest it is quite ugly ;) 
 * All the Text output is just a waste of programspace and this is 
 * intentionally to get a large program ;) 
 *
 * It is a good demonstration how big your programs can get! (and this
 * program only fills 21KB of the 30KB)
 * Consider that every single character of a String consumes one byte 
 * of program space!
 * Also look at it in the hexfile viewer in RP6Loader - nearly half the 
 * program memory is full with ASCII Text.
 *  
 *
 * ****************************************************************************
 */
 
 
/*****************************************************************************/
// Includes:

#include "RP6RobotBaseLib.h" 	

/*****************************************************************************/

// Uncomment only ONE of the following definitions:
//#define FACTORY 
#define HOME

/*****************************************************************************/

void done(void)
{
	writeString_P("Done!\n"); 
}

void bars(uint8_t number)
{
	uint8_t cnt;
	writeChar('\n');
	for(;number > 0; writeChar('\n'), number--)
		for(cnt = 69; cnt ; cnt--)
			writeChar('#');
}

void test(uint8_t number)
{
	bars(2);
	writeString_P("#### TEST #");
	writeInteger(number, DEC);
	writeString_P(" ####\n");
}

void printUBat(uint16_t uBat)
{
	writeIntegerLength((((uBat/102.4f)+0.1f)), DEC, 2);
	writeChar('.');
	writeIntegerLength((((uBat/1.024f)+10)), DEC, 2);
	writeChar('V');
}

char receiveBuffer[UART_RECEIVE_BUFFER_SIZE];

uint8_t getInputLine(void)
{ 
	static uint8_t buffer_pos = 0;
	if(getBufferLength()) 
	{							
		receiveBuffer[buffer_pos] = readChar();
		if(receiveBuffer[buffer_pos]=='\n')
		{
			receiveBuffer[buffer_pos]='\0'; 
			buffer_pos = 0;
			return 1;
		}
		else if(buffer_pos >= 32)
		{									
			receiveBuffer[32]='\0';	
			buffer_pos = 0;
			return 2;
		}
		buffer_pos++;
	}
	return 0;
}

void enterString(void)
{
	while(!getInputLine());
}

uint8_t enterX(void)
{
	enterString();
	return receiveBuffer[0]=='x' || receiveBuffer[0]=='X';
}

/*****************************************************************************/
//

void testPowerOn(void)
{
	test(1);
	writeString_P("\n### POWER ON TEST ###\n");
	writeString_P("Please watch the yellow PowerOn LED and verify that it lights up!\n");
	writeString_P("(it will flash a few times!)\n");
	mSleep(1000);
	powerON();
	mSleep(1000);
	powerOFF();
	mSleep(1000);
	powerON();
	mSleep(2000);
	powerOFF();
	mSleep(500);
	powerON();
	mSleep(500);
	powerOFF();
	done();
}

/*****************************************************************************/
//

void testLEDs(void)
{
	test(2);
	writeString_P("\n### LED Test ###\n");
	writeString_P("Please watch the LEDs and verify that they all work!\n");
	mSleep(1000);
	setLEDs(0b111111);
	mSleep(500);
	setLEDs(0b000000);
	mSleep(500);
	setLEDs(0b111111);
	mSleep(500);
	setLEDs(0b000000);
	mSleep(250);
	uint8_t testLEDs = 1;
	uint8_t i;
	for(i = 0; i < 12; i++){
		setLEDs(testLEDs); 
		testLEDs <<= 1; 
		if(testLEDs > 63)
			testLEDs = 1; 	
		mSleep(400);
	}
	setLEDs(0b111111);
	mSleep(400);
	setLEDs(0b000000);
	done();
}

/*****************************************************************************/
//

void testVoltageSensor(void)
{
	test(3);
	writeString_P("\n### Voltage Sensor Test ###\n");
#ifdef FACTORY
	writeString_P("Please adjust the **laboratory power supply** to 7.20V!\n");
#endif
#ifdef HOME
	writeString_P("Be sure that you are using good accumulators!\n");
	writeString_P("\n### Enter \"x\" and hit return to START this test!\n\n");
	enterX();
    //receiveBytesToBuffer(1, &receiveBuffer[0]);
#endif
	writeString_P("Performing 10 measurements:\n");
	uint16_t ubat;
	uint8_t i;
	for(i = 1; i <= 10; i++)
	{
		writeString_P("Measurement #"); 
		writeInteger(i, DEC);
		writeString_P(": ");
		ubat = readADC(ADC_BAT);
		printUBat(ubat);
#ifdef FACTORY
		if(ubat >= 700 && ubat <= 770)
		{
			writeString_P(" --> OK!\n");
		}
		else 
		{
			writeString_P("\n####### ERROR: VOLTAGE IS TOO ");
			if(ubat < 700) 
			{
				writeString_P("LOW");
			}
			else if(ubat > 770)
			{
				writeString_P("HIGH");
			}
			writeString_P(" (should be min. 6.90V to max. 7.60V)!\n");
		}
#endif
#ifdef HOME
		if(ubat >= 570 && ubat <= 970)
		{
			writeString_P(" --> OK!\n");
		}
		else 
		{
			writeString_P("\nWARNING: VOLTAGE IS TOO ");
			if(ubat < 570) 
			{
				writeString_P("LOW");
			}
			else if(ubat > 970)
			{
				writeString_P("HIGH");
			}
			writeString_P(" (should be min. 5.50V to max. 9.50V)!\n");
		}
#endif
		mSleep(200);
	}
	done();
}

/*****************************************************************************/
//

void testBumpers(void)
{
	test(4);
	writeString_P("\nBumper Test\n");
	writeString_P("Please hit both bumpers and verify \n");
	writeString_P("that both Bumpers are working properly!\n\n");
	writeString_P("### The Test is running now.\n### Enter \"x\" and hit return to STOP this test!\n\n");
	clearReceptionBuffer();
	while(true)
	{
		if(getInputLine())
			break;
		
		static uint8_t info_left = false;
		static uint8_t info_right = false;
		uint8_t bumper_left = getBumperLeft();
		uint8_t bumper_right = getBumperRight();
		
		if(bumper_left) 
		{
			if(!info_left) 
			{			 
				writeString_P("OBSTACLE: LEFT!\n");
				info_left = true;
			}
		}
		else if(info_left) 
		{
			writeString_P("FREE: LEFT!\n");
			info_left = false;
		}
		if(bumper_right) 
		{
			if(!info_right) 
			{
				writeString_P("OBSTACLE: RIGHT!\n");
				info_right = true;
			}
		}
		else if(info_right) 
		{
			writeString_P("FREE: RIGHT!\n");
			info_right = false;
		}
	    statusLEDs.LED4 = (!bumper_left);
		statusLEDs.LED1 = (!bumper_right);
		updateStatusLEDs();
		mSleep(50);
	}
	setLEDs(0b000000);
	done();
}

/*****************************************************************************/
// 

void testLightSensors(void)
{
	test(5);
	writeString_P("\n### Light Sensor Test ###\n");
	writeString_P("Please get yourself a small flashlight!\n");
	writeString_P("While the test runs, move it in front of the Robot\n");
	writeString_P("and watch if the values change accordingly!\n\n");
#ifdef HOME
	writeString_P("### Enter \"x\" and hit return when you are ready!\n");
	enterX();
#endif
	writeString_P("### The Test is running now.\n### Enter \"x\" and hit return to STOP this test!\n\n");
	mSleep(1000);
	startStopwatch1();
	startStopwatch2();
	writeString_P("Performing measurements...:\n");
	uint16_t sens_l;
	uint16_t sens_r;
	clearReceptionBuffer();
	while(true)
	{
		if(getInputLine())
			break;
		
		if(getStopwatch1() > 50) 
		{		
			sens_l = readADC(ADC_LS_L);
			sens_r = readADC(ADC_LS_R);
			
			int16_t dif = ((int16_t)(sens_l - sens_r));
			if(dif > 160) dif = 160;
			if(dif < -160) dif = -160;
	
			// Set LEDs - a small bargraph display that shows which sensor has detected
			// brighter light - and how much brighter it is:
			if(dif > 140)
				setLEDs(0b111000);
			else if(dif > 75)
				setLEDs(0b011000);
			else if(dif > 35)
				setLEDs(0b001000);
			else if(dif > -35)
				setLEDs(0b001001);
			else if(dif > -75)
				setLEDs(0b000001);
			else if(dif > -140)
				setLEDs(0b000011);
			else 
				setLEDs(0b000111);
			setStopwatch1(0);
		}
		
		if(getStopwatch2() > 250) 
		{	
			sens_l = readADC(ADC_LS_L);
			sens_r = readADC(ADC_LS_R);

			writeString_P("Left: ");
			writeIntegerLength(sens_l, DEC, 4);
			writeString_P(",  Right: ");
			writeIntegerLength(sens_r, DEC, 4);
			writeChar('\n');
			setStopwatch2(0);
		}
	}
	stopStopwatch1();
	stopStopwatch2();
	done();
} 

/*****************************************************************************/
//

uint8_t RC5_data;
uint8_t RC5_error;
uint8_t RC5_test;

void receiveRC5Data(RC5data_t rc5data)
{
	writeString_P("RX RC5 Packet --> Toggle Bit:");
	writeChar(rc5data.toggle_bit + '0');
	writeString_P(" | Device Address:");
	writeInteger(rc5data.device, DEC);
	writeString_P(" | Key Code:");
	writeInteger(rc5data.key_code, DEC);
	
	if(RC5_test==2)
	{
		writeString_P(" --> ");
		if(rc5data.key_code == RC5_data 
			&& rc5data.device == (RC5_data & 0b11111) 
			&& rc5data.toggle_bit == ((RC5_data>>5)&1))
		{
			writeString_P("OK!");
			RC5_error = false;
			RC5_test = 10;
		}
		else
		{
			writeString_P("### ERROR!!! ###");
			RC5_error = true;
			RC5_test = 5;
		}
	}
	writeChar('\n');
}


void testACS(void)
{
	test(6);
	writeString_P("\nACS Test\n");
	writeString_P("Please move your hand or other obstacles in front of the Robot\n");
	writeString_P("and verify that both ACS channels are working properly!\n\n");
	writeString_P("ACS is set to MAXIMUM power/range!\n\n");
	
	writeString_P("You can also send RC5 Codes with a TV Remote Control\n");
	writeString_P("to the RP6 - it will display the Toggle Bit, Device Adress\n");
	writeString_P("and Keycode of the RC5 Transmission!\n");
	writeString_P("Make sure your remote control transmits in RC5 and not\n");
	writeString_P("SIRCS or RECS80 etc.! There are several other formats that will NOT work!\n\n");
	writeString_P("### The Test is running now.\n### Enter \"x\" and hit return to STOP this test!\n\n");

	powerON();
	setACSPwrHigh();

	clearReceptionBuffer();
	while(true)