#include "p_serial_mgr.h"
#include "PCircularBuffer.h"
#include "putil.h"
#include "stm32l4xx_hal_uart.h"

#define MAX_SERIAL_BUFFER_SIZE (262) // Actually 261, add 1 for padding

// this protocol is very similar to the digi api v2 (inspired from)
// [0]{1} Start delimiter 0x7E
// [1]{1} Source Address
// [2]{1} Destination Address
// [3]{1} Length Byte
// [4-n]{n} frame data
// [n+1]{1} checksum
// Escape bytes are 0x7D, only frame data can be escaped
// Escaped bytes are followed by the byte to be escaped XOR'd with 0x20
// 0x7D and 0x7E need to be escaped (within the frame data)

typedef enum serial_state_t
{
	SS_IDLE = 0,	// waiting
	SS_START = 1,	// get start byte, interrupt after 4 more bytes
	SS_FRAME = 2,	// get byte
	SS_ESC = 3,		// get byte, dont increment number left
	SS_CHECKSUM = 4 // done
} serial_state_t;

static UART_HandleTypeDef *_serial_huart_inst = NULL;
static serial_state_t sstate = SS_IDLE;
static uint8_t rxc = '\0';

volatile serial_pkt_t pkt_bank[10];
volatile p_cb_serial_pkt_t serial_pkt_cb;

static volatile uint8_t start_index_tracker = 0;
static volatile uint8_t frame_index_tracker = 0;

#pragma message(Reminder "Move away from cirular buffer to a managed queue")

void UART1_RxCpltCallback(UART_HandleTypeDef *huart)
{
	switch (sstate)
	{
		case SS_IDLE: // packet start
		{
			if (rxc == 0x7E)
			{
				sstate = SS_START;
			}
		}
		break;
		case SS_START:
		{
			switch (start_index_tracker)
			{
				case 0: // source addr
				{
					serial_pkt_cb.buffer[serial_pkt_cb.head].src_addr = rxc;
					start_index_tracker++;
				}
				break;
				case 1:
				{
					serial_pkt_cb.buffer[serial_pkt_cb.head].dest_addr = rxc;
					start_index_tracker++;
				}
				break;
				case 2:
				{
					start_index_tracker = 0;
					frame_index_tracker = 0;
					serial_pkt_cb.buffer[serial_pkt_cb.head].len = rxc;
					sstate = SS_FRAME;
				}
				break;
				default:
				{
					// shouldnt get here
					asm volatile("nop");
				}
			};
		}
		break;
		case SS_FRAME:
		{
			if (rxc == 0x7E)
			{
				asm volatile("nop");
// error occured. bail
#pragma message(Reminder "add a safe escape routine for this")
				sstate = SS_IDLE;
				memset(&serial_pkt_cb.buffer[serial_pkt_cb.head], 0, sizeof(serial_pkt_t));
			}
			else if (rxc == 0x7D)
			{
				sstate = SS_ESC;
			}
			else
			{
				serial_pkt_cb.buffer[serial_pkt_cb.head].frame_data[frame_index_tracker++] = rxc;
				if (frame_index_tracker >= serial_pkt_cb.buffer[serial_pkt_cb.head].len)
				{
					sstate = SS_CHECKSUM;
				}
			}
		}
		break;
		case SS_ESC:
		{
			serial_pkt_cb.buffer[serial_pkt_cb.head].frame_data[frame_index_tracker++] = rxc ^ 0x20;
			if (frame_index_tracker >= serial_pkt_cb.buffer[serial_pkt_cb.head].len)
			{
				sstate = SS_CHECKSUM;
			}
			else
			{
				sstate = SS_FRAME;
			}
		}
		break;
		case SS_CHECKSUM:
		{
			frame_index_tracker = 0;
			serial_pkt_cb.buffer[serial_pkt_cb.head].checksum = rxc;
			serial_pkt_cb.buffer[serial_pkt_cb.head].b_ready = true;
			serial_pkt_cb.head = (serial_pkt_cb.head + 1) % serial_pkt_cb.max_len;
			sstate = SS_IDLE;
		}
		break;
		default:
		{
			PDEBUG("Shouldn't have hit this!\n");
		}
	};
	HAL_UART_Receive_IT(_serial_huart_inst, &rxc, 1);
	// serial_cb.push(&serial_cb, serial_cb_rxc);
	// HAL_GPIO_WritePin(USART1_DE_GPIO_Port, USART1_DE_Pin, 1);
	// huart2_rxc = huart1_rxc;

	// HAL_UART_Transmit(&huart2, &huart2_rxc, 1, 500);
	// HAL_UART_Receive_IT(&huart1, &huart1_rxc, 1);
	// HAL_GPIO_WritePin(USART1_DE_GPIO_Port, USART1_DE_Pin, 0);
}
void p_serial_mgr_init(UART_HandleTypeDef *huart)
{
	_serial_huart_inst = huart;
	_serial_huart_inst->RxCpltCallback = UART1_RxCpltCallback;

	memset(pkt_bank, 0, sizeof(serial_pkt_t) * 10);
	p_cb_serial_pkt_init(&serial_pkt_cb, pkt_bank, 10);
}

serial_pkt_t *p_serial_mgr_service(void)
{
	// this will be less garbage when i switch to a queue
	for (int ind = 0; ind < serial_pkt_cb.max_len; ind++)
	{
		if (serial_pkt_cb.buffer[ind].b_ready)
		{
			return &serial_pkt_cb.buffer[ind];
		}
	}
	return NULL;
}

void p_serial_mgr_start()
{
	HAL_UART_Receive_IT(_serial_huart_inst, &rxc, 1);
}