// =============================================================================
//  pid_outer_task.cpp -- 10 Hz outer-loop task (Sprint 3 / Sprint 5)
// =============================================================================

#include "pid_outer_task.h"

#include <Arduino.h>
#include <cmath>

#include "../modes/standby.h"
#include "../protocols/nmea2000_consumer.h"
#include "../safety/watchdog.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"
#include "pid_inner_task.h"

namespace arautopilot::pid {

namespace {
constexpr const char* TAG = "AR/PID";

portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
PidOuter g_outer{};

float g_heading_setpoint_deg  = 0.0f;
float g_cog_setpoint_deg      = 0.0f;
float g_xte_gain              = 0.5f;   // deg / metre
float g_xte_max_correction    = 20.0f;  // deg

float g_last_rudder_setpoint_deg = 0.0f;
float g_last_error_deg           = 0.0f;
float g_last_xte_correction_deg  = 0.0f;
float g_speed_kn                 = 15.0f;

// ---------------------------------------------------------------------------
//  XTE correction helper (TRACK_KEEPING)
//    xte_m > 0 → vessel to starboard → steer port → negative correction
// ---------------------------------------------------------------------------
static float _xte_correction(float xte_m, float gain, float max_deg) {
    if (std::isnan(xte_m)) return 0.0f;
    float c = -gain * xte_m;
    if (c >  max_deg) c =  max_deg;
    if (c < -max_deg) c = -max_deg;
    return c;
}

void OuterLoopTask(void* /*pv*/) {
    AR_LOGI(TAG, "pid_outer task started on core %d (10 Hz)", xPortGetCoreID());
    safety::watchdog_subscribe_current_task();

    TickType_t last_wake = xTaskGetTickCount();
    for (;;) {
        float heading_sp, cog_sp, xte_gain, xte_max, speed_kn;
        portENTER_CRITICAL(&g_mux);
        heading_sp = g_heading_setpoint_deg;
        cog_sp     = g_cog_setpoint_deg;
        xte_gain   = g_xte_gain;
        xte_max    = g_xte_max_correction;
        speed_kn   = g_speed_kn;
        portEXIT_CRITICAL(&g_mux);

        const auto n2k       = protocols::nmea2000::nmea2000_latest();
        const auto cog_sog   = protocols::nmea2000::nmea2000_cog_sog();
        const auto nav_data  = protocols::nmea2000::nmea2000_nav_data();
        const auto mode      = modes::current_mode();

        const bool in_hh  = (mode == modes::Mode::HEADING_HOLD);
        const bool in_tc  = (mode == modes::Mode::TRUE_COURSE);
        const bool in_tk  = (mode == modes::Mode::TRACK_KEEPING);

        float rudder_sp     = 0.0f;
        float error         = 0.0f;
        float xte_corr      = 0.0f;

        if (in_hh) {
            // ---------------------------------------------------------------
            //  HEADING_HOLD: feedback = magnetic/true heading (PGN 127250)
            // ---------------------------------------------------------------
            const bool allowed = n2k.heading_valid;
            rudder_sp = g_outer.step(
                heading_sp,
                n2k.heading_deg,
                n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
                speed_kn,
                allowed
            );
            error = allowed ? (heading_sp - n2k.heading_deg) : 0.0f;
            pid_inner_set_setpoint_deg(rudder_sp);

        } else if (in_tc) {
            // ---------------------------------------------------------------
            //  TRUE_COURSE: feedback = COG (PGN 129026)
            // ---------------------------------------------------------------
            const bool allowed = cog_sog.valid;
            const float sog    = cog_sog.valid ? cog_sog.sog_kn : speed_kn;
            rudder_sp = g_outer.step(
                cog_sp,
                cog_sog.cog_deg,
                n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
                sog,
                allowed
            );
            error = allowed ? (cog_sp - cog_sog.cog_deg) : 0.0f;
            pid_inner_set_setpoint_deg(rudder_sp);

        } else if (in_tk) {
            // ---------------------------------------------------------------
            //  TRACK_KEEPING: COG hold + XTE proportional correction
            // ---------------------------------------------------------------
            const bool cog_ok = cog_sog.valid;
            const bool xte_ok = nav_data.valid;
            xte_corr = xte_ok ? _xte_correction(nav_data.xte_m, xte_gain, xte_max) : 0.0f;
            const float eff_sp = fmodf(cog_sp + xte_corr + 360.0f, 360.0f);
            const float sog    = cog_ok ? cog_sog.sog_kn : speed_kn;
            rudder_sp = g_outer.step(
                eff_sp,
                cog_sog.cog_deg,
                n2k.rot_valid ? n2k.rate_of_turn_dps : 0.0f,
                sog,
                cog_ok
            );
            error = cog_ok ? (cog_sp - cog_sog.cog_deg) : 0.0f;
            pid_inner_set_setpoint_deg(rudder_sp);

        } else {
            // ---------------------------------------------------------------
            //  STANDBY / DODGE: idle tick — bleed integrator, don't push
            // ---------------------------------------------------------------
            g_outer.step(0.0f, 0.0f, 0.0f, speed_kn, false);
        }

        portENTER_CRITICAL(&g_mux);
        g_last_rudder_setpoint_deg = rudder_sp;
        g_last_error_deg           = error;
        g_last_xte_correction_deg  = xte_corr;
        if (in_tc || in_tk) {
            // Update SOG from live data
            if (cog_sog.valid) g_speed_kn = cog_sog.sog_kn;
        }
        portEXIT_CRITICAL(&g_mux);

        safety::watchdog_feed();
        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(AR_PERIOD_MS_PID_OUTER));
    }
}

}  // namespace

// ---------------------------------------------------------------------------
//  Init / start
// ---------------------------------------------------------------------------

void pid_outer_task_init() {
    PidOuterConfig cfg;
    cfg.schedule_size   = 3;
    cfg.schedule[0]     = {5.0f,  1.20f, 0.03f, 0.80f};
    cfg.schedule[1]     = {15.0f, 0.90f, 0.02f, 1.20f};
    cfg.schedule[2]     = {28.0f, 0.55f, 0.01f, 1.80f};
    cfg.rot_ff_gain     = 1.5f;
    g_outer.update_config(cfg);
    g_outer.reset();
    AR_LOGI(TAG,
            "pid_outer_init: schedule={5kn,15kn,28kn} rot_ff=%.2f freq=%.1fHz",
            cfg.rot_ff_gain, cfg.freq_hz);
}

void pid_outer_task_start() {
    xTaskCreatePinnedToCore(OuterLoopTask, "pid_outer", AR_TASK_STACK_PID_OUTER,
                            nullptr, AR_TASK_PRIO_PID_OUTER, nullptr,
                            AR_TASK_CORE_REALTIME);
}

// ---------------------------------------------------------------------------
//  Setpoints
// ---------------------------------------------------------------------------

void pid_outer_set_heading_setpoint_deg(float sp) {
    while (sp <   0.0f) sp += 360.0f;
    while (sp >= 360.0f) sp -= 360.0f;
    portENTER_CRITICAL(&g_mux);
    g_heading_setpoint_deg = sp;
    portEXIT_CRITICAL(&g_mux);
}

float pid_outer_heading_setpoint_deg() {
    portENTER_CRITICAL(&g_mux);
    float v = g_heading_setpoint_deg;
    portEXIT_CRITICAL(&g_mux);
    return v;
}

void pid_outer_set_cog_setpoint_deg(float sp) {
    while (sp <   0.0f) sp += 360.0f;
    while (sp >= 360.0f) sp -= 360.0f;
    portENTER_CRITICAL(&g_mux);
    g_cog_setpoint_deg = sp;
    portEXIT_CRITICAL(&g_mux);
}

float pid_outer_cog_setpoint_deg() {
    portENTER_CRITICAL(&g_mux);
    float v = g_cog_setpoint_deg;
    portEXIT_CRITICAL(&g_mux);
    return v;
}

// ---------------------------------------------------------------------------
//  XTE parameters
// ---------------------------------------------------------------------------

void pid_outer_set_xte_gain(float gain) {
    if (gain < 0.0f) gain = 0.0f;
    portENTER_CRITICAL(&g_mux);
    g_xte_gain = gain;
    portEXIT_CRITICAL(&g_mux);
}

float pid_outer_xte_gain() {
    portENTER_CRITICAL(&g_mux);
    float v = g_xte_gain;
    portEXIT_CRITICAL(&g_mux);
    return v;
}

void pid_outer_set_xte_max_correction(float deg) {
    if (deg < 0.0f) deg = 0.0f;
    if (deg > 90.0f) deg = 90.0f;
    portENTER_CRITICAL(&g_mux);
    g_xte_max_correction = deg;
    portEXIT_CRITICAL(&g_mux);
}

float pid_outer_xte_max_correction() {
    portENTER_CRITICAL(&g_mux);
    float v = g_xte_max_correction;
    portEXIT_CRITICAL(&g_mux);
    return v;
}

// ---------------------------------------------------------------------------
//  Telemetry
// ---------------------------------------------------------------------------

float pid_outer_last_rudder_setpoint_deg() {
    portENTER_CRITICAL(&g_mux);
    float v = g_last_rudder_setpoint_deg;
    portEXIT_CRITICAL(&g_mux);
    return v;
}

float pid_outer_last_error_deg() {
    portENTER_CRITICAL(&g_mux);
    float v = g_last_error_deg;
    portEXIT_CRITICAL(&g_mux);
    return v;
}

float pid_outer_last_xte_correction_deg() {
    portENTER_CRITICAL(&g_mux);
    float v = g_last_xte_correction_deg;
    portEXIT_CRITICAL(&g_mux);
    return v;
}

// ---------------------------------------------------------------------------
//  SOG / gain scheduling
// ---------------------------------------------------------------------------

void pid_outer_set_speed_kn(float speed_kn) {
    if (speed_kn < 0.0f)  speed_kn = 0.0f;
    if (speed_kn > 80.0f) speed_kn = 80.0f;
    portENTER_CRITICAL(&g_mux);
    g_speed_kn = speed_kn;
    portEXIT_CRITICAL(&g_mux);
}

float pid_outer_speed_kn() {
    portENTER_CRITICAL(&g_mux);
    float v = g_speed_kn;
    portEXIT_CRITICAL(&g_mux);
    return v;
}

void pid_outer_update_gains(float kp, float ki, float kd) {
    PidOuterConfig cfg = g_outer.config();
    cfg.base_kp      = kp;
    cfg.base_ki      = ki;
    cfg.base_kd      = kd;
    cfg.schedule_size = 0;  // explicit gains disable the schedule
    portENTER_CRITICAL(&g_mux);
    g_outer.update_config(cfg);
    portEXIT_CRITICAL(&g_mux);
    AR_LOGI(TAG, "pid_outer gains updated: kp=%.3f ki=%.3f kd=%.3f", kp, ki, kd);
}

void pid_outer_get_gains(float& kp, float& ki, float& kd) {
    portENTER_CRITICAL(&g_mux);
    const auto& cfg = g_outer.config();
    kp = cfg.base_kp;
    ki = cfg.base_ki;
    kd = cfg.base_kd;
    portEXIT_CRITICAL(&g_mux);
}

}  // namespace arautopilot::pid
