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

extern TIM_HandleTypeDef htim4;

void user_pm_frequency_update_callback(void);

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

#include "user_power_mgmt_profiles.h"

static user_pm_vfs_preset_t s_current_preset;

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

/**
 * @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) {

    if(preset > USER_PM_VFS_END) return HAL_ERROR;

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

    // Update global variables
    SystemCoreClockUpdate();

    // This will happen in a critical section, no context switch will occur.
    s_current_preset = preset;

    // Call user defined callbacks.
    user_pm_frequency_update_callback();

    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) {
//    user_pm_vfs_preset_t prev_preset = s_current_preset;

//    if(user_pm_scale_vfs(USER_PM_VFS_XLOW_2) != HAL_OK) {
//        user_pm_error_handler();
//    }

    if(sleep_msec > 65535) sleep_msec = 65535; // Cap. Maximum to 65535ms.
    if(sleep_msec == 0) sleep_msec = 1;

    // Configure TIM6 period.
    __HAL_TIM_SET_AUTORELOAD(&htim4, sleep_msec);
    __HAL_TIM_SET_COUNTER(&htim4, 0UL);

    // We do not register any callbacks here since PRIMASK = 1 will cause no ISR be called.

    // For some yet UNKNOWN reason, TIM UPDATE IRQ flag will be
    // pended even BEFORE start counting.
    __HAL_TIM_CLEAR_IT(&htim4, TIM_IT_UPDATE);

    HAL_TIM_Base_Start_IT(&htim4);

    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_10, GPIO_PIN_RESET);
    HAL_PWR_EnterSLEEPMode(PWR_MAINREGULATOR_ON, PWR_SLEEPENTRY_WFI);
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_10, GPIO_PIN_SET);

    HAL_TIM_Base_Stop_IT(&htim4);

    // Restore previous VFS profile
//    if(user_pm_scale_vfs(prev_preset) != HAL_OK) {
//        user_pm_error_handler();
//    }

    // In case we woken up by another interrupt, return actual sleep'd time.
    if(__HAL_TIM_GET_FLAG(&htim4, TIM_FLAG_UPDATE)) {
        return sleep_msec;
    }
    return __HAL_TIM_GET_COUNTER(&htim4);
}

__weak void user_pm_frequency_update_callback(void) {
    // User application will implement this.
}