#include <memory.h>
#include <stdint.h>

#include <stdlib.h>
#include <stm32h7xx_hal.h>

#include <stm32h7xx_ll_rng.h>

#include "FreeRTOSConfig.h"

#include "cliutils/cli.h"
#include "cmds.h"
#include "ethernet/ethernet_lwip.h"

#include "standard_output/standard_output.h"

#include <cmsis_os2.h>

void init_pll() {
    RCC_OscInitTypeDef osc;
    RCC_ClkInitTypeDef clk;

    // clear the structures
    memset(&osc, 0, sizeof(RCC_OscInitTypeDef));
    memset(&clk, 0, sizeof(RCC_ClkInitTypeDef));

    // ---------------------

    // Configure the PLL.

    osc.OscillatorType = RCC_OSCILLATORTYPE_HSE;
    osc.HSEState = RCC_HSE_ON;
    osc.HSIState = RCC_HSI_ON;
    osc.HSI48State = RCC_HSI48_ON;
    // osc.HSICalibrationValue = LL_RCC_HSI_GetCalibTrimming();
    osc.PLL.PLLState = RCC_PLL_ON;         // PLL is on
    osc.PLL.PLLSource = RCC_PLLSOURCE_HSE; // 8MHz HSE is the input
    osc.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;   // Wide-range VCO
    osc.PLL.PLLRGE = RCC_PLL1VCIRANGE_3;   // input frequency between 8MHz and 16MHz
    osc.PLL.PLLM = 2;                      // 25MHz -> 12.5MHz
    osc.PLL.PLLN = 44;                     // 12.5MHz -> 550MHz
    osc.PLL.PLLP = 1;                      // 550MHz -> 550MHz
    osc.PLL.PLLQ = 11;                     // 550MHz -> 50MHz
    osc.PLL.PLLR = 11;                     // 550MHz -> 50MHz
    osc.PLL.PLLFRACN = 0;                  // no fractional tuning

    HAL_RCC_OscConfig(&osc);

    // initialize clock tree
    clk.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK |
                     RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2 |
                     RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1);
    clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    clk.SYSCLKDivider = RCC_SYSCLK_DIV1;
    clk.AHBCLKDivider = RCC_HCLK_DIV2;
    clk.APB1CLKDivider = RCC_APB1_DIV2;
    clk.APB2CLKDivider = RCC_APB2_DIV2;
    clk.APB3CLKDivider = RCC_APB3_DIV2;
    clk.APB4CLKDivider = RCC_APB4_DIV2;

    HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_3); // set a FLASH latency supporting maximum speed

    // ---------------------
}

void init_osc_and_clk() {
    // ensure that the MCU uses the internal LDO instead of anything else
    HAL_PWREx_ConfigSupply(PWR_DIRECT_SMPS_SUPPLY);

    // configure internal voltage regulator to VOS0 to unlock main clock frequencies above 400MHz
    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);
    while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {
    }

    // initialize the main PLL
    init_pll();

    // turn on additional clock sources and prepare GPIOs for high-speed operation
    __HAL_RCC_HSI48_ENABLE();
    __HAL_RCC_SYSCFG_CLK_ENABLE();
    __HAL_RCC_CSI_ENABLE();
    HAL_EnableCompensationCell();

    // compute SYSCLK values (HSE_VALUE must be correct when invoking this)
    SystemCoreClockUpdate();

    // set tick frequency
    HAL_SetTickFreq(HAL_TICK_FREQ_1KHZ);
}

void print_welcome_message() {
    MSGraw("\033[2J\033[H");
    MSG(ANSI_COLOR_BGREEN "Greetings!" ANSI_COLOR_BYELLOW " Starting up... \n" ANSI_COLOR_RESET);
}

static void init_led() {
    __HAL_RCC_GPIOB_CLK_ENABLE();

    GPIO_InitTypeDef init;
    init.Mode = GPIO_MODE_OUTPUT_PP;
    init.Pin = GPIO_PIN_14;
    init.Pull = GPIO_NOPULL;
    init.Alternate = 0;
    init.Speed = GPIO_SPEED_HIGH;
    HAL_GPIO_Init(GPIOB, &init);
}

void task_startup(void *arg) {
#ifdef DEBUG
    osDelay(1000);
#endif

    // initialize on-board LED(s)
    init_led();

    // initialize the CLI
    cli_init();

    // print greetings
    print_welcome_message();

    // initialize Ethernet
    init_ethernet();

    // initialize commands
    cmd_init();

    // -------------

    for (;;) {
        osDelay(1000);
    }
}

void init_randomizer() {
    __HAL_RCC_RNG_CLK_ENABLE();

    LL_RNG_Enable(RNG);

    while (!LL_RNG_IsActiveFlag_DRDY(RNG)) {
    }

    srand(LL_RNG_ReadRandData32(RNG));
}

int main(void) {
    // initialize FPU and several system blocks
    SystemInit();

    // initialize HAL library
    HAL_Init();

    // initialize oscillator and clocking
    init_osc_and_clk();

    // make random a bit more undeterministic
    init_randomizer();

    // -------------

    // initialize the FreeRTOS kernel
    osKernelInitialize();

    // create startup thread
    osThreadAttr_t attr;
    memset(&attr, 0, sizeof(attr));
    attr.stack_size = 2048;
    attr.name = "init";
    osThreadNew(task_startup, NULL, &attr);

    // start the FreeRTOS!
    osKernelStart();

    for (;;) {
    }
}

// void SysTick_Handler() {
//     HAL_IncTick();
// }

// ------------------------

uint8_t ucHeap[configTOTAL_HEAP_SIZE] __attribute__((section(".FreeRTOSHeapSection")));

// ------------------------

void vApplicationTickHook(void) {
    HAL_IncTick();
}

void vApplicationIdleHook(void) {
    return;
}