#include "plc.h"
#include "main.h"
#include "cmsis_os.h"
#include <string.h>
#include <stdlib.h>

plc_data_typeDef PLC_Data[AMOUNT_BLOCK_PLC] = {0}; // по колличеству опрашиваемых блоков

plc_common_typeDef plc_common = {0, 1};

extern UART_HandleTypeDef huart2;
extern xQueueHandle	QueueCmdTxPLC, QueueCmdRxPLC;
/* ----------------------------------------------------------------------------------------- */
/* ######################################################################################### */
/* PLC CALCULATE CRC ----------------------------------------------------------------------- */
uint16_t plc_calc_cs(uint8_t *buff, uint16_t len)
{
	uint32_t cs16 = 0;
	for(uint16_t i = 0; i < len; i++) cs16 += buff[i];
	
	cs16 = (0x00ff&cs16) + (cs16>>8);
  cs16 = (0x00ff&cs16) + (cs16>>8);	
	return cs16;
}
/* ----------------------------------------------------------------------------------------- */
/* ######################################################################################### */
/* PLC ESCAPE ENCODER ---------------------------------------------------------------------- */
uint16_t escape_encoder(uint8_t *buff, uint16_t len_buf, uint8_t *out_biff)
{
	_Bool flag = 0;
	uint16_t tmpLen = 0;
	for(uint16_t i = 0; i < len_buf; i++){
		if(!flag){
			if(buff[i] == 0x7D) flag = 1;
			else out_biff[tmpLen++] = buff[i];
		}
		else {
			if((buff[i] == 0x5E) || (buff[i] == 0x5D)){
				out_biff[tmpLen++] = buff[i] | 0x20;
				flag = 0;
			}
			else return 0;
		}
	}
	return tmpLen;
}
/* ----------------------------------------------------------------------------------------- */
/* ######################################################################################### */
/* PLC PARSE PACKET RX --------------------------------------------------------------------- */
uint8_t _parse_packet_receive(plc_RxData_typeDef *ParseRx, plc_buff_typeDef *buff)
{
	uint8_t *tmpBuff = (uint8_t*)malloc((sizeof(uint8_t) * buff->len));
	if(tmpBuff == NULL) return 0;
	uint16_t tmpLen = 0;
	//_Bool flag = 0;

	ParseRx->LenBuff = 0;
	ParseRx->Query = 0;
	ParseRx->Valid = 0;
	
	
	tmpLen = escape_encoder(buff->data, buff->len, tmpBuff);
	if(tmpLen == 0){
		free(tmpBuff);
		return 0;
	}
	/*for(uint16_t i = 0; i < buff->len; i++){
		if(!flag){
			if(buff->data[i] == 0x7D) flag = 1;
			else tmpBuff[tmpLen++] = buff->data[i];
		}
		else {
			if((buff->data[i] == 0x5E) || (buff->data[i] == 0x5D)){
				tmpBuff[tmpLen++] = buff->data[i] | 0x20;
				flag = 0;
			}
			else {
				free(tmpBuff);
				return 0;
			}
		}
	}*/
	
	if(plc_calc_cs(tmpBuff, tmpLen) != 0x00FF){
		free(tmpBuff);
		return 0;
	}
	
	if(!(tmpBuff[0] & 0x80)){ // если это запрос данных
		uint8_t route = tmpBuff[tmpBuff[0] / 2 + 1];
		uint8_t odd_route = tmpBuff[0] % 2; // узнаем тетраду
		if(odd_route)route >>= 4; // если старшая тетрада
		route &= 0x3F;
		if(route == PLC_ADDR_BOARD)ParseRx->Query = 1;
	}

	ParseRx->Pointer = tmpBuff[0];

	ParseRx->LenBuff = tmpLen - 2;
	memcpy(ParseRx->Buff, &tmpBuff[1], tmpLen - 1);
	
	ParseRx->Valid = 1;
	
	free(tmpBuff);
	return 1;
}

/* ----------------------------------------------------------------------------------------- */
/* ######################################################################################### */
/* PLC CODER ESQ --------------------------------------------------------------------------- */
void _pack_esq(plc_buff_typeDef *buff, uint8_t x)
{
	if((x == 0x7d) || (x == 0x7e)){
		buff->data[buff->len++] = 0x7D;
		x &= 0xDF;
	}
	buff->data[buff->len++] = x;
}

/* ----------------------------------------------------------------------------------------- */
/* ######################################################################################### */
/* PLC PREPARE PACKET TX ------------------------------------------------------------------- */
void _prepare_packet_send(plc_TxData_typeDef *PrepareTx, plc_buff_typeDef *buff)
{
	uint8_t cs = ~((uint8_t)plc_calc_cs(&PrepareTx->Pointer, PrepareTx->LenBuff + 1));
	buff->len = 0;
	buff->data[buff->len++] = 0x7E;
	_pack_esq(buff, PrepareTx->Pointer);

	for(uint16_t i = 0; i < PrepareTx->LenBuff; i++) _pack_esq(buff, PrepareTx->Buff[i]);
	//_pack_esq(buff, ~((uint8_t)plc_calc_cs(&buff->data[1], buff->len - 1)));
	_pack_esq(buff, cs);
	
	//buff->data[buff->len++] = ~((uint8_t)plc_calc_cs(&buff->data[1], buff->len - 1)); // не считаем первый	 0x7E
	buff->data[buff->len++] = 0x7E;
}

uint8_t my_cmd_plc(plc_RxData_typeDef *PLCRxData, plc_TxData_typeDef *PLCTxData) // PLCRxData - принятый пакет, PLCTxData - передаем
{
	if(PLCRxData->Buff[0] == 0x01){
		
	}
	return 0;
}

/* ----------------------------------------------------------------------------------------- */
/* ######################################################################################### */
/* PLC RECEIVE ----------------------------------------------------------------------------- */
void plc_receive(uint8_t ch)
{
	portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
	static plc_buff_typeDef OutBuff = {0}, InBuff = {0};
	static plc_TxData_typeDef PLCTxData = {0};
	static plc_RxData_typeDef PLCRxData = {0};
	static uint8_t pointer;
	
	static volatile struct {
		_Bool Marker: 1;
		_Bool MarkerSearch: 1;
		_Bool Send: 1;
	}flag = {0, 0, 0};
	
	/**********************************************************/
	if(!flag.Send){ // если мы не ищем маркер
		if(xQueueReceiveFromISR(QueueCmdTxPLC, &PLCTxData, &xHigherPriorityTaskWoken) == pdTRUE){ // если есть данные в очереди
			_prepare_packet_send(&PLCTxData, &OutBuff); // готовим данные для передачи
			flag.MarkerSearch = 1; // флаг для поиска маркера
			if(!(PLCTxData.Pointer & 0x80)) flag.Send = 1; // если передаем команду
		}
	}
	
	if(ch == 0x7E){ // конец пакета
		if(flag.Marker){
			if(pointer == PLC_MARKER_BOARD){ // если мой маркер, то передаем
				HAL_GPIO_WritePin(USART_TX_EN_GPIO_Port, USART_TX_EN_Pin, GPIO_PIN_SET);
				HAL_UART_Transmit(&huart2, OutBuff.data, OutBuff.len, 1000);
				HAL_GPIO_WritePin(USART_TX_EN_GPIO_Port, USART_TX_EN_Pin, GPIO_PIN_RESET);
				flag.MarkerSearch = 0;
				flag.Send = 0;
				pointer = 0x00;
			}
			flag.Marker = 0;
		}
		if(InBuff.len > 1){ // если длинна пакета больше 1, то обрабатываем пакет
			_parse_packet_receive(&PLCRxData, &InBuff);
			xQueueSendFromISR(QueueCmdRxPLC, &PLCRxData, &xHigherPriorityTaskWoken);
		}
		InBuff.len = 0;	
	}
	else{
		if(flag.MarkerSearch){ // если есть флаг для поиска маркера
			pointer = ch;
			flag.Marker = 1;
		}
		InBuff.data[InBuff.len++] = ch; // копим данные
		if(InBuff.len == (MAX_LEN_PLC_BUFF - 1)) InBuff.len = 0;
	}
}