#include "control.h"

#include <math.h>
#include <time.h>

#include "esp_log.h"
#include "esp_task_wdt.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "freertos/task.h"
#include "relay.h"
#include "settings.h"
#include "zigbee_client.h"

static const char *TAG = "control";

static SemaphoreHandle_t s_lock;
static control_state_t s_state;

/* Freeze-protection duty cycle bookkeeping, used only in CONTROL_SRC_NONE. */
static int64_t s_freeze_phase_since_us;
static bool s_freeze_heating;

static inline float clampf(float v, float lo, float hi)
{
    return v < lo ? lo : (v > hi ? hi : v);
}

/* -------------------------------------------------------- room picking -- */

/* Freshest-first: is this room usable as a governing source right now? */
static bool room_is_fresh(int idx)
{
    return zigbee_client_reading_age_s(idx) <= CONTROL_STALE_AFTER_S;
}

/*
 * Coldest fresh room. PRD 4.2 recommends min across rooms as the default
 * formula: nobody is cold, at the cost of overheating elsewhere.
 */
static int coldest_fresh_room(float *temp_out)
{
    int best = -1;
    float coldest = 0;

    for (int i = 0; i < zigbee_client_room_count(); i++) {
        room_reading_t r;
        if (!zigbee_client_get_reading(i, &r) || !r.valid || !room_is_fresh(i)) {
            continue;
        }
        if (best < 0 || r.temperature_c < coldest) {
            best = i;
            coldest = r.temperature_c;
        }
    }

    if (best >= 0 && temp_out) {
        *temp_out = coldest;
    }
    return best;
}

/* Least-stale room with any reading at all, for the aged-data rung. */
static int freshest_known_room(float *temp_out)
{
    int best = -1;
    uint32_t best_age = UINT32_MAX;

    for (int i = 0; i < zigbee_client_room_count(); i++) {
        room_reading_t r;
        if (!zigbee_client_get_reading(i, &r) || !r.valid) {
            continue;
        }
        uint32_t age = zigbee_client_reading_age_s(i);
        if (best < 0 || age < best_age) {
            best = i;
            best_age = age;
            if (temp_out) {
                *temp_out = r.temperature_c;
            }
        }
    }
    return best;
}

/*
 * Which room the active mode wants to govern, ignoring freshness.
 *
 * Returns -1 for formula mode, where no single room is "the" room until we
 * have looked at the readings.
 */
static int mode_preferred_room(const settings_t *cfg, bool *clock_valid_out)
{
    *clock_valid_out = true;

    switch ((room_mode_t)cfg->mode) {
    case ROOM_MODE_MANUAL:
        return cfg->selected_room;

    case ROOM_MODE_SCHEDULE: {
        time_t now = time(NULL);
        /*
         * Before SNTP syncs, the clock reads 1970 and every schedule rule
         * would resolve to whatever covers midnight. Acting on that is worse
         * than not acting: fall through to the formula and say so on screen.
         */
        if (now < 1600000000) {
            *clock_valid_out = false;
            return -1;
        }
        struct tm tm;
        localtime_r(&now, &tm);
        return settings_schedule_room_for_time(tm.tm_hour, tm.tm_min);
    }

    case ROOM_MODE_FORMULA:
    default:
        return -1;
    }
}

/* --------------------------------------------------- the fallback ladder -- */

/*
 * PRD 5.5, in order:
 *   1. the selected room, if fresh
 *   2. another room's fresh reading
 *   3. (a local sensor, if one were fitted — none is)
 *   4. aged data with the setpoint clamped conservatively
 *   5. no data at all: open-loop freeze protection
 *
 * Rungs 4 and 5 always raise a visible fault. Neither ever holds the relay
 * closed on its own; the relay layer's interlocks still apply on top.
 */
static void evaluate_source(const settings_t *cfg, control_state_t *st)
{
    bool clock_valid = true;
    int preferred = mode_preferred_room(cfg, &clock_valid);
    st->schedule_clock_valid = clock_valid;

    /* Rung 1: the mode's chosen room, fresh. */
    if (preferred >= 0 && preferred < zigbee_client_room_count() &&
        room_is_fresh(preferred)) {
        room_reading_t r;
        zigbee_client_get_reading(preferred, &r);
        st->governing_room = preferred;
        st->governing_temp_c = r.temperature_c;
        st->source = CONTROL_SRC_SELECTED;
        st->effective_setpoint_c = st->setpoint_c;
        return;
    }

    /*
     * Rung 2: any fresh room, coldest wins.
     *
     * In formula mode this is the normal path rather than a fallback, so it
     * is only reported as CONTROL_SRC_OTHER_ROOM — which the UI shows as a
     * fault — when a specific room was actually asked for and failed.
     */
    float temp = 0;
    int fresh = coldest_fresh_room(&temp);
    if (fresh >= 0) {
        st->governing_room = fresh;
        st->governing_temp_c = temp;
        st->source = (preferred < 0 && clock_valid)
                         ? CONTROL_SRC_SELECTED
                         : CONTROL_SRC_OTHER_ROOM;
        st->effective_setpoint_c = st->setpoint_c;
        return;
    }

    /* Rung 4: nothing fresh anywhere. Run on the least-stale reading we have,
     * with the setpoint clamped down so a wrong number cannot cook the house. */
    int known = freshest_known_room(&temp);
    if (known >= 0) {
        st->governing_room = known;
        st->governing_temp_c = temp;
        st->source = CONTROL_SRC_STALE;
        st->effective_setpoint_c =
            fminf(st->setpoint_c, CONTROL_FALLBACK_SETPOINT_C);
        return;
    }

    /* Rung 5: no reading since boot. */
    st->governing_room = -1;
    st->governing_temp_c = NAN;
    st->source = CONTROL_SRC_NONE;
    st->effective_setpoint_c = CONTROL_FALLBACK_SETPOINT_C;
}

/* ----------------------------------------------------------- decision --- */

/*
 * Open-loop freeze protection. Only reachable with no reading at all.
 *
 * Deliberately a low duty cycle rather than "off": PRD 5.5 says stale or
 * missing data must not silently mean no heat, because frozen pipes are the
 * failure this is guarding against.
 */
static bool freeze_protection_wants_heat(void)
{
    int64_t now = esp_timer_get_time();
    if (s_freeze_phase_since_us == 0) {
        s_freeze_phase_since_us = now;
        s_freeze_heating = false;
    }

    uint32_t phase_s = (uint32_t)((now - s_freeze_phase_since_us) / 1000000);
    uint32_t limit = s_freeze_heating ? CONTROL_FREEZE_DUTY_ON_S
                                      : CONTROL_FREEZE_DUTY_OFF_S;

    if (phase_s >= limit) {
        s_freeze_heating = !s_freeze_heating;
        s_freeze_phase_since_us = now;
    }

    return s_freeze_heating;
}

/* Hysteresis. Below setpoint-deadband call for heat, above setpoint+deadband
 * stop, in between hold whatever we were doing. */
static bool decide_heat(const control_state_t *st, bool currently_on)
{
    if (st->source == CONTROL_SRC_NONE) {
        return freeze_protection_wants_heat();
    }

    float t = st->governing_temp_c;
    if (isnan(t)) {
        return false;
    }

    if (t < st->effective_setpoint_c - CONTROL_DEADBAND_C) {
        return true;
    }
    if (t > st->effective_setpoint_c + CONTROL_DEADBAND_C) {
        return false;
    }
    return currently_on;
}

/* ---------------------------------------------------------- main task --- */

static void control_task(void *arg)
{
    (void)arg;

    esp_task_wdt_add(NULL);

    control_source_t last_source = CONTROL_SRC_SELECTED;
    bool last_call = false;

    while (1) {
        esp_task_wdt_reset();

        settings_t cfg;
        settings_get(&cfg);

        control_state_t st;
        xSemaphoreTake(s_lock, portMAX_DELAY);
        st = s_state;
        xSemaphoreGive(s_lock);

        st.setpoint_c = clampf(cfg.setpoint_c,
                               CONTROL_SETPOINT_MIN_C, CONTROL_SETPOINT_MAX_C);
        st.mode = (room_mode_t)cfg.mode;

        evaluate_source(&cfg, &st);

        /* Reset the freeze-protection phase whenever real data returns, so a
         * brief outage does not leave us mid-duty-cycle. */
        if (st.source != CONTROL_SRC_NONE) {
            s_freeze_phase_since_us = 0;
            s_freeze_heating = false;
        }

        st.calling_for_heat = decide_heat(&st, relay_is_on());

        /*
         * Always call, every cycle, including when asking for off. This is the
         * relay watchdog refresh: silence is what tells the relay layer the
         * control loop has died.
         */
        relay_request_heat(st.calling_for_heat);

        xSemaphoreTake(s_lock, portMAX_DELAY);
        s_state = st;
        xSemaphoreGive(s_lock);

        if (st.source != last_source) {
            if (st.source == CONTROL_SRC_SELECTED) {
                ESP_LOGI(TAG, "source ok: %s",
                         zigbee_client_room_name(st.governing_room));
            } else {
                ESP_LOGW(TAG, "degraded to %s", control_source_str(st.source));
            }
            last_source = st.source;
        }

        if (st.calling_for_heat != last_call) {
            if (isnan(st.governing_temp_c)) {
                ESP_LOGI(TAG, "heat %s (no reading, freeze protection)",
                         st.calling_for_heat ? "requested" : "released");
            } else {
                ESP_LOGI(TAG, "heat %s: %.1fC vs setpoint %.1fC",
                         st.calling_for_heat ? "requested" : "released",
                         st.governing_temp_c, st.effective_setpoint_c);
            }
            last_call = st.calling_for_heat;
        }

        /*
         * Sleep in 1s slices, feeding the task watchdog each one. A single
         * CONTROL_INTERVAL_S delay would finish fractionally *after* the TWDT
         * deadline every iteration (both are 10s) and trip it spuriously —
         * while still keeping the evaluation cadence at CONTROL_INTERVAL_S.
         */
        for (int i = 0; i < CONTROL_INTERVAL_S; i++) {
            esp_task_wdt_reset();
            vTaskDelay(pdMS_TO_TICKS(1000));
        }
    }
}

/* ---------------------------------------------------------------- api --- */

esp_err_t control_init(void)
{
    if (s_lock) {
        return ESP_OK;
    }

    s_lock = xSemaphoreCreateMutex();
    if (!s_lock) {
        return ESP_ERR_NO_MEM;
    }

    settings_t cfg;
    settings_get(&cfg);

    s_state = (control_state_t){
        .setpoint_c = cfg.setpoint_c,
        .effective_setpoint_c = cfg.setpoint_c,
        .governing_temp_c = NAN,
        .governing_room = -1,
        .mode = (room_mode_t)cfg.mode,
        .source = CONTROL_SRC_NONE,
        .calling_for_heat = false,
        .schedule_clock_valid = false,
    };

    if (xTaskCreate(control_task, "control", 4096, NULL, 5, NULL) != pdPASS) {
        ESP_LOGE(TAG, "task create failed");
        return ESP_ERR_NO_MEM;
    }

    ESP_LOGI(TAG, "started: %ds cadence, deadband +/-%.1fC, stale after %ds",
             CONTROL_INTERVAL_S, CONTROL_DEADBAND_C, CONTROL_STALE_AFTER_S);
    return ESP_OK;
}

void control_get_state(control_state_t *out)
{
    if (!out) {
        return;
    }
    if (!s_lock) {
        *out = (control_state_t){
            .governing_temp_c = NAN,
            .governing_room = -1,
            .source = CONTROL_SRC_NONE,
        };
        return;
    }

    xSemaphoreTake(s_lock, portMAX_DELAY);
    *out = s_state;
    xSemaphoreGive(s_lock);
}

void control_set_setpoint(float celsius)
{
    settings_set_setpoint(clampf(celsius,
                                 CONTROL_SETPOINT_MIN_C, CONTROL_SETPOINT_MAX_C));
}

void control_adjust_setpoint(float delta_c)
{
    settings_t cfg;
    settings_get(&cfg);
    control_set_setpoint(cfg.setpoint_c + delta_c);
}

float control_get_setpoint(void)
{
    settings_t cfg;
    settings_get(&cfg);
    return clampf(cfg.setpoint_c, CONTROL_SETPOINT_MIN_C, CONTROL_SETPOINT_MAX_C);
}

void control_set_mode(room_mode_t mode)
{
    if (mode < ROOM_MODE_COUNT) {
        settings_set_mode((uint8_t)mode);
    }
}

room_mode_t control_get_mode(void)
{
    settings_t cfg;
    settings_get(&cfg);
    return (room_mode_t)cfg.mode;
}

void control_select_room(int room_index)
{
    if (room_index < 0 || room_index >= zigbee_client_room_count()) {
        return;
    }
    settings_set_selected_room((uint8_t)room_index);
    /* PRD 4.2: manual override must always be reachable regardless of the
     * active mode. Picking a room *is* that override. */
    settings_set_mode(ROOM_MODE_MANUAL);
}

int control_get_selected_room(void)
{
    settings_t cfg;
    settings_get(&cfg);
    return cfg.selected_room;
}

const char *control_source_str(control_source_t src)
{
    switch (src) {
    case CONTROL_SRC_SELECTED:   return "ok";
    case CONTROL_SRC_OTHER_ROOM: return "sensor stale, using another room";
    case CONTROL_SRC_STALE:      return "all sensors stale, setpoint reduced";
    case CONTROL_SRC_NONE:       return "no sensor data, freeze protection";
    default:                     return "unknown";
    }
}

const char *control_mode_str(room_mode_t mode)
{
    switch (mode) {
    case ROOM_MODE_MANUAL:   return "Manual";
    case ROOM_MODE_SCHEDULE: return "Schedule";
    case ROOM_MODE_FORMULA:  return "Coldest";
    default:                 return "?";
    }
}