#include "devmode.h"

#include <hardware.h>

#include <fifo.h>

#include <assert.h>
#include <stddef.h>
#include <string.h>

u16
crc16(const void *buf, size_t len);

volatile struct fifo rx_fifo = {0, 0, {0}};

enum ack {
	ACK = 0x00,
	NACK = 0xFF
};

void
rx_isr(void) __critical __interrupt(0)
{
	fifo_push(&rx_fifo, sio_a_data);
}

static volatile uint32_t millis = 0;

void
ctc1_isr(void) __critical __interrupt(1)
{
	millis += 5;
}

uint32_t
clock(void)
{
	volatile uint32_t ret;
	DI;
	ret = millis;
	EI;
	return ret;
}

void
putbyte(unsigned char b)
{
	unsigned char ctrl = 0;

	sio_a_data = b;

	while (!(ctrl & 0x04)) {
		sio_a_ctrl = 0;
		ctrl = sio_a_ctrl;
	}
}

static volatile int32_t errno = 0;

u8
getbyte(void)
{
	u8 b;
	uint32_t ms = clock();
	errno = 0;
	while (fifo_empty(&rx_fifo)) {
		if (clock() - ms > TIMEOUT_MS) {
			errno = ERR_TIMEOUT;
			return 0;
		}
	}
	DI;
	b = fifo_pop(&rx_fifo);
	EI;
	return b;
}

void
flush(void)
{
	DI;
	fifo_clear(&rx_fifo);
	EI;
}


// Hamming(7,4) encoding
u8
encode(u8 x)
{
	u8 y = 0;
	const u8 c[4] = {0x61, 0x52, 0x34, 0x78};

	for (u8 i = 0; i < 4; ++i)
		y ^= ((x >> i) & 1) ? c[i] : 0;

	return y;
}

// Hamming(7,4) decoding
u8
decode(u8 x)
{
	u8 p = 0;
	const u8 r[7] = {6, 5, 3, 7, 1, 2, 4};

	for (int i = 0; i < 7; ++i)
		p ^= ((x >> i) & 1) ? r[i] : 0;

	// Assume simple error, attempt correction
	if (p) {
		size_t i = 0;

		for (i = 0; i < LENGTH(r); ++i) {
			if (r[i] == x)
				break;
		}

		x ^= (1 << i);
	}

	return x & 0x0F;
}


int
read(void *buf, size_t count)
{
	u8 b;
	u8 *p = buf;

	for (int n = 0; n < count; ++n) {
		b = decode(getbyte());
		if (errno)
			return errno;

		b |= (decode(getbyte()) << 4);
		if (errno)
			return errno;

		p[n] = b;
	}

	return 0;
}

int
write(const void *buf, size_t count)
{
	const u8 *p = buf;

	for (size_t i = 0; i < count; ++i) {
		putbyte(encode(p[i] & 0x0F));
		putbyte(encode((p[i] >> 4) & 0x0F));
	}

	return 0;
}

int
read_header(struct header *header)
{
	int err;
	u8 ack;
	u16 checksum;

	while (1) {
		if ((err = read(header, sizeof(*header))))
			return err;

		checksum = header->checksum;
		header->checksum = 0;

		if (checksum == crc16(header, sizeof(*header))) {
			header->checksum = checksum;
			ack = ACK;
			write(&ack, sizeof(ack));
			return 0;
		} else {
			ack = NACK;
			write(&ack, sizeof(ack));
		}
	}
}

int
read_buf(size_t len, void *buf)
{
	int err;
	u8 ack;
	u16 checksum;

	for (int i = 0; i < MAX_TRANS_ATTEMPTS; ++i) {
		// TODO: reduce code?
		if ((err = read(&checksum, sizeof(checksum)))
		    || (err = read(buf, len)))
			break;

		if (checksum == crc16(buf, len)) {
			ack = ACK;
			write(&ack, sizeof(ack));
			return 0;
		} else {
			ack = NACK;
			write(&ack, sizeof(ack));
		}
	}

	return -1;
}

int
write_buf(size_t len, const void *buf)
{
	int err;
	u8 ack = NACK;
	u16 checksum = crc16(buf, len);

	for (int i = 0; i < MAX_TRANS_ATTEMPTS; ++i){
		write(&checksum, sizeof(checksum));
		write(buf, len);

		// If TIMEOUT sending just give up
		if ((err = read(&ack, sizeof(ack))))
			return err;

		if (ack == ACK)
			return 0;
	}

	return -1;
}

// TODO: Restart after timeouts
void
loop(void)
{
	struct header header;
	u8 buf[MAX_PACKET_SIZE];

	while (1) {
		if (read_header(&header)) {
			flush();
			continue;
		}

		switch (header.type) {
		case CMD_BOOT:
			((void (*)(void))header.address)();
			break;

		case CMD_READ:
			write_buf(header.length, (const void *)header.address);
			break;

		case CMD_WRITE:
			if (!read_buf(header.length, buf))
				memcpy((void *)header.address, buf, header.length);
			break;

		case CMD_ECHO:
			if (!read_buf(header.length, buf))
				write_buf(header.length, buf);
			break;

		default:
			break;
		}

		flush();
	}
}

static inline void
init_ctc(void)
{
	ctc_channel_0 = CTC_CTRL(0);

	/* 200Hz clock */
	ctc_channel_1 = CTC_CTRL(CTC_INT_BIT | CTC_PRESCALER_BIT
				 | CTC_TIME_CONST_BIT | CTC_RST_BIT);
	ctc_channel_1 = 0;

	ctc_channel_2 = CTC_CTRL(0);
	ctc_channel_3 = CTC_CTRL(0);

	// Interrupt table for CTC
	// Final address is (Ireg << 8) | ctc_isr_ptr | {00/01/10/11} | 0
	ctc_channel_0 = CTC_INT_VEC(0x10);
}

static inline void
init_pio(void)
{
	// Disable PIO interrupts
	port_a_ctrl = PIO_INT_CTRL(0);
	port_b_ctrl = PIO_INT_CTRL(0);
}

static inline void
init_sio(void)
{
	static const u8 sio_a_cfg[] = {
		// Reset channel
		0b00011000,
		// wr4: X16 clock (115200), one stop bit, no parity
		4, 0b01000100,
		// wr1: interrupt on every Rx, no wait function
		1, SIO_RX_INT_MD0 | SIO_RX_INT_MD1,
		// wr3: enable Rx - 8 bit char
		3, 0b11000001
		// wr5: enable Tx - 8 bit char
		5, 0b01101000
	};

	static const u8 sio_b_cfg[] = {
		// Reset channel
		0b00011000,
		2, 0x1C // load interrupt vector, int_table_rx
	};

	for (u8 i = 0; i < LENGTH(sio_a_cfg); ++i)
		sio_a_ctrl = sio_a_cfg[i];

	for (u8 i = 0; i < LENGTH(sio_b_cfg); ++i)
		sio_b_ctrl = sio_b_cfg[i];
}

void
main(void)
{
	init_ctc();
	init_pio();
	init_sio();

	IM(2);
	EI;

	loop();
}