symplex 0.42.0

//! Control systems analysis: state-space models, transfer functions,
//! stability analysis, and controller design utilities.
//!
//! # State-Space Models
//!
//! A linear time-invariant (LTI) system in state-space form:
//!     ẋ = Ax + Bu
//!     y = Cx + Du
//!
//! # Transfer Functions
//!
//! G(s) = num(s) / den(s) — a ratio of polynomials in the Laplace variable s.

use crate::domains::matrix::Matrix;
use crate::prelude::*;

/// Ascending coefficients of `expr` viewed as a polynomial in `var`,
/// allowing symbolic (var-free) coefficients.
///
/// [`Ex::coeffs`] only handles rational coefficients; transfer functions
/// such as `K·ωₙ² / (s² + 2ζωₙ s + ωₙ²)` need the symbolic variant.
/// Returns `None` if `var` occurs in a non-polynomial position.
fn poly_coeffs_symbolic(expr: &Ex, var: &Ex) -> Option<Vec<Ex>> {
    let var_id = expr.checked_id(var);
    let mut inner = expr.inner.write();
    let ids =
        crate::transforms::solve::symbolic_poly_coeffs(&mut inner.arena, expr.raw_id(), var_id)?;
    drop(inner);
    Some(ids.into_iter().map(|id| expr.wrap(id)).collect())
}

/// Roots of `p`, a polynomial in `var`, repeated with multiplicity (SymPy's
/// `roots(…, multiple=True)`, which `TransferFunction.poles()` uses).
///
/// They are the eigenvalues of the companion matrix of `p`, so they agree
/// with [`StateSpace::poles`] of the realisation.  When `p` is not a
/// polynomial in `var`, or the eigenvalue solver does not find every root,
/// the distinct roots of the solver are returned.
fn roots_with_multiplicity(p: &Ex, var: &Ex) -> Vec<Ex> {
    if let Some(mut c) = poly_coeffs_symbolic(p, var) {
        while c.len() > 1 && c.last().is_some_and(is_exact_zero) {
            c.pop();
        }
        let n = c.len() - 1;
        if n == 0 {
            // A nonzero constant has no roots; the zero polynomial is left
            // to the solver.
            if !c.first().is_some_and(is_exact_zero) {
                return Vec::new();
            }
        } else {
            let ctx = p.context();
            let lead = &c[n];
            let companion = Matrix::from_fn(n, n, |i, j| {
                if i + 1 < n {
                    if j == i + 1 { ctx.one() } else { ctx.zero() }
                } else {
                    (-(&c[j] / lead)).eval()
                }
            });
            if let Ok(ev) = companion.and_then(|m| m.eigenvals())
                && ev.len() == n
            {
                return ev;
            }
        }
    }
    p.solve_or_empty(var)
}

// ═══════════════════════════════════════════════════════════════════════════
// State-Space Model
// ═══════════════════════════════════════════════════════════════════════════

/// A linear time-invariant (LTI) state-space model.
///
/// ẋ = Ax + Bu
/// y = Cx + Du
///
/// where:
/// - `x` is the state vector (n×1)
/// - `u` is the input vector (m×1)
/// - `y` is the output vector (p×1)
/// - `A` is the state matrix (n×n)
/// - `B` is the input matrix (n×m)
/// - `C` is the output matrix (p×n)
/// - `D` is the feedthrough matrix (p×m)
///
/// The fields are private: [`StateSpace::new`] is the only way in, so every
/// model has conformant shapes.  Read them with [`a`](Self::a),
/// [`b`](Self::b), [`c`](Self::c) and [`d`](Self::d).
#[derive(Clone, Debug)]
pub struct StateSpace {
    a: Matrix,
    b: Matrix,
    c: Matrix,
    d: Matrix,
}

impl StateSpace {
    /// Extract a [`Context`] from this model's state matrix.
    fn ctx(&self) -> crate::api::context::Context {
        self.a.get(0, 0).context()
    }

    /// Create a new state-space model from the four system matrices.
    ///
    /// # Errors
    ///
    /// Returns [`SymplexError::InvalidArgument`] if the dimensions are
    /// inconsistent:
    /// - A must be n×n (square)
    /// - B must be n×m
    /// - C must be p×n
    /// - D must be p×m
    ///
    /// # Examples
    ///
    /// ```
    /// use symplex::prelude::*;
    ///
    /// let ctx = Context::new();
    /// let ss = StateSpace::new(
    ///     matrix![ctx, [0, 1], [-2, -3]],
    ///     matrix![ctx, [0], [1]],
    ///     matrix![ctx, [1, 0]],
    ///     matrix![ctx, [0]],
    /// )
    /// .unwrap();
    /// assert_eq!((ss.num_states(), ss.num_inputs(), ss.num_outputs()), (2, 1, 1));
    /// // B has 1 row, A has 2.
    /// assert!(
    ///     StateSpace::new(
    ///         matrix![ctx, [0, 1], [-2, -3]],
    ///         matrix![ctx, [1]],
    ///         matrix![ctx, [1, 0]],
    ///         matrix![ctx, [0]],
    ///     )
    ///     .is_err()
    /// );
    /// ```
    pub fn new(a: Matrix, b: Matrix, c: Matrix, d: Matrix) -> Result<Self, SymplexError> {
        let bad = |reason: String| Err(SymplexError::invalid_argument("StateSpace::new", reason));
        let n = a.nrows();
        if a.ncols() != n {
            return bad(format!("A must be square, got {}×{}", n, a.ncols()));
        }
        if b.nrows() != n {
            return bad(format!(
                "B must have {n} rows (same as A), got {}",
                b.nrows()
            ));
        }
        if c.ncols() != n {
            return bad(format!(
                "C must have {n} columns (same as A), got {}",
                c.ncols()
            ));
        }
        let (m, p) = (b.ncols(), c.nrows());
        if d.shape() != (p, m) {
            return bad(format!(
                "D must be {p}×{m} (rows of C × columns of B), got {}×{}",
                d.nrows(),
                d.ncols()
            ));
        }
        Ok(StateSpace { a, b, c, d })
    }

    /// State matrix `A` (n×n).
    pub fn a(&self) -> &Matrix {
        &self.a
    }

    /// Input matrix `B` (n×m).
    pub fn b(&self) -> &Matrix {
        &self.b
    }

    /// Output matrix `C` (p×n).
    pub fn c(&self) -> &Matrix {
        &self.c
    }

    /// Feedthrough matrix `D` (p×m).
    pub fn d(&self) -> &Matrix {
        &self.d
    }

    /// Number of states (dimension of the state vector).
    pub fn num_states(&self) -> usize {
        self.a.nrows()
    }

    /// Number of inputs (dimension of the input vector).
    pub fn num_inputs(&self) -> usize {
        self.b.ncols()
    }

    /// Number of outputs (dimension of the output vector).
    pub fn num_outputs(&self) -> usize {
        self.c.nrows()
    }

    /// Poles of the system (eigenvalues of A), repeated with multiplicity.
    ///
    /// Returns an empty vector if the eigenvalue solver cannot find any
    /// root of the characteristic polynomial.
    pub fn poles(&self) -> Vec<Ex> {
        self.a.eigenvals().unwrap_or_default()
    }

    /// Characteristic polynomial: det(sI - A).
    ///
    /// Returns a polynomial expression in the given variable `s`, or NaN if
    /// the determinant cannot be computed — use
    /// [`try_char_poly`](Self::try_char_poly) for the error.  (`A` is
    /// square: [`StateSpace::new`] validates it.)
    pub fn char_poly(&self, s: &Ex) -> Ex {
        self.try_char_poly(s).unwrap_or_else(|_| self.ctx().nan())
    }

    /// Characteristic polynomial `det(sI − A)` in the variable `s`.
    ///
    /// # Errors
    ///
    /// Returns an error if the determinant cannot be computed.  `A` is
    /// square by construction, so a shape error cannot occur.
    pub fn try_char_poly(&self, s: &Ex) -> Result<Ex, SymplexError> {
        let n = self.num_states();
        let si = Matrix::identity(&self.ctx(), n)?.scale(s);
        si.sub(&self.a)?.det()
    }

    /// Transfer function `G(s) = C (sI − A)⁻¹ B + D` of a single-input,
    /// single-output system.
    ///
    /// The result has denominator `det(sI − A)` and numerator
    /// `C·adj(sI − A)·B + D·det(sI − A)`, both expanded polynomials in
    /// `s`; common factors are **not** cancelled.
    ///
    /// # Errors
    ///
    /// Returns [`SymplexError::InvalidArgument`] if the system is not SISO.
    ///
    /// # Examples
    ///
    /// ```
    /// use symplex::prelude::*;
    ///
    /// let ctx = Context::new();
    /// let s = ctx.symbol("s");
    /// // ẋ = [[0, 1], [-2, -3]] x + [0, 1] u,  y = [1, 0] x
    /// let ss = StateSpace::new(
    ///     matrix![ctx, [0, 1], [-2, -3]],
    ///     matrix![ctx, [0], [1]],
    ///     matrix![ctx, [1, 0]],
    ///     matrix![ctx, [0]],
    /// )
    /// .unwrap();
    /// let g = ss.to_transfer_function(&s).unwrap();
    /// // G(s) = 1 / (s² + 3s + 2)
    /// assert_eq!(g.num, ctx.int(1));
    /// assert_eq!(g.den, &s.powi(2) + &s * 3 + 2);
    /// ```
    pub fn to_transfer_function(&self, s: &Ex) -> Result<TransferFunction, SymplexError> {
        if self.num_inputs() != 1 || self.num_outputs() != 1 {
            return Err(SymplexError::InvalidArgument {
                operation: "StateSpace::to_transfer_function",
                reason: format!(
                    "requires a SISO system, got {} input(s) and {} output(s)",
                    self.num_inputs(),
                    self.num_outputs()
                ),
            });
        }
        let n = self.num_states();
        let si_minus_a = Matrix::identity(&self.ctx(), n)?.scale(s).sub(&self.a)?;
        let den = si_minus_a.det()?.expand();
        let adj = si_minus_a.adjugate()?;
        let c_adj_b = self.c.matmul(&adj)?.matmul(&self.b)?;
        let num = (c_adj_b.get(0, 0) + &(self.d.get(0, 0) * &den)).expand();
        Ok(TransferFunction::new(num, den, s.clone()))
    }

    /// Controllability matrix: \[B, AB, A²B, ..., Aⁿ⁻¹B\].
    ///
    /// For an n-state, m-input system, this is an n×(n·m) matrix.
    /// The system is controllable if and only if this matrix has rank n.
    ///
    /// # Errors
    ///
    /// Returns an error if a matrix product fails.  The shapes of `A` and
    /// `B` are validated by [`StateSpace::new`], so a shape error cannot
    /// occur.
    pub fn controllability_matrix(&self) -> Result<Matrix, SymplexError> {
        let n = self.num_states();
        let mut cols: Vec<Matrix> = vec![self.b.clone()];
        let mut ab = self.a.matmul(&self.b)?;
        for _ in 1..n {
            cols.push(ab.clone());
            ab = self.a.matmul(&ab)?;
        }
        let refs: Vec<&Matrix> = cols.iter().collect();
        Matrix::hstack(&refs)
    }

    /// Observability matrix: \[C; CA; CA²; ...; CAⁿ⁻¹\].
    ///
    /// For an n-state, p-output system, this is an (n·p)×n matrix.
    /// The system is observable if and only if this matrix has rank n.
    ///
    /// # Errors
    ///
    /// Returns an error if a matrix product fails.  The shapes of `A` and
    /// `C` are validated by [`StateSpace::new`], so a shape error cannot
    /// occur.
    pub fn observability_matrix(&self) -> Result<Matrix, SymplexError> {
        let n = self.num_states();
        let mut rows: Vec<Matrix> = vec![self.c.clone()];
        let mut ca = self.c.matmul(&self.a)?;
        for _ in 1..n {
            rows.push(ca.clone());
            ca = ca.matmul(&self.a)?;
        }
        let refs: Vec<&Matrix> = rows.iter().collect();
        Matrix::vstack(&refs)
    }

    /// Check controllability: rank(controllability_matrix) == n.
    ///
    /// Returns `true` if the system is fully state controllable, `false`
    /// if it is not or the controllability matrix cannot be computed.
    pub fn is_controllable(&self) -> bool {
        self.controllability_matrix()
            .is_ok_and(|m| m.rank() == self.num_states())
    }

    /// Check observability: rank(observability_matrix) == n.
    ///
    /// Returns `true` if the system is fully observable, `false` if it is
    /// not or the observability matrix cannot be computed.
    pub fn is_observable(&self) -> bool {
        self.observability_matrix()
            .is_ok_and(|m| m.rank() == self.num_states())
    }

    /// Check stability: all eigenvalues have negative real part.
    ///
    /// Returns `Some(true)` if all poles are in the open left half-plane.
    /// Returns `Some(false)` if any pole can be shown to have non-negative real part.
    /// Returns `None` if stability cannot be determined symbolically.
    ///
    /// Numeric poles are decided by certified evaluation.  When some pole
    /// is symbolic (or not found), the Routh–Hurwitz test
    /// [`is_routh_stable`] decides from the coefficients of `det(sI − A)`,
    /// as SymPy's `get_asymptotic_stability_conditions` does: `A =
    /// [[−k, 1], [0, −k − 1]]` with `k > 0` is stable (before 0.41 every
    /// symbolic pole gave `None`).
    pub fn is_stable(&self) -> Option<bool> {
        let poles = self.poles();
        let mut undecided = poles.len() < self.num_states();
        for pole in &poles {
            // Try real evaluation first
            if let Ok(val) = pole.eval_f64() {
                if val >= 0.0 {
                    return Some(false);
                }
            } else if let Ok(z) = pole.eval_complex64() {
                if z.re >= 0.0 {
                    return Some(false);
                }
            } else {
                undecided = true;
            }
        }
        if !undecided {
            return Some(true);
        }
        let s = self.a.fresh_symbol("s");
        let cp = self.try_char_poly(&s).ok()?.expand();
        let mut coeffs = poly_coeffs_symbolic(&cp, &s)?;
        coeffs.reverse();
        is_routh_stable(&coeffs)
    }

    /// Discretize using zero-order hold (ZOH).
    ///
    /// Aᵈ = eᴬᵈᵗ (matrix exponential)
    /// Bᵈ = A⁻¹(eᴬᵈᵗ - I)B — computed via series to avoid inverting A.
    ///
    /// Uses series approximation for the matrix exponential.
    /// `dt` is the sampling period (symbolic or numeric).
    /// `order` is the Taylor series truncation order.
    ///
    /// The discrete-time B matrix is computed as:
    ///   Bᵈ = (I·dt + A·dt²/2! + A²·dt³/3! + ...)B
    /// which avoids requiring A to be invertible.
    ///
    /// # Errors
    ///
    /// Returns an error if the matrix exponential series or a matrix
    /// product fails.  The shapes of `A` and `B` are validated by
    /// [`StateSpace::new`], so a shape error cannot occur.
    pub fn discretize_zoh(&self, dt: &Ex, order: usize) -> Result<StateSpace, SymplexError> {
        let n = self.num_states();
        let a_dt = self.a.scale(dt);
        let exp_a_dt = a_dt.exp_series(order)?;

        // Bᵈ = (I·dt + A·dt²/2! + A²·dt³/3! + ...)B, the term
        // Aᵏ⁻¹·dtᵏ/k! built from the previous one (an `i64` k! overflowed
        // and panicked for order > 20 before 0.41).
        let ctx = self.ctx();
        let mut term = Matrix::identity(&ctx, n)?.scale(dt);
        let mut b_sum = term.clone();
        for k in 2..=order {
            let step = dt * &ctx.rational(1, i64::try_from(k).unwrap_or(i64::MAX));
            term = term.matmul(&self.a)?.scale(&step);
            b_sum = b_sum.add(&term)?;
        }
        let b_d = b_sum.matmul(&self.b)?;

        // Discretisation preserves every shape, so the result is as
        // well-formed as `self`.
        Ok(StateSpace {
            a: exp_a_dt,
            b: b_d,
            c: self.c.clone(),
            d: self.d.clone(),
        })
    }

    // ── Advanced control: Riccati & pole placement ─────────────────────

    /// Set up the continuous-time algebraic Riccati equation (CARE).
    ///
    /// Returns the residual matrix: AᵀP + PA − PBR⁻¹BᵀP + Q
    /// which should equal zero when P is the solution.
    ///
    /// # Errors
    ///
    /// Returns an error if `R` is singular, or if `P` is not n×n, `Q` is not
    /// n×n or `R` is not m×m.
    pub fn riccati_residual(
        &self,
        p: &Matrix,
        q: &Matrix,
        r: &Matrix,
    ) -> Result<Matrix, SymplexError> {
        let at = self.a.transpose();
        let r_inv = r.inv()?;
        let bt = self.b.transpose();

        let term1 = at.matmul(p)?; // AᵀP
        let term2 = p.matmul(&self.a)?; // PA
        let term3 = p.matmul(&self.b)?.matmul(&r_inv)?.matmul(&bt)?.matmul(p)?; // PBR⁻¹BᵀP

        term1.add(&term2)?.sub(&term3)?.add(q)
    }

    /// Pole placement via Ackermann's formula (single-input systems only).
    ///
    /// Given desired pole locations, computes feedback gain K such that
    /// the eigenvalues of (A − BK) equal the desired poles.
    ///
    /// Only works for single-input (m=1) controllable systems.
    /// The number of desired poles must equal the number of states.
    ///
    /// # Errors
    ///
    /// Returns [`SymplexError::InvalidArgument`] if the system is not
    /// single-input or the number of desired poles is not `n`, and
    /// [`SymplexError::ComputationFailed`] if the system is not controllable.
    pub fn ackermann(&self, desired_poles: &[Ex]) -> Result<Matrix, SymplexError> {
        if self.num_inputs() != 1 {
            return Err(SymplexError::InvalidArgument {
                operation: "StateSpace::ackermann",
                reason: format!(
                    "requires a single-input system, got {} inputs",
                    self.num_inputs()
                ),
            });
        }
        let n = self.num_states();
        if desired_poles.len() != n {
            return Err(SymplexError::InvalidArgument {
                operation: "StateSpace::ackermann",
                reason: format!("need {n} desired poles, got {}", desired_poles.len()),
            });
        }

        let ctrb = self.controllability_matrix()?;
        if ctrb.rank() != n {
            return Err(SymplexError::ComputationFailed {
                operation: "StateSpace::ackermann",
                reason: "system is not controllable".into(),
            });
        }
        let ctrb_inv = ctrb.inv()?;

        // Build the desired characteristic polynomial:
        // p(s) = (s − p₁)(s − p₂)···(s − pₙ)
        // poly_coeffs[0] is the leading coefficient (1),
        // poly_coeffs[k] is the coefficient of s^(n-k).
        let mut poly_coeffs: Vec<Ex> = vec![self.ctx().int(1)];
        for pole in desired_poles {
            let neg_pole = -(pole.clone());
            let prev = poly_coeffs;
            poly_coeffs = vec![self.ctx().int(0); prev.len() + 1];
            for (i, c) in prev.into_iter().enumerate() {
                let c_neg_pole = &c * &neg_pole;
                poly_coeffs[i] = poly_coeffs[i].clone() + c;
                poly_coeffs[i + 1] = poly_coeffs[i + 1].clone() + c_neg_pole;
            }
        }

        // Evaluate p(A) = poly_coeffs[0]·Aⁿ + poly_coeffs[1]·Aⁿ⁻¹ + ··· + poly_coeffs[n]·I
        let mut p_a = Matrix::zeros(&self.ctx(), n, n)?;
        for (i, coeff) in poly_coeffs.iter().enumerate() {
            let power = (poly_coeffs.len() - 1 - i) as u32;
            let a_power = self.a.powi(power)?;
            p_a = p_a.add(&a_power.scale(coeff))?;
        }

        // K = eₙᵀ · C⁻¹ · p(A)
        // where eₙᵀ is the last standard basis row vector,
        // so eₙᵀ · C⁻¹ is the last row of C⁻¹.
        let last_row: Vec<Ex> = (0..n).map(|j| ctrb_inv.get(n - 1, j).clone()).collect();
        let last_row_mat = Matrix::new(vec![last_row])?; // 1×n

        last_row_mat.matmul(&p_a) // 1×n
    }
}

impl std::fmt::Display for StateSpace {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "StateSpace(n={}, m={}, p={})\nA = {}\nB = {}\nC = {}\nD = {}",
            self.num_states(),
            self.num_inputs(),
            self.num_outputs(),
            self.a,
            self.b,
            self.c,
            self.d
        )
    }
}

// ═══════════════════════════════════════════════════════════════════════════
// Transfer Function
// ═══════════════════════════════════════════════════════════════════════════

/// A transfer function G(s) = num(s) / den(s).
///
/// Numerator and denominator are represented as symbolic expressions
/// in the Laplace variable s. This representation supports symbolic
/// manipulation: series/parallel connections, feedback loops, pole/zero
/// analysis, and evaluation.
#[derive(Clone, Debug)]
pub struct TransferFunction {
    /// Numerator polynomial in s.
    pub num: Ex,
    /// Denominator polynomial in s.
    pub den: Ex,
    /// The Laplace variable (typically "s").
    pub var: Ex,
}

impl TransferFunction {
    /// Extract a [`Context`] from this transfer function's numerator.
    fn ctx(&self) -> crate::api::context::Context {
        self.num.context()
    }

    /// Create a new transfer function G(s) = num / den.
    ///
    /// # Arguments
    ///
    /// * `num` - Numerator polynomial expression
    /// * `den` - Denominator polynomial expression
    /// * `var` - The Laplace variable (e.g., `Context::new().symbol("s")`)
    pub fn new(num: Ex, den: Ex, var: Ex) -> Self {
        TransferFunction { num, den, var }
    }

    /// Create a transfer function from coefficient slices.
    ///
    /// Coefficients are in ascending power order:
    /// `coeffs[0] + coeffs[1]*s + coeffs[2]*s² + ...`
    ///
    /// # Example
    ///
    /// ```
    /// use symplex::prelude::*;
    /// use symplex::control::TransferFunction;
    /// let ctx = Context::new();
    /// let s = ctx.symbol("s");
    /// // G(s) = 1 / (s² + 3s + 2)
    /// let g = TransferFunction::from_coeffs(&[1], &[2, 3, 1], &s);
    /// ```
    pub fn from_coeffs(num_coeffs: &[i64], den_coeffs: &[i64], var: &Ex) -> Self {
        let ctx = var.context();
        let build_poly = |coeffs: &[i64]| -> Ex {
            let mut result = ctx.int(0);
            for (i, &c) in coeffs.iter().enumerate() {
                if c != 0 {
                    let term = if i == 0 {
                        ctx.int(c)
                    } else {
                        &ctx.int(c) * &var.powi(i as i64)
                    };
                    result = &result + &term;
                }
            }
            result
        };
        Self::new(build_poly(num_coeffs), build_poly(den_coeffs), var.clone())
    }

    /// Poles: roots of the denominator polynomial, repeated with
    /// multiplicity (as SymPy's `TransferFunction.poles()`; common factors
    /// with the numerator are **not** cancelled).
    ///
    /// `1/(s + 1)²` has the poles `[−1, −1]`.  (Before 0.41 every
    /// repeated pole was listed once, unlike [`StateSpace::poles`].)
    /// Returns an empty vector if the solver cannot find the roots.
    ///
    /// # Examples
    ///
    /// ```
    /// use symplex::prelude::*;
    ///
    /// let ctx = Context::new();
    /// let s = ctx.symbol("s");
    /// let g = TransferFunction::new(ctx.int(1), (&s + 1).powi(2), s.clone());
    /// assert_eq!(g.poles(), vec![ctx.int(-1), ctx.int(-1)]);
    /// ```
    pub fn poles(&self) -> Vec<Ex> {
        roots_with_multiplicity(&self.den, &self.var)
    }

    /// Zeros: roots of the numerator polynomial, repeated with
    /// multiplicity (as SymPy's `TransferFunction.zeros()`).
    ///
    /// Returns an empty vector if the solver cannot find the roots.
    pub fn zeros(&self) -> Vec<Ex> {
        roots_with_multiplicity(&self.num, &self.var)
    }

    /// DC gain: `lim_{s→0} G(s)` (SymPy: `limit(G, s, 0)`).
    ///
    /// The steady-state gain for a step input: `num(0)/den(0)`, complex
    /// infinity `zoo` for a pole at the origin, and for a pole-zero
    /// cancellation at the origin (`num(0) = den(0) = 0`) the ratio of the
    /// lowest-order coefficients.  (Before 0.41 the cancellation gave
    /// `num(0)/den(0) = 0/0 = nan`: `s/(s² + s)` had DC gain `nan`, not 1.)
    ///
    /// # Examples
    ///
    /// ```
    /// use symplex::prelude::*;
    ///
    /// let ctx = Context::new();
    /// let s = ctx.symbol("s");
    /// let g = TransferFunction::from_coeffs(&[0, 1], &[0, 1, 1], &s); // s/(s² + s)
    /// assert_eq!(g.dc_gain(), ctx.int(1));
    /// ```
    pub fn dc_gain(&self) -> Ex {
        let ctx = self.ctx();
        let zero = ctx.int(0);
        let num_0 = self.num.subs(&self.var, &zero).eval();
        let den_0 = self.den.subs(&self.var, &zero).eval();
        if !(is_exact_zero(&num_0) && is_exact_zero(&den_0)) {
            return &num_0 / &den_0;
        }
        // 0/0: cancel the common power of s through the lowest-order
        // nonzero coefficients.
        if let (Some(n), Some(d)) = (
            poly_coeffs_symbolic(&self.num, &self.var),
            poly_coeffs_symbolic(&self.den, &self.var),
        ) {
            let order = |c: &[Ex]| c.iter().position(|e| !is_exact_zero(e));
            return match (order(&n), order(&d)) {
                (None, Some(_)) => zero, // G = 0
                (Some(i), Some(j)) if i > j => zero,
                (Some(i), Some(j)) if i < j => ctx.complex_infinity(),
                (Some(i), Some(j)) => (&n[i] / &d[j]).eval(),
                (_, None) => ctx.nan(), // zero denominator
            };
        }
        (&self.num / &self.den).limit(&self.var, &zero)
    }

    /// `other`'s numerator and denominator in `self`'s Laplace variable.
    ///
    /// Each transfer function is a function of its own variable, so an
    /// `other` written in `p` is renamed to `s` before the two are combined
    /// (before 0.41 the `p` stayed in the result as a constant).
    fn other_in_var(&self, other: &TransferFunction) -> (Ex, Ex) {
        if other.var == self.var {
            (other.num.clone(), other.den.clone())
        } else {
            (
                other.num.subs(&other.var, &self.var),
                other.den.subs(&other.var, &self.var),
            )
        }
    }

    /// Series connection: G₁(s) · G₂(s).
    ///
    /// The series (cascade) connection multiplies the transfer functions:
    /// G_series = G₁ · G₂ = (num₁·num₂) / (den₁·den₂)
    ///
    /// The result is in `self`'s Laplace variable; if `other` uses another
    /// one it is renamed (as for [`parallel`](Self::parallel) and
    /// [`feedback_with`](Self::feedback_with)).
    pub fn series(&self, other: &TransferFunction) -> TransferFunction {
        let (num2, den2) = self.other_in_var(other);
        TransferFunction {
            num: &self.num * &num2,
            den: &self.den * &den2,
            var: self.var.clone(),
        }
    }

    /// Parallel connection: G₁(s) + G₂(s).
    ///
    /// The parallel connection adds the transfer functions:
    /// G_parallel = (num₁·den₂ + num₂·den₁) / (den₁·den₂)
    pub fn parallel(&self, other: &TransferFunction) -> TransferFunction {
        let (num2, den2) = self.other_in_var(other);
        TransferFunction {
            num: &(&self.num * &den2) + &(&num2 * &self.den),
            den: &self.den * &den2,
            var: self.var.clone(),
        }
    }

    /// Negative unity feedback: G(s) / (1 + G(s)).
    ///
    /// The closed-loop transfer function with unity negative feedback:
    /// G_cl = num / (den + num)
    pub fn feedback(&self) -> TransferFunction {
        TransferFunction {
            num: self.num.clone(),
            den: &self.den + &self.num,
            var: self.var.clone(),
        }
    }

    /// Feedback with controller H(s): G(s) / (1 + G(s)·H(s)).
    ///
    /// The closed-loop transfer function with feedback element H(s):
    /// G_cl = (num_G · den_H) / (den_G · den_H + num_G · num_H)
    pub fn feedback_with(&self, h: &TransferFunction) -> TransferFunction {
        let (h_num, h_den) = self.other_in_var(h);
        TransferFunction {
            num: &self.num * &h_den,
            den: &(&self.den * &h_den) + &(&self.num * &h_num),
            var: self.var.clone(),
        }
    }

    /// Evaluate the transfer function at a specific value of s.
    ///
    /// Substitutes `s_val` for the Laplace variable and evaluates:
    /// G(s_val) = num(s_val) / den(s_val)
    pub fn eval_at(&self, s_val: &Ex) -> Ex {
        let num = self.num.subs(&self.var, s_val).eval();
        let den = self.den.subs(&self.var, s_val).eval();
        &num / &den
    }

    /// State-space realisation in **controllable canonical form**.
    ///
    /// For a proper transfer function with monic-normalised denominator
    /// `sⁿ + aₙ₋₁sⁿ⁻¹ + … + a₀` and numerator `bₙsⁿ + … + b₀`:
    ///
    /// ```text
    /// A = [ 0    1    0  …  0   ]   B = [0]   C = [b₀−a₀bₙ, …, bₙ₋₁−aₙ₋₁bₙ]   D = [bₙ]
    ///     [ 0    0    1  …  0   ]       [0]
    ///     [ …                  ]       […]
    ///     [−a₀ −a₁ −a₂ … −aₙ₋₁]       [1]
    /// ```
    ///
    /// Coefficients may be symbolic.
    ///
    /// # Errors
    ///
    /// Returns [`SymplexError::InvalidArgument`] if numerator or denominator
    /// is not a polynomial in the Laplace variable, the denominator is
    /// constant, or the transfer function is improper (`deg num > deg den`).
    ///
    /// # Examples
    ///
    /// ```
    /// use symplex::prelude::*;
    ///
    /// let ctx = Context::new();
    /// let s = ctx.symbol("s");
    /// let g = TransferFunction::from_coeffs(&[1], &[2, 3, 1], &s); // 1/(s²+3s+2)
    /// let ss = g.to_state_space().unwrap();
    /// assert_eq!(ss.a(), &matrix![ctx, [0, 1], [-2, -3]]);
    /// assert_eq!(ss.b(), &matrix![ctx, [0], [1]]);
    /// assert_eq!(ss.c(), &matrix![ctx, [1, 0]]);
    /// // Round trip
    /// let back = ss.to_transfer_function(&s).unwrap();
    /// assert_eq!(back.den, g.den.expand());
    /// assert_eq!(back.num, g.num);
    /// ```
    pub fn to_state_space(&self) -> Result<StateSpace, SymplexError> {
        let ctx = self.ctx();
        let inv = |reason: String| SymplexError::InvalidArgument {
            operation: "TransferFunction::to_state_space",
            reason,
        };
        let den = poly_coeffs_symbolic(&self.den, &self.var)
            .ok_or_else(|| inv("denominator is not a polynomial in the Laplace variable".into()))?;
        let num = poly_coeffs_symbolic(&self.num, &self.var)
            .ok_or_else(|| inv("numerator is not a polynomial in the Laplace variable".into()))?;
        let n = den.len().saturating_sub(1);
        if n == 0 {
            return Err(inv("denominator must have degree ≥ 1".into()));
        }
        if num.len() > den.len() {
            return Err(inv(format!(
                "improper transfer function: numerator degree {} > denominator degree {}",
                num.len() - 1,
                n
            )));
        }
        // Normalise so the denominator is monic.
        let lead = &den[n];
        let a_coef: Vec<Ex> = den.iter().take(n).map(|c| (c / lead).eval()).collect();
        let mut b_coef: Vec<Ex> = num.iter().map(|c| (c / lead).eval()).collect();
        b_coef.resize(n + 1, ctx.zero());
        let bn = b_coef[n].clone();

        let a = Matrix::from_fn(n, n, |i, j| {
            if i + 1 < n {
                if j == i + 1 { ctx.one() } else { ctx.zero() }
            } else {
                -&a_coef[j]
            }
        })?;
        let b = Matrix::from_fn(n, 1, |i, _| if i + 1 == n { ctx.one() } else { ctx.zero() })?;
        let c = Matrix::from_fn(1, n, |_, j| (&b_coef[j] - &(&a_coef[j] * &bn)).eval())?;
        let d = Matrix::new(vec![vec![bn]])?;
        StateSpace::new(a, b, c, d)
    }
}

impl std::fmt::Display for TransferFunction {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "({}) / ({})", self.num, self.den)
    }
}

// ═══════════════════════════════════════════════════════════════════════════
// Routh-Hurwitz Stability Criterion
// ═══════════════════════════════════════════════════════════════════════════

/// Compute the Routh array for stability analysis.
///
/// Given coefficients of a characteristic polynomial ordered from
/// highest degree to lowest:
///
///   aₙsⁿ + aₙ₋₁sⁿ⁻¹ + ... + a₁s + a₀
///
/// passed as `coeffs = [aₙ, aₙ₋₁, ..., a₁, a₀]`,
/// this function builds the Routh array and returns it as a vector of rows.
///
/// The first two rows are formed from the even-indexed and odd-indexed
/// coefficients respectively. Subsequent rows are computed as:
///
/// ```text
/// routh[i][j] = (routh[i-1][0] * routh[i-2][j+1] - routh[i-2][0] * routh[i-1][j+1]) / routh[i-1][0]
/// ```
///
/// Leading zero coefficients are dropped (they do not change the
/// polynomial).  Zero tests are exact.  The two special cases follow the
/// textbook rules:
///
/// * a row that vanishes entirely is replaced by the coefficients of the
///   derivative of the auxiliary polynomial formed from the row above it
///   (roots symmetric about the origin, e.g. on the imaginary axis);
/// * a zero first entry in an otherwise nonzero row is replaced by the
///   symbol `ε` (primed if the coefficients already use that name), a
///   positive infinitesimal: the entries below are rational functions of
///   `ε` whose signs are read as `ε → 0⁺`.
///
/// (Before 0.30 zeros were detected with an `f64` threshold of `10⁻³⁰`
/// and replaced by the number `10⁻⁹`, which gave wrong sign patterns when
/// the coefficients were small, and leading zeros were kept.)
///
/// # Errors
///
/// Returns [`SymplexError::InvalidArgument`] if `coeffs` is empty or all
/// coefficients are zero.
///
/// # Examples
///
/// ```
/// use symplex::prelude::*;
/// use symplex::control::routh_array;
///
/// let ctx = Context::new();
/// // s² + 3s + 2
/// let table = routh_array(&[ctx.int(1), ctx.int(3), ctx.int(2)]).unwrap();
/// assert_eq!(table.len(), 3);
/// assert!(routh_array(&[]).is_err());
/// ```
pub fn routh_array(coeffs: &[Ex]) -> Result<Vec<Vec<Ex>>, SymplexError> {
    routh_table(coeffs).map(|t| t.rows)
}

/// The Routh table and whether a special case occurred in it.
struct RouthTable {
    rows: Vec<Vec<Ex>>,
    /// A zero first entry, or an entirely zero row: the polynomial has a
    /// root with non-negative real part.
    degenerate: bool,
}

fn is_exact_zero(e: &Ex) -> bool {
    crate::domains::matrix::ex_is_zero(e) == Some(true)
}

fn routh_table(coeffs: &[Ex]) -> Result<RouthTable, SymplexError> {
    if coeffs.is_empty() {
        return Err(SymplexError::invalid_argument(
            "routh_array",
            "coefficients must not be empty",
        ));
    }
    // Leading zeros do not change the polynomial.
    let first_nonzero = coeffs.iter().position(|c| !is_exact_zero(c));
    let Some(start) = first_nonzero else {
        return Err(SymplexError::invalid_argument(
            "routh_array",
            "the zero polynomial has no Routh array",
        ));
    };
    let coeffs = &coeffs[start..];
    let lead = &coeffs[0];
    let ctx = lead.context();

    let n = coeffs.len();
    if n == 1 {
        return Ok(RouthTable {
            rows: vec![vec![lead.clone()]],
            degenerate: false,
        });
    }

    // Number of columns in the Routh table
    let num_cols = n.div_ceil(2);

    // Build first row: even-indexed coefficients (a_n, a_{n-2}, a_{n-4}, ...)
    let mut row0: Vec<Ex> = Vec::with_capacity(num_cols);
    for i in (0..n).step_by(2) {
        row0.push(coeffs[i].clone());
    }
    // Pad with zeros if needed
    while row0.len() < num_cols {
        row0.push(coeffs[0].context().int(0));
    }

    // Build second row: odd-indexed coefficients (a_{n-1}, a_{n-3}, ...)
    let mut row1: Vec<Ex> = Vec::with_capacity(num_cols);
    for i in (1..n).step_by(2) {
        row1.push(coeffs[i].clone());
    }
    // Pad with zeros if needed
    while row1.len() < num_cols {
        row1.push(coeffs[0].context().int(0));
    }

    let mut table: Vec<Vec<Ex>> = vec![row0, row1];
    let mut degenerate = false;
    // The infinitesimal of the ε method, named apart from the input's symbols.
    let epsilon = {
        let used: Vec<String> = coeffs
            .iter()
            .flat_map(|c| c.free_symbols())
            .map(|s| s.to_string())
            .collect();
        let mut name = String::from("ε");
        while used.contains(&name) {
            name.push('\'');
        }
        ctx.symbol(&name)
    };

    // Build subsequent rows (row i belongs to the power s^(n-1-i)).
    let total_rows = n;
    for i in 2..=total_rows {
        // Special cases in row i − 1 (the row about to become the pivot).
        let last = i - 1;
        if table[last].iter().all(is_exact_zero) {
            // Auxiliary polynomial A(s) = Σₖ row[k]·s^(d−2k) of the row
            // above, d = n − 1 − (last − 1); replace the zero row by A′.
            degenerate = true;
            let d = (n - 1 - (last - 1)) as i64;
            let above = table[last - 1].clone();
            let deriv: Vec<Ex> = above
                .iter()
                .enumerate()
                .map(|(k, c)| (c * (d - 2 * k as i64)).eval())
                .collect();
            table[last] = deriv;
        }
        if is_exact_zero(&table[last][0]) {
            degenerate = true;
            table[last][0] = epsilon.clone();
        }
        if i == total_rows {
            break;
        }
        let prev = &table[i - 1];
        let prev2 = &table[i - 2];
        let pivot = prev[0].clone();

        let mut new_row: Vec<Ex> = Vec::with_capacity(num_cols);
        for j in 0..(num_cols - 1) {
            let prev2_j1 = if j + 1 < prev2.len() {
                prev2[j + 1].clone()
            } else {
                coeffs[0].context().int(0)
            };
            let prev_j1 = if j + 1 < prev.len() {
                prev[j + 1].clone()
            } else {
                coeffs[0].context().int(0)
            };
            // routh[i][j] = (pivot * prev2[j+1] - prev2[0] * prev[j+1]) / pivot
            let numerator = &(&pivot * &prev2_j1) - &(&prev2[0] * &prev_j1);
            let mut entry = (&numerator / &pivot).eval();
            if entry.contains(&epsilon) {
                entry = entry.ratsimp();
            }
            new_row.push(entry);
        }
        // Last column is always zero (or not needed), pad if row is too short
        if new_row.is_empty() {
            new_row.push(coeffs[0].context().int(0));
        }
        table.push(new_row);
    }

    Ok(RouthTable {
        rows: table,
        degenerate,
    })
}

/// Check Routh-Hurwitz stability: all first-column entries must be positive
/// (or all negative) for stability.
///
/// Given coefficients of a characteristic polynomial from highest degree
/// to lowest (`coeffs = [aₙ, aₙ₋₁, ..., a₁, a₀]`), builds the Routh
/// array and checks for sign changes in the first column.
///
/// Returns `Some(true)` if stable (no sign changes), `Some(false)` if
/// unstable (sign changes, or a zero first-column entry / zero row, which
/// means a root with non-negative real part), or `None` if stability cannot
/// be determined symbolically.  Leading zero coefficients are ignored; an
/// empty or all-zero coefficient list is `Some(false)`.
///
/// A coefficient that is provably zero or of the opposite sign to the
/// leading one is `Some(false)` before the table is consulted (a Hurwitz
/// polynomial has all its coefficients of one sign); this decides symbolic
/// coefficients whose table entries are undecidable: `s³ + (k − 1)·s² −
/// k·s + 1` with `k > 0` (before 0.41 `None`: the sign of `k − 1` is
/// unknown).
pub fn is_routh_stable(coeffs: &[Ex]) -> Option<bool> {
    // The only errors are an empty or zero coefficient list: not stable.
    let Ok(table) = routh_table(coeffs) else {
        return Some(false);
    };
    // Necessary condition: a Hurwitz polynomial has all its coefficients
    // nonzero and of the leading coefficient's sign (see above).
    let mut nonzero = coeffs.iter().skip_while(|c| is_exact_zero(c));
    if let Some(lead) = nonzero.next()
        && crate::domains::matrix::ex_is_zero(lead) == Some(false)
        && let Some(lead_positive) = crate::domains::matrix::ex_is_positive(lead)
    {
        for c in nonzero {
            let wrong_sign = crate::domains::matrix::ex_is_positive(c) == Some(!lead_positive)
                && crate::domains::matrix::ex_is_zero(c) == Some(false);
            if is_exact_zero(c) || wrong_sign {
                return Some(false);
            }
        }
    }
    if table.degenerate {
        return Some(false);
    }

    // All first-column entries must have the same sign; the signs are
    // decided exactly (certified evaluation), not by an f64 threshold.
    let mut first_sign: Option<bool> = None;
    for row in &table.rows {
        let entry = &row[0];
        // `None`: can't decide symbolically.
        let positive = crate::domains::matrix::ex_is_positive(entry)?;
        if !positive && crate::domains::matrix::ex_is_zero(entry) != Some(false) {
            return None;
        }
        match first_sign {
            None => first_sign = Some(positive),
            Some(s) if s != positive => return Some(false), // Sign change → unstable
            Some(_) => {}
        }
    }
    Some(true)
}

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

    #[test]
    fn state_space_basic_construction() {
        let ctx = crate::api::context::Context::new();
        let a = Matrix::new(vec![
            vec![ctx.int(0), ctx.int(1)],
            vec![ctx.int(-2), ctx.int(-3)],
        ])
        .unwrap();
        let b = Matrix::new(vec![vec![ctx.int(0)], vec![ctx.int(1)]]).unwrap();
        let c = Matrix::new(vec![vec![ctx.int(1), ctx.int(0)]]).unwrap();
        let d = Matrix::new(vec![vec![ctx.int(0)]]).unwrap();
        let ss = StateSpace::new(a, b, c, d).unwrap();
        assert_eq!(ss.num_states(), 2);
        assert_eq!(ss.num_inputs(), 1);
        assert_eq!(ss.num_outputs(), 1);
    }

    #[test]
    fn transfer_function_basic_display() {
        let ctx = crate::api::context::Context::new();
        let s = ctx.symbol("s");
        let tf = TransferFunction::new(ctx.int(1), &s * &s + &s * 3 + 2, s);
        let display = format!("{tf}");
        assert!(
            display.contains("/"),
            "Display should show fraction: {display}"
        );
    }

    #[test]
    fn routh_array_row_count() {
        let ctx = crate::api::context::Context::new();
        // s^2 + 3s + 2 → 3 coefficients → 3 rows
        let coeffs = vec![ctx.int(1), ctx.int(3), ctx.int(2)];
        let table = routh_array(&coeffs).unwrap();
        assert_eq!(table.len(), 3);
    }
}