#include "stm32f4xx_hal.h"
#include "user_power_mgmt.h"

typedef struct {
    RCC_OscInitTypeDef RCC_OscInitStruct;
    RCC_ClkInitTypeDef RCC_ClkInitStruct;
    uint32_t Flash_Latency;
    uint32_t Voltage_Scale;
} user_pm_vfs_t;

// C standard guarantees uninitialized parts are zeros.
static user_pm_vfs_t s_user_vfs_table[] = {
    {
        .RCC_OscInitStruct = {
            .OscillatorType = RCC_OSCILLATORTYPE_HSI,
            .HSIState = RCC_HSI_ON,
            .PLL.PLLState = RCC_PLL_NONE,
        },
        .RCC_ClkInitStruct = {
            .ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
                       | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2,
            .SYSCLKSource = RCC_SYSCLKSOURCE_HSI,
            .AHBCLKDivider = RCC_SYSCLK_DIV1,
            .APB1CLKDivider = RCC_HCLK_DIV1,
            .APB2CLKDivider = RCC_HCLK_DIV1,
        },
        .Flash_Latency = FLASH_LATENCY_0,
        .Voltage_Scale = PWR_REGULATOR_VOLTAGE_SCALE2,
    },
    {
        .RCC_OscInitStruct = {
            .OscillatorType = RCC_OSCILLATORTYPE_HSE,
            .HSEState = RCC_HSE_ON,
            .PLL.PLLState = RCC_PLL_ON,
            .PLL.PLLSource = RCC_PLLSOURCE_HSE,
            .PLL.PLLM = 4,
            .PLL.PLLN = 168,
            .PLL.PLLP = RCC_PLLP_DIV2,
            .PLL.PLLQ = 4,
        },
        .RCC_ClkInitStruct = {
            .ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2,
            .SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK,
            .AHBCLKDivider = RCC_SYSCLK_DIV1,
            .APB1CLKDivider = RCC_HCLK_DIV4,
            .APB2CLKDivider = RCC_HCLK_DIV2,
        },
        .Flash_Latency = FLASH_LATENCY_5,
        .Voltage_Scale = PWR_REGULATOR_VOLTAGE_SCALE1,
    },
    {
        .RCC_OscInitStruct = {
            .OscillatorType = RCC_OSCILLATORTYPE_HSE,
            .HSEState = RCC_HSE_ON,
            .PLL.PLLState = RCC_PLL_ON,
            .PLL.PLLSource = RCC_PLLSOURCE_HSE,
            .PLL.PLLM = 4,
            .PLL.PLLN = 144,
            .PLL.PLLP = RCC_PLLP_DIV2,
            .PLL.PLLQ = 4,
        },
        .RCC_ClkInitStruct = {
            .ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2,
            .SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK,
            .AHBCLKDivider = RCC_SYSCLK_DIV1,
            .APB1CLKDivider = RCC_HCLK_DIV4,
            .APB2CLKDivider = RCC_HCLK_DIV2,
        },
        .Flash_Latency = FLASH_LATENCY_4,
        .Voltage_Scale = PWR_REGULATOR_VOLTAGE_SCALE2,
    },
    {
        .RCC_OscInitStruct = {
            .OscillatorType = RCC_OSCILLATORTYPE_HSE,
            .HSEState = RCC_HSE_ON,
            .PLL.PLLState = RCC_PLL_ON,
            .PLL.PLLSource = RCC_PLLSOURCE_HSE,
            .PLL.PLLM = 4,
            .PLL.PLLN = 84,
            .PLL.PLLP = RCC_PLLP_DIV2,
            .PLL.PLLQ = 4,
        },
        .RCC_ClkInitStruct = {
            .ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2,
            .SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK,
            .AHBCLKDivider = RCC_SYSCLK_DIV1,
            .APB1CLKDivider = RCC_HCLK_DIV2,
            .APB2CLKDivider = RCC_HCLK_DIV2,
        },
        .Flash_Latency = FLASH_LATENCY_2,
        .Voltage_Scale = PWR_REGULATOR_VOLTAGE_SCALE2,
    },
};

static void user_pm_error_handler(void) {
    while(1) {
        //
    }
}

static void user_pm_enter_sleep(void) {
    //
}

static void user_pm_exit_sleep(void) {
    //
}

/**
 * @brief Set VFS mode, bad things happens if wrong sequence applied.
 * 
 * @param vfs 
 * @return HAL_StatusTypeDef 
 */
static HAL_StatusTypeDef user_pm_set_vfs_mode(user_pm_vfs_t *vfs) {

    //Temporary raise voltage to SCALE 1
    if(HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK) {
        return HAL_ERROR;
    }

    // Set oscillators and PLLs.
    if(HAL_RCC_OscConfig(&vfs->RCC_OscInitStruct) != HAL_OK) {
        return HAL_ERROR;
    }

    // Apply system clock settings and bus clock settings.
    if(HAL_RCC_ClockConfig(&vfs->RCC_ClkInitStruct, vfs->Flash_Latency) != HAL_OK) {
        return HAL_ERROR;
    }

    // Set correct voltage scale.
    if(HAL_PWREx_ControlVoltageScaling(vfs->Voltage_Scale) != HAL_OK) {
        return HAL_ERROR;
    }

    return HAL_OK;
}

/**
 * @brief Scale system frequency to specified preset
 * 
 * @param preset: Target VFS preset
 * @return Status
 */
HAL_StatusTypeDef user_pm_scale_vfs(user_pm_vfs_preset_t preset) {

    // Transition system to a stable state.
    if(user_pm_set_vfs_mode(&s_user_vfs_table[USER_PM_VFS_SAFE]) != HAL_OK) {
        // Something bad happened.
        user_pm_error_handler();
    }

    // Apply new frequency settings.
    if(user_pm_set_vfs_mode(&s_user_vfs_table[preset]) != HAL_OK) {
        // Another bad thing happened.
        user_pm_error_handler();
    }

    return HAL_OK;
}

/**
 * @brief Sleep for specific timeout
 * 
 * @param sleep_msec: Time to sleep
 * @return Time actually in sleep mode
 */
uint32_t user_pm_idle_timeout(uint32_t sleep_msec) {
    return 0;
}