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

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

#include <stm32h7xx_ll_rng.h>

#include "Drivers/BSP/stm32h735g_discovery_audio.h"
#include "FreeRTOSConfig.h"

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

#include "standard_output/serial_io.h"
#include "standard_output/standard_output.h"

#include <cmsis_os2.h>

#include "user/user.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);
}

#define AUDIO_BUF_SIZE (16000)
int16_t buffer[AUDIO_BUF_SIZE];

BSP_AUDIO_Init_t AudioOutInit;

void audio_init() {
    AudioOutInit.Device = AUDIO_OUT_DEVICE_HEADPHONE;
    AudioOutInit.ChannelsNbr = 2;
    AudioOutInit.SampleRate = 16000;
    AudioOutInit.BitsPerSample = AUDIO_RESOLUTION_16B;
    AudioOutInit.Volume = 10;

    BSP_AUDIO_OUT_Init(0, &AudioOutInit);

    float freq = 440;

    for (uint16_t i = 0; i < AUDIO_BUF_SIZE; i++) {
        int16_t sample = sin(freq * ((double)i / AudioOutInit.SampleRate) * 2 * M_PI) * INT16_MAX;
        buffer[2 * i] = sample;
        buffer[2 * i + 1] = sample;
    }

    BSP_AUDIO_OUT_Play(0, (uint8_t *)buffer, sizeof(uint16_t) * AUDIO_BUF_SIZE);
}

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();

    audio_init();

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

    user_init();

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

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

void btn_cb() {
}

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;
}

// --------

/**
 * @brief  This function handles DMA2 Stream0 interrupt request.
 * @retval None
 */
void DMA2_Stream0_IRQHandler(void) {
    BSP_AUDIO_IN_IRQHandler(2, AUDIO_IN_DEVICE_DIGITAL_MIC2);
}

/**
 * @brief  This function handles DMA2 Stream2 interrupt request.
 * @retval None
 */
void DMA2_Stream2_IRQHandler(void) {
    BSP_AUDIO_IN_IRQHandler(2, AUDIO_IN_DEVICE_DIGITAL_MIC4);
}

/**
 * @brief  This function handles DMA2 Stream3 interrupt request.
 * @retval None
 */
void DMA2_Stream3_IRQHandler(void) {
    BSP_AUDIO_IN_IRQHandler(2, AUDIO_IN_DEVICE_DIGITAL_MIC3);
}

/**
 * @brief  This function handles DMA2 Stream4 interrupt request.
 * @retval None
 */
void AUDIO_IN_SAIx_DMAx_IRQHandler(void) {
    BSP_AUDIO_IN_IRQHandler(0, AUDIO_IN_DEVICE_ANALOG_MIC);
}

/**
 * @brief  This function handles DMA2 Stream6 interrupt request.
 * @param  None
 * @retval None
 */
void DMA2_Stream6_IRQHandler(void) {
    BSP_AUDIO_OUT_IRQHandler(0);
}

/**
 * @brief  This function handles DMA2 Stream7 interrupt request.
 * @retval None
 */
void DMA2_Stream7_IRQHandler(void) {
    BSP_AUDIO_IN_IRQHandler(2, AUDIO_IN_DEVICE_DIGITAL_MIC1);
}