#include <zbootloader.h>
#include <zeta.h>
#include <stddef.h>
#include <string.h>

/* volatile uint8_t rx_head = 0; */
/* volatile uint8_t rx_tail = 0; */
/* volatile uint8_t rx_fifo[FIFO_LEN]; */

struct fifo {
    uint8_t head;
    uint8_t tail;
    uint8_t data[FIFO_LEN];
};

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

uint8_t
fifo_pop(struct fifo *fifo)
{
    uint8_t ret = fifo->data[fifo->head];
    if (++fifo->head >= LEN(fifo->data))
        fifo->head = 0;
    return ret;
}

void
fifo_push(struct fifo *fifo, uint8_t v)
{
    fifo->data[fifo->tail] = v;

    if (++fifo->tail >= LEN(fifo->data))
        fifo->tail = 0;
}

bool
fifo_empty(const struct fifo *fifo)
{
    return (fifo->head == fifo->tail);
}

void
fifo_clear(struct fifo *fifo)
{
    fifo->head = 0;
    fifo->tail = 0;
}

static const uint8_t sio_a_cfg[] = {
    0b00011000,                      // Reset channel
    4         ,                      // wr4
    0b00000100,                      // X1 clock, one stop bit, no parity
    1         ,                      // wr1
    SIO_RX_INT_MD0 | SIO_RX_INT_MD1, // interrupt on every Rx, no wait function
    3         ,                      // wr3
    0b11000001,                      // enable Rx - 8 bit char
    5         ,                      // wr5
    0b01101000                       // enable Tx - 8 bit char
};

extern void *rx_isr_ptr;
extern void *ctc0_isr_ptr;
extern void *ctc1_isr_ptr;
extern void *ctc2_isr_ptr;
extern void *ctc3_isr_ptr;

#define ISR_OFFSET(x) ((unsigned int)&x)

static const unsigned char sio_b_cfg[] = {
    0b00011000,   // Reset channel
    2         ,   // load interrupt vector
    ISR_OFFSET(rx_isr_ptr)          // int_table_rx
};

/* int_table: */
/* int_table_rx: */
/*     dw      rx_isr */

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

static volatile uint32_t millis = 0;

void
ctc3_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;

uint8_t
getbyte(void)
{
    uint8_t 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
uint8_t
encode(uint8_t x)
{
    uint8_t y = 0;
    const uint8_t c[4] = {0x61, 0x52, 0x34, 0x78};

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

    return y;
}

// Hamming(7,4) decoding
uint8_t
decode(uint8_t x)
{
    uint8_t p = 0;
    const uint8_t 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 < LEN(r); ++i) {
            if (r[i] == x)
                break;
        }

        x ^= (1 << i);
    }

    return x & 0x0F;
}


int
read(void *buf, size_t count)
{
    uint8_t b;
    uint8_t *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 uint8_t *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;
    uint8_t ack;
    uint16_t 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;
    uint8_t ack;
    uint16_t 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;
    uint8_t ack = NACK;
    uint16_t 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;
    uint8_t 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();
    }
}

int
main(void)
{
    // Init CTC
    // 16 prescaler, timer mode
    ctc_channel_1 = (CTC_CLK_TRG_BIT | CTC_TIME_CONST_BIT | CTC_RST_BIT
                     | CTC_CTRL_OR_VECTOR_BIT);
    ctc_channel_1 = (CPU_FREQ / 16 / 9600);

    // 200Hz clock
    ctc_channel_3 = (CTC_INT_BIT | CTC_PRESCALER_BIT | CTC_CLK_TRG_BIT |
                     CTC_TIME_CONST_BIT | CTC_RST_BIT | CTC_CTRL_OR_VECTOR_BIT);
    ctc_channel_3 = (CPU_FREQ / 256 / 36);
    ctc_channel_0 = ISR_OFFSET(ctc3_isr_ptr) | (3 << 1);

    // Init SIO
    for (uint8_t i = 0; i < LEN(sio_a_cfg); ++i)
        sio_a_ctrl = sio_a_cfg[i];

    for (uint8_t i = 0; i < LEN(sio_b_cfg); ++i)
        sio_b_ctrl = sio_b_cfg[i];

    // Interrupt mode 2
    IM(2);
    // Enable interrupts
    EI;

    loop();
    return 0;
}