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

//****************               Alink (c) Angoosh                   *****************
//****************                  ADCIS s.r.o.                     *****************

//****************                    Alink                          *****************
//                                  ver 1.0.0
//                                  11.11.2019
//************************************************************************************

#include "Alink.h"
#include "main.h"


//void checkA5(uint8_t, uint8_t, uint8_t);
void TxParseMSG(UART_HandleTypeDef*,int,uint8_t,uint8_t,uint8_t);
void RxParseMSG(UART_HandleTypeDef*,uint8_t*);

/*
 * Vysilani zpravy: A5, rec add, ln, my add, 0, data, CRC
 * 		0  verze sw: A5, rec add, 0C, ADDRESS, 0, VERSION, SUB_VERSION, RELEASE, BUGFIX, CRC
 * 		1  status: A5, rec add, 08, ADDRESS, 0, STATUS, CRC
 * 		2  err: A5, rec add, 08, ADDRESS, 1, ERROR, CRC
 *
 * 		parametry fce:
 * 			type: 0,1,2 (verze, status, err)
 * 			mess: 0,1,2 (verze - pouze 0, status - 1,2,3, err - 1,2)
 *
 *
 * 			data bit:
 * 			0 - version
 * 			1 - status - sleep
 * 			2 - status - charging
 * 			3 - status - running
 * 			4 - err - UNDERVOLT
 * 			5 - err - FET_ERR
 */

/**
 * @brief A5 symbol replacement pattern find
 * @param checked byte
 * @param retval for first replacement byte
 * @param retval for second replacement byte
 */
void checkA5(uint8_t byte, uint8_t *arr1, uint8_t *arr2){		//kontrola, zda ve zprave neni A5
	if(byte == 0xA5){
		*arr1 = 0xAA;
		*arr2 = 0xA8;
	}
	else if(byte == 0xAA){
		*arr1 = 0xAA;
		*arr2 = 0xAA;
	}
	else{
		*arr1 = byte;
		*arr2 = 0x00;
	}
}

void A5pattern(uint8_t *messageIn, uint8_t *messageOut){

}
/**
 * @brief CRC calculation function
 * @param data array pointer
 * @param data length
 * @retval crc byte
 */
uint8_t CRC_Calc(uint8_t *data, uint8_t length){
	uint8_t crc = 0;
	for(int i = 0; i<length; i++){
		crc ^= data[i];
	}
	return crc;
}

/**
 * @brief Receive message parser and command executer
 * @param huart typedef
 * @param message
 */
void RxParseMSG(UART_HandleTypeDef *huart, uint8_t *dataBuffer){
	uint8_t isLCB = 0;
	//uint8_t msgMyAddr = dataBuffer[1];
	uint8_t msgLen = dataBuffer[2];
	uint8_t msgTxAddr = dataBuffer[3];
	uint8_t msgCmd = dataBuffer[4];
	uint8_t msgCrc = dataBuffer[msgLen-1];
	uint8_t msgPreData[msgLen-6];
	if(msgLen > 6){
	    for(int i = 0; i<sizeof(msgPreData); i++){
	    	msgPreData[i] = dataBuffer[5+i];
	    }
	}
	uint8_t payload = 0;
	uint8_t msgForCrc[msgLen-2];

	for(int m = 0; m<sizeof(msgForCrc); m++){
		msgForCrc[m] = dataBuffer[m+1];
	}
	if(msgCrc == CRC_Calc(msgForCrc, sizeof(msgForCrc))){
		if(msgTxAddr >= LCBADDRMIN && msgTxAddr <= LCBADDRMAX){
			isLCB = 1;
		}
	}

	uint8_t AAcount = 0;
	for(int i = 0; i<sizeof(msgPreData); i++){
		if(msgPreData[i] == 0xAA){
			AAcount++;
			i++;
		}
	}

	uint8_t msgData[sizeof(msgPreData)-AAcount];
	uint8_t j = 0;
	for(int i = 0; i<sizeof(msgPreData); i++){
		if(msgPreData[i+j] == 0xAA){
			if(msgPreData[i+j+1] == 0xAA){
				msgData[i] = 0xAA;
			}
			else{
				msgData[i] = 0xA5;
			}
			j++;
		}
		else{
			msgData[i] = msgPreData[i+j];
		}
	}

	if(isLCB == 1){
		if(msgLen > 6){
			payload = 1;
		}

		if(msgCmd == SW_VER_ASK){ //verze sw //type 0
			TxParseMSG(huart,0,0x00,0x00,msgTxAddr);
		}
		else if(msgCmd == STATUS){ //stav (charging, off, on) //type 1
			uint8_t msgStatus = 0;
			TxParseMSG(huart,1,0x00,msgStatus,msgTxAddr);
		}
		else if(msgCmd == STATE_ASK){ //status (err) //type 2
			uint8_t msgState = 0;
			TxParseMSG(huart,2,0x00,msgState,msgTxAddr);
		}
#ifdef ALINK_CTRL
		else if(msgCmd == ALINK_STABLE_STATE && payload == 1){//zapnuti/vypnuti ustaleneho stavu, default je vyply
			SteadyState = msgData[0];
		}
		else if(msgCmd == ALINK_BLOCK_PLS && payload == 1){//vyber ktere pulsy blokovat, popis funkce je v @info
			if(msgData[0] == 0x00){
				for(int i = 0; i < 8; i++){
					INJ0.PULSE_DEL_ARR[i] = msgData[1+i];
				}
			}
			else if(msgData[0] == 0x01){
				for(int i = 0; i < 8; i++){
					INJ1.PULSE_DEL_ARR[i] = msgData[1+i];
				}
			}
			else if(msgData[0] == 0x02){
				for(int i = 0; i < 8; i++){
					INJ2.PULSE_DEL_ARR[i] = msgData[1+i];
				}
			}
			else;
		}
		else if(msgCmd == ALINK_BLOCK_PLS_END && payload == 1){//vyber po ktere pulsy blokovat, popis funkce je v @info
			if(msgData[0] == 0x00){
				for(int i = 0; i < 8; i++){
					INJ0.PULSE_DEL_END_ARR[i] = msgData[1+i];
				}
			}
			else if(msgData[0] == 0x01){
				for(int i = 0; i < 8; i++){
					INJ1.PULSE_DEL_END_ARR[i] = msgData[1+i];
				}
			}
			else if(msgData[0] == 0x02){
				for(int i = 0; i < 8; i++){
					INJ2.PULSE_DEL_END_ARR[i] = msgData[1+i];
				}
			}
			else;
		}
		else if(msgCmd == ALINK_FET_SW_PAUSE && payload == 1){//Nastaveni delky pauzy mezi prepinani fetu
			int hi = msgData[1];
			int lo = msgData[2];
			int delay = (hi*256)+lo;

			if(msgData[0] == 0x00){
				INJ0.SWITCH_DELAY = SwitchDelaySetup(INJ0, delay);
			}
			else if(msgData[0] == 0x01){
				INJ1.SWITCH_DELAY = SwitchDelaySetup(INJ1, delay);
			}
			else if(msgData[0] == 0x02){
				INJ2.SWITCH_DELAY = SwitchDelaySetup(INJ2, delay);
			}
			else;
		}
		else if(msgCmd == ALINK_PULSE_BLK_TIM && payload == 1){//po kolika ms blokovat puls 2B
			uint8_t hi = msgData[1];
			uint8_t lo = msgData[2];

			float ARR = 65535;
			float PSC = 53;

			float x = (1/(SysClk/APBDiv))*(PSC+1)*(ARR+1)*1000;
			float us = (x/ARR)*1000;
			int BlockPulseAfterMs = (hi*256)+lo;
			int BlockPulseAfterTicks = BlockPulseAfterMs/us;

			if(msgData[0] == 0x00){
				INJ0.BLOCK_AFTER_TICKS = BlockPulseAfterTicks;
				INJ0.BLOCK_AFTER_US = BlockPulseAfterMs;
			}
			else if(msgData[0] == 0x01){
				INJ1.BLOCK_AFTER_TICKS = BlockPulseAfterTicks;
				INJ1.BLOCK_AFTER_US = BlockPulseAfterMs;
			}
			else if(msgData[0] == 0x02){
				INJ2.BLOCK_AFTER_TICKS = BlockPulseAfterTicks;
				INJ2.BLOCK_AFTER_US = BlockPulseAfterMs;
			}
			else;
		}
#ifdef ALINK_BRD_RST
		else if(msgData[0] == ALINK_BRD_RST){
			TxParseMSG(huart,3,0x00,msgData[0],msgTxAddr);
			HAL_NVIC_SystemReset();
		}
#endif
	}
#endif
	else;
}

/**
 * @brief Response message parser
 * @param huart typedef
 * @param type of message
 * @param command
 * @param data of message
 * @param address of destination device
 */
void TxParseMSG(UART_HandleTypeDef *huart, int type, uint8_t CMD, uint8_t mess, uint8_t addr){
	uint8_t crc = 0;
	int datasize = 0;
	uint8_t data_buffer[40];
	uint8_t data_buffer_size = 0;

//verze softwaru
	if(type == 0){
		uint8_t data[] = {addr,0x0A,ADDRESS,CMD,VERSION,SUB_VERSION,RELEASE,BUGFIX};
		datasize = sizeof(data);

		for(int i = 0; i<sizeof(data); i++){
			uint8_t a,b;
			checkA5(data[i], &a, &b);
			if(a != 0 && b != 0){
				datasize ++;
				data[1]++;
			}
		}

		datasize ++;
		uint8_t datanew[datasize];
		datanew[0] = 0xA5;
		int j = 0;
		for(int i = 1; i<sizeof(datanew); i++){
			uint8_t a,b;
			checkA5(data[i-1], &a, &b);
			if(a != 0 && b != 0){
				datanew[i+j] = a;
				datanew[i+j+1] = b;
				j++;
			}
			else{
				datanew[i+j] = a;
			}
		}
		for(int i = 0; i<sizeof(datanew); i++){
			data_buffer[i] = datanew[i];
			data_buffer_size ++;
		}

		uint8_t dataForCrc[sizeof(datanew-1)];
		for(int i = 0; i < sizeof(dataForCrc); i++){
			dataForCrc[i] = datanew[i+1];
		}
		crc = CRC_Calc(dataForCrc,sizeof(dataForCrc));
	}


//status
	else if(type == 1){
		__IO uint32_t DATA_READ = 0;
		uint8_t data[] = {addr,0x13,ADDRESS,CMD,mess,0,0,0,0,0,0,0,0,0,0,0,0};
		datasize = sizeof(data);

		DATA_READ = *(__IO uint32_t*)SERIAL_ADDRESS;
		data[13] = DATA_READ >> 24;
		data[14] = DATA_READ >> 16;
		data[15] = DATA_READ >> 8;
		data[16] = DATA_READ;

		for(int i = 0; i<sizeof(data); i++){
			uint8_t a,b;
			checkA5(data[i], &a, &b);
			if(a != 0 && b != 0){
				datasize ++;
				data[1]++;
			}
		}
		datasize ++;
		uint8_t datanew[datasize];
		datanew[0] = 0xA5;
		int j = 0;
		for(int i = 1; i<sizeof(datanew); i++){
			uint8_t a,b;
			checkA5(data[i-1], &a, &b);
			if(a != 0 && b != 0){
				datanew[j+i] = a;
				datanew[j+i+1] = b;
				j++;
			}
			else{
				datanew[j+i] = a;
			}
		}
		for(int i = 0; i<sizeof(datanew); i++){
			data_buffer[i] = datanew[i];
			data_buffer_size ++;
		}

		uint8_t dataForCrc[sizeof(datanew-1)];
		for(int i = 0; i < sizeof(dataForCrc); i++){
			dataForCrc[i] = datanew[i+1];
		}
		crc = CRC_Calc(dataForCrc,sizeof(dataForCrc));
	}

//error report
	else if(type == 2){
		uint8_t data[] = {addr,0x07,ADDRESS,CMD,mess};
		datasize = sizeof(data);

		for(int i = 0; i<sizeof(data); i++){
			uint8_t a,b;
			checkA5(data[i], &a, &b);
			if(a != 0 && b != 0){
				datasize ++;
				data[1]++;
			}
		}
		datasize ++;
		uint8_t datanew[datasize];
		datanew[0] = 0xA5;
		int j = 0;
		for(int i = 1; i<sizeof(datanew); i++){
			uint8_t a,b;
			checkA5(data[i-1], &a, &b);
			if(a != 0 && b != 0){
				datanew[j+i] = a;
				datanew[j+i+1] = b;
				j++;
			}
			else{
				datanew[j+i] = a;
			}
		}
		for(int i = 0; i<sizeof(datanew); i++){
			data_buffer[i] = datanew[i];
			data_buffer_size ++;
		}

		uint8_t dataForCrc[sizeof(datanew-1)];
		for(int i = 0; i < sizeof(dataForCrc); i++){
			dataForCrc[i] = datanew[i+1];
		}
		crc = CRC_Calc(dataForCrc,sizeof(dataForCrc));
	}
//else messages
	else if(type == 3){
		uint8_t data[] = {addr,0x07,ADDRESS,CMD,mess};
		datasize = sizeof(data);

		for(int i = 0; i<sizeof(data); i++){
			uint8_t a,b;
			checkA5(data[i], &a, &b);
			if(a != 0 && b != 0){
				datasize ++;
				data[1]++;
			}
		}
		datasize ++;
		uint8_t datanew[datasize];
		datanew[0] = 0xA5;
		int j = 0;
		for(int i = 1; i<sizeof(datanew); i++){
			uint8_t a,b;
			checkA5(data[i-1], &a, &b);
			if(a != 0 && b != 0){
				datanew[j+i] = a;
				datanew[j+i+1] = b;
				j++;
			}
			else{
				datanew[j+i] = a;
			}
		}
		for(int i = 0; i<sizeof(datanew); i++){
			data_buffer[i] = datanew[i];
			data_buffer_size ++;
		}

		uint8_t dataForCrc[sizeof(datanew-1)];
		for(int i = 0; i < sizeof(dataForCrc); i++){
			dataForCrc[i] = datanew[i+1];
		}
		crc = CRC_Calc(dataForCrc,sizeof(dataForCrc));
	}
	else{}

	datasize ++;

	uint8_t a,b;

	checkA5(crc, &a, &b);
	if(a != 0 && b != 0){
		datasize ++;
		uint8_t msg[datasize];
		for(int i = 0; i<datasize-2; i++){
			msg[i] = data_buffer[i];
		}
		msg[sizeof(msg)-2] = a;
		msg[sizeof(msg)-1] = b;
		msg[2]++;
		HAL_UART_Transmit(huart, (uint8_t *)&msg, sizeof(msg), 0xFFFF);
	}
	else{
		uint8_t msg[datasize];
		for(int i = 0; i<datasize-1; i++){
			msg[i] = data_buffer[i];
		}
		msg[sizeof(msg)-1] = a;
		HAL_UART_Transmit(huart, (uint8_t *)&msg, sizeof(msg), 0xFFFF);
	}
}