#include <global.h>

#include <adc.h>
#include <lcd.h>
#include <led.h>
#include <usb_if.h>


int bt_inputs() {
    bool bt0 = !!(GPIOB->IDR & (1<<2));
    bool bt1 = !!(GPIOB->IDR & (1<<1));
    bool bt2 = !!(GPIOB->IDR & (1<<0));
    bool bt3 = !!(GPIOB->IDR & (1<<12));
    bool bt4 = !!(GPIOB->IDR & (1<<4));
    bool bt5 = !!(GPIOA->IDR & (1<<15));
    return (bt5<<5) | (bt4<<4) | (bt3<<3) | (bt2<<2) | (bt1<<1) | (bt0<<0);
}

int main(void) {
    /* Enable HSE w/ 8 MHz crystal */
    /* FIXME */
    //RCC->CR |= RCC_CR_HSEON;
    //while (!(RCC->CR & RCC_CR_HSERDY))
    //    ;

    /* Configure PLL multiplier, clock dividers and MCO */
    /* The ADS131M02 datasheet recommends an 8.192 MHz input clock for high-resolution mode. */
    RCC->CFGR = (5<<RCC_CFGR_MCO_Pos)    | /* Forward 8 MHz HSI to MCO 1:1 */
        (4<<RCC_CFGR_PLLMUL_Pos) | /* Set PLL multiplier to 6x, leave divider at 1 */
        (1<<RCC_CFGR_PLLSRC_Pos) | /* Select HSI as PLL source */
        /* Leave HPRE at 1, not dividing clock. */
        (0<<RCC_CFGR_PPRE_Pos);    /* Use SYSCLK for APB bus clock */

    /* Enable PLL */
    RCC->CR |= RCC_CR_PLLON;
    while (!(RCC->CR & RCC_CR_PLLRDY))
        ;

    FLASH->ACR = FLASH_ACR_PRFTBE | (1<<FLASH_ACR_LATENCY_Pos);

    /* Switch system clock to PLL */
    RCC->CFGR |= (2<<RCC_CFGR_SW_Pos);
    while (((RCC->CFGR & RCC_CFGR_SWS_Msk) >> RCC_CFGR_SWS_Pos) != 2)
        ;

    /* Switch on HSI48 clock for USB */
    RCC->CR2 |= RCC_CR2_HSI48ON;
    while (!(RCC->CR2 & RCC_CR2_HSI48RDY))
        ;

    /* Enable peripheral clocks */
    RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN | RCC_AHBENR_GPIOCEN | RCC_AHBENR_DMAEN;
    RCC->APB2ENR |= RCC_APB2ENR_TIM16EN | RCC_APB2ENR_USART1EN | RCC_APB2ENR_SPI1EN | RCC_APB2ENR_ADCEN | RCC_APB2ENR_TIM15EN | RCC_APB2ENR_SYSCFGEN | RCC_APB2ENR_TIM17EN; 
    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN | RCC_APB1ENR_TIM3EN | RCC_APB1ENR_SPI2EN | RCC_APB1ENR_USART3EN | RCC_APB1ENR_I2C1EN | RCC_APB1ENR_USBEN | RCC_APB1ENR_CRSEN;
    RCC->CFGR3 |= RCC_CFGR3_I2C1SW;

    /* Enable USB clock recovery */
    CRS->CR = CRS_CR_AUTOTRIMEN | CRS_CR_CEN;

#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)))
#define PULLUP(pin) (1<<(2*pin))
#define PULLDOWN(pin) (2<<(2*pin))

    /* GPIOA:
     *  0 - !ADC_DRDY
     *  1 -  ADC_SYNC
     *  2 - !ADC_CS
     *  3 -  USB_VSENSE
     *  4 -  TP1
     *  5 -  SCK
     *  6 -  HIPO
     *  7 -  HOPI
     *  8 -  ADC_CLK
     *  9 -  DBG_TX
     * 10 -  DBG_RX
     * 11 -  USB_DM
     * 12 -  USB_DP
     * 13 -  SWDIO
     * 14 -  SWCLK
     * 15 -  BT5
     */
    GPIOA->MODER &= (~(CLEAR(15))); /* Clear JTAG TDI pin mode */
    GPIOA->MODER |= AF(0) | OUT(1) | OUT(2) | ANALOG(3) | OUT(4) | AF(5) | AF(6) | AF(7) | AF(8) | AF(9) | AF(10) | IN(11) 
        | IN(12) | IN(13) | IN(14) | IN(15);
    GPIOA->AFR[0] = AFRL( 0, 2) | /* TIM2 ETR */
                    AFRL( 2, 2) | /* TIM2 CH3 to ADC CS */
                    AFRL( 5, 0) | /* SPI1 SCK (to ADC) */
                    AFRL( 6, 0) | /* SPI1 HIPO */
                    AFRL( 7, 0);  /* SPI1 HOPI */
    GPIOA->AFR[1] = AFRH( 8, 0) | /* MCO (ADC clock) */
                    AFRH( 9, 1) | /* USART1 TX (debug USART) */
                    AFRH(10, 1);  /* USART1 RX */
    GPIOA->BSRR = 1<<2; /* De-assert ADC !CS */
    GPIOA->OSPEEDR = (3<<GPIO_OSPEEDR_OSPEEDR1_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR2_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR5_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR6_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR7_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR8_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR9_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR10_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR13_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR14_Pos);
    GPIOA->PUPDR |= PULLUP(15); /* BT5 */
    GPIOA->BSRR = (1<<1) | (1<<2); /* ADC ~SYNC, ~CS */

    /* GPIOB: 
     *  0 -  BT2
     *  1 -  BT1
     *  2 -  BT0
     *  3 -  BT4
     *  4 -  ENC_A
     *  5 -  ENC_B
     *  6 -  SCL
     *  7 -  SDA
     *  8 -  ENC_0
     *  9 -  unused
     * 10 -  RS485_DI
     * 11 -  RS485_DO
     * 12 -  BT3
     * 13 -  LED_SCK
     * 14 -  RS485_DE
     * 15 -  LED_HOPI
     */

    GPIOB->MODER = IN(0) | IN(1) | IN(2) | IN(3) | AF(4) | AF(5) | AF(6) | AF(7) | AF(8) | IN(9) | AF(10) | AF(11) |
        IN(12) | AF(13) | AF(14) | AF(15);
    GPIOB->AFR[0] = AFRL( 4, 1) | /* TIM3 CH1 (Encoder) */
                    AFRL( 5, 1) | /* TIM3 CH2 */
                    AFRL( 6, 1) | /* I2C1 SCL (Display) */
                    AFRL( 7, 1);  /* I2C1 SDA */
    GPIOB->AFR[1] = AFRH( 8, 2) | /* TIM16 CH1 (Encoder zero pulse) */
                    AFRH(10, 4) | /* USART3 TX (RS485 bus) */
                    AFRH(11, 4) | /* USART3 RX */
                    AFRH(13, 0) | /* SPI2 SCK (LEDs) */
                    AFRH(14, 4) | /* USART3 RTS (RS485 DE via remap in USART regs) */
                    AFRH(15, 0);  /* SPI2 HOPI */
    //GPIOB->OTYPER = (1<<13) | (1<<15);
    GPIOB->OSPEEDR = (3<<GPIO_OSPEEDR_OSPEEDR6_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR7_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR10_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR11_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR13_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR14_Pos) |
                     (3<<GPIO_OSPEEDR_OSPEEDR15_Pos);
    GPIOB->PUPDR |= PULLUP(0) | /* BT2 */
                    PULLUP(1) | /* BT1 */
                    PULLUP(2) | /* BT0 */
                    PULLUP(3) | /* BT4 */
                    PULLUP(12); /* BT3 */

    /* GPIOC:
     * PC13 - DFU button
     */

    SystemCoreClockUpdate();

    led_init();
    adc_init();
    lcd_init();
    const char *lines[4] = {"LINE 1 LINE 1 LINE 1",
                            "LINE 2 LINE 2 LINE 2",
                            "LINE 3 LINE 3 LINE 3",
                            "LINE 4 LINE 4 LINE 4"};
    lcd_write_dma(lines);
    usb_init();

//    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;
//
//    USART1->TDR = 0; /* Kick off transmission */
    for (size_t i=0; i<10; i++) {
        st_led.led[i].r = i*10;
        st_led.led[i].g = i*20;
        st_led.led[i].b = i*3;
    }
    while (23) {
        led_set_global_brightness(2);
        for (size_t i=0; i<10; i++) {
            st_led.led[i].r += 1;
            st_led.led[i].g += 2;
            st_led.led[i].b += 3;
        }
        for (size_t i=0; i<3000000; i++) {
            asm volatile ("nop");
        }
    }
}

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)
{
    memcpy(dest, src, n);
}

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

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