#include "control.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "inputs.h"
#include "outputs.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 SUMMER_MODE_TEMPERATURE_THRESHOLD_HIGH \
    20.0f // Summer mode will be activated
#define SUMMER_MODE_TEMPERATURE_THRESHOLD_LOW \
    15.0f // Summer mode will be deactivated --> Heating starts
#define CIRCULATION_PUMP_TEMPERATURE_THRESHOLD \
    30.0f // Min threshold of chamber for circulation pump 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 gControlState = CONTROL_STARTING;
// Control table for daily schedules
static const sControlDay gControlTable[] = {
    {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}}},
};
static sControlTemperatureEntry gCurrentControlEntry =
    gControlTable[0].aTemperatureEntries[0];
static SemaphoreHandle_t xMutexAccessControl = NULL;

// Function prototypes
void taskControl(void *pvParameters);
void findControlCurrentTemperatureEntry(void);
void setControlState(eControlState state);

void initControl(void)
{

    xMutexAccessControl = xSemaphoreCreateRecursiveMutex();
    if (xMutexAccessControl == NULL)
    {
        ESP_LOGE(TAG, "Unable to create mutex");
    }
    xSemaphoreGiveRecursive(xMutexAccessControl);

    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 bSummerMode = false;
    eBurnerState burnerState = BURNER_UNKNOWN;
    int64_t i64BurnerEnableTimestamp = esp_timer_get_time();

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

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

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

        findControlCurrentTemperatureEntry();

        if (getOutdoorTemperature().fDampedValue >=
            SUMMER_MODE_TEMPERATURE_THRESHOLD_HIGH)
        {
            bSummerMode = true;
        }
        else if (getOutdoorTemperature().fDampedValue <=
                 SUMMER_MODE_TEMPERATURE_THRESHOLD_LOW)
        {
            bSummerMode = false;
        }

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

        // Disable burner if target temperature is reached or a fault occurred
        if (bHeatingInAction)
        {
            if ((getChamberTemperature().fCurrentValue >=
                 getControlCurrentTemperatureEntry().fChamberTemperature) ||
                (getChamberTemperature().predict60s.fValue >=
                 getControlCurrentTemperatureEntry().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 (burnerState == BURNER_UNKNOWN)
                {
                    if (getBurnerError() == FAULT)
                    {
                        // ESP_LOGW(TAG, "Burner fault detected: Disabling burner");
                        bHeatingInAction = false;
                        burnerState = BURNER_FAULT;
                        setControlState(CONTROL_FAULT_BURNER);
                        setBurnerState(DISABLED);
                        setSafetyControlState(ENABLED);
                    }
                    else
                    {
                        // ESP_LOGI(TAG, "No burner fault detected: Marking burner as
                        // fired");
                        burnerState = BURNER_FIRED;
                    }
                }
            }
        }

        // Manage circulation pump
        if (getChamberTemperature().fCurrentValue <=
            CIRCULATION_PUMP_TEMPERATURE_THRESHOLD)
        {
            // ESP_LOGI(TAG, "Burner cooled down: Disabling circulation pump");
            setCirculationPumpState(DISABLED);
        }
        else
        {
            // ESP_LOGI(TAG, "Burner heated: Enabling circulation pump");
            setCirculationPumpState(ENABLED);
        }
    } // End of while(1)
}

void setControlState(eControlState state)
{

    if (xSemaphoreTakeRecursive(xMutexAccessControl, pdMS_TO_TICKS(5000)) == pdTRUE)
    {
        gControlState = state;
        xSemaphoreGiveRecursive(xMutexAccessControl);
    }
    else
    {
        ESP_LOGE(TAG, "Unable to take mutex: setControlState()");
    }
}

eControlState getControlState(void)
{

    eControlState ret = CONTROL_FAULT_SAFETY;

    if (xSemaphoreTakeRecursive(xMutexAccessControl, pdMS_TO_TICKS(5000)) == pdTRUE)
    {
        ret = gControlState;
        xSemaphoreGiveRecursive(xMutexAccessControl);
    }
    else
    {
        ESP_LOGE(TAG, "Unable to take mutex: getControlState()");
    }

    return ret;
}

eControlWeekday getControlCurrentWeekday(void)
{
    // Get current time
    time_t now;
    struct tm timeinfo;
    time(&now);
    localtime_r(&now, &timeinfo);

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

/**
 * @brief Finds the active temperature control entry for the current time.
 *
 * Searches through the weekly schedule to find the most recent entry
 * that should be active at the current date/time. Falls back to the
 * last entry in the week if no suitable entry is found.
 */
/**
 * @brief Finds the active temperature control entry for the current time.
 *
 * Searches through the weekly schedule to find the most recent entry
 * that should be active at the current date/time. Falls back to the
 * last entry in the week if no suitable entry is found.
 */
void findControlCurrentTemperatureEntry(void)
{
    eControlWeekday currentDay = getControlCurrentWeekday();

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

    int currentHour = timeinfo.tm_hour;
    int currentMinute = timeinfo.tm_min;

    if (xSemaphoreTakeRecursive(xMutexAccessControl, pdMS_TO_TICKS(5000)) == pdTRUE)
    {

        // ESP_LOGI(TAG, "Searching for control entry - Day: %d, Time: %02d:%02d", currentDay, currentHour, currentMinute);

        // Search through all days and entries
        for (int dayIndex = 0; dayIndex < 7; dayIndex++)
        {
            const sControlDay *day = &gControlTable[dayIndex];

            for (int entryIndex = 0; entryIndex < day->entryCount; entryIndex++)
            {
                const sControlTemperatureEntry *entry = &day->aTemperatureEntries[entryIndex];

                // Check if this entry is in the future (next active entry)
                bool isFutureDay = (day->day > currentDay);
                bool isTodayFutureTime = (day->day == currentDay) &&
                                         ((entry->timestamp.hour > currentHour) ||
                                          (entry->timestamp.hour == currentHour &&
                                           entry->timestamp.minute > currentMinute));

                if (isFutureDay || isTodayFutureTime)
                {

                    // Found next scheduled entry, so determine the previous (active) one
                    if (entryIndex > 0)
                    {
                        // Use previous entry from same day
                        gCurrentControlEntry = day->aTemperatureEntries[entryIndex - 1];
                    }
                    else if (dayIndex > 0)
                    {
                        // Use last entry from previous day
                        const sControlDay *previousDay = &gControlTable[dayIndex - 1];
                        gCurrentControlEntry = previousDay->aTemperatureEntries[previousDay->entryCount - 1];
                    }
                    else
                    {
                        // First entry of the week - wrap to last entry of Sunday
                        const sControlDay *sunday = &gControlTable[6];
                        gCurrentControlEntry = sunday->aTemperatureEntries[sunday->entryCount - 1];
                    }
                    /*
                    ESP_LOGI(TAG, "Active entry found - Time: %02d:%02d, "
                             "Return Temp: %lf, Chamber Temp: %lf",
                             gCurrentControlEntry.timestamp.hour,
                             gCurrentControlEntry.timestamp.minute,
                             gCurrentControlEntry.fReturnFlowTemperature,
                             gCurrentControlEntry.fChamberTemperature);
                             */
                    return;
                }
            }
        }

        // If we reached here, current time is after all entries this week
        // Use the last entry (Sunday evening)
        const sControlDay *sunday = &gControlTable[6];
        gCurrentControlEntry = sunday->aTemperatureEntries[sunday->entryCount - 1];

        // ESP_LOGI(TAG, "Using last entry of week - Time: %02d:%02d", gCurrentControlEntry.timestamp.hour, gCurrentControlEntry.timestamp.minute);
        xSemaphoreGiveRecursive(xMutexAccessControl);
    }
    else
    {
        ESP_LOGE(TAG, "Unable to take mutex: findControlCurrentTemperatureEntry()");
    }
}

sControlTemperatureEntry getControlCurrentTemperatureEntry(void)
{
    sControlTemperatureEntry ret = gControlTable[0].aTemperatureEntries[0];
    if (xSemaphoreTakeRecursive(xMutexAccessControl, pdMS_TO_TICKS(5000)) == pdTRUE)
    {
        ret = gCurrentControlEntry;
        xSemaphoreGiveRecursive(xMutexAccessControl);
    }
    else
    {
        ESP_LOGE(TAG, "Unable to take mutex: getControlCurrentTemperatureEntry()");
    }

    return ret;
}