// =============================================================================
//  pid_outer.h -- outer heading-control PID (header-only, host-testable)
// =============================================================================
//
//  Line-by-line port of arautopilot/studio/simulator/pid_outer.py. Same
//  algorithm, same variables, same numerics. The Python module is the
//  reference; this header must stay byte-equivalent in behaviour.
//
//  No Arduino dependencies -- compiles on ESP32 + host g++/clang/MSVC.
// =============================================================================

#pragma once

#include <cstdint>

namespace arautopilot::pid {

struct OuterGainPoint {
    float speed_kn;
    float kp;
    float ki;
    float kd;
};

constexpr std::size_t MAX_OUTER_SCHEDULE_POINTS = 8;

struct PidOuterConfig {
    // Sampling
    float freq_hz{10.0f};

    // Base gains (used if schedule_size == 0)
    float base_kp{0.9f};
    float base_ki{0.02f};
    float base_kd{1.2f};

    // SOG-indexed gain schedule (fixed-capacity array; schedule_size <= MAX)
    OuterGainPoint schedule[MAX_OUTER_SCHEDULE_POINTS]{};
    std::size_t schedule_size{0};

    // Setpoint handling
    float deadband_deg{0.5f};
    float integral_clamp{35.0f};

    // Output saturation
    float max_rudder_deg{35.0f};
    float rate_limit_dps{30.0f};

    // Anti-windup
    float aw_gain{-1.0f};  // -1 sentinel -> use 1/active_kp

    // Derivative LPF
    float d_lpf_tau_s{0.10f};

    // ROT feed-forward (brief sec. 6)
    float rot_ff_gain{1.5f};

    float dt() const { return 1.0f / freq_hz; }
};

struct PidOuterState {
    float integral{0.0f};
    float prev_error{0.0f};
    float prev_d_term{0.0f};
    float prev_rudder_setpoint{0.0f};
    float last_output_deg{0.0f};
};

namespace detail_outer {

inline float clamp_f(float x, float lo, float hi) {
    if (x < lo) return lo;
    if (x > hi) return hi;
    return x;
}

inline float lpf_alpha(float tau_s, float dt) {
    if (tau_s <= 0.0f) return 1.0f;
    return dt / (tau_s + dt);
}

/// Shortest-arc signed error between two compass headings in degrees.
/// Returns value in (-180, +180]; positive means turn starboard.
inline float heading_error_deg(float setpoint, float measured) {
    float delta = setpoint - measured;
    // wrap to [0, 360)
    delta = delta - 360.0f * (float)((int)(delta / 360.0f));
    if (delta < 0.0f) delta += 360.0f;
    if (delta > 180.0f) delta -= 360.0f;
    return delta;
}

inline void interpolate_gains(
    const PidOuterConfig& cfg, float speed_kn,
    float& kp_out, float& ki_out, float& kd_out
) {
    if (cfg.schedule_size == 0) {
        kp_out = cfg.base_kp;
        ki_out = cfg.base_ki;
        kd_out = cfg.base_kd;
        return;
    }
    // Schedule entries are expected to be sorted by speed_kn ascending
    // (enforced by the constructor / config-update path).
    if (speed_kn <= cfg.schedule[0].speed_kn) {
        kp_out = cfg.schedule[0].kp;
        ki_out = cfg.schedule[0].ki;
        kd_out = cfg.schedule[0].kd;
        return;
    }
    const std::size_t last = cfg.schedule_size - 1;
    if (speed_kn >= cfg.schedule[last].speed_kn) {
        kp_out = cfg.schedule[last].kp;
        ki_out = cfg.schedule[last].ki;
        kd_out = cfg.schedule[last].kd;
        return;
    }
    for (std::size_t i = 0; i < last; ++i) {
        const auto& a = cfg.schedule[i];
        const auto& b = cfg.schedule[i + 1];
        if (a.speed_kn <= speed_kn && speed_kn <= b.speed_kn) {
            const float span = b.speed_kn - a.speed_kn;
            const float t = (span == 0.0f) ? 0.0f
                                            : (speed_kn - a.speed_kn) / span;
            kp_out = a.kp + t * (b.kp - a.kp);
            ki_out = a.ki + t * (b.ki - a.ki);
            kd_out = a.kd + t * (b.kd - a.kd);
            return;
        }
    }
    // Unreachable.
    kp_out = cfg.base_kp;
    ki_out = cfg.base_ki;
    kd_out = cfg.base_kd;
}

}  // namespace detail_outer

class PidOuter {
   public:
    PidOuter() = default;
    explicit PidOuter(const PidOuterConfig& config) : config_(config) {}

    void reset() { state_ = PidOuterState{}; }

    void update_config(const PidOuterConfig& config) { config_ = config; }

    const PidOuterConfig& config() const { return config_; }
    const PidOuterState& state() const { return state_; }

    /// One controller tick. Returns the signed rudder setpoint in degrees,
    /// already clamped to [-max_rudder_deg, +max_rudder_deg] and
    /// rate-limited.
    float step(
        float heading_setpoint_deg,
        float heading_measured_deg,
        float rate_of_turn_dps,
        float speed_kn,
        bool allowed = true
    ) {
        const PidOuterConfig& cfg = config_;
        PidOuterState& st = state_;
        const float dt = cfg.dt();

        // Error with shortest-arc + deadband.
        const float raw_err = detail_outer::heading_error_deg(
            heading_setpoint_deg, heading_measured_deg
        );
        float error;
        if (raw_err >= -cfg.deadband_deg && raw_err <= cfg.deadband_deg) {
            error = 0.0f;
        } else {
            const float sign = (raw_err > 0.0f) ? 1.0f : -1.0f;
            error = raw_err - sign * cfg.deadband_deg;
        }

        if (!allowed) {
            st.integral *= 0.95f;
            st.prev_error = error;
            st.last_output_deg = 0.0f;
            return 0.0f;
        }

        float kp = 0.0f, ki = 0.0f, kd = 0.0f;
        detail_outer::interpolate_gains(cfg, speed_kn, kp, ki, kd);

        const float p_term = kp * error;

        st.integral += ki * error * dt;
        st.integral = detail_outer::clamp_f(
            st.integral, -cfg.integral_clamp, cfg.integral_clamp
        );

        const float d_raw = (dt > 0.0f) ? kd * (error - st.prev_error) / dt
                                          : 0.0f;
        const float alpha = detail_outer::lpf_alpha(cfg.d_lpf_tau_s, dt);
        const float d_term = (1.0f - alpha) * st.prev_d_term + alpha * d_raw;
        st.prev_d_term = d_term;

        const float rot_ff_term = -cfg.rot_ff_gain * rate_of_turn_dps;

        const float raw_rudder = p_term + st.integral + d_term + rot_ff_term;

        float rudder = detail_outer::clamp_f(raw_rudder, -cfg.max_rudder_deg,
                                              cfg.max_rudder_deg);
        if (raw_rudder != rudder) {
            float aw;
            if (cfg.aw_gain >= 0.0f) {
                aw = cfg.aw_gain;
            } else if (kp != 0.0f) {
                aw = 1.0f / kp;
            } else {
                aw = 0.0f;
            }
            st.integral -= aw * (raw_rudder - rudder) * dt;
            st.integral = detail_outer::clamp_f(
                st.integral, -cfg.integral_clamp, cfg.integral_clamp
            );
        }

        // Rate-limit the produced rudder setpoint.
        const float max_delta = cfg.rate_limit_dps * dt;
        const float delta = rudder - st.prev_rudder_setpoint;
        if (delta > max_delta) {
            rudder = st.prev_rudder_setpoint + max_delta;
        } else if (delta < -max_delta) {
            rudder = st.prev_rudder_setpoint - max_delta;
        }
        st.prev_rudder_setpoint = rudder;

        st.prev_error = error;
        st.last_output_deg = rudder;
        return rudder;
    }

   private:
    PidOuterConfig config_{};
    PidOuterState state_{};
};

}  // namespace arautopilot::pid
