/*
 * ts_comms.cpp
 *
 *  Created on: 22 Oct 2020
 *      Author: Dmitrijs
 */

#include "main.h"
#include "dbw.h"

comms_status RX;
comms_status TX;
unsigned char SCI_flags;
const char Signature[20];
const char Revision[20];
char crc32_bufer[8];

void (*Store_config)(void) = Write_Config;
const char Signature[20] = { "speeduino DBW 2.0.0" };
const char Revision[20] = { "speeduino DBW 2.0.0" };

volatile var_t tx_bufer;

void Comms_Init(void) {
	Comms_Reset(&TX);
	Comms_Reset(&RX);

	USART1->ISR &= ~USART_ISR_RXNE;
	USART1->CR1 |= USART_CR1_RXNEIE;

}

void USART1_IRQHandler(void) {
	uint8_t tmp;
	if (USART1->ISR & USART_ISR_RXNE) {
		tmp = USART1->RDR;
		USART1->ISR &= ~USART_ISR_RXNE;
		//if there are any RX errors reset comms else process data
		if (USART1->ISR & 0x000F) {
			Comms_Reset(&RX);
			USART1->ISR &= ~0x000F;
		} else {
			Rx_Char(tmp);
		}
	}
//transmit interrupt
	if (USART1->ISR & USART_ISR_TC) {
		if (TX.bytes_done < TX.cnt) {
			USART1->TDR = *TX.address;
			TX.address++;
			TX.bytes_done++;
		} else
			TX_Done();

	}
}

void Comms_Reset(comms_status *CS) {

	CS->mode = IDLE;					//Mode Idle
	CS->cnt = 0;					//Nothing to receive
	CS->bytes_done = 0;				//Nothing received
	CS->crc32 = 0;					//Reset crc32
	CS->pause_flag = 0;
	CS->address = &CS->cmd;		//Point incoming characters to cmd_bufer
	CS->timeout = 0;
}

void Rx_Char(unsigned char data) {

	RX.bytes_done++;

	if (RX.bytes_done == 1) {
		if (data == 'A') {
			SCI_flags |= SCI_FLAGS_TX_VARIABLES;
			Comms_Reset(&RX);
		} else if (data == 'Q') {
			TX_Schedule((unsigned char*) Revision, 20);
			Comms_Reset(&RX);
		} else if (data == 'S') {
			TX_Schedule((unsigned char*) Signature, 20);
			Comms_Reset(&RX);
		} else if (data == 'B') {
			Store_config();
			Comms_Reset(&RX);
		} else if ((data == 'r') || (data == 'w') || (data == 'k')
				|| (data == 'b') || (data == 'z')) {
			RX.timeout = 500;
			RX.cmd = data;
		} else {
			Comms_Reset(&RX);
			return;
		}

	} else if (RX.bytes_done == 2) {
		RX.CANid = data;
	} else if (RX.bytes_done == 3) {
		RX.table_index = data;
		if (RX.cmd == 'b') {
			Store_config();
			Comms_Reset(&RX);
			return;
		}
	} else if (RX.bytes_done == 4) {
		RX.offset = 0x00;
		RX.offset = (unsigned short) data;
	} else if (RX.bytes_done == 5) {
		RX.offset |= ((unsigned short) data) << 8;
	} else if (RX.bytes_done == 6) {
		RX.data_size = 0x00;
		RX.data_size = (unsigned short) data;
	} else if (RX.bytes_done == 7) {
		RX.data_size += ((unsigned short) data) << 8;

		if (RX.CANid == TS_CAN_ID) {

			if (RX.cmd == 'k') {
				SCI_flags |= SCI_FLAGS_CRC32;
				Comms_Reset(&RX);
				return;
			} else if (RX.cmd == 'z') {
				// process command buttons
				if (RX.data_size == 0x0100) {
					Apply_Sensor_Calibration();
				} else if (RX.data_size == 0x0200) {
					DBW_Stop();
					config->config_bits |= CONFIG_TESTMODE_F;
				} else if (RX.data_size == 0x0300) {
					config->config_bits &= ~CONFIG_TESTMODE_F;
				} else if (RX.data_size == 0x0400) {
					//Start TPS AUTOCAL
					ac_timer = 200;
					ac_mode = 1;
				}
				Comms_Reset(&RX);
			} else if (RX.cmd == 'r') {
				// Read different tables
				if (RX.table_index == 0x01) {
					TX_Schedule((unsigned char*) config + RX.offset,
							RX.data_size);
				} else if (RX.table_index == 0x07) {
					Copy_Tx_Vars();
					TX_Schedule((unsigned char*) &tx_bufer + RX.offset,
							RX.data_size);
				} else if (RX.table_index == 0x0e) {
					TX_Schedule((unsigned char*) Signature, 20);
				} else if (RX.table_index == 0x0f) {
					TX_Schedule((unsigned char*) Revision, 20);
				}
				Comms_Reset(&RX);
				return;
			} else {
				//Comms are not reset for 'w' command
				// setup write pointer
				RX.cnt = RX.bytes_done + RX.data_size;
				if (RX.table_index == 0x01) {
					RX.address = (unsigned char*) config + RX.offset;
				} else {
					RX.address = (unsigned char*) RX.can_bufer;
					// Do not allow more then 256 bytes to be written to CAN buffer
					if (RX.data_size > 256)
						Comms_Reset(&RX);
				}

				return;
			}

		} else {
			// if received CANID != TS_CANID
			if ((RX.cmd == 'k') || (RX.cmd == 'r')) {
				//TODO pass data to CAN device
				Comms_Reset(&RX);
				return;
			}
			Comms_Reset(&RX);
			return;
		}
	} else {
		//receive data TX.bytes_done >7
		*(RX.address) = data;
		RX.address++;

		if (RX.bytes_done >= RX.cnt) {
			if (RX.table_index == 0x0d) {
				//TODO Call command by index
				Comms_Reset(&RX);
			} else {
				// Expecting here ends data write chain
				Comms_Reset(&RX);
			}

		}

	}

} // RX Char

void TX_Schedule(unsigned char *data, unsigned short count) {
	//Schedule transmission
	TX.mode = TRANSMISSION_IN_PROGRESS;	// Set TX mode to transmit

	TX.cnt = count;			// Set number of byte to be transmited
	TX.bytes_done = 0;		// Set number of bytes transmitted
	TX.address = data;		// Store updated address

	//transmit first data byte
	USART1->ICR |= USART_ICR_TCCF;
	USART1->TDR = *data;
	USART1->CR1 |= USART_CR1_TCIE;

	TX.bytes_done++;
	TX.address++;

//		if(TX.cnt==1)
//			{
//			TX_Done();
//			}
//		else{
//
//
//			}

	return;
}

void Poll_Tx(void) {

	if (TX.mode == TRANSMISSION_IN_PROGRESS) {
		// do nothing because transmitter is busy;
	}
	// Check if variables should be transmitted
	else if (SCI_flags & SCI_FLAGS_TX_VARIABLES) {
		//Copy variables to tx_bufer
		Copy_Tx_Vars();
		TX_Schedule((unsigned char*) &tx_bufer, sizeof(var_t));
		SCI_flags &= ~SCI_FLAGS_TX_VARIABLES;
		return;

	} else if (SCI_flags & SCI_FLAGS_CRC32) {
		SCI_flags &= ~SCI_FLAGS_CRC32;
		CRC32();
		TX_Schedule((unsigned char*) &crc32_bufer[0], 4);
		return;

	}

}

void TX_Done(void) {
	Comms_Reset(&TX);
	//disable TX interrupt
	USART1->CR1 &= ~USART_CR1_TCIE;
	USART1->ICR |= USART_ICR_TCCF;

}

void CRC32(void) {
	unsigned int tmp;

	tmp = (unsigned int) crc32((void*) config, sizeof(config_t));
	crc32_bufer[0] = (unsigned char) ((tmp >> 24) & 0x000000ff);
	crc32_bufer[1] = (unsigned char) ((tmp >> 16) & 0x000000ff);
	crc32_bufer[2] = (unsigned char) ((tmp >> 8) & 0x000000ff);
	crc32_bufer[3] = (unsigned char) ((tmp) & 0x000000ff);
}

void Copy_Tx_Vars(void) {

//TODO copy all variables  from *var to tx_bufer to make sure
// nothing is changed while transmission is in progress
//tx_bufer.a = var->a;

	tx_bufer.clock = var.clock;
	tx_bufer.status0 = var.rpm;
	tx_bufer.status1 = var.status1;
	tx_bufer.status2 = var.status2;
	tx_bufer.status3 = var.status3;

	tx_bufer.pps1_adc = var.pps1_adc;
	tx_bufer.pps2_adc = var.pps2_adc;
	tx_bufer.tps1_adc = var.tps1_adc;
	tx_bufer.tps2_adc = var.tps2_adc;
	tx_bufer.motor_current_adc = var.motor_current_adc;
	tx_bufer.vbat_adc = var.vbat_adc;

	tx_bufer.pps1 = var.pps1;
	tx_bufer.pps2 = var.pps2;
	tx_bufer.tps1 = var.tps1;
	tx_bufer.tps2 = var.tps2;
	tx_bufer.pps = var.pps;
	tx_bufer.tps = var.tps;
	tx_bufer.tps_error = var.tps_error;
	tx_bufer.motor_pwm = var.motor_pwm;
	tx_bufer.idle_dc = var.idle_dc;
	tx_bufer.pps_delta = var.pps_delta;
	tx_bufer.tps_delta = var.tps_delta;
	tx_bufer.tps_target = var.tps_target;
}

#define WIDTH  (8)
#define TOPBIT (1 << (WIDTH - 1))
#define POLYNOMIAL 0xD8

crc_t calc_crc(crc_t message[], int nBytes) {
	crc_t remainder = 0;

	/*
	 * Perform modulo-2 division, a byte at a time.
	 */
	for (int byte = 0; byte < nBytes; ++byte) {
		/*
		 * Bring the next byte into the remainder.
		 */
		remainder ^= (message[byte] << (WIDTH - 8));

		/*
		 * Perform modulo-2 division, a bit at a time.
		 */
		for (unsigned char bit = 8; bit > 0; --bit) {
			/*
			 * Try to divide the current data bit.
			 */
			if (remainder & TOPBIT) {
				remainder = (remainder << 1) ^ POLYNOMIAL;
			} else {
				remainder = (remainder << 1);
			}
		}
	}

	/*
	 * The final remainder is the CRC result.
	 */
	return remainder;
}

static uint32_t crc32_tab[] = { 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
		0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4,
		0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
		0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb,
		0xf4d4b551, 0x83d385c7, 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
		0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e,
		0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
		0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75,
		0xdcd60dcf, 0xabd13d59, 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
		0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808,
		0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
		0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f,
		0x9fbfe4a5, 0xe8b8d433, 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
		0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162,
		0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
		0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49,
		0x8cd37cf3, 0xfbd44c65, 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
		0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc,
		0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
		0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3,
		0xb966d409, 0xce61e49f, 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
		0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6,
		0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
		0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d,
		0x0a00ae27, 0x7d079eb1, 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
		0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0,
		0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
		0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767,
		0x3fb506dd, 0x48b2364b, 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
		0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a,
		0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
		0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31,
		0x2cd99e8b, 0x5bdeae1d, 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
		0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14,
		0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
		0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b,
		0x6fb077e1, 0x18b74777, 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
		0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee,
		0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
		0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5,
		0x47b2cf7f, 0x30b5ffe9, 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
		0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8,
		0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d };

/**
 * Online CRC calculator:
 * http://www.zorc.breitbandkatze.de/crc.html
 */
uint32_t crc32(void *buf, uint32_t size) {
	return crc32inc(buf, 0, size);
}

uint32_t crc32inc(void *buf, uint32_t crc, uint32_t size) {
	uint8_t *p;

	p = (uint8_t*) buf;
	crc = crc ^ 0xFFFFFFFF;

	while (size--) {
		crc = crc32_tab[(crc ^ *p++) & 0xFF] ^ (crc >> 8);
	}

	return crc ^ 0xFFFFFFFF;
}

void Write_Config(void) {
	__disable_irq();
	uint32_t error;
	FLASH_EraseInitTypeDef erase_pages;
	erase_pages.NbPages = 2;
	erase_pages.TypeErase = TYPEERASE_PAGES;
	erase_pages.PageAddress = (uint32_t) &config_flash;
	HAL_FLASH_Unlock();
	HAL_FLASHEx_Erase(&erase_pages, &error);
	HAL_FLASH_Lock();

	uint16_t num_wrt;
	if (sizeof(config_t) & 0x0001)
		num_wrt = (sizeof(config_t) << 1) + 1;
	else
		num_wrt = sizeof(config_t) << 1;
	if (num_wrt > 2048)
		Error_Handler();

	HAL_FLASH_Unlock();
	uint16_t *data, *flash_address;
	data = (uint16_t*) &config_ram;
	flash_address = (uint16_t*) &config_flash;
	for (int i = 0; i < num_wrt; i++) {
		HAL_FLASH_Program(TYPEPROGRAM_HALFWORD, (uint32_t) flash_address,
				*data);
		data++;
		flash_address++;
	}
	HAL_FLASH_Lock();
	__enable_irq();
}

void TS_Comms_RX_Timeout(void) {
	if (RX.mode != IDLE) {
		if (RX.timeout) {
			RX.timeout--;
			if (RX.timeout == 0) {
				Comms_Reset(&RX);
			}
		}
	}
}