#include "board.h"
#include <stm32f407xx.h>

#define RCC_PLL_N 0x150
#define RCC_PLL_Q 0x7

void Board::init_start() {
    RCC->CR |= ((uint32_t)RCC_CR_HSEON);
    while (!(RCC->CR & RCC_CR_HSERDY))
        ;

    FLASH->ACR |= FLASH_ACR_PRFTEN;
    FLASH->ACR &= (uint32_t)((uint32_t)~FLASH_ACR_LATENCY);
    FLASH->ACR |= (uint32_t)FLASH_ACR_LATENCY_5WS; // 5 WS (6 CPU cycles)
    FLASH->ACR |= FLASH_ACR_ICEN | FLASH_ACR_DCEN;

    RCC->CFGR |= RCC_CFGR_PPRE2_DIV2;
    RCC->CFGR |= RCC_CFGR_PPRE1_DIV4;
    RCC->PLLCFGR = RCC_PLLCFGR_PLLM_3 | (RCC_PLL_N << RCC_PLLCFGR_PLLN_Pos) | RCC_PLLCFGR_PLLSRC_HSE |
                   (RCC_PLL_Q << RCC_PLLCFGR_PLLQ_Pos);

    RCC->CFGR |= (RCC_CFGR_PPRE1_0 | RCC_CFGR_PPRE1_2);

    RCC->CR |= RCC_CR_PLLON;

    while ((RCC->CR & RCC_CR_PLLRDY) == 0) {
    };
    RCC->CFGR &= (uint32_t)((uint32_t) ~(RCC_CFGR_SW));
    RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL;
    while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)0x08)
        ;

    SystemCoreClockUpdate();

    RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;
    GPIOD->MODER |= GPIO_MODER_MODER12_0;
}

void Board::enable_timer() {
    SysTick_Config(SystemCoreClock / 1000);
}

volatile uint64_t current_time = 0;

rl::TimePoint Board::get_now() {
    ROTOR_LIGHT_DISABLE_INTERRUPTS();
    auto copy = current_time;
    ROTOR_LIGHT_ENABLE_INTERRUPTS();
    return copy;
}

void Board::toggle_led() {
    GPIOD->ODR ^= GPIO_ODR_OD12;
}

void Board::delay() {
    auto end = Board::get_now() + 1000;
    while (Board::get_now() < end);
}


extern "C" {
void SysTick_Handler(void) {
    ++current_time;
}
}
