#include <ESP32Servo.h>
#include <Preferences.h>
#include "app.h"
#include "config.h"

static Servo servo;
static Preferences prefs;

static int volumeToUs(uint8_t volume) {
    return map(volume, 0, 100, SERVO_MIN_US, SERVO_MAX_US);
}

static void applyRelay(bool on) {
    digitalWrite(PIN_RELAY, on ? RELAY_ACTIVE_LEVEL : !RELAY_ACTIVE_LEVEL);
}

// Owns the servo, the relay and NVS. Other tasks only send commands.
void controlTask(void *arg) {
    pinMode(PIN_RELAY, OUTPUT);

    prefs.begin("volume", false);
    AppState st;
    st.volume = prefs.getUChar("vol", 30);
    st.relay = prefs.getBool("relay", false);
    setState(st);
    applyRelay(st.relay);

    servo.setPeriodHertz(50);
    int currentUs = volumeToUs(st.volume);
    servo.attach(PIN_SERVO, SERVO_MIN_US, SERVO_MAX_US);
    servo.writeMicroseconds(currentUs);
    TickType_t lastMove = xTaskGetTickCount();

    bool dirty = false;
    TickType_t lastChange = 0;

    Serial.printf("[control] restored volume=%u relay=%d\n", st.volume, st.relay);

    for (;;) {
        int targetUs = volumeToUs(st.volume);
        bool moving = currentUs != targetUs;
        TickType_t wait = (moving || dirty || servo.attached())
                              ? pdMS_TO_TICKS(SERVO_UPDATE_MS)
                              : portMAX_DELAY;

        Command cmd;
        if (xQueueReceive(cmdQueue, &cmd, wait) == pdTRUE) {
            AppState prev = st;
            switch (cmd.type) {
                case CmdType::VolumeDelta:
                    st.volume = constrain((int)st.volume + cmd.value, 0, 100);
                    break;
                case CmdType::VolumeSet:
                    st.volume = constrain((int)cmd.value, 0, 100);
                    break;
                case CmdType::RelayToggle:
                    st.relay = !st.relay;
                    break;
                case CmdType::RelaySet:
                    st.relay = cmd.value != 0;
                    break;
            }
            if (st.relay != prev.relay) {
                applyRelay(st.relay);
                Serial.printf("[control] relay %s\n", st.relay ? "ON" : "OFF");
            }
            if (st.volume != prev.volume || st.relay != prev.relay) {
                setState(st);
                dirty = true;
                lastChange = xTaskGetTickCount();
            }
            targetUs = volumeToUs(st.volume);
        }

        TickType_t now = xTaskGetTickCount();

        // Smooth servo movement toward the target
        if (currentUs != targetUs) {
            int diff = targetUs - currentUs;
            int step = constrain(diff, -SERVO_SPEED_US_PER_STEP, SERVO_SPEED_US_PER_STEP);
            currentUs += step;
            if (!servo.attached()) servo.attach(PIN_SERVO, SERVO_MIN_US, SERVO_MAX_US);
            servo.writeMicroseconds(currentUs);
            lastMove = now;
        } else if (servo.attached() && now - lastMove >= pdMS_TO_TICKS(SERVO_DETACH_IDLE_MS)) {
            servo.detach();
        }

        // Delayed save to spare flash wear while the encoder is being turned
        if (dirty && now - lastChange >= pdMS_TO_TICKS(SAVE_DELAY_MS)) {
            prefs.putUChar("vol", st.volume);
            prefs.putBool("relay", st.relay);
            dirty = false;
            Serial.printf("[control] saved volume=%u relay=%d\n", st.volume, st.relay);
        }
    }
}