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

#define PERIODIC_INTERVAL 1U // Run control loop every 1 second

// Temperature thresholds
#define RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY 30.0f
#define RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT 25.0f
#define CHAMBER_TEMPERATURE_TARGET 80.0f            // Max cutoff temperature
#define CHAMBER_TEMPERATURE_THRESHOLD 45.0f         // Min threshold for burner enable
#define OUTDOOR_TEMPERATURE_THRESHOLD 15.0f         // Min threshold for burner enable
#define BURNER_FAULT_DETECTION_THRESHOLD (60U * 4U) // Burner fault detection after 4 minutes

static const char *TAG = "smart-oil-heater-control-system-control";
static eControlState sControlState = CONTROL_STARTING;

// Control table for daily schedules
static sControlDay aControlTable[] = {
    {MONDAY, 2U, {{{4, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMBER_TEMPERATURE_TARGET}, {{22, 0}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMBER_TEMPERATURE_TARGET}}},
    {TUESDAY, 2U, {{{4, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMBER_TEMPERATURE_TARGET}, {{22, 0}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMBER_TEMPERATURE_TARGET}}},
    {WEDNESDAY, 2U, {{{4, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMBER_TEMPERATURE_TARGET}, {{22, 0}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMBER_TEMPERATURE_TARGET}}},
    {THURSDAY, 2U, {{{4, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMBER_TEMPERATURE_TARGET}, {{22, 0}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMBER_TEMPERATURE_TARGET}}},
    {FRIDAY, 2U, {{{4, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMBER_TEMPERATURE_TARGET}, {{23, 0}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMBER_TEMPERATURE_TARGET}}},
    {SATURDAY, 2U, {{{6, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMBER_TEMPERATURE_TARGET}, {{23, 30}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMBER_TEMPERATURE_TARGET}}},
    {SUNDAY, 2U, {{{6, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMBER_TEMPERATURE_TARGET}, {{22, 30}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMBER_TEMPERATURE_TARGET}}},
};

// Function prototypes
void taskControl(void *pvParameters);
eControlWeekday getCurrentWeekday(void);
sControlTemperatureEntry getCurrentTemperatureEntry(void);

void initControl(void)
{
    BaseType_t taskCreated = xTaskCreate(
        taskControl,   // Function to implement the task
        "taskControl", // Task name
        8192,          // 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 taskControl(void *pvParameters)
{
    bool bHeatingInAction = false;
    eBurnerState eBurnerState = BURNER_UNKNOWN;
    int64_t i64BurnerEnableTimestamp = esp_timer_get_time();

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

        // Handle safety faults
        if (getSafetyState() != SAFETY_NO_ERROR)
        {
            ESP_LOGW(TAG, "Control not possible due to safety fault!");
            sControlState = CONTROL_FAULT_SAFETY;
            if (bHeatingInAction)
            {
                ESP_LOGW(TAG, "Disabling burner due to safety fault");
                bHeatingInAction = false;
                setBurnerState(DISABLED);
                setSafetyControlState(ENABLED);
            }
            continue;
        }

        // Handle SNTP faults
        if (getSntpState() != SYNC_SUCCESSFUL)
        {
            ESP_LOGW(TAG, "Control not possible due to SNTP fault!");
            sControlState = CONTROL_FAULT_SNTP;
            if (bHeatingInAction)
            {
                ESP_LOGW(TAG, "Disabling burner due to SNTP fault");
                bHeatingInAction = false;
                setBurnerState(DISABLED);
                setSafetyControlState(ENABLED);
            }
            continue;
        }

        // Get current temperature entry
        sControlTemperatureEntry currentControlEntry = getCurrentTemperatureEntry();

        // Enable burner if outdoor temperature is low and return flow temperature is cooled down
        if (!bHeatingInAction && (eBurnerState != BURNER_FAULT))
        {
            if (getOutdoorTemperature().average60s.fValue >= OUTDOOR_TEMPERATURE_THRESHOLD)
            {
                // ESP_LOGI(TAG, "Outdoor temperature too warm: Disabling heating");
                setBurnerState(DISABLED);
                setSafetyControlState(DISABLED);
                sControlState = CONTROL_OUTDOOR_TOO_WARM;
            }
            else if ((getReturnFlowTemperature().average60s.fValue <= currentControlEntry.fReturnFlowTemperature) &&
                     (getChamberTemperature().fCurrentValue <= CHAMBER_TEMPERATURE_THRESHOLD))
            {
                ESP_LOGI(TAG, "Enabling burner: Return flow temperature target reached");
                eBurnerState = BURNER_UNKNOWN;
                bHeatingInAction = true;
                setBurnerState(ENABLED);
                setSafetyControlState(ENABLED);
                i64BurnerEnableTimestamp = esp_timer_get_time();
                sControlState = CONTROL_HEATING;
            }
            else
            {
                // ESP_LOGI(TAG, "Return flow temperature is still to warm: Disabling heating");
                sControlState = CONTROL_RETURN_FLOW_TOO_WARM;
            }
        }

        // Disable burner if target temperature is reached or a fault occurred
        if (bHeatingInAction)
        {
            if ((getChamberTemperature().fCurrentValue >= currentControlEntry.fChamberTemperature) ||
                (getChamberTemperature().predict60s.fValue >= currentControlEntry.fChamberTemperature))
            {
                ESP_LOGI(TAG, "Chamber target temperature reached: Disabling burner");
                bHeatingInAction = false;
                setBurnerState(DISABLED);
                setSafetyControlState(ENABLED);
            }
            else if (esp_timer_get_time() - i64BurnerEnableTimestamp >= BURNER_FAULT_DETECTION_THRESHOLD * 1000000U)
            {
                if (eBurnerState == BURNER_UNKNOWN)
                {
                    if (getBurnerError() == FAULT)
                    {
                        // ESP_LOGI(TAG, "Burner fault detected after threshold!");
                        bHeatingInAction = false;
                        eBurnerState = BURNER_FAULT;
                        sControlState = CONTROL_FAULT_BURNER;
                        setBurnerState(DISABLED);
                        setSafetyControlState(ENABLED);
                    }
                    else
                    {
                        // ESP_LOGI(TAG, "No Burner fault detected after threshold!");
                        eBurnerState = BURNER_FIRED;
                    }
                }
            }
        }

        // Handle circulation pump state
        if ((getReturnFlowTemperature().average60s.fValue <= currentControlEntry.fReturnFlowTemperature) &&
            (getChamberTemperature().fCurrentValue <= CHAMBER_TEMPERATURE_THRESHOLD))
        {
            // ESP_LOGI(TAG, "Burner is cooled down: Disable circulation pump");
            setCirculationPumpState(DISABLED);
        }
        else
        {
            // ESP_LOGI(TAG, "Burner is heated: Enable circulation pump");
            setCirculationPumpState(ENABLED);
        }
    } // End of while(1)
}

eControlState getControlState(void)
{
    return sControlState;
}

eControlWeekday getCurrentWeekday(void)
{
    time_t now;
    struct tm *timeinfo;

    time(&now);
    timeinfo = localtime(&now);

    int day = timeinfo->tm_wday;
    return (eControlWeekday)((day == 0) ? 6 : day - 1);
}

sControlTemperatureEntry getCurrentTemperatureEntry(void)
{
    sControlTemperatureEntry result = aControlTable[0].aTemperatureEntries[0];
    eControlWeekday currentDay = getCurrentWeekday();

    time_t now;
    struct tm timeinfo;
    time(&now);
    localtime_r(&now, &timeinfo);

    int hour = timeinfo.tm_hour;
    int minute = timeinfo.tm_min;

    for (int i = 0; i < sizeof(aControlTable) / sizeof(aControlTable[0]); i++)
    {
        for (int j = 0; j < aControlTable[i].entryCount; j++)
        {
            if ((aControlTable[i].day > currentDay) ||
                (aControlTable[i].day == currentDay && aControlTable[i].aTemperatureEntries[j].timestamp.hour > hour) ||
                (aControlTable[i].day == currentDay && aControlTable[i].aTemperatureEntries[j].timestamp.hour == hour && aControlTable[i].aTemperatureEntries[j].timestamp.minute >= minute))
            {
                return aControlTable[i].aTemperatureEntries[j];
            }
            result = aControlTable[i].aTemperatureEntries[j];
        }
    }
    return result;
}