#include "BSP_POWER.h"
#include "lptim.h"
#include "ulog.h"
#include "rtc.h"
#include "fdcan.h"
#include "i2c.h"
#include "sdmmc.h"
#include "cmsis_os2.h"
#include "BSP_GPIO.h"
#include "BSP_ADC.h"

#define LPTIM_CLK 500     // Hz
#define SLEEP_TICK_TIME 1  // seconds to wait
#define STAY_AWAKE_TIME 60 // seconds to stay awake without K15



// Start the sleep counter check
void BSP_POWER_Init() {
    uint16_t counter = LPTIM_CLK * SLEEP_TICK_TIME;
    HAL_LPTIM_Counter_Start_IT(&hlptim4, counter);
}


// This should be called as soon as possible after wakeup
void BSP_POWER_WakeUp() {

    /* Check if the system has resumed from StandBy mode */
    if(__HAL_PWR_GET_FLAG(PWR_FLAG_SB) != RESET)
    {
        /* Clear Standby flag */
        __HAL_PWR_CLEAR_FLAG(PWR_FLAG_SB);
    }


    if (__HAL_RTC_WAKEUPTIMER_GET_FLAG(&hrtc, RTC_FLAG_WUTF) != 0U)
    {
        __HAL_RTC_WAKEUPTIMER_CLEAR_FLAG(&hrtc, RTC_FLAG_WUTF);
    }


}

/* The Following Wakeup sequence is highly recommended prior to each Standby mode entry
    mainly when using more than one wakeup source this is to not miss any wakeup event.
    - Disable all used wakeup sources,
    - Clear all related wakeup flags,
    - Re-enable all used wakeup sources,
    - Enter the Standby mode.
*/
void BSP_POWER_EnterStandby() {

    // We need to disable all Periperals to minimize parasitic currents
    HAL_I2C_DeInit(&hi2c1);
    HAL_I2C_MspDeInit(&hi2c1);
    HAL_I2C_DeInit(&hi2c2);
    HAL_I2C_MspDeInit(&hi2c2);
    HAL_FDCAN_DeInit(&hfdcan1);
    HAL_FDCAN_MspDeInit(&hfdcan1);
    HAL_FDCAN_DeInit(&hfdcan2);
    HAL_FDCAN_MspDeInit(&hfdcan2);
    HAL_SD_MspDeInit(&hsd1);

    // These Pins interfere with the WAKEUP PINS
    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1| GPIO_PIN_3);

    // cut the power for the Slaves and unused peripherals
    BSP_GPIO_ClsOff();
    BSP_GPIO_PeriperalsOff();
    BSP_GPIO_RadioOff();



    // Here now the showdown 
    PWREx_WakeupPinTypeDef sPinParams;

    /* Disable used wakeup source: PWR_WAKEUP_PIN1 */
    HAL_PWR_DisableWakeUpPin(PWR_WAKEUP_PIN1);
    HAL_PWR_DisableWakeUpPin(PWR_WAKEUP_PIN2);
    HAL_RTCEx_DeactivateWakeUpTimer(&hrtc);

    HAL_PWREx_ClearPendingEvent();
    /* Clear all related wakeup flags */
    HAL_PWREx_ClearWakeupFlag(PWR_WAKEUP_FLAG_ALL);
    HAL_PWREx_ClearWakeupFlag(PWR_WAKEUP_PIN1);
    HAL_PWREx_ClearWakeupFlag(PWR_WAKEUP_PIN2);

    /* Clear all related wakeup flags */
    __HAL_PWR_CLEAR_FLAG(PWR_FLAG_WU);


    //HAL_RTCEx_SetWakeUpTimer_IT(&hrtc, 0xFFFF, RTC_WAKEUPCLOCK_RTCCLK_DIV16);
    HAL_PWREx_ClearWakeupFlag(PWR_WAKEUP_PIN1);

    sPinParams.WakeUpPin    = PWR_WAKEUP_PIN1;
    sPinParams.PinPolarity  = PWR_PIN_POLARITY_HIGH;
    sPinParams.PinPull      = PWR_PIN_PULL_DOWN;
    HAL_PWREx_EnableWakeUpPin(&sPinParams);

    sPinParams.WakeUpPin    = PWR_WAKEUP_PIN2;
    sPinParams.PinPolarity  = PWR_PIN_POLARITY_HIGH;
    sPinParams.PinPull      = PWR_PIN_PULL_DOWN;
    HAL_PWREx_EnableWakeUpPin(&sPinParams);


    /* Enter the Standby mode */
    HAL_PWR_EnterSTANDBYMode();

}

uint32_t sec_without_k15 = 0;

void HAL_LPTIM_AutoReloadMatchCallback(LPTIM_HandleTypeDef *hlptim) {

    if(hlptim == &hlptim4) {
        // 1s timer  check if K15 is set
        if (!BSP_GPIO_K15isSet()) {
            sec_without_k15++;
        } else {
            sec_without_k15 = 0;
        }

        if (sec_without_k15 > STAY_AWAKE_TIME) {
            BSP_POWER_EnterStandby();
        }
    }

}



void BSP_POWER_FullPowerMode() {

    BSP_GPIO_ClsOn();
    BSP_GPIO_PeriperalsOn();
    BSP_GPIO_RadioOn();
    BSP_ADC_Start();
    osDelay(10);
    StartPowerTasks();

}