#include <global.h>
#include <string.h>
#include "8b10b.h"
#include "crc32.h"
#include "xorshift.h"
#include "protocol.h"
#include "generated/waveform_tables.h"

volatile uint64_t sys_time_us;

static uint32_t read_fuse_monitor(void);
static void set_rj45_leds(uint32_t leds);
static void set_status_leds(uint32_t leds);
static void dma_tx_constant(size_t table_size, uint16_t constant);
static void dma_tx_waveform(size_t table_size, const uint16_t *table);

#define SYNC_INTERVAL 13

static size_t tx_bitpos = 0;
static size_t tx_sympos = 0;
static int tx_last_bit = 0;
static struct state_8b10b_enc encoder_state_8b10b;
static uint32_t packet_rng_state;

union tx_buf_union {
    struct data_packet packet;
    uint8_t bytes[sizeof(struct data_packet)];
};

static union tx_buf_union tx_buf[3];
static union tx_buf_union *tx_buf_read = &tx_buf[0];
static union tx_buf_union *tx_buf_idle = &tx_buf[1];
static union tx_buf_union *tx_buf_write = &tx_buf[2];
static bool idle_buf_ready = false;

void update_tx_buf(void);
int hex_to_int(char *hex, size_t len);

int hex_to_int(char *hex, size_t len) {
    int rv = 0;
    while (len--) {
        rv = rv<<4;
        char c = hex[len];
        if ('0' <= c && c <= '9')
            c = c - '0';
        else if ('a' <= c && c <= 'f')
            c = c - 'a' + 0xa;
        else if ('A' <= c && c <= 'F')
            c = c - 'A' + 0xa;
        else
            c = 0;
        rv |= c;
    }
    return rv;
}

enum leds {
    LED_ON = 1,
    LED_PING = 2,
    LED_OVERHEAT = 4,
    LED_CONTROL_ERR = 8,
    LED_INPUT_ERR = 16,
    LED_OUTPUT_ERR = 32
};

char rxbuf[256];
size_t rxp = 0;

int main(void) {
    /* Configure clocks for 64 MHz system clock.
     * 
     * HSE @ 8 MHz --[PLL x16 /2]--> PLL "R" clock @ 64 MHz
     */
    /* Enable peripherals */
    RCC->APBENR1 |= RCC_APBENR1_PWREN;
    /* Enable High-speed external crystal oscillator. The board has an 8 MHz crystal. */
    RCC->CR |= RCC_CR_HSEON;
    while (!(RCC->CR & RCC_CR_HSERDY)) {
        /* wait for HSE osc to stabilize. */
    }
    /* Increase flash wait states to 2 required for operation above 48 MHz */
    FLASH->ACR = (FLASH->ACR & ~FLASH_ACR_LATENCY_Msk) | (2<<FLASH_ACR_LATENCY_Pos);
    while ((FLASH->ACR & FLASH_ACR_LATENCY_Msk) != (2<<FLASH_ACR_LATENCY_Pos)) {
        /* wait for flash controller to acknowledge change. */
    }
    /* Configure PLL with multiplier 16, divisor 2 for "R" output, and enable "R" (sysclk) output */
    RCC->PLLCFGR = (16<<RCC_PLLCFGR_PLLN_Pos) | (3<<RCC_PLLCFGR_PLLSRC_Pos) | (1<<RCC_PLLCFGR_PLLR_Pos) | RCC_PLLCFGR_PLLREN;
    RCC->CR |= RCC_CR_PLLON;
    while (!(RCC->CR & RCC_CR_PLLRDY)) {
        /* wait for PLL to stabilize. */
    }
    /* Switch SYSCLK to PLL source. */
    RCC->CFGR |= (2<<RCC_CFGR_SW_Pos);
    while ((RCC->CFGR & RCC_CFGR_SWS_Msk) != (2<<RCC_CFGR_SWS_Pos)) {
        /* wait for RCC to switch over. */
    }

    RCC->AHBENR |= RCC_AHBENR_DMA1EN;
    RCC->APBENR1 |= RCC_APBENR1_USART3EN | RCC_APBENR1_I2C1EN;
    RCC->APBENR2 |= RCC_APBENR2_USART1EN | RCC_APBENR2_TIM1EN;
    RCC->IOPENR |= RCC_IOPENR_GPIOAEN | RCC_IOPENR_GPIOBEN | RCC_IOPENR_GPIOCEN | RCC_IOPENR_GPIODEN;

    /* GPIOA:
     * A0: MON_H
     * A1: MON_FAULT_CURRENT
     * A2: MON_L
     * A3: (testpoint)
     * A4: VIN_MON
     * A5: (testpoint)
     * A6: RJ45 LED 2
     * A7: Pulse RX
     * A8: Fuse monitor 6
     * A9: RS485 TX
     * A10: RS485 RX
     * A11: Fuse monitor 1
     * A12: RS485 DE
     * A13: SWDIO
     * A14: SWCLK
     * A15: Fuse monitor 4
     */
    GPIOA->MODER = 
        ANALOG(0) | ANALOG(1) | ANALOG(2) | ANALOG(4) |
        IN(3) | IN(5) |
        OUT(6) |
        ANALOG(7) |
        IN(8) | IN(11) | IN(15) |
        AF(9) | AF(10) | AF(12) |
        AF(13) | AF(14);
    GPIOA->AFR[1] = AFRH(9, 1) | AFRH(10, 1) | AFRH(12, 1) | AFRH(13, 0) | AFRH(14, 0);
    GPIOA->OSPEEDR = (3<<(2*9)) | (3<<(2*12)) | (3<<(2*13));

    /* GPIOB:
     * B0: Driver A low (TIM1_CH2N)
     * B1: Driver B low (TIM1_CH3N)
     * B2: RJ45 LED 1
     * B3: Driver A high (TIM1_CH2)
     * B4: V_ISO_SENSE
     * B5: (testpoint)
     * B6: SCL
     * B7: SDA
     * B8: DBG_TX
     * B9: DBG_RX
     * B10: LED 3 "On"
     * B11: LED 5 "RS458 Ping"
     * B12: LED 1 "Overheating"
     * B13: LED 6 "Control Error"
     * B14: LED 4 "Input Error"
     * B15: LED 2 "Output Error"
     */
    GPIOB->MODER =
        AF(0) | AF(1) | AF(3) |
        OUT(2) |
        IN(4) |
        IN(5) |
        AF(6) | AF(7) |
        AF(8) | AF(9) |
        OUT(10) | OUT(11) | OUT(12) | OUT(13) | OUT(14) | OUT(15);
    GPIOB->AFR[0] = AFRL(0, 2) | AFRL(1, 2) | AFRL(3, 1) | AFRL(6, 6) | AFRL(7, 6);
    GPIOB->AFR[1] = AFRH(8, 4) | AFRH(9, 4);
    GPIOB->OSPEEDR = (3<<0) | (3<<1) | (3<<3);

    /* GPIOC:
     * C0, C3: (testpoint)
     * C1: Fan 2 PWM
     * C2: Fan 1 PWM
     * C4: RJ45 LED 4
     * C5: RJ45 LED 3
     * C6: Fuse monitor 7
     * C7: Fuse monitor 2
     * C8: Fuse monitor 5
     * C9: (testpoint)
     * C10: Driver B high
     * C11-C15: (testpoint)
     */
    GPIOC->MODER =
        IN(0) | OUT(1) | OUT(2) | IN(3) | IN(9) | IN(11) | IN(12) | IN(13) | IN(14) | IN(15) |
        OUT(4) | OUT(5) |
        IN(6) | IN(7) | IN(8) |
        AF(10);
    GPIOC->OTYPER = (1<<1) | (1<<2);
    GPIOC->AFR[1] = AFRH(10, 2);
    GPIOC->OSPEEDR = (3<<10);
    GPIOC->BRR = (1<<1) | (1<<2); /* Turn down fans (most fans don't turn off at 0% PWM) */

    /* GPIOD:
     * D0-D6: (testpoint)
     * D8: Fuse monitor 3
     * D9: Fuse monitor 0
     */
    GPIOD->MODER = IN(0) | IN(1) | IN(2) | IN(3) | IN(4) | IN(5) | IN(6) |
        IN(8) | IN(9);

    for (int i=0; i<10; i++) {
        set_status_leds(LED_ON);
        delay_us(250*1000);
        set_status_leds(0);
        delay_us(250*1000);
    }

    GPIOC->BSRR = (1<<1) | (1<<2); /* Enable fans */

    for (int i=0; i<10; i++) {
        set_status_leds(LED_ON);
        delay_us(250*1000);
        set_status_leds(0);
        delay_us(250*1000);
    }

    TIM1->CCMR1 = (6<<TIM_CCMR1_OC2M_Pos) | TIM_CCMR1_OC2PE;
    TIM1->CCMR2 = (6<<TIM_CCMR2_OC3M_Pos) | TIM_CCMR2_OC3PE;
    TIM1->CCER = TIM_CCER_CC2E | TIM_CCER_CC2NE | TIM_CCER_CC2NP | TIM_CCER_CC3E | TIM_CCER_CC3NE | TIM_CCER_CC3P;
    TIM1->BDTR = (8<<TIM_BDTR_DTG_Pos) | TIM_BDTR_MOE;
    TIM1->DCR = (14<<TIM_DCR_DBA_Pos) | (1<<TIM_DCR_DBL_Pos);
    TIM1->PSC = 3;
    TIM1->ARR = 250;
    TIM1->CCR2 = 64;
    TIM1->CCR3 = 192;
    TIM1->DIER = TIM_DIER_UDE;
    TIM1->CR1 |= TIM_CR1_CEN;

    DMAMUX1->CCR = 25;
    DMA1_Channel1->CPAR = (uint32_t)&TIM1->DMAR;
    NVIC_EnableIRQ(DMA1_Channel1_IRQn);
    NVIC_SetPriority(DMA1_Channel1_IRQn, 0);
    dma_tx_constant(COUNT_OF(waveform_zero_one), 0x00);
    xfr_8b10b_encode_reset(&encoder_state_8b10b);

    USART1->CR1 = /* 8-bit -> M1, M0 clear */
        /* OVER8 clear. Use default 16x oversampling */
        /* CMIF clear */
        /* MME clear */
        /* WAKE clear */
        /* PCE, PS clear */
        /* RXNEIE, other interrupts clear */
          USART_CR1_TE
        | USART_CR1_RE;
    USART1->CR3 |= USART_CR3_DEM; /* Output DE signal on RTS pin */
    USART1->BRR = 6667; /* Set baudrate to 9600 Bd */
    USART1->CR1 |= USART_CR1_UE; /* And... go! */

    int rj45_rx_ctr = 0;
    int ping_ctr = 0;
    int control_err_ctr = 0;
    while (23) {
        if (rj45_rx_ctr) {
            set_rj45_leds(0x5);
            rj45_rx_ctr--;

        } else {
            set_rj45_leds(0x0);
        }

        int leds = LED_ON;
        if (ping_ctr) {
            leds |= LED_PING;
            ping_ctr--;
        }
        if (control_err_ctr) {
            leds |= LED_CONTROL_ERR;
            control_err_ctr--;
        }
        set_status_leds(leds);

        int isr = USART1->ISR;
        if ((isr & USART_ISR_ORE) || (isr & USART_ISR_FE)) {
            USART1->ICR = USART_ICR_ORECF | USART_ICR_FECF;
        }

        if (isr & USART_ISR_RXNE_RXFNE) {
            rj45_rx_ctr = 20000;
            char c = USART1->RDR;
            if (c == '\n') {
                if (rxp > 2 && rxp != 4*16*3) {
                    control_err_ctr = 1000000;
                    rxp = 0;
                    continue;
                }

                for (size_t i=0; i<16; i++) {
                    int brightness = hex_to_int(&rxbuf[DRIVER_ADDR*16*3 + i*3], 1);
                    int channels = hex_to_int(&rxbuf[DRIVER_ADDR*16*3 + i*3 + 1], 2);

                    tx_buf_write->packet.channels[i] = channels;

                    if ((i&1) == 0) {
                        tx_buf_write->packet.brightness[i>>1] = brightness;
                    } else {
                        tx_buf_write->packet.brightness[i>>1] |= brightness<<4;
                    }
                }
                rxp = 0;
                ping_ctr = 500000;
                update_tx_buf();

            } else {
                if (rxp <= sizeof(rxbuf)) {
                    rxbuf[rxp] = c;
                    rxp ++;
                }
            }
        }
    }
}

void dma_tx_waveform(size_t table_size, const uint16_t *table) {
    DMA1_Channel1->CCR = 0;
    DMA1_Channel1->CCR = (1<<DMA_CCR_MSIZE_Pos) | (1<<DMA_CCR_PSIZE_Pos) | DMA_CCR_MINC | DMA_CCR_DIR | DMA_CCR_TCIE;
    DMA1_Channel1->CNDTR = table_size;
    DMA1_Channel1->CMAR = (uint32_t)table;
    DMA1_Channel1->CCR |= DMA_CCR_EN;
}

void dma_tx_constant(size_t table_size, uint16_t constant) {
    static uint16_t tx_constant[2];
    tx_constant[0] = constant;
    tx_constant[1] = constant;

    DMA1_Channel1->CCR = 0;
    DMA1_Channel1->CCR = (1<<DMA_CCR_MSIZE_Pos) | (1<<DMA_CCR_PSIZE_Pos) | DMA_CCR_DIR | DMA_CCR_TCIE;
    DMA1_Channel1->CNDTR = table_size;
    DMA1_Channel1->CMAR = (uint32_t)&tx_constant;
    DMA1_Channel1->CCR |= DMA_CCR_EN;
}

int8_t bit_arr[4096];
size_t bit_pos = 0;
int16_t sym_arr[512];
size_t sym_pos = 0;
uint16_t cnd_arr[512];
size_t cnd_pos = 0;

void DMA1_Channel1_IRQHandler() {
    static int transfer_errors = 0;
    static int current_symbol = 0;

    if (DMA1->ISR & DMA_ISR_TEIF1) {
        transfer_errors ++;
    }

    DMA1->IFCR = DMA_IFCR_CGIF1;

    int bit = !!(current_symbol & (1<<tx_bitpos));
    bit_arr[bit_pos++] = bit;
    if (bit_pos == COUNT_OF(bit_arr)) {
        bit_pos = 0;
    }

    if (tx_last_bit == bit) {
        dma_tx_constant(COUNT_OF(waveform_zero_one), bit ? WAVEFORM_CONST_ONE : WAVEFORM_CONST_ZERO);
    } else if (bit) {
        dma_tx_waveform(COUNT_OF(waveform_zero_one), waveform_zero_one);
    } else {
        dma_tx_waveform(COUNT_OF(waveform_zero_one), waveform_one_zero);
    }

    tx_last_bit = bit;

    if (tx_bitpos == 0) {
        tx_bitpos = 9;

        sym_arr[sym_pos] = -255;

        if (tx_sympos == sizeof(struct data_packet)) {
            if (idle_buf_ready) {
                union tx_buf_union *tmp = tx_buf_idle;
                tx_buf_idle = tx_buf_read;
                tx_buf_read = tmp;
                idle_buf_ready = false;

                current_symbol = xfr_8b10b_encode(&encoder_state_8b10b, -K28_1);
                sym_arr[sym_pos] = current_symbol;
                packet_rng_state = xorshift32(1);

                tx_sympos = 0;

            } else {
                current_symbol = xfr_8b10b_encode(&encoder_state_8b10b, -K23_7);
                sym_arr[sym_pos] = current_symbol;
            }

        } else {
            uint8_t b = tx_buf_read->bytes[tx_sympos];
            packet_rng_state = xorshift32(packet_rng_state);
            b ^= packet_rng_state;
            current_symbol = xfr_8b10b_encode(&encoder_state_8b10b, b);
            sym_arr[sym_pos] = current_symbol;
            tx_sympos ++;
        }

        sym_pos++;
        if (sym_pos == COUNT_OF(sym_arr)) {
            sym_pos = 0;
        }

    } else {
        tx_bitpos --;
    }
}

void update_tx_buf() {
    uint32_t crc = crc32_reset();
    for (size_t i=0; i<offsetof(struct data_packet, crc); i++) {
        crc = crc32_update(crc, tx_buf_write->bytes[i]);
    }
    tx_buf_write->packet.crc = crc32_finalize(crc);

    __disable_irq();
    union tx_buf_union *tmp = tx_buf_idle;
    tx_buf_idle = tx_buf_write;
    tx_buf_write = tmp;
    idle_buf_ready = true;
    __enable_irq();
}

uint32_t read_fuse_monitor() {
    uint32_t idr_a = GPIOA->IDR;
    uint32_t idr_c = GPIOC->IDR;
    uint32_t idr_d = GPIOD->IDR;

    int fm0 = !!(idr_d & (1<<9));
    int fm1 = !!(idr_a & (1<<11));
    int fm2 = !!(idr_c & (1<<7));
    int fm3 = !!(idr_d & (1<<8));
    int fm4 = !!(idr_a & (1<<15));
    int fm5 = !!(idr_c & (1<<8));
    int fm6 = !!(idr_a & (1<<8));
    int fm7 = !!(idr_c & (1<<6));

    return (fm0<<0) | (fm1<<1) | (fm2<<2) | (fm3<<3) | (fm4<<4) | (fm5<<5) | (fm6<<6) | (fm7<<7);
}

void set_rj45_leds(uint32_t leds) {
    leds = ~leds;

    if (leds&1) {
        GPIOB->BSRR = (1<<2); 
    } else {
        GPIOB->BSRR = (1<<2)<<16; 
    }

    if (leds&2) {
        GPIOA->BSRR = (1<<6); 
    } else {
        GPIOA->BSRR = (1<<6)<<16; 
    }

    if (leds&4) {
        GPIOC->BSRR = (1<<5); 
    } else {
        GPIOC->BSRR = (1<<5)<<16; 
    }

    if (leds&8) {
        GPIOC->BSRR = (1<<4); 
    } else {
        GPIOC->BSRR = (1<<4)<<16; 
    }
}

void set_status_leds(uint32_t leds) {
    GPIOB->BSRR = ((0x3f<<10)<<16) | (((~leds)&0x3f)<<10);
}

void SysTick_Handler() {
    sys_time_us += SYSTICK_INTERVAL_US;
}

void HardFault_Handler() {
    asm volatile ("bkpt");
}

void delay_us(int duration_us) {
    while (duration_us--) {
        for (int i=0; i<3; i++) {
            asm volatile ("nop");
        }
    }
}

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 *memmove(void *dest, const void *src, size_t n)
{
    return memcpy(dest, src, n);
}

void *memset(void *dest, int c, size_t n)
{
    unsigned char *d = dest;
    while (n--) {
        *d++ = c;
    }
	return dest;
}

size_t strlen(const char *s)
{
    const char *start = s;
    while (*s) {
        s++;
    }
    return s - start;
}

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