//! Steady-state **override / selector control** network.
//!
//! Real supervisory controllers drive a *single* actuator from *several*
//! competing objectives: a primary setpoint (e.g. capacity, superheat) plus a
//! set of operating-envelope protections (SST low, SDT high, DGT high,
//! min/max frequency, …). Only one objective is "in authority" at a time; the
//! others act as overrides that take over when a limit is about to be crossed.
//!
//! This mirrors the `BOLT.Control.SteadyState.SetpointControl` library used in
//! the reference Modelica chillers (61WH / 61AQ / NG-Screw), where the pattern
//! is `ErrorCalculation` blocks feeding a tree of `Min` / `Max` selectors into a
//! single `SetpointController`. See also the ALES/UTC report *Supervisory
//! Control Formulation: Centrifugal System* (Mancuso & Morari, 2016).
//!
//! # Formulation
//!
//! Each objective `i` computes a **normalized** error
//!
//! ```text
//!   e_i = gain_i · (setpoint_i − measurement_i)
//! ```
//!
//! The `gain_i` normalizes every objective to a comparable scale (e.g.
//! `1/(freq_max − freq_min)`, `−1/(T_dgt_max − T_dgt_min)`), so that the
//! selector compares apples to apples — this is the "same-gain" principle from
//! the reference: after normalization a *single* unit controller integrates the
//! selected error.
//!
//! Errors are folded left-to-right into a single selected error `E`:
//!
//! ```text
//!   acc_0 = e_0
//!   acc_i = combine_i(acc_{i-1}, e_i)     with combine_i ∈ {Min, Max}
//!   E     = acc_{n-1}
//! ```
//!
//! The fold order encodes **priority**: place higher-priority protections later
//! in the chain (this reproduces the linear `min/max/min/…` selector chains of
//! `CompressorControl` / `EXVControl`).
//!
//! # Smoothing (convergence)
//!
//! `Min` / `Max` are replaced by the C^∞ `softMin` / `softMax`
//! (`entropyk_core::smoothing`) with sharpness `alpha`. Using a smooth selector
//! with an **exact analytic Jacobian** (rather than a non-smooth `min`/`max`
//! with a semismooth Newton step) is the "Jacobian-smoothing" approach that the
//! nonlinear-complementarity literature reports as markedly more robust and
//! faster to converge (fewer Newton iterations, no chattering at the selector
//! kinks). `alpha` can be annealed toward zero by an outer continuation loop for
//! a sharp final solution.

use entropyk_core::smoothing::{smooth_max, smooth_min};

use super::constraint::ComponentOutput;

/// How an objective combines with the running selected error.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Combine {
    /// Take the (smooth) minimum of the accumulator and this objective's error.
    Min,
    /// Take the (smooth) maximum of the accumulator and this objective's error.
    Max,
}

/// A single control objective feeding an override network.
///
/// The normalized error is `gain · (setpoint − measurement)`, where
/// `measurement` is the current value of [`Objective::output`].
#[derive(Debug, Clone)]
pub struct Objective {
    /// The measured plant output for this objective.
    pub output: ComponentOutput,
    /// Target value for the measured output (SI units).
    pub setpoint: f64,
    /// Normalization/sign gain for this objective's error.
    pub gain: f64,
    /// Selector applied between the running accumulator and this objective.
    /// Ignored for the first objective (which seeds the accumulator).
    pub combine: Combine,
}

impl Objective {
    /// Builds an objective with the given output, setpoint, gain and combinator.
    pub fn new(output: ComponentOutput, setpoint: f64, gain: f64, combine: Combine) -> Self {
        Self {
            output,
            setpoint,
            gain,
            combine,
        }
    }

    /// Normalized error `e = gain · (setpoint − measurement)`.
    #[inline]
    pub fn error(&self, measurement: f64) -> f64 {
        self.gain * (self.setpoint - measurement)
    }
}

/// `softMin` value and partials `(value, ∂/∂a, ∂/∂b)`.
#[inline]
fn soft_min_partials(a: f64, b: f64, k: f64) -> (f64, f64, f64) {
    let d = ((a - b) * (a - b) + k * k).sqrt();
    let s = if d > 0.0 { (a - b) / d } else { 0.0 };
    (smooth_min(a, b, k), 0.5 * (1.0 - s), 0.5 * (1.0 + s))
}

/// `softMax` value and partials `(value, ∂/∂a, ∂/∂b)`.
#[inline]
fn soft_max_partials(a: f64, b: f64, k: f64) -> (f64, f64, f64) {
    let d = ((a - b) * (a - b) + k * k).sqrt();
    let s = if d > 0.0 { (a - b) / d } else { 0.0 };
    (smooth_max(a, b, k), 0.5 * (1.0 + s), 0.5 * (1.0 - s))
}

/// Evaluates the selected error `E` for the given objectives and their measured
/// values (`measured[i]` corresponds to `objectives[i]`).
///
/// Panics in debug builds if the slice lengths differ. Returns `0.0` for an
/// empty objective list.
pub fn eval_error_signal(objectives: &[Objective], measured: &[f64], alpha: f64) -> f64 {
    debug_assert_eq!(objectives.len(), measured.len());
    if objectives.is_empty() {
        return 0.0;
    }
    let mut acc = objectives[0].error(measured[0]);
    for i in 1..objectives.len() {
        let e = objectives[i].error(measured[i]);
        acc = match objectives[i].combine {
            Combine::Min => smooth_min(acc, e, alpha),
            Combine::Max => smooth_max(acc, e, alpha),
        };
    }
    acc
}

/// Computes the selector weights `w_i = ∂E/∂e_i` for each objective via a
/// forward/backward sweep over the fold. These let the caller assemble the
/// exact plant-coupling Jacobian: `∂E/∂measurement_i = w_i · (−gain_i)`.
pub fn eval_error_weights(objectives: &[Objective], measured: &[f64], alpha: f64) -> Vec<f64> {
    debug_assert_eq!(objectives.len(), measured.len());
    let n = objectives.len();
    let mut weights = vec![0.0; n];
    if n == 0 {
        return weights;
    }
    if n == 1 {
        weights[0] = 1.0;
        return weights;
    }

    // Forward: accumulate value and store per-step partials.
    let mut pa = vec![0.0; n]; // ∂acc_i/∂acc_{i-1}
    let mut pb = vec![0.0; n]; // ∂acc_i/∂e_i
    let mut acc = objectives[0].error(measured[0]);
    for i in 1..n {
        let e = objectives[i].error(measured[i]);
        let (val, da, db) = match objectives[i].combine {
            Combine::Min => soft_min_partials(acc, e, alpha),
            Combine::Max => soft_max_partials(acc, e, alpha),
        };
        pa[i] = da;
        pb[i] = db;
        acc = val;
    }

    // Backward: propagate ∂E/∂acc back to each e_i.
    let mut g = 1.0;
    for i in (1..n).rev() {
        weights[i] = g * pb[i];
        g *= pa[i];
    }
    weights[0] = g;
    weights
}

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

    fn obj(setpoint: f64, gain: f64, combine: Combine) -> Objective {
        Objective::new(
            ComponentOutput::Temperature {
                component_id: "c".to_string(),
            },
            setpoint,
            gain,
            combine,
        )
    }

    #[test]
    fn single_objective_is_plain_error() {
        let objs = vec![obj(5.0, -0.5, Combine::Min)];
        let e = eval_error_signal(&objs, &[7.0], 1e-3);
        assert!((e - (-0.5 * (5.0 - 7.0))).abs() < 1e-12);
        let w = eval_error_weights(&objs, &[7.0], 1e-3);
        assert_eq!(w, vec![1.0]);
    }

    #[test]
    fn min_selects_smaller_error_and_routes_weight() {
        // Two objectives; e_0 large, e_1 small → Min picks ~e_1, so weight ~1 on
        // objective 1 and ~0 on objective 0.
        let objs = vec![obj(10.0, 1.0, Combine::Min), obj(0.0, 1.0, Combine::Min)];
        // measured: obj0 at 5 → e0 = 5; obj1 at 5 → e1 = -5. min → ~-5.
        let e = eval_error_signal(&objs, &[5.0, 5.0], 1e-4);
        assert!((e - (-5.0)).abs() < 1e-2, "E={e}");
        let w = eval_error_weights(&objs, &[5.0, 5.0], 1e-4);
        assert!(w[1] > 0.98 && w[0] < 0.02, "weights={w:?}");
        // Weights of a smooth selector sum to 1 (convex combination).
        assert!((w[0] + w[1] - 1.0).abs() < 1e-9);
    }

    #[test]
    fn weights_match_finite_difference() {
        let objs = vec![
            obj(8.0, 0.7, Combine::Min),
            obj(2.0, -1.3, Combine::Max),
            obj(-1.0, 0.9, Combine::Min),
        ];
        let measured = [6.0, 3.0, 0.5];
        let alpha = 0.05;
        let w = eval_error_weights(&objs, &measured, alpha);
        let h = 1e-6;
        for i in 0..objs.len() {
            // dE/de_i via FD on the measurement, then convert: dE/dm_i = -gain_i·w_i.
            let mut mp = measured;
            let mut mm = measured;
            mp[i] += h;
            mm[i] -= h;
            let de_dm = (eval_error_signal(&objs, &mp, alpha)
                - eval_error_signal(&objs, &mm, alpha))
                / (2.0 * h);
            let expected = -objs[i].gain * w[i];
            assert!(
                (de_dm - expected).abs() < 1e-4,
                "objective {i}: FD {de_dm} vs analytic {expected}"
            );
        }
    }
}