// =============================================================================
//  safety/safety_monitor.cpp -- 50 Hz safety task (Sprint 6)
// =============================================================================

#include "safety_monitor.h"

#include <Arduino.h>
#include <esp_system.h>

#include "../hal/di_do.h"
#include "../hal/pinout.h"
#include "../hal/rudder_actuator.h"
#include "../hal/rudder_sensor.h"
#include "../modes/standby.h"
#include "../pid/pid_inner_task.h"
#include "../pid/pid_outer_task.h"
#include "../protocols/nmea2000_consumer.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"
#include "watchdog.h"

namespace arautopilot::safety {

namespace {

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

// ----- Thresholds (mirrors pinout.h constants and brief section 7) -----------
// Off-course
constexpr float OFF_COURSE_DEG         = AR_DEFAULT_OFF_COURSE_DEG;
constexpr float SEVERE_OFF_COURSE_DEG  = AR_SEVERE_OFF_COURSE_DEG;
constexpr uint32_t SEVERE_HOLD_MS      = AR_SEVERE_OFF_COURSE_HOLD_MS;

// Rudder not responding: setpoint must have moved by this much before we check
constexpr float RUDDER_DEADBAND_DEG    = 1.0f;
// If setpoint moves outside deadband and rudder doesn't follow within this, alarm
constexpr uint32_t RUDDER_TIMEOUT_MS   = 3000;

// Battery / current ADC  (linear: 0..4095 raw → 0..3.3 V; then scaled by
// voltage divider / shunt amplifier on the PCB).
// Calibration: V_bat = raw * 3.3 / 4095 * 6.0  (1:6 voltage divider assumed)
constexpr float BATTERY_SCALE          = 3.3f * 6.0f / 4095.0f;
// Current: I_act = raw * 3.3 / 4095 * 50.0  (example transducer)
constexpr float CURRENT_SCALE          = 3.3f * 50.0f / 4095.0f;
// Alarm thresholds
constexpr float VOLTAGE_LOW_V          = 10.5f;
constexpr float CURRENT_HIGH_A         = 30.0f;

// ----- Shared alarm state ----------------------------------------------------
portMUX_TYPE g_alarm_mux               = portMUX_INITIALIZER_UNLOCKED;
AlarmBits    g_alarms                  = {};

// ----- Rudder-not-responding state -------------------------------------------
float    g_rnr_setpoint_snapshot       = 0.0f;  // setpoint at timer start
uint32_t g_rnr_timer_start_ms          = 0;
bool     g_rnr_timer_running           = false;

// ----- Severe off-course timer -----------------------------------------------
uint32_t g_severe_oc_start_ms          = 0;
bool     g_severe_oc_timer_running     = false;

// ----- Helpers ---------------------------------------------------------------

inline float shortest_arc_deg(float sp, float meas) {
    float e = sp - meas;
    while (e >  180.0f) e -= 360.0f;
    while (e < -180.0f) e += 360.0f;
    return e;
}

void update_alarms(const uint32_t now) {
    using namespace arautopilot;

    AlarmBits bits = {};

    const bool engaged = !modes::is_standby();

    // ----- Heading lost ------------------------------------------------------
    bits.heading_lost = engaged && protocols::nmea2000::nmea2000_is_stale();

    // ----- VMS critical (DI4) ------------------------------------------------
    bits.vms_critical = hal::di_read(PIN_DI4_EXTERNAL_ALARM);

    // ----- Limit switch reached (DI2 or DI3) ---------------------------------
    bits.limit_reached = hal::di_read(PIN_DI2_LIMIT_SWITCH_PORT)
                       || hal::di_read(PIN_DI3_LIMIT_SWITCH_STBD);

    // ----- Off-course (outer-loop error) -------------------------------------
    // Requires pilot to be actively steering a heading.
    if (engaged) {
        // pid_outer_last_error_deg() is signed shortest-arc (setpoint - meas).
        const float err_abs = fabsf(pid::pid_outer_last_error_deg());

        if (err_abs >= SEVERE_OFF_COURSE_DEG) {
            bits.off_course = true;  // severe also implies off_course
            if (!g_severe_oc_timer_running) {
                g_severe_oc_start_ms      = now;
                g_severe_oc_timer_running = true;
            }
            if ((now - g_severe_oc_start_ms) >= SEVERE_HOLD_MS) {
                bits.off_course_severe = true;
            }
        } else {
            g_severe_oc_timer_running = false;
            if (err_abs >= OFF_COURSE_DEG) {
                bits.off_course = true;
            }
        }
    } else {
        g_severe_oc_timer_running = false;
    }

    // ----- Rudder not responding ---------------------------------------------
    // We watch whether the inner-loop rudder setpoint has moved (i.e. outer
    // loop or manual command is asking for motion) and whether the rudder
    // sensor follows within RUDDER_TIMEOUT_MS.
    if (engaged) {
        const float sp  = pid::pid_inner_setpoint_deg();
        const auto  rdr = hal::rudder_sensor_latest();

        if (!g_rnr_timer_running) {
            // Start timer if setpoint is outside deadband from actual angle.
            if (rdr.valid &&
                fabsf(sp - rdr.angle_deg) > RUDDER_DEADBAND_DEG) {
                g_rnr_setpoint_snapshot = sp;
                g_rnr_timer_start_ms    = now;
                g_rnr_timer_running     = true;
            }
        } else {
            // Timer running: check if rudder has caught up.
            if (!rdr.valid ||
                fabsf(rdr.angle_deg - g_rnr_setpoint_snapshot) < RUDDER_DEADBAND_DEG) {
                // Rudder moved (or sensor gone invalid -- don't alarm on that).
                g_rnr_timer_running = false;
            } else if ((now - g_rnr_timer_start_ms) >= RUDDER_TIMEOUT_MS) {
                bits.rudder_not_resp = true;
            }
        }
    } else {
        g_rnr_timer_running = false;
    }

    // ----- Battery voltage ---------------------------------------------------
    {
        const int raw   = analogRead(PIN_AI2_BATTERY_VOLTAGE);
        const float vbat = (float)raw * BATTERY_SCALE;
        bits.voltage_low = (vbat < VOLTAGE_LOW_V);
    }

    // ----- Actuator current --------------------------------------------------
    {
        const int raw    = analogRead(PIN_AI3_ACTUATOR_CURRENT);
        const float iact = (float)raw * CURRENT_SCALE;
        bits.actuator_overcurr = (iact > CURRENT_HIGH_A);
    }

    // ----- Watchdog tripped (sticky, set on boot) ----------------------------
    // Preserved across the update so we don't clear it here.
    portENTER_CRITICAL(&g_alarm_mux);
    bits.watchdog_tripped = g_alarms.watchdog_tripped;
    g_alarms = bits;
    portEXIT_CRITICAL(&g_alarm_mux);
}

// ----- Actions triggered by alarms ------------------------------------------

void react_to_alarms(const AlarmBits& bits) {
    // EMERGENCY conditions that require immediate forced-STANDBY.
    if (bits.off_course_severe || bits.rudder_not_resp ||
        bits.heading_lost      || bits.watchdog_tripped ||
        bits.vms_critical) {

        if (!modes::is_standby()) {
            AR_LOGE(TAG,
                "emergency alarm asserted -- forcing STANDBY "
                "(oc_severe=%d rnr=%d hdg_lost=%d wdog=%d vms=%d)",
                (int)bits.off_course_severe, (int)bits.rudder_not_resp,
                (int)bits.heading_lost, (int)bits.watchdog_tripped,
                (int)bits.vms_critical);
            modes::force_standby("safety alarm");
            hal::rudder_actuator_power_off();
        }
    }

    // Actuator overcurrent / voltage low → also disengage (HIGH severity).
    if (bits.actuator_overcurr || bits.voltage_low) {
        if (!modes::is_standby()) {
            AR_LOGE(TAG,
                "power alarm -- forcing STANDBY (overcurr=%d vlow=%d)",
                (int)bits.actuator_overcurr, (int)bits.voltage_low);
            modes::force_standby("power alarm");
            hal::rudder_actuator_power_off();
        }
    }

    // Audible buzzer: any active alarm.
    hal::do_write(PIN_DO4_BUZZER, bits.any());
}

// ----- FreeRTOS task ---------------------------------------------------------

void SafetyTask(void* /*pv*/) {
    AR_LOGI(TAG, "safety_monitor task started on core %d (50 Hz)",
            xPortGetCoreID());

    watchdog_subscribe_current_task();

    TickType_t last_wake = xTaskGetTickCount();
    for (;;) {
        hal::di_poll();
        const uint32_t now = millis();

        // ---- DI1: Engage/Disengage physical button --------------------------
        if (hal::di_rising_edge(PIN_DI1_DISENGAGE_BUTTON)) {
            modes::force_standby("DI1 physical button");
            hal::rudder_actuator_power_off();
        }

        // ---- Both limit switches: cut power (wiring fault) ------------------
        if (hal::di_read(PIN_DI2_LIMIT_SWITCH_PORT) &&
            hal::di_read(PIN_DI3_LIMIT_SWITCH_STBD)) {
            AR_LOGE(TAG,
                "both limit switches asserted simultaneously -- "
                "cutting actuator power (wiring fault?)");
            hal::rudder_actuator_power_off();
        }

        // ---- Sprint 6: evaluate full alarm catalogue ------------------------
        update_alarms(now);
        react_to_alarms(safety_alarm_bits());

        watchdog_feed();
        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_SAFETY));
    }
}

}  // namespace

// -----------------------------------------------------------------------------
//  Public API
// -----------------------------------------------------------------------------

void safety_monitor_start_task() {
    xTaskCreatePinnedToCore(SafetyTask, "safety_monitor",
                            AR_TASK_STACK_SAFETY, nullptr,
                            AR_TASK_PRIO_SAFETY, nullptr,
                            AR_TASK_CORE_REALTIME);
}

AlarmBits safety_alarm_bits() {
    AlarmBits copy;
    portENTER_CRITICAL(&g_alarm_mux);
    copy = g_alarms;
    portEXIT_CRITICAL(&g_alarm_mux);
    return copy;
}

void safety_set_watchdog_tripped() {
    portENTER_CRITICAL(&g_alarm_mux);
    g_alarms.watchdog_tripped = true;
    portEXIT_CRITICAL(&g_alarm_mux);
    AR_LOGW(TAG, "watchdog tripped flag set (reset reason: TASK_WDT)");
}

}  // namespace arautopilot::safety
