#include "Vehicle.h"
#include "BSP_GPIO.h"
#include "stdbool.h"
#include "BSP_ADC.h"

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

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

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


void Vehicle_Init() {
    engineRunning = false;
}

bool Vehicle_isHeadlightOn()
{
    return BSP_GPIO_HeadLightIsSet();
}

bool Vehicle_isEngineRunning() {


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

bool Vehicle_isK15On() {
    return BSP_GPIO_K15isSet();
}






#include "CLS.h"
#include "CLS_BSP.h"
#include "CLSAddress.h"

static CLS_VehicleStatus_t status = {0};
_Static_assert(sizeof(status) == 8, "CLS_HeatbeatData_t is not 8 bytes");

void CLS_VehicleHeatbeat(void *argument) {
    CLS_BSP_TxHeaderType cls_vehicle_header = CREATE_BSP_CAN_HEADER(GENERATE_CLS_ADDRESS(CLS_CODE_STATUS,0,CLS_CH_STA_VEHICLE), CLS_BSP_DLC_BYTES_8);

    
    status.k15 = Vehicle_isK15On();
    status.headlight = Vehicle_isHeadlightOn();
    status.engine = Vehicle_isEngineRunning();
    status.speed = (uint8_t)Vehicle_Speed();
    status.unlocked = Vehicle_UnlockedSignal();

    CLS_BSP_CAN_AddMessageToSend(&cls_vehicle_header, (uint8_t*)&status);

}