//! Steady-state saturated PI supervisory control, expressed as algebraic
//! residual pairs solved jointly with the plant model.
//!
//! # Motivation
//!
//! Real HVAC machines are run by supervisory controllers: a compressor tracks a
//! leaving-water-temperature (LWT) setpoint, an expansion valve tracks a suction
//! saturated temperature (SST) / superheat setpoint, a hot-gas-bypass valve
//! keeps the compressor off its surge line, etc. To *qualify a machine with its
//! controls* at steady state, each control loop must be represented as equations
//! that are solved together with the thermodynamic residuals — not applied as an
//! outer optimisation loop.
//!
//! A naive embedding (`measure(x) − setpoint = 0` plus a hard-clipped actuator)
//! suffers from **integrator wind-up**: when the actuator saturates, the
//! setpoint can no longer be met, yet the hard constraint still demands it,
//! leaving the system either infeasible or stuck against a bound with no smooth
//! release.
//!
//! # Formulation
//!
//! This module implements the steady-state saturated-PI formulation: for a loop
//! with actuator `u ∈ [u_min, u_max]`, controlled output `y`, and setpoint
//! `y_ref`, we introduce **one internal variable `x`** and **two residuals**:
//!
//! ```text
//!   r_u = u − ( Z · S(x) + Y )                         (actuator law)
//!   r_y = K · ( y_ref − y ) − ( x − S(x) )             (control law)
//! ```
//!
//! with
//!
//! ```text
//!   S(x) = ( |x + Q| − |x − Q| ) / 2   = clamp(x, −Q, Q)   (saturation)
//!   Z    = ( u_max − u_min ) / 2
//!   Y    = ( u_max + u_min ) / 2
//! ```
//!
//! * `K` carries the sign of the proportional gain (`+1` when raising `u` raises
//!   `y`, `−1` otherwise). With the offset-free control law below its **magnitude
//!   no longer changes the tracked value** (only convergence scaling), so tuning
//!   reduces to picking the correct sign.
//! * `Q > 0` sets the width of the linear (unsaturated) band of the internal
//!   variable.
//!
//! # Offset-free (integral-equivalent) formulation
//!
//! This is the canonical steady-state saturated-PI formulation of Mancuso &
//! Morari (*Supervisory Control Formulation: Centrifugal System*, UTC, 2016,
//! eq. A.1): `K·e + S(x) − x = 0` with `e = y_ref − y`, i.e.
//! `r_y = K·(y_ref − y) − (x − S(x))`. The key term is **`x − S(x)`**, which is
//! **exactly zero inside the band** (`S(x) = x`): the control-law residual then
//! collapses to `K·(y_ref − y) = 0 ⇒ y = y_ref` — **perfect steady-state
//! tracking, independent of `K` and of where the actuator sits in its range**.
//! (An earlier `x + S(x)` form was a droop/proportional controller with a
//! residual steady-state offset `≈ 2x/K`; it is replaced by this offset-free
//! form to remove the fragile gain tuning it required.)
//!
//! **Behaviour.** When the internal variable `x` sits inside `(−Q, Q)`,
//! `S(x) = x`, so `x − S(x) = 0 ⇒ y = y_ref` (perfect tracking) and `u` is free
//! between its bounds. When `x` leaves the band, `S(x)` saturates to `±Q`,
//! `x − S(x) = ±(x − Q) ≠ 0`, pinning `u` to `u_max` / `u_min` while the
//! tracking error is *released* (no wind-up): the solver naturally finds the
//! best achievable `y` at the saturated actuator. This reproduces the
//! anti-wind-up behaviour of a real supervisory PI controller at steady state.
//!
//! # Differentiability
//!
//! The exact `S(x)` is only C⁰ (kinks at `x = ±Q`), which hurts Newton
//! convergence. [`Saturation::Smooth`] replaces `|·|` with the analytic
//! [`smooth_abs`](entropyk_core::smoothing::smooth_abs) so the whole loop has a
//! continuous Jacobian, at the cost of a small, tunable rounding of the corners.

use entropyk_core::smoothing::{smooth_abs, smooth_abs_derivative};

use super::bounded::BoundedVariableId;
use super::constraint::ComponentOutput;
use super::constraint::ConstraintId;
use super::override_network::{eval_error_signal, eval_error_weights, Objective};

/// Saturation-function variant used inside a [`SaturatedController`].
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum Saturation {
    /// Exact `S(x) = clamp(x, −Q, Q)`. Continuous but not differentiable at the
    /// two corners `x = ±Q`.
    Hard,
    /// C¹ approximation using [`smooth_abs`] with rounding parameter `eps`
    /// (larger `eps` ⇒ softer corners, smaller ⇒ closer to [`Saturation::Hard`]).
    Smooth { eps: f64 },
}

impl Default for Saturation {
    fn default() -> Self {
        // A mild rounding that keeps the Jacobian continuous without materially
        // shifting the saturation corners for typical Q≈1 loops.
        Saturation::Smooth { eps: 1e-3 }
    }
}

/// Errors raised while constructing a [`SaturatedController`].
#[derive(Debug, Clone, PartialEq)]
pub enum SaturatedControlError {
    /// `u_max` was not strictly greater than `u_min`.
    InvalidBounds { u_min: f64, u_max: f64 },
    /// `Q` was not strictly positive.
    NonPositiveQ(f64),
    /// The gain `K` was zero (the loop would be inert).
    ZeroGain,
    /// A supplied parameter was not finite.
    NonFinite(&'static str),
}

impl std::fmt::Display for SaturatedControlError {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            SaturatedControlError::InvalidBounds { u_min, u_max } => write!(
                f,
                "actuator bounds invalid: u_max ({u_max}) must exceed u_min ({u_min})"
            ),
            SaturatedControlError::NonPositiveQ(q) => {
                write!(f, "saturation band Q ({q}) must be strictly positive")
            }
            SaturatedControlError::ZeroGain => write!(f, "loop gain K must be non-zero"),
            SaturatedControlError::NonFinite(p) => write!(f, "parameter `{p}` must be finite"),
        }
    }
}

impl std::error::Error for SaturatedControlError {}

/// A single steady-state saturated-PI control loop.
///
/// It links a measurable plant output (`output`, the controlled variable `y`)
/// to a bounded actuator (`actuator`, the manipulated variable `u`) so that the
/// solver drives `y` to `setpoint` while respecting `[u_min, u_max]` with
/// anti-wind-up. Each loop contributes two residuals and two unknowns (`u` and
/// the internal variable `x`) to the global system.
#[derive(Debug, Clone)]
pub struct SaturatedController {
    id: ConstraintId,
    output: ComponentOutput,
    actuator: BoundedVariableId,
    setpoint: f64,
    u_min: f64,
    u_max: f64,
    q: f64,
    k: f64,
    saturation: Saturation,
    /// Optional override/selector network. When non-empty, the control law is
    /// driven by the selected error `E` over these objectives instead of the
    /// single `(output, setpoint, k)` triple above. See [`super::override_network`].
    objectives: Vec<Objective>,
    /// Selector smoothing sharpness for the override network (`softMin`/`softMax`).
    alpha: f64,
    /// Override **activation** homotopy parameter `λ ∈ [0, 1]`. The effective
    /// selected error blends the primary objective with the full network:
    /// `E_eff = (1−λ)·e_primary + λ·E`. At `λ = 0` only the primary objective is
    /// active (a feasible baseline that always solves like the single-loop
    /// controller); at `λ = 1` the full override network is in force. Ramping `λ`
    /// from 0 → 1 with warm starts engages protections gradually and keeps every
    /// intermediate solve near-feasible — the robust cure for hard cold-start
    /// override switching. Default `1.0` (fully active).
    activation: f64,
}

impl SaturatedController {
    /// Builds a controller loop.
    ///
    /// * `output` — the controlled variable `y` (a measurable plant output).
    /// * `actuator` — the manipulated variable `u` (a bounded control variable).
    /// * `setpoint` — the target value `y_ref` for `y`.
    /// * `u_min`, `u_max` — the actuator saturation bounds.
    ///
    /// Defaults: unit gain (`K = +1`), unit band (`Q = 1`), and the default
    /// [`Saturation`]. Tune with [`Self::with_gain`], [`Self::with_band`] and
    /// [`Self::with_saturation`].
    pub fn new(
        id: ConstraintId,
        output: ComponentOutput,
        actuator: BoundedVariableId,
        setpoint: f64,
        u_min: f64,
        u_max: f64,
    ) -> Result<Self, SaturatedControlError> {
        if !setpoint.is_finite() {
            return Err(SaturatedControlError::NonFinite("setpoint"));
        }
        if !u_min.is_finite() {
            return Err(SaturatedControlError::NonFinite("u_min"));
        }
        if !u_max.is_finite() {
            return Err(SaturatedControlError::NonFinite("u_max"));
        }
        if u_max <= u_min {
            return Err(SaturatedControlError::InvalidBounds { u_min, u_max });
        }
        Ok(Self {
            id,
            output,
            actuator,
            setpoint,
            u_min,
            u_max,
            q: 1.0,
            k: 1.0,
            saturation: Saturation::default(),
            objectives: Vec::new(),
            alpha: 1e-3,
            activation: 1.0,
        })
    }

    /// Sets the loop gain `K` (its sign must match the actuator→output
    /// sensitivity; magnitude scales the internal variable).
    pub fn with_gain(mut self, k: f64) -> Result<Self, SaturatedControlError> {
        if !k.is_finite() {
            return Err(SaturatedControlError::NonFinite("k"));
        }
        if k == 0.0 {
            return Err(SaturatedControlError::ZeroGain);
        }
        self.k = k;
        Ok(self)
    }

    /// Sets the saturation band half-width `Q > 0`.
    pub fn with_band(mut self, q: f64) -> Result<Self, SaturatedControlError> {
        if !q.is_finite() {
            return Err(SaturatedControlError::NonFinite("q"));
        }
        if q <= 0.0 {
            return Err(SaturatedControlError::NonPositiveQ(q));
        }
        self.q = q;
        Ok(self)
    }

    /// Selects the saturation-function variant (hard vs. C¹ smooth).
    pub fn with_saturation(mut self, saturation: Saturation) -> Self {
        self.saturation = saturation;
        self
    }

    /// Attaches an override/selector network of [`Objective`]s. When set, the
    /// control-law residual is driven by the selected error `E` (a `softMin`/
    /// `softMax` fold over the objectives) instead of the single `(output,
    /// setpoint, gain)` triple. The primary setpoint should be the first
    /// objective; higher-priority protections come later in the chain.
    ///
    /// The per-objective `gain`s carry the normalization and sign, so the loop
    /// gain `K` is implicitly `1` in network mode.
    pub fn with_objectives(mut self, objectives: Vec<Objective>) -> Self {
        self.objectives = objectives;
        self
    }

    /// Sets the selector smoothing sharpness `alpha > 0` for the override
    /// network (smaller ⇒ sharper `min`/`max`, larger ⇒ smoother/more robust).
    pub fn with_alpha(mut self, alpha: f64) -> Self {
        if alpha.is_finite() && alpha > 0.0 {
            self.alpha = alpha;
        }
        self
    }

    /// In-place setter for the selector smoothing sharpness `alpha`. Used by the
    /// warm-started **alpha-continuation** (homotopy on the selector sharpness):
    /// solve first with a large `alpha` (very smooth, well-conditioned), then
    /// anneal toward the target while warm-starting from the previous solution.
    /// Ignores non-finite or non-positive values.
    pub fn set_alpha(&mut self, alpha: f64) {
        if alpha.is_finite() && alpha > 0.0 {
            self.alpha = alpha;
        }
    }

    /// Whether this controller uses an override/selector network.
    pub fn is_network(&self) -> bool {
        !self.objectives.is_empty()
    }

    /// The override objectives (empty in single-objective mode).
    pub fn objectives(&self) -> &[Objective] {
        &self.objectives
    }

    /// Selector smoothing sharpness for the override network.
    pub fn alpha(&self) -> f64 {
        self.alpha
    }

    /// Raw selected error `E` over the override network for the given measured
    /// objective values (`measured[i]` ↔ `objectives()[i]`), ignoring activation.
    pub fn error_signal(&self, measured: &[f64]) -> f64 {
        eval_error_signal(&self.objectives, measured, self.alpha)
    }

    /// Raw selector weights `w_i = ∂E/∂e_i` for the override network, ignoring
    /// activation.
    pub fn error_weights(&self, measured: &[f64]) -> Vec<f64> {
        eval_error_weights(&self.objectives, measured, self.alpha)
    }

    /// Override activation homotopy parameter `λ ∈ [0, 1]`.
    pub fn activation(&self) -> f64 {
        self.activation
    }

    /// In-place setter for the override activation `λ` (clamped to `[0, 1]`),
    /// used by the warm-started activation continuation.
    pub fn set_activation(&mut self, activation: f64) {
        if activation.is_finite() {
            self.activation = activation.clamp(0.0, 1.0);
        }
    }

    /// **Activation-blended** selected error
    /// `E_eff = (1−λ)·e_primary + λ·E`. Equal to the raw network error at
    /// `λ = 1`, and to the primary objective's error alone at `λ = 0`.
    pub fn network_error(&self, measured: &[f64]) -> f64 {
        let full = eval_error_signal(&self.objectives, measured, self.alpha);
        if self.activation >= 1.0 || self.objectives.is_empty() {
            return full;
        }
        let primary = self.objectives[0].error(measured[0]);
        (1.0 - self.activation) * primary + self.activation * full
    }

    /// Activation-blended selector weights `∂E_eff/∂e_i`.
    pub fn network_error_weights(&self, measured: &[f64]) -> Vec<f64> {
        let mut w = eval_error_weights(&self.objectives, measured, self.alpha);
        if self.activation >= 1.0 || w.is_empty() {
            return w;
        }
        for wi in w.iter_mut() {
            *wi *= self.activation;
        }
        w[0] += 1.0 - self.activation;
        w
    }

    /// Control-law residual in override-network mode:
    /// `r_y = E − (x − S(x))`, where `E` is the selected error.
    pub fn residual_y_network(&self, e: f64, x: f64) -> f64 {
        e - (x - self.saturation_s(x))
    }

    /// The controller's identifier.
    pub fn id(&self) -> &ConstraintId {
        &self.id
    }

    /// The controlled plant output `y`.
    pub fn output(&self) -> &ComponentOutput {
        &self.output
    }

    /// The manipulated actuator `u`.
    pub fn actuator(&self) -> &BoundedVariableId {
        &self.actuator
    }

    /// The output setpoint `y_ref`.
    pub fn setpoint(&self) -> f64 {
        self.setpoint
    }

    /// Actuator lower bound `u_min`.
    pub fn u_min(&self) -> f64 {
        self.u_min
    }

    /// Actuator upper bound `u_max`.
    pub fn u_max(&self) -> f64 {
        self.u_max
    }

    /// `Z = (u_max − u_min) / 2` — half the actuator span.
    pub fn z(&self) -> f64 {
        0.5 * (self.u_max - self.u_min)
    }

    /// `Y = (u_max + u_min) / 2` — actuator mid-point.
    pub fn y_offset(&self) -> f64 {
        0.5 * (self.u_max + self.u_min)
    }

    /// Loop gain `K` used by the control-law residual.
    pub fn gain(&self) -> f64 {
        self.k
    }

    /// Saturation function `S(x)` (exact or smoothed per [`Saturation`]).
    ///
    /// Exact form: `S(x) = (|x + Q| − |x − Q|)/2 = clamp(x, −Q, Q)`.
    pub fn saturation_s(&self, x: f64) -> f64 {
        match self.saturation {
            Saturation::Hard => x.clamp(-self.q, self.q),
            Saturation::Smooth { eps } => {
                0.5 * (smooth_abs(x + self.q, eps) - smooth_abs(x - self.q, eps))
            }
        }
    }

    /// Derivative `S'(x)` of [`Self::saturation_s`].
    pub fn saturation_ds(&self, x: f64) -> f64 {
        match self.saturation {
            Saturation::Hard => {
                if x > -self.q && x < self.q {
                    1.0
                } else {
                    0.0
                }
            }
            Saturation::Smooth { eps } => {
                0.5 * (smooth_abs_derivative(x + self.q, eps)
                    - smooth_abs_derivative(x - self.q, eps))
            }
        }
    }

    /// Actuator-law residual `r_u = u − (Z·S(x) + Y)`.
    ///
    /// Zero when the actuator equals the value dictated by the internal
    /// variable through the saturation map.
    pub fn residual_u(&self, u: f64, x: f64) -> f64 {
        u - (self.z() * self.saturation_s(x) + self.y_offset())
    }

    /// Control-law residual `r_y = K·(y_ref − y) − (x − S(x))`
    /// (offset-free / integral-equivalent form, Mancuso & Morari eq. A.1).
    ///
    /// Inside the band `x − S(x) = 0`, so the residual is `K·(y_ref − y)` and
    /// vanishes exactly at `y = y_ref` (perfect steady-state tracking). Once `x`
    /// (hence `u`) saturates, `x − S(x) ≠ 0` releases the tracking error
    /// smoothly (anti-wind-up).
    pub fn residual_y(&self, y: f64, x: f64) -> f64 {
        self.k * (self.setpoint - y) - (x - self.saturation_s(x))
    }

    /// Jacobian partials of the actuator-law residual `r_u`.
    ///
    /// Returns `(∂r_u/∂u, ∂r_u/∂x)`. The dependence on the plant state enters
    /// only through `u` and `x`, both system unknowns.
    pub fn d_residual_u(&self, x: f64) -> (f64, f64) {
        (1.0, -self.z() * self.saturation_ds(x))
    }

    /// Jacobian partials of the control-law residual `r_y`.
    ///
    /// Returns `(∂r_y/∂y, ∂r_y/∂x)`. `∂r_y/∂y = −K` is the coupling into the
    /// plant (via whatever state `y` is measured from); `∂r_y/∂x = −(1 − S'(x))`.
    ///
    /// In the unsaturated band `S'(x) = 1`, so `∂r_y/∂x = 0`: the control-law
    /// row constrains only `y` (`y = y_ref`) and is closed through the plant
    /// coupling `∂r_y/∂y = −K` (well-posed whenever the loop is controllable,
    /// `∂y/∂u ≠ 0`). Under saturation `S'(x) → 0`, restoring `∂r_y/∂x → −1`.
    pub fn d_residual_y(&self, x: f64) -> (f64, f64) {
        (-self.k, -(1.0 - self.saturation_ds(x)))
    }
}

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

    fn ctrl(saturation: Saturation) -> SaturatedController {
        SaturatedController::new(
            ConstraintId::new("lwt"),
            ComponentOutput::Temperature {
                component_id: "evaporator".to_string(),
            },
            BoundedVariableId::new("compressor_f_m"),
            280.0, // y_ref
            0.5,   // u_min
            2.0,   // u_max
        )
        .unwrap()
        .with_saturation(saturation)
    }

    #[test]
    fn test_saturation_equals_hard_clamp() {
        let c = ctrl(Saturation::Hard).with_band(1.0).unwrap();
        assert_eq!(c.saturation_s(0.4), 0.4);
        assert_eq!(c.saturation_s(2.0), 1.0);
        assert_eq!(c.saturation_s(-3.0), -1.0);
        // Exact analytic identity S(x) = (|x+Q|-|x-Q|)/2.
        for &x in &[-2.0f64, -0.7, 0.0, 0.3, 5.0] {
            let exact = 0.5 * ((x + 1.0).abs() - (x - 1.0).abs());
            assert!((c.saturation_s(x) - exact).abs() < 1e-12);
        }
    }

    #[test]
    fn test_z_and_y_offset() {
        let c = ctrl(Saturation::Hard);
        assert!((c.z() - 0.75).abs() < 1e-12); // (2.0-0.5)/2
        assert!((c.y_offset() - 1.25).abs() < 1e-12); // (2.0+0.5)/2
    }

    #[test]
    fn test_unsaturated_loop_tracks_setpoint() {
        // Offset-free: in the linear band S(x)=x ⇒ x−S(x)=0, so r_y=0 collapses
        // to K(y_ref−y)=0 ⇒ y=y_ref for ANY x in (−Q,Q), and r_u=0 ⇒ u=Z·x+Y.
        let c = ctrl(Saturation::Hard).with_band(1.0).unwrap();
        let x = 0.3;
        // Perfect tracking, independent of the internal variable x.
        let y = c.setpoint();
        let u = c.z() * x + c.y_offset();
        assert!(c.residual_y(y, x).abs() < 1e-12);
        assert!(c.residual_u(u, x).abs() < 1e-12);
        // Actuator lands strictly inside its bounds (not saturated).
        assert!(u > c.u_min() && u < c.u_max());
    }

    #[test]
    fn test_saturated_actuator_releases_tracking_error() {
        // Drive the internal variable well past the band: the actuator law must
        // pin u to u_max and the control law must NOT force y = y_ref (the error
        // is released — anti-wind-up).
        let c = ctrl(Saturation::Hard).with_band(1.0).unwrap();
        let x = 10.0; // deep in saturation, S(x)=Q=1
        let u = c.z() * c.saturation_s(x) + c.y_offset();
        assert!((u - c.u_max()).abs() < 1e-12, "u must pin to u_max: {u}");
        // At r_y = 0: K(y_ref − y) = x − S(x) = 9 ⇒ y = y_ref − 9 ≠ y_ref.
        let y = c.setpoint() - (x - c.saturation_s(x));
        assert!(c.residual_y(y, x).abs() < 1e-12);
        assert!(
            (y - c.setpoint()).abs() > 1.0,
            "tracking error must be released under saturation"
        );
    }

    #[test]
    fn test_smooth_saturation_is_c1_and_close_to_hard() {
        let hard = ctrl(Saturation::Hard).with_band(1.0).unwrap();
        let soft = ctrl(Saturation::Smooth { eps: 1e-3 })
            .with_band(1.0)
            .unwrap();
        // Away from the corners the smooth and hard forms nearly coincide.
        for &x in &[-3.0, -0.5, 0.0, 0.5, 3.0] {
            assert!(
                (soft.saturation_s(x) - hard.saturation_s(x)).abs() < 5e-3,
                "smooth S deviates too far at x={x}"
            );
        }
        // Finite-difference check of the analytic smooth derivative.
        let x = 0.9;
        let h = 1e-6;
        let fd = (soft.saturation_s(x + h) - soft.saturation_s(x - h)) / (2.0 * h);
        assert!(
            (soft.saturation_ds(x) - fd).abs() < 1e-4,
            "S'(x) mismatch: analytic {} vs FD {}",
            soft.saturation_ds(x),
            fd
        );
    }

    #[test]
    fn test_residual_jacobians_match_finite_difference() {
        let c = ctrl(Saturation::Smooth { eps: 1e-3 })
            .with_band(1.0)
            .unwrap()
            .with_gain(-1.5)
            .unwrap();
        let (x, u, y) = (0.6, 1.1, 279.0);
        let h = 1e-6;

        // ∂r_u/∂u and ∂r_u/∂x
        let (dru_du, dru_dx) = c.d_residual_u(x);
        let fd_dru_du = (c.residual_u(u + h, x) - c.residual_u(u - h, x)) / (2.0 * h);
        let fd_dru_dx = (c.residual_u(u, x + h) - c.residual_u(u, x - h)) / (2.0 * h);
        assert!((dru_du - fd_dru_du).abs() < 1e-5);
        assert!((dru_dx - fd_dru_dx).abs() < 1e-4);

        // ∂r_y/∂y and ∂r_y/∂x
        let (dry_dy, dry_dx) = c.d_residual_y(x);
        let fd_dry_dy = (c.residual_y(y + h, x) - c.residual_y(y - h, x)) / (2.0 * h);
        let fd_dry_dx = (c.residual_y(y, x + h) - c.residual_y(y, x - h)) / (2.0 * h);
        assert!((dry_dy - fd_dry_dy).abs() < 1e-5);
        assert!((dry_dx - fd_dry_dx).abs() < 1e-4);
    }

    #[test]
    fn test_construction_validates_parameters() {
        let out = ComponentOutput::Temperature {
            component_id: "e".to_string(),
        };
        // u_max <= u_min rejected.
        assert!(matches!(
            SaturatedController::new(
                ConstraintId::new("c"),
                out.clone(),
                BoundedVariableId::new("a"),
                1.0,
                2.0,
                2.0
            ),
            Err(SaturatedControlError::InvalidBounds { .. })
        ));
        // Zero gain rejected.
        let c = SaturatedController::new(
            ConstraintId::new("c"),
            out.clone(),
            BoundedVariableId::new("a"),
            1.0,
            0.0,
            1.0,
        )
        .unwrap();
        assert!(matches!(
            c.with_gain(0.0),
            Err(SaturatedControlError::ZeroGain)
        ));
        // Non-positive Q rejected.
        let c2 = SaturatedController::new(
            ConstraintId::new("c"),
            out,
            BoundedVariableId::new("a"),
            1.0,
            0.0,
            1.0,
        )
        .unwrap();
        assert!(matches!(
            c2.with_band(0.0),
            Err(SaturatedControlError::NonPositiveQ(_))
        ));
    }
}
