// =============================================================================
//  pid_inner.h -- inner rudder-position PID (header-only, host-testable)
// =============================================================================
//
//  Line-by-line port of arautopilot/studio/simulator/pid_inner.py. Same
//  algorithm, same variables, same numerics. The Python module is the
//  reference; this header must stay byte-equivalent in behaviour. Drift is
//  caught by the cross-validation test (Python module loaded directly +
//  C++ compiled via ctypes -> same trajectory within float tolerance).
//
//  No Arduino dependencies -- this file compiles on the ESP32 toolchain AND
//  on host g++/clang/MSVC for native Unity tests.
// =============================================================================

#pragma once

#include <algorithm>
#include <cstdint>

namespace arautopilot::pid {

struct PidInnerConfig {
    // Gains
    float kp{2.5f};
    float ki{0.15f};
    float kd{0.30f};

    // Sampling
    float freq_hz{50.0f};

    // Setpoint handling
    float deadband_deg{0.5f};
    float rate_limit_dps{30.0f};

    // Output saturation
    float output_min_pct{-100.0f};
    float output_max_pct{+100.0f};

    // Anti-windup
    float integral_clamp{30.0f};
    // aw_gain: if < 0 -> use 1/kp when kp != 0, else 0. Default sentinel -1.
    float aw_gain{-1.0f};

    // Derivative low-pass
    float d_lpf_tau_s{0.05f};

    // Actuator non-linearity compensation
    float deadband_pct{7.0f};
    float min_useful_pwm_pct{12.0f};
    float asymmetry_stbd_over_port{1.0f};

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

struct PidInnerState {
    float integral{0.0f};
    float prev_error{0.0f};
    float prev_d_term{0.0f};
    float prev_setpoint_deg{0.0f};
    float last_output_pct{0.0f};
};

namespace detail {

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);
}

}  // namespace detail

class PidInner {
   public:
    PidInner() = default;
    explicit PidInner(const PidInnerConfig& config) : config_(config) {}

    void reset(float measured_deg = 0.0f, float setpoint_deg = 0.0f) {
        state_ = PidInnerState{};
        state_.prev_setpoint_deg = setpoint_deg;
        (void)measured_deg;  // kept in signature for parity with Python
    }

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

    const PidInnerConfig& config() const { return config_; }
    const PidInnerState& state() const { return state_; }
    PidInnerState& mutable_state() { return state_; }

    /// One controller tick. Returns the signed PWM command in percent.
    /// When ``allowed`` is false the output is forced to zero and the
    /// integrator bleeds toward zero (anti-windup during manual operation).
    float step(float setpoint_deg, float measured_deg, bool allowed = true) {
        const PidInnerConfig& cfg = config_;
        PidInnerState& st = state_;
        const float dt = cfg.dt();

        // Rate-limit the setpoint.
        const float target = rate_limit_setpoint(setpoint_deg);
        st.prev_setpoint_deg = target;

        // Error with deadband.
        const float raw_error = target - measured_deg;
        float error;
        if (raw_error >= -cfg.deadband_deg && raw_error <= cfg.deadband_deg) {
            error = 0.0f;
        } else {
            const float sign = (raw_error > 0.0f) ? 1.0f : -1.0f;
            error = raw_error - sign * cfg.deadband_deg;
        }

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

        // Proportional.
        const float p_term = cfg.kp * error;

        // Integral (provisional).
        st.integral += cfg.ki * error * dt;
        st.integral = detail::clamp_f(st.integral, -cfg.integral_clamp,
                                        cfg.integral_clamp);

        // Derivative with low-pass.
        const float d_raw = (dt > 0.0f)
                                ? cfg.kd * (error - st.prev_error) / dt
                                : 0.0f;
        const float alpha = detail::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 raw_output = p_term + st.integral + d_term;

        // Saturate + back-calculation anti-windup.
        const float output = detail::clamp_f(raw_output, cfg.output_min_pct,
                                              cfg.output_max_pct);
        if (raw_output != output) {
            float aw;
            if (cfg.aw_gain >= 0.0f) {
                aw = cfg.aw_gain;
            } else if (cfg.kp != 0.0f) {
                aw = 1.0f / cfg.kp;
            } else {
                aw = 0.0f;
            }
            st.integral -= aw * (raw_output - output) * dt;
            st.integral = detail::clamp_f(st.integral, -cfg.integral_clamp,
                                            cfg.integral_clamp);
        }

        const float cmd = compensate(output);

        st.prev_error = error;
        st.last_output_pct = cmd;
        return cmd;
    }

   private:
    float rate_limit_setpoint(float requested_deg) const {
        const float max_delta = config_.rate_limit_dps * config_.dt();
        const float delta = requested_deg - state_.prev_setpoint_deg;
        if (delta > max_delta) return state_.prev_setpoint_deg + max_delta;
        if (delta < -max_delta) return state_.prev_setpoint_deg - max_delta;
        return requested_deg;
    }

    float compensate(float raw_pct) const {
        const PidInnerConfig& cfg = config_;
        if (raw_pct == 0.0f) return 0.0f;
        float magnitude = (raw_pct < 0.0f) ? -raw_pct : raw_pct;
        if (magnitude <= cfg.deadband_pct) return 0.0f;
        if (magnitude < cfg.min_useful_pwm_pct) magnitude = cfg.min_useful_pwm_pct;
        const float sign = (raw_pct > 0.0f) ? 1.0f : -1.0f;
        float cmd = sign * magnitude;
        if (cmd > 0.0f && cfg.asymmetry_stbd_over_port != 0.0f) {
            cmd /= cfg.asymmetry_stbd_over_port;
        } else if (cmd < 0.0f) {
            cmd *= cfg.asymmetry_stbd_over_port;
        }
        return detail::clamp_f(cmd, cfg.output_min_pct, cfg.output_max_pct);
    }

    PidInnerConfig config_{};
    PidInnerState state_{};
};

}  // namespace arautopilot::pid
