#include "hal/uart2.h"
#include "libs/utils.h"
#include "libs/task.h"
#include "serial.h"
static uart_t uart2;

static u32 uart_poll_task(void);

void serial_init(void){
	circle_buffer_init(&uart2.c_tx_buff , uart2.tx_buf, sizeof(uart2.tx_buf));
	circle_buffer_init(&uart2.c_rx_buff , uart2.rx_buf, sizeof(uart2.rx_buf));
	UART2_Init();
	task_start(uart_poll_task, 0);
}

void serial_write(u8 *data, int len){
	circle_put_data(&uart2.c_tx_buff, data, len);
	UART2_EnableTx();
}
__weak void protocol_recv_frame(u8 *data, u16 rx_len) {}

static bool uart_on_rx_frame(uart_t *uart)
{
	u16 crc0 = shark_decode_u16(uart->rx_frame + uart->rx_length);
	u16 crc1 = shark_crc16_check(uart->rx_frame, uart->rx_length);

	if (crc0 != crc1) {
		return false;
	}
	protocol_recv_frame((u8 *)uart->rx_frame, uart->rx_length);
	return true;
}


static void uart_rx_poll(uart_t *uart) {
	u8 data;
	while(1) {
		if (circle_get_one_data(&uart->c_rx_buff, &data) != 1) {
			break;
		}
		
		switch(data){
			case CH_START:
				uart->rx_length = 0;
				uart->escape = false;
				break;
			case CH_END:
				if (uart->rx_length > 2 && uart->rx_length != 0xFFFF){
					uart->rx_length -= 2; //skip crc
					uart_on_rx_frame(uart);
				}
				uart->rx_length = 0xFFFF;
				break;
			case CH_ESC:
				uart->escape = true;
				break;
			default:
				if (uart->escape) {
					uart->escape = false;
					switch (data) {
						case CH_ESC_START:
							data = CH_START;
							break;

						case CH_ESC_END:
							data = CH_END;
							break;

						case CH_ESC_ESC:
							data = CH_ESC;
							break;

						default:
							data = 0xFF;
					}
				}

				if (uart->rx_length < sizeof(uart->rx_frame)) {
					uart->rx_frame[uart->rx_length] = data;
					uart->rx_length++;
				} else {
					uart->rx_length = 0xFFFF;
				}			
		}
	}
}

static u32 uart_poll_task(void){
	uart_rx_poll(&uart2);
	return 0;
}


static void uart_write_byte(uart_t *uart, u8 value)
{
	circle_put_data(&uart->c_tx_buff, &value, 1);
}

static void uart_write_byte_esc(uart_t *uart, u8 value)
{
	switch (value) {
	case CH_START:
		uart_write_byte(uart, CH_ESC);
		value = CH_ESC_START;
		break;

	case CH_END:
		uart_write_byte(uart, CH_ESC);
		value = CH_ESC_END;
		break;

	case CH_ESC:
		uart_write_byte(uart, CH_ESC);
		value = CH_ESC_ESC;
		break;
	}

	uart_write_byte(uart, value);
}

static void uart_write_esc(uart_t *uart, const u8 *buff, u16 length)
{
	const u8 *buff_end;

	for (buff_end = buff + length; buff < buff_end; buff++) {
		uart_write_byte_esc(uart, *buff);
	}
}

static void uart_tx_start(uart_t *uart)
{
	uart_write_byte(uart, CH_START);
	uart->tx_crc16 = 0;
}

static void uart_tx_continue(uart_t *uart, const void *buff, u16 length)
{
	uart_write_esc(uart, (const u8 *) buff, length);
	uart->tx_crc16 = shark_crc16_update(uart->tx_crc16, (const u8 *) buff, length);
}

static void uart_tx_end(uart_t *uart)
{
	uart_write_esc(uart, (u8 *)&uart->tx_crc16, sizeof(uart->tx_crc16));
	uart_write_byte(uart, CH_END);
	UART2_EnableTx();
}

void uart_write_frame(uint8_t *bytes, int len){
	uart_t *uart = &uart2;
	uart_tx_start(uart);
	uart_tx_continue(uart, bytes, len);
	uart_tx_end(uart);
}

bool uart_tx_finished(void) {
	u8 data;
	if (circle_get_one_data(&uart2.c_tx_buff, &data) <= 0){
		return true;
	}
	UART2_TxData(data);
	return false;
}

void uart_rx_put(u8 ch) {
	circle_put_one_data(&uart2.c_rx_buff, ch);
}

int fputc(int c, FILE *fp){
	u8 data = c;
	serial_write(&data, 1);
	return 1;
}