#include <stdint.h>
#include <stm32h747xx.h>
#include <core_cm7.h>
#include <stdlib.h>
#include "delay.h"
#include "usb_device.h"
#include "usb_device_cdc.h"
#include "clocks.h"
#include "exti.h"
#include "registers.h"
#include "pin.h"
#include "fmc.h"

#define LED1_GPIO GPIOI
#define LED1_PIN 12
/* #define LED2_PIN 13 */

pin_t wkup;
pin_t led1;
exti_t exti_wkup;


void init_fmc(fmc_t *fmc)
{
  // select pll 2 r as fmc clock
  reg_write_bits(&RCC->D1CCIPR, 2 << (RCC_D1CCIPR_FMCSEL_Pos), RCC_D1CCIPR_FMCSEL_Msk);

  RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
  fmc_init(fmc);

  #define BURST_LENGTH_1 0x0000
  #define BURST_TYPE_SEQUENTIAL 0x0000
  #define CAS_LATENCY_3 0x0030
  #define OPERATING_MODE_STANDARD 0x0000
  #define WRITEBURST_MODE_SINGLE 0x0200
  uint16_t sdram_mode_register = BURST_LENGTH_1 | BURST_TYPE_SEQUENTIAL |
                                 CAS_LATENCY_3 | OPERATING_MODE_STANDARD |
                                 WRITEBURST_MODE_SINGLE;

  fmc_sdram_timing_t sdram_timing = {
    .startup_delay_us =  100,        // 100 µs
    .max_sd_clock_hz =  100000000,   // 100 MHz
    .refresh_period_ns =  15625,     // 64ms / (4096 rows) = 15625ns
    .mode_register_to_active =  2,   // tMRD = 2 cycles
    .exit_self_refresh =  7,         // tXSR = 70ns
    .active_to_precharge =  4,       // tRAS = 42ns
    .row_cycle =  7,                 // tRC = 70ns
    .row_precharge =  2,             // tRP = 18ns
    .row_to_column =  2,             // tRCD = 18ns
  };
  fmc_sdram_configuration_t sdram_config = {
    .column_bits =  9,
    .row_bits =  12,
    .memory_data_width =  32, // 32-bit
    .internal_banks =  4,     // 4 internal banks
    .cas_latency =  3,        // CAS latency = 3
    .write_protection =  false,
    .read_burst =  true,
    .read_pipe_delay_cycles =  0,
  };

  {
    pin_t pin_addr[12];
    pin_t pin_data[32];
    pin_t pin_ba[2];
    pin_t pin_nbl[4];
    pin_t pin_sdcke1;
    pin_t pin_sdclk;
    pin_t pin_sdncas;
    pin_t pin_sdne1;
    pin_t pin_sdras;
    pin_t pin_sdnwe;

    // A0-A11
    pin_init(&pin_addr[0], GPIOF, 0);
    pin_init(&pin_addr[1], GPIOF, 1);
    pin_init(&pin_addr[2], GPIOF, 2);
    pin_init(&pin_addr[3], GPIOF, 3);
    pin_init(&pin_addr[4], GPIOF, 4);
    pin_init(&pin_addr[5], GPIOF, 5);
    pin_init(&pin_addr[6], GPIOF, 12);
    pin_init(&pin_addr[7], GPIOF, 13);
    pin_init(&pin_addr[8], GPIOF, 14);
    pin_init(&pin_addr[9], GPIOF, 15);
    pin_init(&pin_addr[10], GPIOG, 0);
    pin_init(&pin_addr[11], GPIOG, 1);
    // BA0-BA1
    pin_init(&pin_ba[0], GPIOG, 4);
    pin_init(&pin_ba[1], GPIOG, 5);
    // D0-D31
    pin_init(&pin_data[0], GPIOD, 14);
    pin_init(&pin_data[1], GPIOD, 15);
    pin_init(&pin_data[2], GPIOD, 0);
    pin_init(&pin_data[3], GPIOD, 1);
    pin_init(&pin_data[4], GPIOE, 7);
    pin_init(&pin_data[5], GPIOE, 8);
    pin_init(&pin_data[6], GPIOE, 9);
    pin_init(&pin_data[7], GPIOE, 10);
    pin_init(&pin_data[8], GPIOE, 11);
    pin_init(&pin_data[9], GPIOE, 12);
    pin_init(&pin_data[10], GPIOE, 13);
    pin_init(&pin_data[11], GPIOE, 14);
    pin_init(&pin_data[12], GPIOE, 15);
    pin_init(&pin_data[13], GPIOD, 8);
    pin_init(&pin_data[14], GPIOD, 9);
    pin_init(&pin_data[15], GPIOD, 10);
    pin_init(&pin_data[16], GPIOH, 8);
    pin_init(&pin_data[17], GPIOH, 9);
    pin_init(&pin_data[18], GPIOH, 10);
    pin_init(&pin_data[19], GPIOH, 11);
    pin_init(&pin_data[20], GPIOH, 12);
    pin_init(&pin_data[21], GPIOH, 13);
    pin_init(&pin_data[22], GPIOH, 14);
    pin_init(&pin_data[23], GPIOH, 15);
    pin_init(&pin_data[24], GPIOI, 0);
    pin_init(&pin_data[25], GPIOI, 1);
    pin_init(&pin_data[26], GPIOI, 2);
    pin_init(&pin_data[27], GPIOI, 3);
    pin_init(&pin_data[28], GPIOI, 6);
    pin_init(&pin_data[29], GPIOI, 7);
    pin_init(&pin_data[30], GPIOI, 9);
    pin_init(&pin_data[31], GPIOI, 10);

    pin_init(&pin_nbl[0], GPIOE, 0);
    pin_init(&pin_nbl[1], GPIOE, 1);
    pin_init(&pin_nbl[2], GPIOI, 4);
    pin_init(&pin_nbl[3], GPIOI, 5);

    pin_init(&pin_sdcke1, GPIOH, 7);
    pin_init(&pin_sdclk, GPIOG, 8);
    pin_init(&pin_sdncas, GPIOG, 15);
    pin_init(&pin_sdne1, GPIOH, 6);
    pin_init(&pin_sdras, GPIOF, 11);
    pin_init(&pin_sdnwe, GPIOH, 5);

    // A0-A11
    pin_into_alternate_highspeed(&pin_addr[0], 12);
    pin_into_alternate_highspeed(&pin_addr[1], 12);
    pin_into_alternate_highspeed(&pin_addr[2], 12);
    pin_into_alternate_highspeed(&pin_addr[3], 12);
    pin_into_alternate_highspeed(&pin_addr[4], 12);
    pin_into_alternate_highspeed(&pin_addr[5], 12);
    pin_into_alternate_highspeed(&pin_addr[6], 12);
    pin_into_alternate_highspeed(&pin_addr[7], 12);
    pin_into_alternate_highspeed(&pin_addr[8], 12);
    pin_into_alternate_highspeed(&pin_addr[9], 12);
    pin_into_alternate_highspeed(&pin_addr[10], 12);
    pin_into_alternate_highspeed(&pin_addr[11], 12);
    // BA0-BA1
    pin_into_alternate_highspeed(&pin_ba[0], 12);
    pin_into_alternate_highspeed(&pin_ba[1], 12);
    // D0-D31
    pin_into_alternate_highspeed(&pin_data[0], 12);
    pin_into_alternate_highspeed(&pin_data[1], 12);
    pin_into_alternate_highspeed(&pin_data[2], 12);
    pin_into_alternate_highspeed(&pin_data[3], 12);
    pin_into_alternate_highspeed(&pin_data[4], 12);
    pin_into_alternate_highspeed(&pin_data[5], 12);
    pin_into_alternate_highspeed(&pin_data[6], 12);
    pin_into_alternate_highspeed(&pin_data[7], 12);
    pin_into_alternate_highspeed(&pin_data[8], 12);
    pin_into_alternate_highspeed(&pin_data[9], 12);
    pin_into_alternate_highspeed(&pin_data[10], 12);
    pin_into_alternate_highspeed(&pin_data[11], 12);
    pin_into_alternate_highspeed(&pin_data[12], 12);
    pin_into_alternate_highspeed(&pin_data[13], 12);
    pin_into_alternate_highspeed(&pin_data[14], 12);
    pin_into_alternate_highspeed(&pin_data[15], 12);
    pin_into_alternate_highspeed(&pin_data[16], 12);
    pin_into_alternate_highspeed(&pin_data[17], 12);
    pin_into_alternate_highspeed(&pin_data[18], 12);
    pin_into_alternate_highspeed(&pin_data[19], 12);
    pin_into_alternate_highspeed(&pin_data[20], 12);
    pin_into_alternate_highspeed(&pin_data[21], 12);
    pin_into_alternate_highspeed(&pin_data[22], 12);
    pin_into_alternate_highspeed(&pin_data[23], 12);
    pin_into_alternate_highspeed(&pin_data[24], 12);
    pin_into_alternate_highspeed(&pin_data[25], 12);
    pin_into_alternate_highspeed(&pin_data[26], 12);
    pin_into_alternate_highspeed(&pin_data[27], 12);
    pin_into_alternate_highspeed(&pin_data[28], 12);
    pin_into_alternate_highspeed(&pin_data[29], 12);
    pin_into_alternate_highspeed(&pin_data[30], 12);
    pin_into_alternate_highspeed(&pin_data[31], 12);

    pin_into_alternate_highspeed(&pin_nbl[0], 12);
    pin_into_alternate_highspeed(&pin_nbl[1], 12);
    pin_into_alternate_highspeed(&pin_nbl[2], 12);
    pin_into_alternate_highspeed(&pin_nbl[3], 12);

    pin_into_alternate_highspeed(&pin_sdcke1, 12);
    pin_into_alternate_highspeed(&pin_sdclk, 12);
    pin_into_alternate_highspeed(&pin_sdncas, 12);
    pin_into_alternate_highspeed(&pin_sdne1, 12);
    pin_into_alternate_highspeed(&pin_sdras, 12);
    pin_into_alternate_highspeed(&pin_sdnwe, 12);

    fmc_sdram_configure(fmc, SDRAM_BANK2, sdram_config, sdram_timing, sdram_mode_register);
  }
}

usb_device_t* init_usb()
{
  pin_t ulpi_clk, ulpi_stp, ulpi_dir, ulpi_nxt,
    ulpi_d0, ulpi_d1, ulpi_d2, ulpi_d3, ulpi_d4,
    ulpi_d5, ulpi_d6, ulpi_d7, otg_hs_overcurrent;
  pin_init(&ulpi_clk, GPIOA, 5);
  pin_init(&ulpi_stp, GPIOC, 0);
  pin_init(&ulpi_dir, GPIOI, 11);
  pin_init(&ulpi_nxt, GPIOH, 4);

  pin_init(&ulpi_d0, GPIOA, 3);
  pin_init(&ulpi_d1, GPIOB, 0);
  pin_init(&ulpi_d2, GPIOB, 1);
  pin_init(&ulpi_d3, GPIOB, 10);
  pin_init(&ulpi_d4, GPIOB, 11);
  pin_init(&ulpi_d5, GPIOB, 12);
  pin_init(&ulpi_d6, GPIOB, 13);
  pin_init(&ulpi_d7, GPIOB, 5);
  pin_init(&otg_hs_overcurrent, GPIOJ, 1);

  pin_into_alternate_highspeed(&ulpi_clk, 10);
  pin_into_alternate_highspeed(&ulpi_stp, 10);
  pin_into_alternate_highspeed(&ulpi_dir, 10);
  pin_into_alternate_highspeed(&ulpi_nxt, 10);
  pin_into_alternate_highspeed(&ulpi_d0, 10);
  pin_into_alternate_highspeed(&ulpi_d1, 10);
  pin_into_alternate_highspeed(&ulpi_d2, 10);
  pin_into_alternate_highspeed(&ulpi_d3, 10);
  pin_into_alternate_highspeed(&ulpi_d4, 10);
  pin_into_alternate_highspeed(&ulpi_d5, 10);
  pin_into_alternate_highspeed(&ulpi_d6, 10);
  pin_into_alternate_highspeed(&ulpi_d7, 10);

  pin_into_input_highspeed(&otg_hs_overcurrent);

  void *usb_dev = usb_device_init(USB_OTG_HS1, &USB_CLASS_CDC_ACM,
                                  0x1234, 0x1111, u"Frantisek Bohacek",
                                  u"Display", 1, NULL);
  cdc_acm_configure(usb_dev, 512);
  usb_device_setup(usb_dev);

  return usb_dev;
}

void app_loop(usb_device_t* usb_dev);

void main()
{
  clocks_wait_ready(CLOCK_HSI);

  // Clock gating
  RCC->APB4ENR |= RCC_APB4ENR_SYSCFGEN;
  RCC->AHB4ENR |= RCC_AHB4ENR_GPIOAEN | RCC_AHB4ENR_GPIOBEN |
                  RCC_AHB4ENR_GPIOCEN | RCC_AHB4ENR_GPIODEN |
                  RCC_AHB4ENR_GPIOEEN | RCC_AHB4ENR_GPIOFEN |
                  RCC_AHB4ENR_GPIOGEN | RCC_AHB4ENR_GPIOHEN |
                  RCC_AHB4ENR_GPIOIEN | RCC_AHB4ENR_GPIOJEN;

  systick_configure();

  // Clocks section
  // Enable hsi48 for usb
  clocks_enable(CLOCK_HSI48);
  clocks_wait_ready(CLOCK_HSI48);

  // Enable HSE for PLLs
  clocks_enable(CLOCK_HSE);
  clocks_wait_ready(CLOCK_HSE);

  // HSE 25 MHz -> divide by 25 -> 1 MHz -> need VCOSEL 1
  // HSE / 25 * 400 = FVCO = 400 MHz
  // PLL2 R = 200 MHz
  clocks_pll_configure(CLOCK_PLL2, 1, 25, PLL_SOURCE_HSE, 400, 2, 2, 2);
  clocks_pll_enable(CLOCK_PLL2);
  clocks_pll_wait_ready(CLOCK_PLL2, 300);

  // 1. Set pwr configuration
  reg_write_bits(&PWR->CR3,
                 (1 << PWR_CR3_SMPSEN_Pos) | (0 << PWR_CR3_LDOEN_Msk) |
                 (0 << PWR_CR3_BYPASS_Msk),
                 PWR_CR3_LDOEN_Msk | PWR_CR3_SMPSEN_Msk | PWR_CR3_BYPASS_Msk);

  // 2. Verify pwr configuration. If not working, stay here looping!
  // NOTE: this probably means you need to power cycle the board. The
  // configuration can be written only when the chip has just been connected.
  while (!(PWR->CR3 & PWR_CR3_SMPSEN) || (PWR->CR3 & PWR_CR3_LDOEN) || (PWR->CR3 & PWR_CR3_BYPASS));

  // 3. Change VOS
  // 3.1. Check current configuration (VOS 3)
  while (!(PWR->CSR1 & PWR_CSR1_ACTVOSRDY));

  // 3.2. VOS 1 transition
  reg_write_bits(&PWR->D3CR, (11 << PWR_D3CR_VOS_Pos), PWR_D3CR_VOS_Msk);
  while (!(PWR->CSR1 & PWR_D3CR_VOSRDY));

  // 3.3. VOS 0 transition
  reg_set_bits(&SYSCFG->PWRCR, SYSCFG_PWRCR_ODEN);
  while (!(PWR->CSR1 & PWR_D3CR_VOSRDY));

  // 25 MHz / 10 -> 2.5 MHz * 384 = 960 MHz (FVCO)
  // PLL1_P = 960 / 2 => 480 MHz
  clocks_pll_configure(CLOCK_PLL1, 0, 10, PLL_SOURCE_HSE, 384, 2, 0, 0);
  clocks_pll_enable(CLOCK_PLL1);
  clocks_pll_wait_ready(CLOCK_PLL1, 300);

  // Flash setup
  /* uint32_t sysck_freq_hz = 480000000; // 480 MHz */
  /* uint32_t d1cpre_freq_hz = sysck_freq_hz / 1; // 480 Mhz */
  /* uint32_t rcc_hclk = d1cpre_freq_hz / 2; // 240 MHz */

  // 240 MHz means wait states = 4, progr delay = 2
  reg_write_bits(&FLASH->ACR,
                 (2 << FLASH_ACR_WRHIGHFREQ_Pos) | (4 << FLASH_ACR_LATENCY_Pos),
                 FLASH_ACR_WRHIGHFREQ_Msk | FLASH_ACR_LATENCY_Msk);
  while (((FLASH->ACR & FLASH_ACR_LATENCY_Msk) >> FLASH_ACR_LATENCY_Pos) != 4);

  // D1CPRE = sysck / 1 => 480 MHz, HPRE = sysck / 2 => 240 MHz
  reg_write_bits(&RCC->D1CFGR,
                 (0 << RCC_D1CFGR_D1CPRE_Pos) | (1 << RCC_D1CFGR_HPRE_Pos) |
                 (0 << RCC_D1CFGR_D1CPRE_Pos),
                 RCC_D1CFGR_D1CPRE_Msk | RCC_D1CFGR_HPRE_Msk | RCC_D1CFGR_D1CPRE_Msk);

  // Sysck = PLL1_P
  clocks_system_clock_source(CLOCK_SOURCE_PLL_1_P_CK, 1, 1, 1, 300);

  fmc_t fmc;
  init_fmc(&fmc);

  uint8_t* sdram_data = (uint8_t*)fmc_sdram_allocate(&fmc, SDRAM_BANK2, 1024);

  for (uint32_t i = 0; i < 1024; i++) {
    *(sdram_data + i) = i;
  }

  // Pins init
  pin_init(&led1, LED1_GPIO, LED1_PIN);
  pin_into_output_pushpull(&led1);
  pin_toggle(&led1);
  pin_toggle(&led1);

  // pc13 input - wakeup button
  pin_init(&wkup, GPIOC, 13);
  pin_into_input(&wkup);

  exti_init(&exti_wkup, 13, EXTI, SYSCFG);
  exti_external_interrupt(&exti_wkup, EXTI_GPIOC);
  exti_rising_interrupt(&exti_wkup);
  exti_enable_interrupt(&exti_wkup);

  __enable_irq();

  usb_device_t* usb_dev = init_usb();
  usb_device_wait_for_handshake(usb_dev);

  app_loop(usb_dev);
}

void app_loop(usb_device_t* usb_dev)
{
  uint8_t data[64];
  while (1) {
    uint16_t received = cdc_data_receive(usb_dev, data, 64);

    for (uint16_t i = 0; i < received; i++) {
      data[i] = data[i] + 1;
    }

    if (received > 0) {
      cdc_data_send_blocking(usb_dev, data, received);
    }

    if (pin_read(&wkup)) {
      cdc_data_send_blocking(usb_dev, (uint8_t*)"Hello world!\r\n", 0);
    }
  }
}


// Interrupt handlers

void hard_fault_handler() {
  volatile uint32_t cfsr = SCB->CFSR;
  volatile uint32_t hfsr = SCB->HFSR;
  volatile uint32_t bfar = SCB->BFAR;

  while(1) {}
}

void exti15_10_handler(void)
{
  if (exti_is_interrupt(&exti_wkup)) {
    exti_clear_interrupt(&exti_wkup);
    pin_toggle(&led1);
  }
}