#include <global.h>

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

static uint8_t crc8_calc(uint8_t *data, size_t len);
static bool parity_calc(uint8_t *data, size_t len);
uint32_t mems_trx_word(uint32_t data);
uint32_t mems_trx_cmd(uint32_t cmd);
void mems_write_reg(int addr, int val);
uint32_t mems_read_reg(int addr);
int16_t mems_read_meas(int ch);
void mems_spi_init(void);
void mems_init(void);

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 res_seq;
    uint16_t meas_data[16];
    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);
}

enum mems_regs {
    MEMS_REG_CTRL0,             /* 0 */
    MEMS_REG_CTRL1,             /* 1 */
    MEMS_REG_CONFIG,            /* 2 */
    MEMS_REG_STATUS0,           /* 3 */
    MEMS_REG_STATUS1,           /* 4 */
    MEMS_REG_STATUS2,           /* 5 */
    MEMS_REG_CHIP_REVID,        /* 6 */
    MEMS_REG_ACC_CHX_LOW,       /* 7 */
    MEMS_REG_ACC_CHX_HIGH,      /* 8 */
    MEMS_REG_ACC_CHY_LOW,       /* 9 */
    MEMS_REG_ACC_CHY_HIGH,      /* 10 */
    MEMS_REG_OSC_COUNTER,       /* 11 */
    MEMS_REG_ID_SENSOR_TYPE,    /* 12 */
    MEMS_REG_ID_VEH_MANUF,      /* 13 */
    MEMS_REG_ID_SENSOR_MANUF,   /* 14 */
    MEMS_REG_ID_LOT0,           /* 15 */
    MEMS_REG_ID_LOT1,           /* 16 */
    MEMS_REG_ID_LOT2,           /* 17 */
    MEMS_REG_ID_LOT3,           /* 18 */
    MEMS_REG_ID_WAFER,          /* 19 */
    MEMS_REG_ID_COOR_X,         /* 20 */
    MEMS_REG_ID_COOR_Y,         /* 21 */
    MEMS_REG_RESET,             /* 22 */
    MEMS_REG_OFF_CHX_HIGH,      /* 23 */
    MEMS_REG_OFF_CHX_LOW,       /* 24 */
    MEMS_REG_OFF_CHY_HIGH,      /* 25 */
    MEMS_REG_OFF_CHY_LOW,       /* 26 */
};

uint8_t crc8_calc(uint8_t *data, size_t len) {
    int acc = 0;
    for (size_t i=0; i<len; i++) {
        acc ^= data[i];
        for (size_t j=0; j<8; j++) {
            acc <<= 1;
            if (acc & 0x100) {
                acc ^= 0x197; /* 0x100 | poly */
            }
        }
    }
    if (acc & 0x100)
        asm volatile ("bkpt");
    return acc;
}

#define MEMS_OPCODE_Msk 0x3
#define MEMS_OPCODE_Pos 30
#define MEMS_ADDR_Msk 0x1f
#define MEMS_ADDR_Pos 21
#define MEMS_DATA_Msk 0xff
#define MEMS_DATA_Pos 13
#define MEMS_P_Pos 28
#define MEMS_SEN_Pos 29
#define MEMS_MEAS_Pos 12
#define MEMS_MEAS_Msk 0x3fff

bool parity_calc(uint8_t *data, size_t len) {
    bool acc = 0;
    for (size_t i=0; i<len; i++) {
        uint8_t b = data[i];
        for (size_t j=0; j<8; j++) {
            if (b&1) {
                acc = !acc;
            }
            b >>= 1;
        }
    }
    return acc;
}

uint32_t mems_trx_word(uint32_t data) {
    /* CAUTION: ST's SPI peripherals behave differently depending on DR register access size, yet the CMSIS headers
     * expose it as an 32-bit uint only. In this case, we actually want a 32-bit access.
     */
    uint16_t *dr = (uint16_t *)&SPI1->DR;
    *dr = data>>16;
    while (SPI1->SR & SPI_SR_BSY)
        ;
    uint32_t out = (*dr) << 16;
    *dr = data&0xffff;
    while (SPI1->SR & SPI_SR_BSY)
        ;
    out |= *dr;
    return out;
}

uint32_t mems_trx_cmd(uint32_t cmd) {
    GPIOA->BRR = 1<<15; /* De-assert !CS */

    uint8_t bytes[3] = {(cmd>>16)&0xff, (cmd>>8)&0xff, cmd&0xff};
    uint8_t crc = crc8_calc(bytes, 3);
    int parity = !!parity_calc(bytes, 3);

    uint32_t out = mems_trx_word(cmd | (parity<<MEMS_P_Pos) | crc);
    GPIOA->BSRR = 1<<15; /* Assert !CS */

    return out;
}

void mems_write_reg(int addr, int val) {
    addr &= MEMS_ADDR_Msk;
    val &= MEMS_DATA_Msk;
    (void)mems_trx_cmd((1<<MEMS_OPCODE_Pos) | (addr<<MEMS_ADDR_Pos) | (val<<MEMS_DATA_Pos));
}

uint32_t mems_read_reg(int addr) {
    addr &= MEMS_ADDR_Msk;
    mems_trx_cmd((3<<MEMS_OPCODE_Pos) | (addr<<MEMS_ADDR_Pos));
    for (int i=0; i<2000; i++)
        asm volatile ("nop");
    uint32_t rv = mems_trx_cmd(3<<MEMS_OPCODE_Pos);
    return (rv >> MEMS_DATA_Pos) & MEMS_DATA_Msk;
}

int16_t mems_read_meas(int ch) {
    ch &= 3;
    mems_trx_cmd((ch<<MEMS_OPCODE_Pos) | (1<<MEMS_SEN_Pos));
    for (int i=0; i<10; i++)
        asm volatile ("nop");
    uint32_t rv = mems_trx_cmd(3<<MEMS_OPCODE_Pos);
    /* shift 14-bit data left to align the MSB with the int16_t's sign bit */
    int16_t data = (rv >> MEMS_MEAS_Pos) << 2;
    /* Now do an arithmetic division to sign extend */
    return data / 4;
}

void mems_spi_init(void) {
    SPI1->CR1 = (1<<SPI_CR1_BR_Pos) | SPI_CR1_MSTR | SPI_CR1_SSM | SPI_CR1_SSI;
    SPI1->CR2 = (15<<SPI_CR2_DS_Pos);
    SPI1->CR1 |= SPI_CR1_SPE;
}

void mems_init(void) {
    mems_spi_init();

    for (size_t i=0; i<10000; i++) {
        asm volatile("nop");
    }

    /* Take accelerometer out of initialization phase */
    mems_write_reg(MEMS_REG_CTRL0, 0x01);
}

#define WIN_LEN 8
int16_t meas_buf[WIN_LEN * 3] = {0};
size_t meas_buf_wptr = 0;
size_t meas_buf_rptr = 0;
int res_seq = 0;

void TIM1_BRK_TIM15_IRQHandler (void) {
    TIM15->SR = 0;
    int16_t data = mems_read_meas(0);

    /* write into meas_buf as circular buffer */
    meas_buf[meas_buf_wptr] = data;
    meas_buf_wptr += 1;
    if (meas_buf_wptr >= COUNT_OF(meas_buf)) {
        meas_buf_wptr = 0;
    }

    /* set read pointer to oldest 8-measurement block by rounding down meas_buf_wptr by 8, then adding 8 and wrapping */
    size_t tmp = 8 * (meas_buf_wptr / 8 + 1);
    if (tmp >= COUNT_OF(meas_buf)) {
        tmp = 0;
    }

    /* Update sequence pointer when the transmission window changes. */
    if (tmp != meas_buf_rptr) {
        res_seq += 1;
        meas_buf_rptr = tmp;
    }
}

int main(void) {

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

#define AFRL(pin, val) ((val) << ((pin)*4))
#define AFRH(pin, val) ((val) << (((pin)-8)*4))
#define AF(pin) (2<<(2*(pin)))
#define OUT(pin) (1<<(2*(pin)))
#define IN(pin) (0)
#define ANALOG(pin) (3<<(2*(pin)))
#define CLEAR(pin) (3<<(2*(pin)))

    /* GPIO pin config:
     * A9: USART 1 TX -> LED
     * A10: USART 1 RX -> debug
     * A15: Accelerometer CS
     * A5/6/7: SPI SCK/MISO/MOSI for Accelerometer
     */
    GPIOA->MODER &= ~(CLEAR(15)); /* Clear JTAG TDI pin mode */
    GPIOA->MODER |= AF(9) | AF(10) | OUT(15) | AF(5) | AF(6) | AF(7);
    GPIOA->AFR[0] = AFRL(5, 5) | AFRL(6, 5) | AFRL(7, 5);
    GPIOA->AFR[1] = AFRH(9, 7) | AFRH(10, 7);
    GPIOA->BSRR = 1<<15; /* De-assert accelerometer !CS */

    SystemCoreClockUpdate();
    int apb2_clock = SystemCoreClock / APB2_PRESC;
    
    TIM15->PSC = apb2_clock / 1000000 * 100 - 1; /* 100us ticks */
    TIM15->ARR = 1000 - 1; /* 100ms overflow interrupt interval */
    TIM15->DIER = TIM_DIER_UIE;
    TIM15->CR1 = TIM_CR1_CEN;
    NVIC_EnableIRQ(TIM1_BRK_TIM15_IRQn);

    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;

    mems_init();

    struct tx_state tx_st = { 0 };
    struct res_pkt res_buf = { 0 };
    uint8_t tx_buf[512];
    /*
    int req_seq = 0;
    struct req_pkt req_buf = { 0 };
    uint8_t rx_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 */
    while (23) {
        if (tx_st.remaining_bytes == 0) {
            res_buf.res_seq = res_seq;
            memcpy(res_buf.meas_data, meas_buf + meas_buf_rptr, 8 * sizeof(meas_buf[0]));
            memcpy(res_buf.meas_data + 8, meas_buf + ((meas_buf_rptr + 8) % COUNT_OF(meas_buf)) , 8 * sizeof(meas_buf[0]));
            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));;
        }

        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;
            (void) c;
            /*
            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 || (int)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 *memcpy(void *restrict dest, const void *restrict src, size_t n)
{
	unsigned char *d = dest;
	const unsigned char *s = src;

	for (; n; n--) *d++ = *s++;
	return dest;
}

void *memset(void *dest, int c, size_t n)
{
	unsigned char *s = dest;
	size_t k;

	/* Fill head and tail with minimal branching. Each
	 * conditional ensures that all the subsequently used
	 * offsets are well-defined and in the dest region. */

	if (!n) return dest;
	s[0] = c;
	s[n-1] = c;
	if (n <= 2) return dest;
	s[1] = c;
	s[2] = c;
	s[n-2] = c;
	s[n-3] = c;
	if (n <= 6) return dest;
	s[3] = c;
	s[n-4] = c;
	if (n <= 8) return dest;

	/* Advance pointer to align it at a 4-byte boundary,
	 * and truncate n to a multiple of 4. The previous code
	 * already took care of any head/tail that get cut off
	 * by the alignment. */

	k = -(uintptr_t)s & 3;
	s += k;
	n -= k;
	n &= -4;

	for (; n; n--, s++) *s = c;

	return dest;
}

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