/* 
 * ****************************************************************************
 * Crawlee Firmware on RP6 ROBOT SYSTEM 
 * ****************************************************************************
 * Author(s): Christoph Schultz
 * Version 1.0 - 2013-09-12 
 * Copyright 2013
 * ****************************************************************************
 * Description:
 * TThe firmware shall allow to access all sensors and actuators via UART
 * ****************************************************************************
 */

/*****************************************************************************/
// Includes:

#include "RP6RobotBaseLib.h" 	// The RP6 Robot Base Library.
								// Always needs to be included!


// The behaviour command data type:
typedef struct {
	uint8_t  speed_left;  // left speed (is used for rotation and
						  // move distance commands - if these commands are
						  // active, speed_right is ignored!)
	uint8_t  speed_right; // right speed
	unsigned dir:2;       // direction (FWD, BWD, LEFT, RIGHT)
	unsigned move:1;      // move flag
	unsigned rotate:1;    // rotate flag
	uint16_t move_value;  // move value is used for distance and angle values
	uint8_t  state;       // state of the behaviour
} behaviour_command_t;

#define IDLE  0

behaviour_command_t mv_cmd = {0, 0, FWD, false, false, 0, IDLE};

#define P_BUFFLEN 16

char buffer[P_BUFFLEN];
uint8_t ind;

uint8_t acs_level;

int8_t speed;
int8_t offset;
uint8_t direction;

uint16_t movetimer;

void reportACSState(void)
{
		char bl,br;

		writeString_P(">S");
		switch(acs_level)
		{
			case 0:
				writeChar('0');
				break;
			case 1:
				writeChar('1');
				break;
			case 2:
				writeChar('2');
				break;
			case 3:
				writeChar('3');
				break;
		}
		writeChar('\n');

		if(obstacle_left)
			bl = '1';
		else
			bl = '0';

		if(obstacle_right)
			br = '1';
		else
			br = '0';

		writeString_P(">L");
		writeChar(bl);
		writeChar('\n');

		writeString_P(">R");
		writeChar(br);
		writeChar('\n');
}

void reportBatteryState(void)
{

	writeString_P(">");
	writeInteger(adcBat,DEC);
	writeChar('\n');
}

void reportEngineState(void)
{

	writeString_P(">L");
	writeInteger(adcMotorCurrentLeft,DEC);
	writeChar('\n');

	writeString_P(">R");
	writeInteger(adcMotorCurrentRight,DEC);
	writeChar('\n');
}

void reportLightState(void)
{
	uint8_t ind;
	char temp[16];

	for(int i = 0; i < 16; i ++)
	{
		temp[i] = '\0';
	}
	snprintf(temp,16,"%d",adcLSL);

	writeString_P(">L");
	ind = 0;
	while(ind < 16 && temp[ind] != '\0')
	{
		writeChar(temp[ind]);
		ind ++;
	}
	writeChar('\n');

	for(int i = 0; i < 16; i ++)
	{
		temp[i] = '\0';
	}
	snprintf(temp,16,"%d",adcLSR);

	writeString_P(">R");
	ind = 0;
	while(ind < 16 && temp[ind] != '\0')
	{
		writeChar(temp[ind]);
		ind ++;
	}
	writeChar('\n');

}

void reportBumperState(void)
{
	char bl,br;

	if(bumper_left)
		bl = '1';
	else
		bl = '0';

	if(bumper_right)
		br = '1';
	else
		br = '0';

	writeString_P(">L");
	writeChar(bl);
	writeChar('\n');

	writeString_P(">R");
	writeChar(br);
	writeChar('\n');
}

/*****************************************************************************/
// Parser
void parseSpeedCommand(const char* cmd)
{
	int8_t spTmp;
	spTmp = atoi(cmd);
	if(spTmp < 0)
	{
		spTmp = -spTmp;
		direction = BWD;
	}
	else
	{
		direction = FWD;
	}
	speed = spTmp;
	if(offset > speed)
		offset = speed;
	if(offset < -speed)
		offset = -speed;
	offset = spTmp;
	writeString_P(">");
	writeInteger(spTmp, DEC);
	writeChar('\n');
}
void parseOffsetCommand(const char* cmd)
{
	int8_t spTmp;
	spTmp = atoi(cmd);
	if(spTmp > speed)
		spTmp = speed;
	if(spTmp < -((int8_t)speed))
		spTmp = -((int8_t)speed);
	offset = spTmp;
	writeString_P(">");
	writeInteger(spTmp, DEC);
	writeChar('\n');
}

void parseACSCommand(const char* cmd)
{
	switch(cmd[0])
	{
		case '+':
			if(acs_level < 3)
				acs_level ++;
			break;
		case '-':
			if(acs_level > 0)
				acs_level --;
			break;
		case '0':
			acs_level = 0;
			break;
		case '1':
			acs_level = 1;
			break;
		case '2':
			acs_level = 2;
			break;
		case '3':
			acs_level = 3;
			break;
		default:
			reportACSState();
			break;
	}
}

void parseMoveTimedCommand(const char* buffer)
{
	movetimer = 10;
	if(buffer[0] != '\0')
	{
		movetimer = atoi(&buffer[0]);
	}
	writeString_P(">");
	writeInteger(movetimer,DEC);

	mv_cmd.dir = direction;
	mv_cmd.speed_left = speed+offset;
	mv_cmd.speed_right = speed-offset;
	mv_cmd.move = 0;
	mv_cmd.rotate = 0;
	mv_cmd.move_value = 0;

	setStopwatch6(0);
}

void parseCommands(void)
{
	uint16_t dist;
	//Loop input
	for(int i = 0; i < ind; i ++)
	{
		writeChar(buffer[i]);
	}

	switch(buffer[0])
	{
		case 'c':
		case 'C':
			writeString_P(" - move timed\n");
			parseMoveTimedCommand((const char*)&buffer[1]);
			break;
		case 'o':
		case 'O':
			writeString_P(" - set offset\n");
			parseOffsetCommand((const char*)&buffer[1]);
			break;
		case 'q':
		case 'Q':
			writeString_P(" - set speed\n");
			parseSpeedCommand((const char*)&buffer[1]);
			break;
		case 'd':
		case 'D':
				dist = 15;
				writeString_P(" - rotate right\n");
				if(buffer[1] != '\0')
				{
					dist = atoi(&buffer[1]);
				}
				mv_cmd.dir = RIGHT;
				mv_cmd.speed_left = speed;
				mv_cmd.speed_right = speed;
				mv_cmd.move = 0;
				mv_cmd.rotate = 1;
				mv_cmd.move_value = dist;
				break;
		case 'a':
		case 'A':
			dist = 15;
			writeString_P(" - rotate left\n");
			if(buffer[1] != '\0')
			{
				dist = atoi(&buffer[1]);
			}
			mv_cmd.dir = LEFT;
			mv_cmd.speed_left = speed;
			mv_cmd.speed_right = speed;
			mv_cmd.move = 0;
			mv_cmd.rotate = 1;
			mv_cmd.move_value = dist;
			break;
		case 's':
		case 'S':
			dist = 150;
			writeString_P(" - move backward\n");
			if(buffer[1] != '\0')
			{
				dist = atoi(&buffer[1]);
			}
			mv_cmd.dir = BWD;
			mv_cmd.speed_left = speed;
			mv_cmd.speed_right = speed;
			mv_cmd.move = 1;
			mv_cmd.rotate = 0;
			mv_cmd.move_value = dist;
			break;
		case 'w':
		case 'W':
			dist = 150;
			writeString_P(" - move forward\n");
			if(buffer[1] != '\0')
			{
				dist = atoi(&buffer[1]);
			}
			mv_cmd.dir = FWD;
			mv_cmd.speed_left = speed;
			mv_cmd.speed_right = speed;
			mv_cmd.move = 1;
			mv_cmd.rotate = 0;
			mv_cmd.move_value = dist;
			break;
		case 'i':
		case 'I':
			writeString_P(" - obstacle sensor status/ctrl\n");
			parseACSCommand((const char*)&buffer[1]);
			break;
		case 'l':
		case 'L':
			writeString_P(" - Report light sensor status\n");
			//No parameters - nothing to parse
			reportLightState();
			break;
		case 'V':
		case 'v':
			writeString_P(" - Report VDD status");
			writeChar('\n');
			//No parameters - nothing to parse
			reportBatteryState();
			break;
		case 'e':
		case 'E':
			writeString_P(" - Engine status");
			writeChar('\n');
			//No parameters - nothing to parse
			reportEngineState();
			break;
		case 'b':
		case 'B':
			writeString_P(" - Report bumper status");
			writeChar('\n');
			//No parameters - nothing to parse
			reportBumperState();
			break;
		default:
			writeChar('\n');
			writeString_P("!E0 - Unknown command!\n");
			break;
	}
}

void clearBuffer(void)
{
	ind = 0;
	for(int i = 1; i < P_BUFFLEN; i ++)
	{
		buffer[i] = '\0';
	}
}

void checkCommands(void)
{
	while(getBufferLength())    // Check if we have data
	{
			//Read character
			buffer[ind] = readChar(); // get next character from reception buffer
			//Check if newline has been detected
			if(buffer[ind] == '\n')
			{
				buffer[ind] = '\0';
				parseCommands();
				clearBuffer();
			}
			else if(buffer[ind] == '\r') //Remove \r; otherwise increase index
			{
				buffer[ind] = '\0';
			}
			else if(ind < P_BUFFLEN - 1) //If buffer has still capacity increase index, last char has to remain '\0'
			{
				ind ++;
			}
			else
			{
				writeString_P("!E1 - Buffer length exceeded!\n");
				//Clear all buffers
				clearBuffer();
				clearReceptionBuffer();
			}
	}
}

void updateACS(void)
{
	static uint8_t old_acs = 255;

	if(acs_level == old_acs)
		return;
	else
		old_acs = acs_level;

	if(acs_level == 1)
	{
		setACSPwrLow();
	}
	else if(acs_level == 2)
	{
		setACSPwrMed();
	}
	else if(acs_level == 3)
	{
		setACSPwrHigh();
	}
	else
	{
		setACSPwrOff();
	}
}

void acsStateChanged(void)
{

		if(obstacle_left && obstacle_right)
			statusLEDs.byte = 0b100100;
		else
			statusLEDs.byte = 0b000000;
		statusLEDs.LED5 = obstacle_left;
		statusLEDs.LED4 = (!obstacle_left);
		statusLEDs.LED2 = obstacle_right;
		statusLEDs.LED1 = (!obstacle_right);
		updateStatusLEDs();
}

/**
 * This function processes the movement commands that the behaviours generate.
 * Depending on the values in the behaviour_command_t struct, it sets motor
 * speed, moves a given distance or rotates.
 */
void moveCommand(behaviour_command_t * cmd)
{
	if(cmd->move_value > 0)
	{
		if(cmd->rotate)
			rotate(cmd->speed_left, cmd->dir, cmd->move_value, false);
		else if(cmd->move)
			move(cmd->speed_left, cmd->dir, DIST_MM(cmd->move_value), false);
		cmd->move_value = 0;
	}
	else if(!(cmd->move || cmd->rotate))
	{
		changeDirection(cmd->dir);

		if(getStopwatch6()<=movetimer)
		{
			moveAtSpeed(cmd->speed_left,cmd->speed_right);
			startStopwatch6();
		}
		else
		{
			moveAtSpeed(0,0);
			stopStopwatch6();
		}
	}
	else if(isMovementComplete())
	{
		cmd->rotate = false;
		cmd->move = false;
		cmd->speed_left = 0;
		cmd->speed_right = 0;
	}
}


/*****************************************************************************/
// Main function - The program starts here:

int main(void)
{
	initRobotBase(); // Always call this first! The Processor will not work
					 // correctly otherwise.

	setLEDs(0b111111); // Turn all LEDs on
	mSleep(500);       // delay 500ms
	setLEDs(0b000000); // All LEDs off

	// Write a message to UART:
	writeString_P("\nHello! My name is Crawlee #0 (b9) 2013-11-04!\n\n");
	
	clearBuffer();
	clearReceptionBuffer();

	acs_level = 2;
	ACS_setStateChangedHandler(acsStateChanged);

	speed = 80;
	offset = 0;
	direction = FWD;

	powerON(); // Turn on Encoders, Current sensing, ACS and Power LED.

	enableACS();

	setACSPwrMed();

	setLEDs(0b001001);

	// ---------------------------------------
	// Main loop:
	while(true)
	{
		//Check commands
		checkCommands();

		//Update ACS
		updateACS();

		//Execute move command
		moveCommand(&mv_cmd);

		//Perform system tasks and update data-fields
		task_RP6System();

		//Sleep to save power
		//-ATTENTION:ACS Function does not work well with this mSleep command!
		//-It has been therefore deactivated in b8 - CS
		//mSleep(5);
	}
	// End of main loop
	// ---------------------------------------

	return 0;
}

/*****************************************************************************/
// EOF