#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "safety.h"
#include "outputs.h"
#include "inputs.h"

#define PERIODIC_INTERVAL 1U // run safety checks every 1sec

typedef struct _TemperatureSensorLimit
{
    float max; // Maximum temperature limit
    float min; // Minimum temperature limit
} sTemperatureSensorLimit;

static const char *TAG = "smart-oil-heater-control-system-safety";
static SemaphoreHandle_t xMutexAccessSafety = NULL;
sSafetyStateElement safetyStates[NUMBER_OF_ERROR_STATES] = {
    {0, "Emergency Stop"},
    {0, "Safety Door Open"},
    {0, "Overheating"},
    {0, "Low Battery"}};

static const sTemperatureSensorLimit sChamperTemperatureLimit = {75.0f, 15.0f};

void taskSafety(void *pvParameters);
void checkSensorSanity(void);
void setSafeState(void);

void initSafety(void)
{
    xMutexAccessSafety = xSemaphoreCreateBinary();
    if (xMutexAccessSafety == NULL)
    {
        ESP_LOGE(TAG, "Unable to create mutex");
    }
    xSemaphoreGive(xMutexAccessSafety);

    BaseType_t taskCreated = xTaskCreate(
        taskSafety,   // Function to implement the task
        "taskSafety", // Task name
        2048,         // Stack size (in words, not bytes)
        NULL,         // Parameters to the task function (none in this case)
        5,            // Task priority (higher number = higher priority)
        NULL          // Task handle (optional)
    );

    if (taskCreated == pdPASS)
    {
        ESP_LOGI(TAG, "Task created successfully!");
    }
    else
    {
        ESP_LOGE(TAG, "Failed to create task");
    }
}

void taskSafety(void *pvParameters)
{
    while (1)
    {
        vTaskDelay(PERIODIC_INTERVAL * 1000U / portTICK_PERIOD_MS);

        checkSensorSanity();
    }
}

void checkSensorSanity(void)
{
    static float fChamperTemperatureLast;
    static float fOutdoorTemperatureLast;
    static float fInletFlowTemperatureLast;
    static float fReturnFlowTemperatureLast;

    const float fChamperTemperatureCurrent = getChamberTemperature(CURRENT);
    if (fChamperTemperatureCurrent == fChamperTemperatureLast)
    {
        ESP_LOGE(TAG, "Champer Temperature Sensor reported unchanged value!");
    }
    else
    {
        fChamperTemperatureLast = fChamperTemperatureCurrent;

        if (fChamperTemperatureCurrent > sChamperTemperatureLimit.max)
        {
            ESP_LOGE(TAG, "Champer Temperature Sensor reported too high value!");
        }
        else if (fChamperTemperatureCurrent < sChamperTemperatureLimit.min)
        {
            ESP_LOGE(TAG, "Champer Temperature Sensor reported too low value!");
        }
        else
        {
            // everything ok
        }
    }
}

void setSafeState(void)
{
    setCirculationPumpState(DISABLED);
    setBurnerState(DISABLED);
}

sSafetyStateElement *getSafetyStates(void)
{
    return safetyStates;
}