/* Kernel includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"

#include "xparameters.h"	/* XPAR parameters */
#include "xuartps.h"		/* UART Driver */
#include <math.h>

#include "vehicle.h"
#include "lidarControl.h"
#include "motor.h"

static XUartPs uartController;
static XUartPs computerCom;

static u8 RECBuffer[66];
static u8 SCANdata[682][2];
static u8 GRID[GRID_SIZE][GRID_SIZE];
static float SIN[900];
static float COS[900];
static float STEPangle[682];
static Robot VEHICLE;

void main_av(void){
//	int i = 0;
	motorFun();
/*	initUART();
	initTrigLookUpTables();
	initGrid();
	sendLIDARcmd(BM_CMD, 4);
	recvLIDARdata();
*/

	for(;;){
		requestDistanceData();
		updateGrid();
		//i = printGrid();
		//sendLIDARcmd(QT_CMD, 4);
	}

}

void initUART(){
	XUartPs_Config *uartConfig;

	uartConfig = XUartPs_LookupConfig(XPAR_PS7_UART_0_DEVICE_ID);
	XUartPs_CfgInitialize(&uartController, uartConfig, uartConfig->BaseAddress);
	XUartPs_SetBaudRate(&uartController, 115200);

	uartConfig = XUartPs_LookupConfig(XPAR_PS7_UART_1_DEVICE_ID);
	XUartPs_CfgInitialize(&computerCom, uartConfig, uartConfig->BaseAddress);
	XUartPs_SetBaudRate(&computerCom, 115200);

}

void initGrid(){
	int i = 0;
	int j = 0;

	VEHICLE.x = GRID_SIZE / 2;
	VEHICLE.y = GRID_SIZE / 2;
	VEHICLE.angle = 90;

	for(i = 0; i < GRID_SIZE; ++i){
		for(j = 0; j < GRID_SIZE; ++j){
			if((i == 0) || (i == GRID_SIZE - 1) || (j == 0) || (j == GRID_SIZE - 1)){
				GRID[i][j] = OBSTACLE;
			}
			else{
				GRID[i][j] = CLEAR;
			}
		}
	}
	GRID[VEHICLE.x][VEHICLE.y] = 'X';

	return;
}

void initTrigLookUpTables(){
	int i = 0;
	for(i = 0; i < 900; ++i){
		SIN[i] = sin((i/10.0)  * (PI / 180.0));
		COS[i] = cos((i/10.0)  * (PI / 180.0));
		if (i >= 682){
			continue;
		}
		STEPangle[i] = i * STEP_VALUE;
	}
}

int sendLIDARcmd(u8 *command, int size){
	int sentCount = 0;

	sentCount += XUartPs_Send(&uartController, &command[0], size);

	while (sentCount < size){
		sentCount += XUartPs_Send(&uartController, &command[sentCount], 1);
	}
	return sentCount;
}

int recvLIDARdata(){
	int recvCount = 0;

	while(recvCount < MAX_INPUT){
		recvCount += XUartPs_Recv(&uartController, &RECBuffer[recvCount], 65);
		if (recvCount < 2){
			continue;
		}
		XUartPs_SetOptions(&uartController, XUARTPS_OPTION_RESET_TX);
		if ((RECBuffer[recvCount - 2] == '\n') && (RECBuffer[recvCount - 1] == '\n')){
			return recvCount;
		}
	}
	return recvCount;
}

int parseLIDARinput(int index){
	u8 commandEcho[20];
	u8 status[10];
	u8 timeStamp[10];
	int i = 0;
	int bufferIndex = 0;

	for(i = 0; i < 15; ++i){
		commandEcho[i] = RECBuffer[bufferIndex];
		++bufferIndex;
		if (RECBuffer[bufferIndex - 1] == '\n'){
			break;
		}
	}
	for(i = 0; i < 10; ++i){
		status[i] = RECBuffer[bufferIndex];
		++bufferIndex;
		if (RECBuffer[bufferIndex - 1] == '\n'){
			break;
		}
	}
	for(i = 0; i < 10; ++i){
		timeStamp[i] = RECBuffer[bufferIndex];
		++bufferIndex;
		if (RECBuffer[bufferIndex - 1] == '\n'){
			break;
		}
	}

	for (; bufferIndex < 65; bufferIndex += 2){
		if ((RECBuffer[bufferIndex] == '\n') || (RECBuffer[bufferIndex + 1] == '\n')){
			return index;
		}
		SCANdata[index][0] = RECBuffer[bufferIndex];
		SCANdata[index][1] = RECBuffer[bufferIndex + 1];
		++index;
	}
	return index;
}

void requestDistanceData(){
	int index = 0;

	sendLIDARcmd((u8*)"GS0044006201\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0063008101\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0082010001\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0101011901\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0120013801\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0139015701\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0158017601\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0177019501\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0196021401\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0215023301\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0234025201\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0253027101\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0272029001\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0291030901\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0310032801\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0329034701\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0348036601\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0367038501\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0386040401\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0405042301\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0424044201\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0443046101\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0462048001\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0481049901\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0500051801\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0519053701\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0538055601\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0557057501\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0576059401\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0595061301\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0614063201\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0633065101\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0652067001\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0671068901\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0690070801\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);

	sendLIDARcmd((u8*)"GS0709072501\n", 13);
	recvLIDARdata();
	index = parseLIDARinput(index);
	return;
}
int decode(u8 *code, int byte){
  int value = 0;
  int i;
  for (i = 0; i < byte; ++i) {
    value <<= 6;
    value &= ~0x3f;
    value |= code[i] - 0x30;
  }
  return value;
}

float calcCos(float angle){
	if(angle <= 90){
		return COS[(int)angle * 10];
	}
	else if(angle <= 180){
		return -COS[(int)(180 - angle) * 10];
	}
	else if(angle <= 270){
		return -COS[(int)(270 - angle) * 10];
	}
	else if(angle <= 360){
		return COS[(int)(360 - angle) * 10];
	}
	return 0;
}
float calcSin(float angle){
	if(angle <= 90){
		return SIN[(int)angle * 10];
	}
	else if(angle <= 180){
		return SIN[(int)(180 - angle) * 10];
	}
	else if(angle <= 270){
		return -SIN[(int)(270 - angle) * 10];
	}
	else if(angle <= 360){
		return -SIN[(int)(360 - angle) * 10];
	}
	return 0;
}
void updateGrid(){
	int i = 0;
	int distance = 0;
	int xCol = 0;
	int yCol = 0;
	float angle = 0;
	float angleShift = 0;
	float value = 0.0;

	angleShift = STEPangle[341] - VEHICLE.angle;
	for(i = 0; i < 682; ++i){
		distance = decode(SCANdata[i], 2);
		if(distance > 4000){
			continue;
		}
		angle = STEPangle[i] - angleShift;
		value = calcCos(angle);
		xCol = VEHICLE.x + (int)(((float)distance * value) / (float)GRID_SCALE);
		value = calcSin(angle);
		yCol = VEHICLE.y + (int)(((float)distance * value) / (float)GRID_SCALE);
		if ((xCol >= GRID_SIZE) || (xCol < 0)){
			continue;
		}
		if ((yCol >= GRID_SIZE) || (yCol < 0)){
			continue;
		}
		GRID[xCol][yCol] = OBSTACLE;
	}

}


int printGrid(){
	int sentCount = 0;
	int i, j = 0;
	u8 lineFeed[2];
	lineFeed[0] = '\r';
	lineFeed[1] = '\n';

	for(i = GRID_SIZE; i > 0; --i){
		for(j = 0; j < GRID_SIZE; ++j){
			sentCount += XUartPs_Send(&computerCom, &GRID[j][i], 1);
		}
		sentCount += XUartPs_Send(&computerCom, lineFeed, 2);
	}

	return sentCount;


}