#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 1sec

#define RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY 30.0f
#define RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT 25.0f
#define CHAMPER_TEMPERATURE_TARGET 80.0f            // Max cut off temperature, burner will stop if chamber is even or higher
#define CHAMPER_TEMPERATURE_THRESHOLD 45.0f         // Min threshold temperature for enabling burner, burner will only start if chamber is even or lower
#define OUTDOOR_TEMPERATURE_THRESHOLD 15.0f         // Min threshold temperature for enabling burner, burner will only start if outdoor is even or lower
#define BURNER_FAULT_DETECTION_THRESHOLD (60U * 3U) // Detect burner fault if after 3 minutes no burner start detected

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

static sControlDay aControlTable[] = {
    {MONDAY, 2U, {{{4, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMPER_TEMPERATURE_TARGET}, {{22, 0}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMPER_TEMPERATURE_TARGET}}},
    {TUESDAY, 2U, {{{4, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMPER_TEMPERATURE_TARGET}, {{22, 0}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMPER_TEMPERATURE_TARGET}}},
    {WEDNESDAY, 2U, {{{4, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMPER_TEMPERATURE_TARGET}, {{22, 0}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMPER_TEMPERATURE_TARGET}}},
    {THURSDAY, 2U, {{{4, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMPER_TEMPERATURE_TARGET}, {{22, 0}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMPER_TEMPERATURE_TARGET}}},
    {FRIDAY, 2U, {{{4, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMPER_TEMPERATURE_TARGET}, {{23, 0}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMPER_TEMPERATURE_TARGET}}},
    {SATURDAY, 2U, {{{6, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMPER_TEMPERATURE_TARGET}, {{23, 30}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMPER_TEMPERATURE_TARGET}}},
    {SUNDAY, 2U, {{{6, 45}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_DAY, CHAMPER_TEMPERATURE_TARGET}, {{22, 30}, RETURN_FLOW_TEMPERATURE_LOWER_LIMIT_NIGHT, CHAMPER_TEMPERATURE_TARGET}}},
};

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;
    bool bBurnerFaultDetected = false;
    int64_t i64BurnerEnableTimestamp = esp_timer_get_time();

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

        if (getSafetyState() != SAFETY_NO_ERROR)
        {
            ESP_LOGW(TAG, "Control not possible due to safety fault!");
            sControlState = CONTROL_FAULT_SAFETY;
            if (bHeatingInAction == true)
            {
                ESP_LOGW(TAG, "Control not possible due to safety fault: Disable burner");
                bHeatingInAction = false;
                setCirculationPumpState(ENABLED);
                setBurnerState(DISABLED);
                setSafetyControlState(ENABLED);
            }
            continue;
        }

        if (getSntpState() != SYNC_SUCCESSFUL)
        {
            ESP_LOGW(TAG, "Control not possible due to sntp fault!");
            sControlState = CONTROL_FAULT_SNTP;
            if (bHeatingInAction == true)
            {
                ESP_LOGW(TAG, "Control not possible due to sntp fault: Disable burner");
                bHeatingInAction = false;
                setCirculationPumpState(ENABLED);
                setBurnerState(DISABLED);
                setSafetyControlState(ENABLED);
            }
            continue;
        }

        sControlTemperatureEntry currentControlEntry = getCurrentTemperatureEntry();
        // ESP_LOGI(TAG, "Control Entry Hour: %i Minute: %i ChamberTemp: %lf ReturnFlowTemp: %lf", currentControlEntry.timestamp.hour, currentControlEntry.timestamp.minute, currentControlEntry.fChamberTemperature, currentControlEntry.fReturnFlowTemperature);

        if (bHeatingInAction == true)
        {
            if ((getChamberTemperature().fCurrentValue >= currentControlEntry.fChamberTemperature) || (getChamberTemperature().predict60s.fValue >= currentControlEntry.fChamberTemperature))
            {
                ESP_LOGI(TAG, "Chamber Target Temperature reached: Disable burner");
                bHeatingInAction = false;
                setCirculationPumpState(ENABLED);
                setBurnerState(DISABLED);
                setSafetyControlState(ENABLED);
            }
            else
            {
                if (bHeatingInAction)
                {
                    int64_t i64Delta = esp_timer_get_time() - i64BurnerEnableTimestamp;

                    if ((i64Delta / 1000000U) >= BURNER_FAULT_DETECTION_THRESHOLD)
                    {
                        if (getBurnerError() == FAULT)
                        {
                            ESP_LOGW(TAG, "Detected burner fault after %lli seconds!", (i64Delta / 1000000U));
                            ESP_LOGW(TAG, "Control not possible due to burner fault: Disable burner");
                            sControlState = CONTROL_FAULT_BURNER;
                            bHeatingInAction = false;
                            bBurnerFaultDetected = true;
                            setCirculationPumpState(ENABLED);
                            setBurnerState(DISABLED);
                            setSafetyControlState(ENABLED);
                        }
                    }
                }
            }
        }

        if ((bHeatingInAction == false) && (bBurnerFaultDetected == false))
        {
            if (getOutdoorTemperature().average60s.fValue >= OUTDOOR_TEMPERATURE_THRESHOLD)
            {
                ESP_LOGI(TAG, "Outdoor temperature too warm: Waiting for winter.");
                setCirculationPumpState(DISABLED);
                setBurnerState(DISABLED);
                setSafetyControlState(DISABLED);
                sControlState = CONTROL_OUTDOOR_TOO_WARM;
            }
            else
            {
                if ((getReturnFlowTemperature().average60s.fValue <= currentControlEntry.fReturnFlowTemperature) && (getChamberTemperature().fCurrentValue <= CHAMPER_TEMPERATURE_THRESHOLD))
                {
                    ESP_LOGI(TAG, "Return Flow Target Temperature reached: Enable Burner");
                    bHeatingInAction = true;
                    setCirculationPumpState(ENABLED);
                    setBurnerState(ENABLED);
                    setSafetyControlState(ENABLED);
                    i64BurnerEnableTimestamp = esp_timer_get_time();
                    sControlState = CONTROL_HEATING;
                }
                else
                {
                    sControlState = CONTROL_RETURN_FLOW_TOO_WARM;
                }
            }
        }
    }
}

eControlState getControlState(void)
{
    return sControlState;
}

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

    // Get the current time
    time(&now);
    timeinfo = localtime(&now); // Convert to local time

    // Get the day of the week (0 = Sunday, 1 = Monday, ..., 6 = Saturday)
    int day = timeinfo->tm_wday;

    // Adjust so that Monday = 0, Sunday = 6
    if (day == 0)
    {
        day = 6; // Sunday becomes 6
    }
    else
    {
        day -= 1; // Shift other days to make Monday = 0
    }

    return (eControlWeekday)day;
}

sControlTemperatureEntry getCurrentTemperatureEntry(void)
{
    sControlTemperatureEntry result = aControlTable[0].aTemperatureEntries[0];
    eControlWeekday currentDay = getCurrentWeekday();
    time_t now;
    struct tm timeinfo;

    // Get the current time
    time(&now);
    // Convert to local time structure
    localtime_r(&now, &timeinfo);
    // Extract hour and minute
    int hour = timeinfo.tm_hour;  // Hour (0-23)
    int minute = timeinfo.tm_min; // Minute (0-59)u

    // ESP_LOGI(TAG, "Current Day: %i Hour: %i Minute: %i", currentDay, hour, minute);

    for (int i = 0; i < sizeof(aControlTable) / sizeof(aControlTable[0]); i++)
    {
        /// loops through days
        // ESP_LOGI(TAG, "Day %d: %d", i + 1, aControlTable[i].day);
        // int numberOfEntries = aControlTable[i].entryCount;
        // ESP_LOGI(TAG, "Number of entries: %i", numberOfEntries);

        for (int j = 0; j < aControlTable[i].entryCount; j++)
        {
            if ((aControlTable[i].day) > currentDay)
            {
                // ESP_LOGI(TAG, "DAY Return Control Entry Day: %i Hour: %i Minute: %i ChamberTemp: %lf ReturnFlowTemp: %lf", aControlTable[i].day, aControlTable[i].aTemperatureEntries[j].timestamp.hour, aControlTable[i].aTemperatureEntries[j].timestamp.minute, aControlTable[i].aTemperatureEntries[j].fChamberTemperature, aControlTable[i].aTemperatureEntries[j].fReturnFlowTemperature);
                return result;
            }

            if ((aControlTable[i].day == currentDay) && (aControlTable[i].aTemperatureEntries[j].timestamp.hour > hour))
            {
                // ESP_LOGI(TAG, "HOUR Return Control Entry Day: %i Hour: %i Minute: %i ChamberTemp: %lf ReturnFlowTemp: %lf", aControlTable[i].day, aControlTable[i].aTemperatureEntries[j].timestamp.hour, aControlTable[i].aTemperatureEntries[j].timestamp.minute, aControlTable[i].aTemperatureEntries[j].fChamberTemperature, aControlTable[i].aTemperatureEntries[j].fReturnFlowTemperature);
                return result;
            }

            if ((aControlTable[i].day == currentDay) && (aControlTable[i].aTemperatureEntries[j].timestamp.hour == hour) && (aControlTable[i].aTemperatureEntries[j].timestamp.minute == minute))
            {
                // ESP_LOGI(TAG, "MINUTE Return Control Entry Day: %i Hour: %i Minute: %i ChamberTemp: %lf ReturnFlowTemp: %lf", aControlTable[i].day, aControlTable[i].aTemperatureEntries[j].timestamp.hour, aControlTable[i].aTemperatureEntries[j].timestamp.minute, aControlTable[i].aTemperatureEntries[j].fChamberTemperature, aControlTable[i].aTemperatureEntries[j].fReturnFlowTemperature);
                return result;
            }

            // ESP_LOGI(TAG, "SET Return Control Entry Day: %i Hour: %i Minute: %i ChamberTemp: %lf ReturnFlowTemp: %lf", aControlTable[i].day, aControlTable[i].aTemperatureEntries[j].timestamp.hour, aControlTable[i].aTemperatureEntries[j].timestamp.minute, aControlTable[i].aTemperatureEntries[j].fChamberTemperature, aControlTable[i].aTemperatureEntries[j].fReturnFlowTemperature);
            result = aControlTable[i].aTemperatureEntries[j];
        }
    }
    return result;
}