#include <global.h>

#include "microcobs.h"
#include "crc32.h"

struct __attribute__((packed)) ll_pkt_trailer {
    uint32_t crc32;
};

struct __attribute__((packed)) req_pkt {
    uint32_t req_seq;
    struct ll_pkt_trailer trailer;
};

struct __attribute__((packed)) res_pkt {
    uint32_t req_seq;
    uint32_t res_seq;
    struct ll_pkt_trailer trailer;
};

struct tx_state {
    uint8_t *tx_char;
    int remaining_bytes;
};

static crc32_t pkt_crc(void *pkt, struct ll_pkt_trailer *trailer);
crc32_t pkt_crc(void *pkt, struct ll_pkt_trailer *trailer) {
    crc32_t crc = crc32_reset();
    for (uint8_t *in = (uint8_t *)pkt; in < (uint8_t *)trailer; in++) {
        crc = crc32_update(crc, *in);
    }
    return crc32_finalize(crc);
}

static void packetize(void *pkt, struct ll_pkt_trailer *trailer);
void packetize(void *pkt, struct ll_pkt_trailer *trailer) {
    trailer->crc32 = pkt_crc(pkt, trailer);
}

int main(void) {

    RCC->AHBENR |= RCC_AHBENR_GPIOAEN;
    RCC->APB2ENR |= RCC_APB2ENR_USART1EN;

    GPIOA->MODER |= (2 << GPIO_MODER_MODER9_Pos) | (2 << GPIO_MODER_MODER10_Pos);
    GPIOA->AFR[1] = (7 << (9-8)*4) | (7 << (10-8)*4);

    SystemCoreClockUpdate();
    int apb2_clock = SystemCoreClock / APB2_PRESC;
    
    int baudrate = 115200;

    USART1->CR1 = USART_CR1_TE | USART_CR1_RE;
    USART1->BRR = (apb2_clock + baudrate/2) / baudrate;
    USART1->CR2 |= USART_CR2_RXINV; //| USART_CR2_TXINV;
    USART1->CR1 |= USART_CR1_UE;

    int req_seq = 0;
    int res_seq = 0;
    struct req_pkt req_buf = { 0 };
    struct tx_state tx_st = { 0 };
    struct res_pkt res_buf = { 0 };
    uint8_t rx_buf[512];
    uint8_t tx_buf[512];
    size_t rx_char = 0;
    unsigned int rx_overrun = 0;
    unsigned int rx_cobs_error = 0;
    unsigned int rx_framing_error = 0;
    unsigned int rx_crc_error = 0;

    USART1->TDR = 0; /* Kick off transmission */
    int i = 0;
    while (23) {
        if (tx_st.remaining_bytes == 0) {
            if (i > 100) {
                res_buf.req_seq = req_seq;
                res_buf.res_seq = res_seq;
                res_seq += 1;
                packetize(&res_buf, &res_buf.trailer);
                tx_st.tx_char = tx_buf;
                tx_st.remaining_bytes = cobs_encode((uint8_t *)&res_buf, sizeof(res_buf), tx_buf, sizeof(tx_buf));;
                /*
                for (int k=0; k < 8; k++)
                    tx_buf[k] = 255-k;
                for (int k=0; k < 8; k++)
                    tx_buf[8+k] = (k + 1) % 8;
                tx_st.remaining_bytes = 16;
                */
                i = 0;
            } else {
                i++;
            }
        }

        if (USART1->ISR & USART_ISR_TXE && tx_st.remaining_bytes > 0) {
            USART1->TDR = *(tx_st.tx_char);
            tx_st.tx_char += 1;
            tx_st.remaining_bytes -= 1;
        }

        if (USART1->ISR & USART_ISR_ORE)
            USART1->ICR = USART_ICR_ORECF;

        if (USART1->ISR & USART_ISR_NE)
            USART1->ICR = USART_ICR_NCF;

        if (USART1->ISR & USART_ISR_FE)
            USART1->ICR = USART_ICR_FECF;

        if (USART1->ISR & USART_ISR_RXNE) {
            uint8_t c = USART1->RDR;
            if (!c) {
                if (rx_char < sizeof(rx_buf)) {
                    int rc = cobs_decode(rx_buf, rx_char, (uint8_t *)&req_buf, sizeof(req_buf));
                    if (rc < 0) {
                        rx_cobs_error += 1;
                    } else {
                        if (rc == sizeof(req_buf)) {
                            crc32_t check_crc = pkt_crc(&req_buf, &req_buf.trailer);
                            if (check_crc != req_buf.trailer.crc32 || check_crc == 0 || check_crc == -1) {
                                rx_crc_error += 1;
                            } else {
                                req_seq = req_buf.req_seq;
                            }
                        } else {
                            rx_framing_error += 1;
                        }
                    }
                }
                rx_char = 0;
            } else {
                if (rx_char < sizeof(rx_buf)) {
                    rx_buf[rx_char] = c;
                    rx_char += 1;
                } else {
                    rx_overrun += 1;
                }
            }
        }
    }
}

void __libc_init_array (void) __attribute__((weak));
void __libc_init_array ()
{
}