/***********************************************************************//**
 * @file		SerialCOM.c
 * @brief		Contains all functions for Serial communication
 * @version		1.0
 * @date		19. March 2014
 * @author		Michal Semrad
 **************************************************************************/
#include "SerialCOM.h"

/************************** PRIVATE VARIABLES *************************/

/************************** PRIVATE FUNCTIONS *************************/

/*----------------- INTERRUPT SERVICE ROUTINES --------------------------*/
/*********************************************************************//**
 * @brief	UART3 interrupt handler sub-routine, stores received bytes to xSerial receive queue
 * @param	None
 * @return	None
 **********************************************************************/

void UART3_IRQHandler(void) {
	uint32_t intsrc, tmp, tmp1;
	uint8_t data;

	//LEDTurnOn();
	/* Determine the interrupt source */
	intsrc = UART_GetIntId(LPC_UARTx);
	tmp = intsrc & UART_IIR_INTID_MASK;

	// Receive Line Status
	if (tmp == UART_IIR_INTID_RLS) {
		// Check line status
		tmp1 = UART_GetLineStatus(LPC_UARTx);
		// Mask out the Receive Ready and Transmit Holding empty status
		tmp1 &= (UART_LSR_OE | UART_LSR_PE | UART_LSR_FE | UART_LSR_BI
				| UART_LSR_RXFE);
		// If any error exist
		if (tmp1) {
			LogMessage("Serial line error",17);
		}
	}

	// Receive Data Available or Character time-out
	if ((tmp == UART_IIR_INTID_RDA) || (tmp == UART_IIR_INTID_CTI)) {

		while (UART_Receive((LPC_UART_TypeDef *) LPC_UARTx, &data, 1,NONE_BLOCKING)) {

			if(xQueueSendFromISR( xSerialReceive, &data, NULL)!=pdTRUE )
				LogMessage("Queue is full",13);
		}
	}

}

/*-------------------------PRIVATE FUNCTIONS------------------------------*/
/*********************************************************************//**
 * @brief		HW initialization of line and SerialCOM task creation for packets receiving
 * @param[in]	None
 * @return 		None
 **********************************************************************/

void SerialInit(void) {
	// UART Configuration structure variable
	UART_CFG_Type UARTConfigStruct;
	// UART FIFO configuration Struct variable
	UART_FIFO_CFG_Type UARTFIFOConfigStruct;
	// Pin configuration for UART3
	PINSEL_CFG_Type PinCfg;

	/*
	 * Initialize UART3 pin connect
	 */
	PinCfg.Funcnum = 2;
	PinCfg.OpenDrain = 0;
	PinCfg.Pinmode = 0;
	PinCfg.Pinnum = 0;
	PinCfg.Portnum = 0;
	PINSEL_ConfigPin(&PinCfg);
	PinCfg.Pinnum = 1;
	PINSEL_ConfigPin(&PinCfg);

	/* Initialize UART Configuration parameter structure to default state:
	 * Baudrate = 115200bps
	 * 8 data bit
	 * 1 Stop bit
	 * None parity
	 */
	UART_ConfigStructInit(&UARTConfigStruct);
	//UARTConfigStruct->Baud_rate = 9600;

	UART_Init(LPC_UARTx, &UARTConfigStruct);

	/* Initialize FIFOConfigStruct to default state:
	 * 				- FIFO_DMAMode = DISABLE
	 * 				- FIFO_Level = UART_FIFO_TRGLEV0
	 * 				- FIFO_ResetRxBuf = ENABLE
	 * 				- FIFO_ResetTxBuf = ENABLE
	 * 				- FIFO_State = ENABLE
	 */
	UART_FIFOConfigStructInit(&UARTFIFOConfigStruct);

	// Initialize FIFO for UART0 peripheral
	UART_FIFOConfig(LPC_UARTx, &UARTFIFOConfigStruct);

	// Enable UART Transmit
	UART_TxCmd(LPC_UARTx, ENABLE);

	/* Enable UART Rx interrupt */
	UART_IntConfig((LPC_UART_TypeDef *) LPC_UARTx, UART_INTCFG_RBR, ENABLE);
	/* preemption = 1, sub-priority = 1 */
	//NVIC_SetPriority(UART3_IRQn, ((0x01 << 3) | 0x01));
	NVIC_SetPriority(UART3_IRQn, 0b11111);
	/* Enable Interrupt for UART0 channel */
	NVIC_EnableIRQ(UART3_IRQn);

	SetKinMode(MODESTART);

	xTaskCreate(SerialCOMTask, (signed portCHAR *) "SCOM", 256, NULL, 4, NULL);


}

/********************************************************************//*
 * @brief		packets decoding
 * @param[in]	None
 * @return 		None
 **********************************************************************/
void SerialCOMTask(void * pvParameters) {

	char data;

	static int received = 0;
	static int escBefore = 0;

	while (1) {
		if (xQueueReceive(xSerialReceive, &data, portMAX_DELAY ) == pdTRUE) {
			switch (data) {

			case SLIP_END:
				if (received > 0) {
					uint16_t crc=crc_get((uint8_t *)&packet_buffer[1],received-3);	//compute crc
					uint16_t crc2;
					memcpy(&crc2,&packet_buffer[received-2],2);						//get crc from packet
					if(crc==crc2){													//compare these crc
						SerialProcessPacket(packet_buffer, received);				//process packet
					}
					else{
						LogMessage("Sent again",10);
						SlipSend(ID_SENDAGAIN,NULL,0);								//send request to send packet one more time
					}
				}
				received = 0;
				break;


			case SLIP_ESC:
				escBefore = 1;
				break;


			case SLIP_ESC_END:
				if (escBefore == 1) {
					if (received < MAX_PACKET_L)
						packet_buffer[received++] = SLIP_END;
					escBefore = 0;
				} else {
					if (received < MAX_PACKET_L)
						packet_buffer[received++] = data;
				}
				break;

			case SLIP_ESC_ESC:
				if (escBefore == 1) {
					if (received < MAX_PACKET_L)
						packet_buffer[received++] = SLIP_ESC;
					escBefore = 0;
				} else {
					if (received < MAX_PACKET_L)
						packet_buffer[received++] = data;
				}
				break;

			default:
				if (received < MAX_PACKET_L)
					packet_buffer[received++] = data;
				break;
			}
		}
		else{
			LogMessage("xQueueReceive failed",20);
		}

	}
}


/********************************************************************//*
 * @brief		packets processing
 * @param[in]	packet[] - array of received bytes
 * @param[in]	length - length of this array
 * @return 		None
 **********************************************************************/
void SerialProcessPacket(char packet[], int length)
{
	int temp;
	char pcWriteBuffer[100];//buffer for task list

	if (packet==NULL)return;
	//packet arrived
	switch (packet[0])//packet[0] is ID
	{
	case ID_X:
		CopyToAddress((char*)&xyzabg[0],&packet[1],2);
		SetKinMode(MODEXYZ);
		break;
	case ID_Y:
		CopyToAddress((char*)&xyzabg[1],&packet[1],2);
		SetKinMode(MODEXYZ);
		break;
	case ID_Z:
		CopyToAddress((char*)&xyzabg[2],&packet[1],2);
		SetKinMode(MODEXYZ);
		break;
	case ID_BETA:
		CopyToAddress((char*)&xyzabg[4],&packet[1],2);
		SetKinMode(MODEXYZ);
		break;

	case ID_ALPHA:
		CopyToAddress((char*)&xyzabg[3],&packet[1],2);
		SetKinMode(MODEALPHAGRIP);
		break;
	case ID_GRIP:
		CopyToAddress((char*)&xyzabg[5],&packet[1],2);
		SetKinMode(MODEALPHAGRIP);
		break;
	case ID_Q0:
		CopyToAddress((char*)&anglesfrac[0],&packet[1],2);
		SetKinMode(MODEANGLES);
		break;
	case ID_Q1:
		CopyToAddress((char*)&anglesfrac[1],&packet[1],2);
		SetKinMode(MODEANGLES);
		break;
	case ID_Q2:
		CopyToAddress((char*)&anglesfrac[2],&packet[1],2);
		SetKinMode(MODEANGLES);
		break;
	case ID_Q3:
		CopyToAddress((char*)&anglesfrac[3],&packet[1],2);
		SetKinMode(MODEANGLES);
		break;

	case ID_INSTR:
		memcpy((char*)instructions,&packet[1],length-3);
		SetKinMode(MODEAUT);
		SetInstCount((length-3)/16);
		break;

	case ID_RESET:
		NVIC_SystemReset();
		break;

	case ID_PAUSE:
		vTaskSuspend(xHandleAutomat);
		xTimerStop(Timer20ms,0);
		break;

	case ID_CONTINUE:
		vTaskResume(xHandleAutomat);
		xTimerStart(Timer20ms,0);
		break;

	case ID_STARTPOS:
		SetKinMode(MODESTARTPOS);;
		break;

	case ID_CYCLIC:
		SetExecutType(EXCYCLE);
		break;

	case ID_ONCE:
		SetExecutType(EXONCE);
		break;

	case ID_SPEED:
		memcpy(&temp,&packet[1],4);
		SetSpeed(temp);
		break;

	case ID_QFREEHEAP:
		temp=xPortGetFreeHeapSize();
		SlipSend(ID_FREEHEAP,(char*)&temp, 4);
		break;

	case ID_QTASKLIST:
		vTaskList( (signed char *)pcWriteBuffer );
		SlipSend(ID_TASKLIST,(char*) pcWriteBuffer, 100);
		break;

	case ID_SENDAGAIN:
		SlipSend(id_previous,data_previous,len_previous);
		break;

	case ID_PWMON:
		PWMturnOn();
		SetLedStatus(LEDON);
		SlipSend(ID_ANGLES,(char*)anglesfrac,2*6);
		SlipSend(ID_XYZABG,(char*)xyzabg,2*6);
		break;

	case ID_PWMOFF:
		GoToStartPosition();
		ComputeTimeToWait();			//compute time necessary for move to the start the position
		vTaskDelay(GetDelay()+200);		//wait for manipulator until it gets to the start position
		PWMturnOff();					//turn off servos
		SetLedStatus(LEDOFF);			//switch off the LED
		//NVIC_SystemReset();
		break;

	default:
		LogMessage("unknown ID was received",23);
		//SetError(ERROR_COMMAND);
		break;
	}


	if(packet[0]<=ID_INSTR){//in most cases we have to give semaphore what causes Kinematic task running

		if( xSemaphoreGive( xSemaphoreCOM ) != pdTRUE ){
			LogMessage("xSemaphoreCOM give failed",25);
		}
	}
}

/********************************************************************//*
 * @brief		sending packets
 * @param[in]	id - packet identifikator
 * @param[in]	data - packet data
 * @param[in]	len - packet length
 * @return 		None
 **********************************************************************/
void SlipSend(char id, char * data, int len) {
	int temp = len;
	int i;
	char* tempdata=data;

	id_previous=id;
	data_previous=data;
	len_previous=len;

	UART_SendByte(LPC_UARTx, SLIP_END);
	switch (id) {
	case SLIP_END: //end occours
		UART_SendByte(LPC_UARTx, SLIP_ESC);
		UART_SendByte(LPC_UARTx, SLIP_ESC_END);
		break;
	case SLIP_ESC:
		UART_SendByte(LPC_UARTx, SLIP_ESC);
		UART_SendByte(LPC_UARTx, SLIP_ESC_ESC);
		break;
	default:
		UART_SendByte(LPC_UARTx, id);
	}

	while ((len--) > 0) {
		switch ((unsigned char) *data) {

		case SLIP_END:
			UART_SendByte(LPC_UARTx, SLIP_ESC);
			UART_SendByte(LPC_UARTx, SLIP_ESC_END);
			break;


		case SLIP_ESC:
			UART_SendByte(LPC_UARTx, SLIP_ESC);
			UART_SendByte(LPC_UARTx, SLIP_ESC_ESC);
			break;

		default:
			UART_SendByte(LPC_UARTx, *data);
		}
		data++;
	 // wait for current transmission complete - THR must be empty
	   while (UART_CheckBusy((LPC_UART_TypeDef *)LPC_UARTx));
	}


	uint16_t crc16 = crc_get((uint8_t*)tempdata,temp);
	char crc[2];
	memcpy(crc,&crc16,2);

	for(i=0;i<2;i++){
		switch(crc[i]){
		case SLIP_END:
			UART_SendByte(LPC_UARTx, SLIP_ESC);
			UART_SendByte(LPC_UARTx, SLIP_ESC_END);
			break;

		case SLIP_ESC:
			UART_SendByte(LPC_UARTx, SLIP_ESC);
			UART_SendByte(LPC_UARTx, SLIP_ESC_ESC);
			break;

		default:
			UART_SendByte(LPC_UARTx, crc[i]);
		}
		 // wait for current transmission complete - THR must be empty
		while (UART_CheckBusy((LPC_UART_TypeDef *)LPC_UARTx));
	}

	UART_SendByte(LPC_UARTx, SLIP_END);
	UART_SendByte(LPC_UARTx, SLIP_END);
}



/********************************************************************//*
 * @brief		send text message to the PC
 * @param[in]	message - text message
 * @param[in]	len - message length
 * @return 		None
 **********************************************************************/
void LogMessage(char* message,int len){

	SlipSend(ID_LOGGER,message,len);
}


/********************************************************************//*
 * @brief		get crc of packet
 * @param[in]	data - packet data
 * @param[in]	len - packet length
 * @return 		16bit crc
 **********************************************************************/
uint16_t crc_get(uint8_t * data, int len)
{
	uint8_t crc[2];
  	uint8_t tmp = 0;
  	int i;

  	crc[0] = 0;
  	crc[1] = 0;

  	for(i = 0; i < len; i++)
  	{
  		tmp = crc[0];
  		crc[0] = crc[1] ^ data[i];
  		crc[1] = tmp;
  		crc[0] ^= crc[0] << 4;
  		crc[1] ^= crc[0] >> 4;
  		*((uint16_t *) crc) ^= (*(uint16_t*)(crc)) << 3;
  	} // for i

  	return *((uint16_t *) crc);
} // crc_get()


