/**
 * @file safety.c
 * @brief Implementation of safety monitoring module.
 */

#include "safety.h"

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

#include <string.h>
#include <math.h>

/** @brief Task interval in seconds. */
#define PERIODIC_INTERVAL 1U

/** @brief Grace period for unchanged sensor readings (seconds). */
#define SENSOR_GRACE_PERIOD (CONFIG_SENSOR_GRACE_PERIOD_MINUTES * 60U)

/** @brief Epsilon for float comparison. */
#define FLOAT_EPSILON 0.0001f

static const char *TAG = "safety";

static SemaphoreHandle_t xMutexAccessSafety = NULL;

/** @brief Sensor sanity check configurations. */
static sSensorSanityCheck sanityChecks[NUMBER_OF_SENSOR_SANITY_CHECKS] = {
    {SENSOR_NO_ERROR, "chamber_temperature", {SENSOR_LIMIT_CHAMBER_MAX, SENSOR_LIMIT_CHAMBER_MIN}, 0.0f, 0U, getChamberTemperature},
    {SENSOR_NO_ERROR, "outdoor_temperature", {SENSOR_LIMIT_OUTDOOR_MAX, SENSOR_LIMIT_OUTDOOR_MIN}, 0.0f, 0U, getOutdoorTemperature},
    {SENSOR_NO_ERROR, "inlet_flow_temperature", {SENSOR_LIMIT_INLET_MAX, SENSOR_LIMIT_INLET_MIN}, 0.0f, 0U, getInletFlowTemperature},
    {SENSOR_NO_ERROR, "return_flow_temperature", {SENSOR_LIMIT_RETURN_MAX, SENSOR_LIMIT_RETURN_MIN}, 0.0f, 0U, getReturnFlowTemperature}};

static eSafetyState sSafetyState = SAFETY_NO_ERROR;

/* Private function prototypes */
static void taskSafety(void *pvParameters);
static void checkSensorSanity(void);
static void setSafeState(void);

esp_err_t initSafety(void)
{
    xMutexAccessSafety = xSemaphoreCreateRecursiveMutex();
    if (xMutexAccessSafety == NULL)
    {
        ESP_LOGE(TAG, "Failed to create mutex");
        return ESP_FAIL;
    }
    xSemaphoreGiveRecursive(xMutexAccessSafety);

    BaseType_t taskCreated = xTaskCreate(
        taskSafety,
        "taskSafety",
        4096,
        NULL,
        5,
        NULL);

    if (taskCreated != pdPASS)
    {
        ESP_LOGE(TAG, "Failed to create task");
        return ESP_FAIL;
    }

    setSafeState();

    ESP_LOGI(TAG, "Initialized successfully");
    return ESP_OK;
}

/**
 * @brief Safety monitoring task.
 * @param pvParameters Task parameters (unused).
 */
static void taskSafety(void *pvParameters)
{
    while (1)
    {
        vTaskDelay(PERIODIC_INTERVAL * 1000U / portTICK_PERIOD_MS);

        if (xSemaphoreTakeRecursive(xMutexAccessSafety, portMAX_DELAY) == pdTRUE)
        {
            checkSensorSanity();

            if (sSafetyState != SAFETY_NO_ERROR)
            {
                setSafeState();
            }

            xSemaphoreGiveRecursive(xMutexAccessSafety);
        }
    }
}

/**
 * @brief Check all sensor readings for sanity.
 */
static void checkSensorSanity(void)
{
    sSafetyState = SAFETY_NO_ERROR;
    for (int i = 0; i < NUMBER_OF_SENSOR_SANITY_CHECKS; i++)
    {
        // printf("Check sanity of sensor %s:\n", sanityChecks[i].name);
        // printf("  state: %u\n", sanityChecks[i].state);
        // 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 sMeasurement sCurrentMeasurement = sanityChecks[i].getSensor();

        if (sCurrentMeasurement.state == MEASUREMENT_FAULT)
        {
            ESP_LOGE(TAG, "%s Sensor not found!", sanityChecks[i].name);
            sanityChecks[i].state = SENSOR_NOT_FOUND;
            sSafetyState = SAFETY_SENSOR_ERROR;
        }
        else
        {
            if (fabsf(sCurrentMeasurement.fCurrentValue - sanityChecks[i].fSensorTemperatureLast) < FLOAT_EPSILON)
            {
                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, sCurrentMeasurement.fCurrentValue, sanityChecks[i].fSensorTemperatureLast);
                    sanityChecks[i].state = SENSOR_UNCHANGED;
                    sSafetyState = SAFETY_SENSOR_ERROR;
                }
            }
            else
            {
                sanityChecks[i].uUnchangedCounter = 0U;
                sanityChecks[i].fSensorTemperatureLast = sCurrentMeasurement.fCurrentValue;

                if (sCurrentMeasurement.fCurrentValue > sanityChecks[i].sSensorLimit.max)
                {
                    ESP_LOGE(TAG, "%s Sensor reported too high value! %lf > %lf", sanityChecks[i].name, sCurrentMeasurement.fCurrentValue, sanityChecks[i].sSensorLimit.max);
                    sanityChecks[i].state = SENSOR_TOO_HIGH;
                    sSafetyState = SAFETY_SENSOR_ERROR;
                }
                else if (sCurrentMeasurement.fCurrentValue < sanityChecks[i].sSensorLimit.min)
                {
                    ESP_LOGE(TAG, "%s Sensor reported too low value! %lf < %lf", sanityChecks[i].name, sCurrentMeasurement.fCurrentValue, sanityChecks[i].sSensorLimit.min);
                    sanityChecks[i].state = SENSOR_TOO_LOW;
                    sSafetyState = SAFETY_SENSOR_ERROR;
                }
                else
                {
                    sanityChecks[i].state = SENSOR_NO_ERROR;
                }
            }
        }
    }
}

/**
 * @brief Set system to safe state (burner off, pump on).
 */
static void setSafeState(void)
{
    setCirculationPumpState(ENABLED); // To cool down system
    setBurnerState(DISABLED);         // Deactivate burner
    setSafetyControlState(DISABLED);  // Disable power to Burner
}

void getSensorSanityStates(sSensorSanityCheck *pSensorSanityChecks)
{
    if (xSemaphoreTakeRecursive(xMutexAccessSafety, pdMS_TO_TICKS(5000)) == pdTRUE)
    {
        for (size_t i = 0; i < NUMBER_OF_SENSOR_SANITY_CHECKS; i++)
        {
            // Copy only the needed attributes
            pSensorSanityChecks[i].state = sanityChecks[i].state;
            strncpy(pSensorSanityChecks[i].name, sanityChecks[i].name, MAX_ERROR_STRING_SIZE);
        }
        xSemaphoreGiveRecursive(xMutexAccessSafety);
    }
    else
    {
        ESP_LOGE(TAG, "Unable to take mutex: getSensorSanityStates()");
    }
}

eSafetyState getSafetyState(void)
{
    eSafetyState state = SAFETY_NO_ERROR;
    if (xSemaphoreTakeRecursive(xMutexAccessSafety, pdMS_TO_TICKS(5000)) == pdTRUE)
    {
        state = sSafetyState;
        xSemaphoreGiveRecursive(xMutexAccessSafety);
    }
    else
    {
        state = SAFETY_INTERNAL_ERROR;
        ESP_LOGE(TAG, "Unable to take mutex: getSafetyState()");
    }
    return state;
}