#include "cmsis_os2.h"
#include "LightState.h"
#include "BSP_GPIO.h"
#include "BSP_ADC.h"
#include "LightTask.h"
#include "ulog.h"

// Create the task with a specific priority and stack size
osThreadAttr_t task_attr = {
    .name = "LightStateTask",
    .priority = osPriorityNormal,
    .stack_size = 1024
};

// Function prototype for the task function
void LightStateTask(void *argument);

// Function to start the LightStateTask
void LightStateTask_start(void)
{
    osThreadNew(LightStateTask, NULL, &task_attr);
}



// Check if K15 is on for switching the light state
static bool isK15On()
{
    return BSP_GPIO_K15isSet();
}


// Define the threshold voltage for engine running
#define ENGINE_RUNNING_THRESHOLD 13.3 // 13.5V

// Define thresholds with hysteresis
#define ENGINE_RUNNING_THRESHOLD_HIGH (ENGINE_RUNNING_THRESHOLD + 0.125)
#define ENGINE_RUNNING_THRESHOLD_LOW (ENGINE_RUNNING_THRESHOLD - 0.125)

// Global variable to store the current engine state
static bool engineRunning = false;

// Function to initialize the engine state based on the initial voltage
static void initializeEngineState()
{
    float initialVoltage = BSP_ADC_ReadBusValue();
    if (initialVoltage > ENGINE_RUNNING_THRESHOLD)
    {
        engineRunning = true;
    }
    else
    {
        engineRunning = false;
    }
}

// Ensure the engine state is initialized once
static bool isEngineInitialized = false;

static bool isEngineRunning()
{
    if (!isEngineInitialized)
    {
        initializeEngineState();
        isEngineInitialized = true;
    }

    float voltage = BSP_ADC_ReadBusValue();

    if (engineRunning)
    {
        // If engine is currently running, use the lower threshold to turn it off
        if (voltage < ENGINE_RUNNING_THRESHOLD_LOW)
        {
            engineRunning = false;
        }
    }
    else
    {
        // If engine is currently off, use the higher threshold to turn it on
        if (voltage > ENGINE_RUNNING_THRESHOLD_HIGH)
        {
            engineRunning = true;
        }



    }
    return engineRunning;
}
// check if the headlight is on and the engine is running to switch the light state
// we can check the headlight using the GPIO functions
static bool isHeadlightOn()
{
    return BSP_GPIO_HeadLightIsSet();
}



// Function to determine the next state of the light
// return 1 as default state
// return 2 if the engine is running
// return 3 if the headlight and engine is running
static int determineNextState()
{
    if (isHeadlightOn() && isEngineRunning()) {
        return 2;
    } else if (isEngineRunning()) {
        return 1;
    } else {
        return 0;
    }
}


// Task function
void LightStateTask(void *argument)
{

    // default state
    int current_state = 0;
    // Infinite loop to keep the task running
    while (1)
    {
        // only change the state if the current state is different from the next state
        int next_state = determineNextState();
        if (current_state != next_state) {
            LightTask_setTheme(next_state);
            ULOG_INFO("Light state changed to %d", next_state);
            current_state = next_state;
        }

        // Delay the task for a certain amount of time (in milliseconds)
        osDelay(50);
    }
}