// =============================================================================
//  pid_inner_task.cpp -- 50 Hz inner-loop control task (Sprint 2)
// =============================================================================

#include "pid_inner_task.h"

#include <Arduino.h>

#include "../hal/rudder_actuator.h"
#include "../hal/rudder_sensor.h"
#include "../modes/standby.h"
#include "../safety/watchdog.h"
#include "../system/ar_log.h"
#include "../system/task_config.h"

namespace arautopilot::pid {

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

portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
PidInner g_pid{};
float g_setpoint_deg = 0.0f;
float g_last_output_pct = 0.0f;
float g_last_error_deg = 0.0f;

void InnerLoopTask(void* /*pv*/) {
    AR_LOGI(TAG, "pid_inner task started on core %d (50 Hz)", xPortGetCoreID());
    safety::watchdog_subscribe_current_task();

    TickType_t last_wake = xTaskGetTickCount();
    for (;;) {
        // Snapshot what the operator/outer loop wants.
        float setpoint;
        portENTER_CRITICAL(&g_mux);
        setpoint = g_setpoint_deg;
        portEXIT_CRITICAL(&g_mux);

        // Read the current rudder position.
        const auto rd = hal::rudder_sensor_latest();

        // Only run the controller if we have a valid reading and we are not
        // in STANDBY. The PID's `allowed` parameter cleanly bleeds the
        // integrator while disengaged.
        const bool allowed = rd.valid && !modes::is_standby();
        const float cmd = g_pid.step(setpoint, rd.angle_deg, allowed);

        // Send to the actuator. rudder_command() carries its own safety
        // interlocks (power, mode, limit switches) and will refuse if
        // anything is off, so we don't replicate them here.
        const int8_t pwm_pct =
            (cmd > 127.0f) ? 127 : (cmd < -127.0f) ? -127 : (int8_t)cmd;
        hal::rudder_command(pwm_pct);

        // Publish for Modbus.
        portENTER_CRITICAL(&g_mux);
        g_last_output_pct = cmd;
        g_last_error_deg = setpoint - rd.angle_deg;
        portEXIT_CRITICAL(&g_mux);

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

}  // namespace

void pid_inner_task_init() {
    PidInnerConfig cfg;
    // Inherit seed defaults from PidInnerConfig (which match the 30 m
    // yacht_motor_planeo profile). Real production gains arrive via the
    // .appack at deployment time (Sprint 4) and via Modbus hot-swap at
    // commissioning (Sprint 7).
    g_pid.update_config(cfg);
    g_pid.reset(0.0f, 0.0f);
    AR_LOGI(TAG,
            "pid_inner_init: kp=%.3f ki=%.3f kd=%.3f freq=%.1f Hz "
            "deadband=%.2f deg rate_lim=%.1f dps",
            cfg.kp, cfg.ki, cfg.kd, cfg.freq_hz, cfg.deadband_deg,
            cfg.rate_limit_dps);
}

void pid_inner_task_start() {
    xTaskCreatePinnedToCore(InnerLoopTask, "pid_inner", AR_TASK_STACK_PID_INNER,
                            nullptr, AR_TASK_PRIO_PID_INNER, nullptr,
                            AR_TASK_CORE_REALTIME);
}

void pid_inner_set_setpoint_deg(float setpoint_deg) {
    portENTER_CRITICAL(&g_mux);
    g_setpoint_deg = setpoint_deg;
    portEXIT_CRITICAL(&g_mux);
}

float pid_inner_setpoint_deg() {
    portENTER_CRITICAL(&g_mux);
    float v = g_setpoint_deg;
    portEXIT_CRITICAL(&g_mux);
    return v;
}

float pid_inner_last_output_pct() {
    portENTER_CRITICAL(&g_mux);
    float v = g_last_output_pct;
    portEXIT_CRITICAL(&g_mux);
    return v;
}

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

void pid_inner_update_gains(float kp, float ki, float kd) {
    PidInnerConfig cfg = g_pid.config();
    cfg.kp = kp;
    cfg.ki = ki;
    cfg.kd = kd;
    portENTER_CRITICAL(&g_mux);
    g_pid.update_config(cfg);
    portEXIT_CRITICAL(&g_mux);
    AR_LOGI(TAG, "pid_inner gains updated: kp=%.3f ki=%.3f kd=%.3f", kp, ki, kd);
}

void pid_inner_get_gains(float& kp, float& ki, float& kd) {
    portENTER_CRITICAL(&g_mux);
    const auto& cfg = g_pid.config();
    kp = cfg.kp;
    ki = cfg.ki;
    kd = cfg.kd;
    portEXIT_CRITICAL(&g_mux);
}

}  // namespace arautopilot::pid
