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

#define PERIODIC_INTERVAL 1U            // run safety checks every 1sec
#define SENSOR_GRACE_PERIOD (60U * 30U) // period that a sensor can report the same reading in seconds

static const char *TAG = "smart-oil-heater-control-system-safety";
static SemaphoreHandle_t xMutexAccessSafety = NULL;
sSensorSanityCheck sanityChecks[NUMBER_OF_SENSOR_SANITY_CHECKS] = {
    {0U, "chamber_temperature", {95.0f, -10.0f}, 0.0f, 0U, getChamberTemperature},
    {0U, "outdoor_temperature", {45.0f, -20.0f}, 0.0f, 0U, getOutdoorTemperature},
    {0U, "inlet_flow_temperature", {95.0f, -10.0f}, 0.0f, 0U, getInletFlowTemperature},
    {0U, "return_flow_temperature", {95.0f, -10.0f}, 0.0f, 0U, getReturnFlowTemperature}};

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)
{

    for (int i = 0; i < NUMBER_OF_SENSOR_SANITY_CHECKS; i++)
    {
        // printf("Check sanity of sensor %s:\n", sanityChecks[i].name);
        // printf("  Status: %u\n", sanityChecks[i].status);
        // printf("  Sensor Limits: Max = %.2f, Min = %.2f\n", sanityChecks[i].sSensorLimit.max, sanityChecks[i].sSensorLimit.min);
        // printf("  Last Sensor Temperature: %.2f\n", sanityChecks[i].fSensorTemperatureLast);

        const float fSensorTemperatureCurrent = sanityChecks[i].getSensor(CURRENT);
        if (fSensorTemperatureCurrent == sanityChecks[i].fSensorTemperatureLast)
        {
            sanityChecks[i].uUnchangedCounter++;
            if (sanityChecks[i].uUnchangedCounter >= (SENSOR_GRACE_PERIOD / PERIODIC_INTERVAL))
            {
                ESP_LOGE(TAG, "%s Sensor reported unchanged value! %lf == %lf", sanityChecks[i].name, fSensorTemperatureCurrent, sanityChecks[i].fSensorTemperatureLast);
                sanityChecks[i].status = 1U;
            }
        }
        else
        {
            sanityChecks[i].fSensorTemperatureLast = fSensorTemperatureCurrent;

            if (fSensorTemperatureCurrent > sanityChecks[i].sSensorLimit.max)
            {
                ESP_LOGE(TAG, "%s Sensor reported too high value! %lf > %lf", sanityChecks[i].name, fSensorTemperatureCurrent, sanityChecks[i].sSensorLimit.max);
                sanityChecks[i].status = 1U;
            }
            else if (fSensorTemperatureCurrent < sanityChecks[i].sSensorLimit.min)
            {
                ESP_LOGE(TAG, "%s Sensor reported too low value! %lf < %lf", sanityChecks[i].name, fSensorTemperatureCurrent, sanityChecks[i].sSensorLimit.min);
                sanityChecks[i].status = 1U;
            }
            else
            {
                sanityChecks[i].uUnchangedCounter = 0U;
                sanityChecks[i].status = 0U;
            }
        }
        // printf("  Status: %u\n", sanityChecks[i].status);
    }
}

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

/*
sSafetyStateElement *getSafetyStates(void)
{
    return safetyStates;
}
*/