#include <global.h>
#include <string.h>

volatile uint64_t sys_time_us;

uint32_t read_fuse_monitor(void);
void set_rj45_leds(uint32_t leds);
void set_status_leds(uint32_t leds);

uint16_t waveform[512] = {
    128, 255,
131, 255,
134, 255,
137, 255,
140, 254,
143, 254,
146, 254,
149, 253,
152, 253,
155, 252,
158, 251,
162, 250,
165, 250,
167, 249,
170, 248,
173, 246,
176, 245,
179, 244,
182, 243,
185, 241,
188, 240,
190, 238,
193, 237,
196, 235,
198, 234,
201, 232,
203, 230,
206, 228,
208, 226,
211, 224,
213, 222,
215, 220,
218, 218,
220, 215,
222, 213,
224, 211,
226, 208,
228, 206,
230, 203,
232, 201,
234, 198,
235, 196,
237, 193,
238, 190,
240, 188,
241, 185,
243, 182,
244, 179,
245, 176,
246, 173,
248, 170,
249, 167,
250, 165,
250, 162,
251, 158,
252, 155,
253, 152,
253, 149,
254, 146,
254, 143,
254, 140,
255, 137,
255, 134,
255, 131,
255, 128,
255, 124,
255, 121,
255, 118,
254, 115,
254, 112,
254, 109,
253, 106,
253, 103,
252, 100,
251, 97,
250, 93,
250, 90,
249, 88,
248, 85,
246, 82,
245, 79,
244, 76,
243, 73,
241, 70,
240, 67,
238, 65,
237, 62,
235, 59,
234, 57,
232, 54,
230, 52,
228, 49,
226, 47,
224, 44,
222, 42,
220, 40,
218, 37,
215, 35,
213, 33,
211, 31,
208, 29,
206, 27,
203, 25,
201, 23,
198, 21,
196, 20,
193, 18,
190, 17,
188, 15,
185, 14,
182, 12,
179, 11,
176, 10,
173, 9,
170, 7,
167, 6,
165, 5,
162, 5,
158, 4,
155, 3,
152, 2,
149, 2,
146, 1,
143, 1,
140, 1,
137, 0,
134, 0,
131, 0,
128, 0,
124, 0,
121, 0,
118, 0,
115, 1,
112, 1,
109, 1,
106, 2,
103, 2,
100, 3,
97, 4,
93, 5,
90, 5,
88, 6,
85, 7,
82, 9,
79, 10,
76, 11,
73, 12,
70, 14,
67, 15,
65, 17,
62, 18,
59, 20,
57, 21,
54, 23,
52, 25,
49, 27,
47, 29,
44, 31,
42, 33,
40, 35,
37, 37,
35, 40,
33, 42,
31, 44,
29, 47,
27, 49,
25, 52,
23, 54,
21, 57,
20, 59,
18, 62,
17, 65,
15, 67,
14, 70,
12, 73,
11, 76,
10, 79,
9, 82,
7, 85,
6, 88,
5, 90,
5, 93,
4, 97,
3, 100,
2, 103,
2, 106,
1, 109,
1, 112,
1, 115,
0, 118,
0, 121,
0, 124,
0, 127,
0, 131,
0, 134,
0, 137,
1, 140,
1, 143,
1, 146,
2, 149,
2, 152,
3, 155,
4, 158,
5, 162,
5, 165,
6, 167,
7, 170,
9, 173,
10, 176,
11, 179,
12, 182,
14, 185,
15, 188,
17, 190,
18, 193,
20, 196,
21, 198,
23, 201,
25, 203,
27, 206,
29, 208,
31, 211,
33, 213,
35, 215,
37, 218,
40, 220,
42, 222,
44, 224,
47, 226,
49, 228,
52, 230,
54, 232,
57, 234,
59, 235,
62, 237,
65, 238,
67, 240,
70, 241,
73, 243,
76, 244,
79, 245,
82, 246,
85, 248,
88, 249,
90, 250,
93, 250,
97, 251,
100, 252,
103, 253,
106, 253,
109, 254,
112, 254,
115, 254,
118, 255,
121, 255,
124, 255,
};


int main(void) {
    /* Configure clocks for 168 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 5 required for operation above 136 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)
     * 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) | IN(1) | IN(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->AFR[1] = AFRH(10, 2);
    GPIOC->OSPEEDR = (3<<10);

    /* 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);

    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_CC3E | TIM_CCER_CC3NE;
    TIM1->BDTR = TIM_BDTR_MOE | (32<<TIM_BDTR_DTG_Pos) | TIM_BDTR_MOE;
    TIM1->DCR = (14<<TIM_DCR_DBA_Pos) | (1<<TIM_DCR_DBL_Pos);
    TIM1->PSC = 4;
    TIM1->ARR = 256;
    TIM1->CCR2 = 64;
    TIM1->CCR3 = 192;
    TIM1->DIER = TIM_DIER_UDE;
    TIM1->CR1 |= TIM_CR1_CEN;

    DMAMUX1->CCR = 25;
    DMA1_Channel1->CCR = (1<<DMA_CCR_MSIZE_Pos) | (1<<DMA_CCR_PSIZE_Pos) | DMA_CCR_MINC | DMA_CCR_CIRC | DMA_CCR_DIR | DMA_CCR_TCIE;
    DMA1_Channel1->CNDTR = COUNT_OF(waveform);
    DMA1_Channel1->CPAR = &TIM1->DMAR;
    DMA1_Channel1->CMAR = &waveform;
    DMA1_Channel1->CCR |= DMA_CCR_EN;

    int i = 0;
    int j = 0;
    while (23) {
        i++;
        j++;
        i %= 6;
        j %= 4;
        delay_us(100000);
        set_rj45_leds(1 << j);
        set_status_leds(1 << i);
    }
}

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 () {
}