//! pidwright — a practical discrete-time PID controller.
//!
//! This is the version you actually want in a control loop rather than the
//! three-line textbook one. It includes:
//!
//! * output saturation (min/max limits on the command),
//! * conditional integration (anti-windup): the integral term stops
//!   accumulating while the output is saturated,
//! * derivative-on-measurement instead of derivative-on-error, which avoids
//!   the giant spike ("derivative kick") when the setpoint steps.
//!
//! The controller is agnostic about units and time base; you pass it the
//! sample interval `dt` on every `update`.

/// Configurable, stateful PID controller.
#[derive(Debug, Clone)]
pub struct Pid {
    kp: f64,
    ki: f64,
    kd: f64,

    out_min: f64,
    out_max: f64,

    integral: f64,
    prev_measurement: Option<f64>,
}

impl Pid {
    /// Create a controller from the three gains. Output is unclamped by
    /// default (±∞); call [`Pid::with_limits`] to bound it.
    pub fn new(kp: f64, ki: f64, kd: f64) -> Self {
        Pid {
            kp,
            ki,
            kd,
            out_min: f64::NEG_INFINITY,
            out_max: f64::INFINITY,
            integral: 0.0,
            prev_measurement: None,
        }
    }

    /// Set actuator saturation limits. Panics if `min > max`.
    pub fn with_limits(mut self, min: f64, max: f64) -> Self {
        assert!(min <= max, "output min must not exceed max");
        self.out_min = min;
        self.out_max = max;
        self
    }

    /// Wipe the integrator and derivative history. Do this on a mode change or
    /// after the loop has been open for a while, otherwise stale state kicks
    /// the output when you close the loop again.
    pub fn reset(&mut self) {
        self.integral = 0.0;
        self.prev_measurement = None;
    }

    /// Read-only access to the accumulated integral term (before Ki scaling).
    pub fn integral(&self) -> f64 {
        self.integral
    }

    /// Compute one control output.
    ///
    /// `setpoint` is where we want the process to be, `measurement` is where it
    /// actually is, `dt` is the time since the previous call (seconds, > 0).
    ///
    /// Returns the (already saturated) command.
    pub fn update(&mut self, setpoint: f64, measurement: f64, dt: f64) -> f64 {
        debug_assert!(dt > 0.0, "dt must be positive");
        let error = setpoint - measurement;

        // Proportional.
        let p = self.kp * error;

        // Derivative on the measurement (note the sign: d(error)/dt with a
        // fixed setpoint is -d(measurement)/dt).
        let d = match self.prev_measurement {
            Some(prev) => -self.kd * (measurement - prev) / dt,
            None => 0.0, // first sample: no derivative yet
        };
        self.prev_measurement = Some(measurement);

        // Tentatively integrate, then decide whether to keep it (anti-windup).
        let candidate_integral = self.integral + error * dt;
        let i = self.ki * candidate_integral;

        let unclamped = p + i + d;
        let output = unclamped.clamp(self.out_min, self.out_max);

        // Conditional integration: only commit the new integral if we are not
        // driving further into saturation. This keeps the integrator from
        // winding up while the actuator is pinned.
        let saturated_high = unclamped > self.out_max && error > 0.0;
        let saturated_low = unclamped < self.out_min && error < 0.0;
        if !(saturated_high || saturated_low) {
            self.integral = candidate_integral;
        }

        output
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn pure_proportional_is_gain_times_error() {
        let mut c = Pid::new(2.0, 0.0, 0.0);
        // error = 10 - 4 = 6, output = 2*6 = 12.
        assert!((c.update(10.0, 4.0, 0.1) - 12.0).abs() < 1e-12);
    }

    #[test]
    fn integral_accumulates_over_time() {
        let mut c = Pid::new(0.0, 1.0, 0.0);
        // constant error of 1 for two steps of dt=0.5 → integral = 1.0.
        c.update(1.0, 0.0, 0.5);
        let out = c.update(1.0, 0.0, 0.5);
        assert!((out - 1.0).abs() < 1e-12);
        assert!((c.integral() - 1.0).abs() < 1e-12);
    }

    #[test]
    fn derivative_uses_measurement_not_setpoint() {
        // A step change in setpoint alone must NOT produce a derivative spike.
        let mut c = Pid::new(0.0, 0.0, 5.0);
        c.update(0.0, 0.0, 0.1); // prime prev_measurement = 0
        let out = c.update(100.0, 0.0, 0.1); // setpoint jumps, measurement flat
        assert!(out.abs() < 1e-12, "derivative kicked on setpoint change");
    }

    #[test]
    fn output_is_clamped() {
        let mut c = Pid::new(100.0, 0.0, 0.0).with_limits(-5.0, 5.0);
        assert!((c.update(10.0, 0.0, 0.1) - 5.0).abs() < 1e-12);
        assert!((c.update(-10.0, 0.0, 0.1) + 5.0).abs() < 1e-12);
    }

    #[test]
    fn anti_windup_holds_the_integral_while_saturated() {
        // Big Ki, tight limits: without anti-windup the integral would balloon.
        let mut c = Pid::new(0.0, 10.0, 0.0).with_limits(-1.0, 1.0);
        for _ in 0..50 {
            c.update(1.0, 0.0, 0.1); // persistent positive error, output pinned
        }
        // Integrator should be held near the value that just saturates the
        // output (~0.1 for Ki=10, limit=1), not a runaway thousands.
        assert!(c.integral() < 0.2, "integral wound up: {}", c.integral());
    }

    #[test]
    fn closed_loop_settles_to_setpoint() {
        // Simulate a first-order plant  y' = (u - y) * k  and check the PID
        // drives it to the setpoint with small steady-state error.
        let mut c = Pid::new(2.0, 1.0, 0.05).with_limits(-100.0, 100.0);
        let setpoint = 5.0;
        let mut y = 0.0;
        let dt = 0.05;
        for _ in 0..2000 {
            let u = c.update(setpoint, y, dt);
            y += (u - y) * 1.5 * dt; // simple stable plant
        }
        assert!((y - setpoint).abs() < 1e-2, "settled at {y}");
    }
}
