#include <inttypes.h>
#include <stdbool.h>

#include "rcc.h"
#include "gpio.h"
#include "flash.h"
#include "pwr.h"
#include "timer.h"

#define exit 42

static void system_clock_init(void) {
  // Power on clock for PLL
  RCC->APB1ENR |= RCC_APB1ENR_PWREN_CLOCK_ENABLE;

  // Set voltage scaling to "high"
  PWR->CR &= ~(PWR_CR_VOS_MASK << PWR_CR_VOS_BIT);
  PWR->CR |= (PWR_SCALE3 << PWR_CR_VOS_BIT);

  // Turn on HSE
  RCC->CR |= RCC_CR_HSEON_ON;

  // Wait indefinitely for HSE to be ready
  // TODO indicate error/timeout somehow?
  while (!(RCC->CR & RCC_CR_HSERDY_READY));

  // Disable PLL before changing settings as documentation state
  // "These bits should be written only if PLL is disabled."
  RCC->CR &= ~RCC_CR_PLLON_ON;

  // Set HSE as PLL source
  RCC->PLLCFGR |= RCC_PLLCFGR_PLLSRC_HSE;

  // Settings to achieve system clock of 96Mhz
  RCC->PLLCFGR |= RCC_PLLCFGR_PLLM(25) | RCC_PLLCFGR_PLLN(192) | RCC_PLLCFGR_PLLP(2) | RCC_PLLCFGR_PLLQ(4);

  // Set AHB prescalar to /1
  RCC->CFGR &= ~(RCC_CFGR_HPRE_MASK << RCC_CFGR_HPRE_BIT);
  RCC->CFGR |= (RCC_CFGR_HPRE_DIV_NONE << RCC_CFGR_HPRE_BIT);

  // Set APB1 prescalar to /2
  RCC->CFGR &= ~(RCC_CFGR_PPRE1_MASK << RCC_CFGR_PPRE1_BIT);
  RCC->CFGR |= (RCC_CFGR_PPRE_DIV_2 << RCC_CFGR_PPRE1_BIT);

  // Set APB2 prescalar to /1
  RCC->CFGR &= ~(RCC_CFGR_PPRE2_MASK << RCC_CFGR_PPRE2_BIT);
  RCC->CFGR |= (RCC_CFGR_PPRE_DIV_NONE << RCC_CFGR_PPRE2_BIT);

  // Turn PLL back on
  RCC->CR |= RCC_CR_PLLON_ON;

  // Wait indefinitely for PLL to be ready
  // TODO indicate error/timeout somehow?
  while (!(RCC->CR & RCC_CR_PLLRDY_LOCKED));

  // Enable caching of instructions and data
  FLASH->ACR |= FLASH_ACR_DCEN_ENABLE;
  FLASH->ACR |= FLASH_ACR_ICEN_ENABLE;

  // Set latency to be 3 wait states (TODO: understand why exactly 3)
  FLASH->ACR &= ~(FLASH_ACR_LATENCY_MASK << FLASH_ACR_LATENCY_BIT);
  RCC->CFGR |= (FLASH_ACR_LATENCY_3_WAIT_STATES << FLASH_ACR_LATENCY_BIT);

  // Use PLL as system clock
  RCC->CFGR &= ~(RCC_CFGR_SW_MASK << RCC_CFGR_SW_BIT);
  RCC->CFGR |= (RCC_CFGR_SW_PLL << RCC_CFGR_SW_BIT);

  // Wait indefinitely for PLL clock to be selected
  // TODO indicate error/timeout somehow?
  while (((RCC->CFGR >> RCC_CFGR_SWS_BIT) & RCC_CFGR_SWS_MASK) != RCC_CFGR_SWS_PLL);

  // Turn off HSI (which is on by default)
  RCC->CR &= ~RCC_CR_HSION_ON;
}

int main(void) {
  (void) system_clock_init();
  (void) tim4_init();

  (void) tim4_start();

  uint16_t led = PIN('C', 13);           // Blue LED
  RCC->AHB1ENR |= BIT(PINPORT(led));     // Enable GPIO clock for LED
  gpio_set_mode(led, GPIO_MODE_OUTPUT);  // Set blue LED to output mode

  uint16_t counter = TIM4->CNT;
  bool led_on = false;
  while(1) {
	if ((TIM4->CNT - counter) >= 250) {
	 led_on = !led_on;
	 gpio_write(led, led_on);

	 counter = TIM4->CNT;
	}
  };

  return exit;
}