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

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

#include <stm32h7xx_ll_rng.h>

#include "FreeRTOSConfig.h"

#include "board_support.h"
#include "cliutils/cli.h"
#include "cmds.h"
#include "etherlib/global_state.h"

#include "ethernet/ethernet_etherlib.h"
#include "standard_output/serial_io.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 Main 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 (44)
    osc.PLL.PLLP = 1;                      // 550MHz -> 550MHz
    osc.PLL.PLLQ = 5;                      // 550MHz -> 110MHz
    osc.PLL.PLLR = 5;                      // 550MHz -> 110MHz
    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_4); // 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);
}

int udp_recv_cb(const Pckt *packet, PcktSieveLayerTag tag) {
    char str[packet->payloadSize + 1];
    memcpy(str, packet->payload, packet->payloadSize);
    str[packet->payloadSize] = '\0';
    MSG("%s\n", str);
    return 0;
}

bool btn_pressed = false;

void task_startup(void *arg) {
    // initialize on-board LEDs and buttons
    leds_init();
    btn_init();

    // initialize the CLI
    cli_init();

    // print greetings
    print_welcome_message();

    // initialize Ethernet
    init_ethernet();

    // initialize commands
    cmd_init();

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

    cbd cb = udp_new_connblock(E.ethIntf, IPv4_IF_ADDR, 4000, udp_recv_cb);

    for (;;) {
        osDelay(1000);

        if (is_etherlib_initialized()) {
            timer_tick(E.tmr, 1000000);
        }

        if (btn_pressed) {
            udp_sendto(cb, "Üdv!\n", 7, IPv4(10, 42, 0, 1), 4100);
            btn_pressed = false;
        }
    }
}

void btn_cb() {
    btn_pressed = true;
}

void init_randomizer() {
    __HAL_RCC_RNG_CLK_ENABLE();

    LL_RNG_Enable(RNG);

    while (!LL_RNG_IsActiveFlag_DRDY(RNG)) {
    }

    srand(LL_RNG_ReadRandData32(RNG));
}

void init_mpu() {
    HAL_MPU_Disable();
}

static CRC_HandleTypeDef crc;

void init_crc() {
    __HAL_RCC_CRC_CLK_ENABLE();

    crc.Instance = CRC;
    crc.Init.DefaultPolynomialUse = DEFAULT_POLYNOMIAL_ENABLE;
    crc.Init.DefaultInitValueUse = DEFAULT_INIT_VALUE_ENABLE;
    crc.Init.InputDataInversionMode = CRC_INPUTDATA_INVERSION_NONE;
    crc.Init.OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_DISABLE;
    crc.InputDataFormat = CRC_INPUTDATA_FORMAT_BYTES;
    if (HAL_CRC_Init(&crc) != HAL_OK) {
        MSG("Nem sikerült a CRC-modult felinicializálni!\n");
    }
    __HAL_CRC_DR_RESET(&crc);
}

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 less deterministic
    init_randomizer();

    // initialize the CRC module
    init_crc();

    // initialize MPU
    init_mpu();

    // initialize standard output
    serial_io_init();

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

    // 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 (;;) {
    }
}

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

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

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

void vApplicationTickHook(void) {
    HAL_IncTick();
}

void vApplicationIdleHook(void) {
    return;
}

// --------