// =============================================================================
//  hal/knob_encoder.cpp -- Rotary encoder for heading knob (Sprint 7)
// =============================================================================

#include "knob_encoder.h"

#include <Arduino.h>

#include "../hal/pinout.h"
#include "../modes/standby.h"
#include "../pid/pid_outer_task.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"

namespace arautopilot::hal {

namespace {

constexpr const char* TAG = "AR/KNOB";

// Encoder pins (reuse DO6/DO7 pads in commissioning variant).
constexpr uint8_t PIN_A = PIN_DO6_RESERVED;  // GPIO 16
constexpr uint8_t PIN_B = PIN_DO7_RESERVED;  // GPIO 17

// ----- Quadrature state machine (Gray-code decode) -------------------------
//
// State transitions: 4 states × 2 new pins → delta
// Table index: (prev_state << 2) | new_ab
// Value: +1 (CW), -1 (CCW), 0 (no move or invalid)
//
static const int8_t QEM[16] = {
//  AB=00  01  10  11
      0,  -1,  +1,  0,   // prev=00
     +1,   0,   0, -1,   // prev=01
     -1,   0,   0, +1,   // prev=10
      0,  +1,  -1,  0,   // prev=11
};

volatile int32_t g_pulse_count = 0;
volatile uint8_t g_prev_state  = 0;

portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;

// Armed state
volatile bool    g_armed        = false;
volatile uint32_t g_arm_time_ms = 0;

void IRAM_ATTR encoder_isr() {
    uint8_t a = (uint8_t)digitalRead(PIN_A);
    uint8_t b = (uint8_t)digitalRead(PIN_B);
    uint8_t state = (a << 1) | b;
    int8_t delta = QEM[(g_prev_state << 2) | state];
    if (delta != 0) {
        portENTER_CRITICAL_ISR(&g_mux);
        g_pulse_count += delta;
        portEXIT_CRITICAL_ISR(&g_mux);
    }
    g_prev_state = state;
}

void KnobTask(void* /*pv*/) {
    AR_LOGI(TAG, "knob_encoder task started on core %d", xPortGetCoreID());

    int32_t last_count = 0;

    for (;;) {
        const uint32_t now = millis();

        // Auto-disarm after timeout.
        portENTER_CRITICAL(&g_mux);
        bool armed = g_armed;
        if (armed && (now - g_arm_time_ms) > KNOB_ARM_TIMEOUT_MS) {
            g_armed = false;
            armed = false;
            AR_LOGI(TAG, "knob auto-disarmed (timeout)");
        }
        portEXIT_CRITICAL(&g_mux);

        portENTER_CRITICAL(&g_mux);
        int32_t count = g_pulse_count;
        portEXIT_CRITICAL(&g_mux);

        int32_t delta = count - last_count;
        last_count = count;

        if (delta != 0 && armed && !modes::is_standby()) {
            // Apply ±KNOB_STEP_DEG per pulse, normalised to 0..360.
            float sp = pid::pid_outer_heading_setpoint_deg();
            sp = fmodf(sp + (float)delta * KNOB_STEP_DEG + 360.0f, 360.0f);
            pid::pid_outer_set_heading_setpoint_deg(sp);
            AR_LOGD(TAG, "knob: delta=%d new_sp=%.1f", delta, sp);
        }

        vTaskDelay(pdMS_TO_TICKS(20));  // 50 Hz check rate
    }
}

}  // namespace

void knob_encoder_init() {
    pinMode(PIN_A, INPUT_PULLUP);
    pinMode(PIN_B, INPUT_PULLUP);
    g_prev_state = (uint8_t)((digitalRead(PIN_A) << 1) | digitalRead(PIN_B));
    attachInterrupt(digitalPinToInterrupt(PIN_A), encoder_isr, CHANGE);
    attachInterrupt(digitalPinToInterrupt(PIN_B), encoder_isr, CHANGE);
    AR_LOGI(TAG, "knob encoder on GPIO A=%d B=%d", PIN_A, PIN_B);
}

void knob_encoder_start_task() {
    xTaskCreatePinnedToCore(KnobTask, "knob_enc",
                            2048, nullptr,
                            1,    // low priority
                            nullptr,
                            AR_TASK_CORE_COMMS);
}

void knob_encoder_set_armed(bool armed) {
    portENTER_CRITICAL(&g_mux);
    g_armed       = armed;
    g_arm_time_ms = millis();
    portEXIT_CRITICAL(&g_mux);
    AR_LOGI(TAG, "knob %s", armed ? "ARMED" : "disarmed");
}

bool knob_encoder_is_armed() {
    return g_armed;
}

}  // namespace arautopilot::hal
