automatica 1.0.0

//! # Linear system
//!
//! This module contains the state-space representation of a linear system.
//! * poles calculation
//! * controllability matrix
//! * observability matrix
//! * conversion from a generic transfer function
//! * calculation the equilibrium point of the system.
//! * system stability
//!
//! [continuous](continuous/index.html) module contains the specialized
//! structs and methods for continuous systems.
//!
//! [discrete](discrete/index.html) module contains the specialized structs and
//! methods for discrete systems.
//!
//! The [solver](solver/index.html) module contains the methods for the time
//! evaluation of continuous systems.

use std::{
    fmt,
    fmt::{Debug, Display, Formatter},
    marker::PhantomData,
    ops::{Add, Div, Mul, Neg, Sub},
};

use ndarray::{s, Array2};

use crate::{
    enums::Time,
    error::{Error, ErrorKind},
    matrix::{ar_to_dm, identity, MatrixMul, ScalarMul, Trace},
    polynomial_matrix::PolyMatrix,
    transfer_function::TfGen,
    wrappers::{PWrapper, PP},
    Abs, Complex, Inv, NumCast, One, Pow, Sign, Sqrt, Zero,
};

pub mod continuous;
pub mod discrete;
pub mod solver;

/// State-space representation of a linear system
///
/// ```text
/// xdot(t) = A * x(t) + B * u(t)
/// y(t)    = C * x(t) + D * u(t)
/// ```
#[derive(Clone, Debug, PartialEq)]
pub struct SsGen<T, U: Time> {
    /// A matrix
    pub(super) a: Array2<T>,
    /// B matrix
    pub(super) b: Array2<T>,
    /// C matrix
    pub(super) c: Array2<T>,
    /// D matrix
    pub(super) d: Array2<T>,
    /// Dimensions
    dim: Dim,
    /// Tag for continuous or discrete time
    time: PhantomData<U>,
}

/// Dim of the linear system.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Dim {
    /// Number of states
    states: usize,
    /// Number of inputs
    inputs: usize,
    /// Number of outputs
    outputs: usize,
}

/// Implementation of the methods for the Dim struct.
impl Dim {
    /// Get the number of states.
    #[must_use]
    pub fn states(&self) -> usize {
        self.states
    }

    /// Get the number of inputs.
    #[must_use]
    pub fn inputs(&self) -> usize {
        self.inputs
    }

    /// Get the number of outputs.
    #[must_use]
    pub fn outputs(&self) -> usize {
        self.outputs
    }
}

/// Implementation of the methods for the state-space
impl<T, U: Time> SsGen<T, U>
where
    T: Clone,
{
    /// Create a new state-space representation
    ///
    /// # Arguments
    ///
    /// * `states` - number of states (n)
    /// * `inputs` - number of inputs (m)
    /// * `outputs` - number of outputs (p)
    /// * `a` - A matrix (nxn), row major matrix supplied as slice
    /// * `b` - B matrix (nxm), row major matrix supplied as slice
    /// * `c` - C matrix (pxn), row major matrix supplied as slice
    /// * `d` - D matrix (pxm), row major matrix supplied as slice
    ///
    /// # Panics
    ///
    /// Panics if matrix dimensions do not match
    ///
    /// # Example
    ///
    /// ```
    /// use automatica::Ss;
    /// let sys = Ss::new_from_slice(2, 1, 1, &[-2., 0., 3., -7.], &[1., 3.], &[-1., 0.5], &[0.1]);
    /// ```
    pub fn new_from_slice(
        states: usize,
        inputs: usize,
        outputs: usize,
        a: &[T],
        b: &[T],
        c: &[T],
        d: &[T],
    ) -> Self {
        let states_matrix = Array2::from_shape_vec((states, states), a.into()).unwrap();
        debug_assert!(states_matrix.is_square());
        Self {
            a: states_matrix,
            b: Array2::from_shape_vec((states, inputs), b.into()).unwrap(),
            c: Array2::from_shape_vec((outputs, states), c.into()).unwrap(),
            d: Array2::from_shape_vec((outputs, inputs), d.into()).unwrap(),
            dim: Dim {
                states,
                inputs,
                outputs,
            },
            time: PhantomData,
        }
    }

    /// Get the dimensions of the system (states, inputs, outputs).
    ///
    /// # Example
    ///
    /// ```
    /// use automatica::Ssd;
    /// let sys = Ssd::new_from_slice(2, 1, 1, &[-2., 0., 3., -7.], &[1., 3.], &[-1., 0.5], &[0.1]);
    /// let dimensions = sys.dim();
    /// ```
    #[must_use]
    pub fn dim(&self) -> Dim {
        self.dim
    }
}

impl<U> SsGen<f64, U>
where
    U: Time,
{
    /// Calculate the poles of the system
    ///
    /// # Example
    ///
    /// ```
    /// use automatica::Ss;
    /// let sys = Ss::<f64>::new_from_slice(2, 1, 1, &[-2., 0., 3., -7.], &[1., 3.], &[-1., 0.5], &[0.1]);
    /// let poles = sys.poles();
    /// assert_eq!(-2., poles[0].re);
    /// assert_eq!(-7., poles[1].re);
    /// ```
    #[must_use]
    pub fn poles(&self) -> Vec<Complex<f64>> {
        self.poles_impl()
    }
}

impl<U> SsGen<f32, U>
where
    U: Time,
{
    /// Calculate the poles of the system
    ///
    /// # Example
    ///
    /// ```
    /// use automatica::Ss;
    /// let sys = Ss::<f32>::new_from_slice(2, 1, 1, &[-2., 0., 3., -7.], &[1., 3.], &[-1., 0.5], &[0.1]);
    /// let poles = sys.poles();
    /// assert_eq!(-2., poles[0].re);
    /// assert_eq!(-7., poles[1].re);
    /// ```
    #[must_use]
    pub fn poles(&self) -> Vec<Complex<f32>> {
        self.poles_impl()
    }
}

impl<T, U> SsGen<T, U>
where
    T: Abs
        + Copy
        + Inv<Output = T>
        + One
        + PartialOrd
        + Pow<T>
        + nalgebra::RealField
        + Sign
        + Sqrt
        + Zero,
    U: Time,
{
    #[must_use]
    fn poles_impl(&self) -> Vec<Complex<T>> {
        match self.dim().states() {
            1 => vec![Complex::new(self.a[(0, 0)], <T as Zero>::zero())],
            2 => {
                let m00 = self.a[(0, 0)];
                let m01 = self.a[(0, 1)];
                let m10 = self.a[(1, 0)];
                let m11 = self.a[(1, 1)];
                let trace = m00 + m11;
                let determinant = m00 * m11 - m01 * m10;

                let (eig1, eig2) =
                    polynomen::complex_quadratic_roots(PWrapper(-trace), PWrapper(determinant));
                vec![
                    Complex::new(eig1.0 .0, eig1.1 .0),
                    Complex::new(eig2.0 .0, eig2.1 .0),
                ]
            }
            _ => ar_to_dm(&self.a)
                .complex_eigenvalues()
                .as_slice()
                .iter()
                .map(|x| Complex::new(x.re, x.im))
                .collect::<Vec<_>>(),
        }
    }
}

/// Controllability matrix implementation.
///
/// `Mr = [B AB (A^2)B ... (A^(n-1))B]` -> (n, mn) matrix.
///
/// # Arguments
///
/// * `n` - Number of states
/// * `m` - Number of inputs
/// * `a` - A matrix
/// * `b` - B matrix
fn controllability_impl<T>(n: usize, m: usize, a: &Array2<T>, b: &Array2<T>) -> Array2<T>
where
    T: Add<Output = T> + Clone + Mul<Output = T> + Zero,
{
    // Create the entire matrix ahead to avoid multiple allocations.
    let mut mr = Array2::<T>::from_elem((n, n * m), T::zero());
    // mr.columns_range_mut(0..m).copy_from(b);
    mr.slice_mut(s![.., 0..m]).assign(b);
    // Create a temporary matrix for the multiplication, since the Mr matrix
    // cannot be used both as reference and mutable reference.
    let mut rhs = Array2::<T>::from_elem((n, m), T::zero());
    for i in 1..n {
        rhs.assign(&mr.slice(s![.., ((i - 1) * m)..(i * m)]));
        // Multiply A by the result of the previous step.
        // The result is directly inserted into Mr.
        // a.mul_to(&rhs, &mut mr.columns_range_mut((i * m)..((i + 1) * m)));
        let column = a.mmul(&rhs);
        mr.slice_mut(s![.., (i * m)..((i + 1) * m)]).assign(&column);
    }
    mr
}

/// Observability matrix implementation.
///
/// `Mo = [C' A'C' (A'^2)C ... (A'^(n-1))C']` -> (n, pn) matrix.
///
/// # Arguments
///
/// * `n` - Number of states
/// * `p` - Number of outputs
/// * `a` - A matrix
/// * `c` - C matrix
fn observability_impl<T>(n: usize, p: usize, a: &Array2<T>, c: &Array2<T>) -> Array2<T>
where
    T: Add<Output = T> + Clone + Mul<Output = T> + Zero,
{
    // Create the entire matrix ahead to avoid multiple allocations.
    let mut mo = Array2::from_elem((n, n * p), T::zero());
    mo.slice_mut(s![.., 0..p]).assign(&c.t());
    let at = a.clone().reversed_axes();
    // Create a temporary matrix for the multiplication, since the Mo matrix
    // cannot be used both as reference and mutable reference.
    let mut rhs = Array2::from_elem((n, p), T::zero());
    for i in 1..n {
        rhs.assign(&mo.slice(s![.., ((i - 1) * p)..(i * p)]));
        // Multiply A by the result of the previous step.
        // The result is directly inserted into Mo;
        let column = at.mmul(&rhs);
        mo.slice_mut(s![.., (i * p)..((i + 1) * p)]).assign(&column);
    }
    mo
}

impl<T, U: Time> SsGen<T, U>
where
    T: Add<Output = T> + Clone + Mul<Output = T> + Zero,
{
    /// Controllability matrix
    ///
    /// `Mr = [B AB A^2B ... A^(n-1)B]` -> (n, mn) matrix.
    ///
    /// The return value is: `(rows, cols, vector with data in row major mode)`
    ///
    /// # Example
    /// ```
    /// use automatica::{linear_system::SsGen, Discrete};
    /// let a = [-1., 3., 0., 2.];
    /// let b = [1., 2.];
    /// let c = [1., 1.];
    /// let d = [0.];
    /// let sys = SsGen::<f64, Discrete>::new_from_slice(2, 1, 1, &a, &b, &c, &d);
    /// let mr = sys.controllability();
    /// assert_eq!((2, 2, vec![1., 5., 2., 4.]), mr);
    /// ```
    #[must_use]
    pub fn controllability(&self) -> (usize, usize, Vec<T>) {
        let n = self.dim.states;
        let m = self.dim.inputs;
        let mr = controllability_impl(n, m, &self.a, &self.b);
        (n, n * m, mr.iter().cloned().collect())
    }

    /// Observability matrix
    ///
    /// `Mo = [C' A'C' A'^2B ... A'^(n-1)C']` -> (n, pn) matrix.
    ///
    /// The return value is: `(rows, cols, vector with data in row major mode)`
    ///
    /// # Example
    /// ```
    /// use automatica::{linear_system::SsGen, Continuous};
    /// let a = [-1., 3., 0., 2.];
    /// let b = [1., 2.];
    /// let c = [1., 1.];
    /// let d = [0.];
    /// let sys = SsGen::<f64, Continuous>::new_from_slice(2, 1, 1, &a, &b, &c, &d);
    /// let mr = sys.observability();
    /// assert_eq!((2, 2, vec![1., -1., 1., 5.]), mr);
    /// ```
    #[must_use]
    pub fn observability(&self) -> (usize, usize, Vec<T>) {
        let n = self.dim.states;
        let p = self.dim.outputs;
        let mo = observability_impl(n, p, &self.a, &self.c);
        (n, n * p, mo.iter().cloned().collect())
    }
}

/// Faddeev-LeVerrier algorithm
///
/// [Wikipedia](https://en.wikipedia.org/wiki/Faddeev%E2%80%93LeVerrier_algorithm)
///
/// B(s) =       B1*s^(n-1) + B2*s^(n-2) + B3*s^(n-3) + ...
/// a(s) = s^n + a1*s^(n-1) + a2*s^(n-2) + a3*s^(n-3) + ...
///
/// with B1 = I = eye(n,n)
/// a1 = -trace(A); ak = -1/k * trace(A*Bk)
/// Bk = a_(k-1)*I + A*B_(k-1)
#[allow(non_snake_case)]
pub(super) fn leverrier<T>(A: &Array2<T>) -> Result<(PP<T>, PolyMatrix<T>), Error>
where
    T: Add<Output = T>
        + Clone
        + Inv<Output = T>
        + Mul<Output = T>
        + Neg<Output = T>
        + NumCast
        + One
        + PartialEq
        + Zero,
{
    let size = A.nrows(); // A is a square matrix.
    let mut a = vec![T::one()];
    let a1 = -A.trace();
    a.insert(0, a1.clone());

    let I = identity(size);
    let B1 = I.clone();
    let mut B = vec![B1.clone()];
    if size == 1 {
        return Ok((PP::new_from_coeffs(&a), PolyMatrix::new_from_coeffs(&B)));
    }

    let mut ak = a1;
    let mut ABk = A.mmul(&B1);
    for k in 2..=size {
        let Bk = (&I).smul(ak) + &ABk;

        ABk = A.mmul(&Bk);
        // Casting usize to T may causes a loss of precision.
        ak = -T::from(k)
            .ok_or_else(|| Error::new_internal(ErrorKind::NumericConversion))?
            .inv()
            * ABk.trace();

        B.insert(0, Bk);
        a.insert(0, ak.clone());
    }
    Ok((PP::new_from_coeffs(&a), PolyMatrix::new_from_coeffs(&B)))
}

impl<T, U> SsGen<T, U>
where
    T: Add<Output = T>
        + Clone
        + Div<Output = T>
        + Mul<Output = T>
        + Neg<Output = T>
        + One
        + PartialEq
        + Sub<Output = T>
        + Zero,
    U: Time,
{
    /// Convert a transfer function representation into state space representation.
    /// Conversion is done using the observability canonical form.
    ///
    /// ```text
    ///        b_n*s^n + b_(n-1)*s^(n-1) + ... + b_1*s + b_0
    /// G(s) = ---------------------------------------------
    ///          s^n + a_(n-1)*s^(n-1) + ... + a_1*s + a_0
    ///     ┌                   ┐        ┌         ┐
    ///     │ 0 0 0 . 0 -a_0    │        │ b'_0    │
    ///     │ 1 0 0 . 0 -a_1    │        │ b'_1    │
    /// A = │ 0 1 0 . 0 -a_2    │,   B = │ b'_2    │
    ///     │ . . . . . .       │        │ .       │
    ///     │ 0 0 0 . 1 -a_(n-1)│        │ b'_(n-1)│
    ///     └                   ┘        └         ┘
    ///     ┌           ┐                ┌    ┐
    /// C = │0 0 0 . 0 1│,           D = │b'_n│
    ///     └           ┘                └    ┘
    ///
    /// b'_n = b_n,   b'_i = b_i - a_i*b'_n,   i = 0, ..., n-1
    /// ```
    ///
    /// # Arguments
    ///
    /// `tf` - transfer function
    ///
    /// # Errors
    ///
    /// It returns an error if the transfer function has no poles.
    pub fn new_observability_realization(tf: &TfGen<T, U>) -> Result<Self, Error> {
        // Get the denominator in the monic form maintaining the original gain.
        let tf_norm = tf.normalize();
        let order = match tf_norm.den_int().0.degree() {
            Some(d) => d,
            None => return Err(Error::new_internal(ErrorKind::ZeroPolynomialDenominator)),
        };
        let num = {
            // Extend the numerator coefficients with zeros to the length of the
            // denominator polynomial.
            let mut num = tf_norm.num_int().clone();
            num.0.extend(order);
            num
        };

        // Calculate the observability canonical form.
        let a = observability_canonical_form(&tf_norm)?;

        // Get the number of states n.
        let states = a.nrows();
        // Get the highest coefficient of the numerator.
        let b_n = num.0[order].0.clone();

        // Create a nx1 vector with b'i = bi - ai * b'n
        let b = Array2::from_shape_fn((states, 1), |(i, _j)| {
            num.0[i].0.clone() - tf_norm.den_int().0[i].0.clone() * b_n.clone()
        });

        // Crate a 1xn vector with all zeros but the last that is 1.
        let c = {
            let mut c = Array2::from_elem((1, states), T::zero());
            c[(0, states - 1)] = T::one();
            c
        };

        // Crate a 1x1 matrix with the highest coefficient of the numerator.
        let d = Array2::from_elem((1, 1), b_n);

        // A single transfer function has only one input and one output.
        Ok(Self {
            a,
            b,
            c,
            d,
            dim: Dim {
                states,
                inputs: 1,
                outputs: 1,
            },
            time: PhantomData,
        })
    }

    /// Convert a transfer function representation into state space representation.
    /// Conversion is done using the controllability canonical form.
    ///
    /// ```text
    ///        b_n*s^n + b_(n-1)*s^(n-1) + ... + b_1*s + b_0
    /// G(s) = ---------------------------------------------
    ///          s^n + a_(n-1)*s^(n-1) + ... + a_1*s + a_0
    ///     ┌                          ┐        ┌   ┐
    ///     │  0    1    0   .  0      │        │ 0 │
    ///     │  0    0    1   .  0      │        │ 0 │
    /// A = │  0    0    0   .  0      │,   B = │ 0 │
    ///     │  .    .    .   .  .      │        │ . │
    ///     │  0    0    0   .  1      │        │ 0 │
    ///     │ -a_0 -a_1 -a_2 . -a_(n-1)│        │ 1 │
    ///     └                          ┘        └   ┘
    ///     ┌                         ┐         ┌    ┐
    /// C = │b'_0 b'_1 b'_2 . b'_(n-1)│,    D = │b'_n│
    ///     └                         ┘         └    ┘
    ///
    /// b'_n = b_n,   b'_i = b_i - a_i*b'_n,   i = 0, ..., n-1
    /// ```
    ///
    /// # Arguments
    ///
    /// `tf` - transfer function
    ///
    /// # Errors
    ///
    /// It returns an error if the transfer function has no poles.
    pub fn new_controllability_realization(tf: &TfGen<T, U>) -> Result<Self, Error> {
        // Get the denominator in the monic form maintaining the original gain.
        let tf_norm = tf.normalize();
        let order = match tf_norm.den_int().0.degree() {
            Some(d) => d,
            None => return Err(Error::new_internal(ErrorKind::ZeroPolynomialDenominator)),
        };
        let num = {
            // Extend the numerator coefficients with zeros to the length of the
            // denominator polynomial.
            let mut num = tf_norm.num_int().clone();
            num.0.extend(order);
            num
        };

        // Calculate the observability canonical form.
        let a = controllability_canonical_form(&tf_norm)?;

        // Get the number of states n.
        let states = a.nrows();
        // Get the highest coefficient of the numerator.
        let b_n = num.0[order].0.clone();

        // Crate a nx1 vector with all zeros but the last that is 1.
        let b = {
            let mut b = Array2::from_elem((states, 1), T::zero());
            b[(states - 1, 0)] = T::one();
            b
        };

        // Create a 1xn vector with b'i = bi - ai * b'n
        let c = Array2::from_shape_fn((1, states), |(_i, j)| {
            num.0[j].0.clone() - tf_norm.den_int().0[j].0.clone() * b_n.clone()
        });

        // Crate a 1x1 matrix with the highest coefficient of the numerator.
        let d = Array2::from_elem((1, 1), b_n);

        // A single transfer function has only one input and one output.
        Ok(Self {
            a,
            b,
            c,
            d,
            dim: Dim {
                states,
                inputs: 1,
                outputs: 1,
            },
            time: PhantomData,
        })
    }
}

/// Build the observability canonical form of the states (A) matrix.
///
/// The transfer function shall be in normalized from, i.e. the highest
/// coefficient of the denominator shall be 1.
fn observability_canonical_form<T, U>(tf: &TfGen<T, U>) -> Result<Array2<T>, Error>
where
    T: Clone + Neg<Output = T> + One + PartialEq + Zero,
    U: Time,
{
    // Assert that the denominator is in monic form.
    debug_assert!(One::is_one(&tf.den_int().0.leading_coeff().0));
    match tf.den_int().0.degree() {
        Some(degree) if degree > 0 => {
            let comp = Array2::from_shape_fn((degree, degree), |(i, j)| {
                if j == degree - 1 {
                    -tf.den_int().0[i].0.clone()
                } else if i == j + 1 {
                    T::one()
                } else {
                    T::zero()
                }
            });
            debug_assert!(comp.is_square());
            Ok(comp)
        }
        _ => Err(Error::new_internal(ErrorKind::NoPolesDenominator)),
    }
}

/// Build the controllability canonical form of the states (A) matrix.
///
/// The transfer function shall be in normalized from, i.e. the highest
/// coefficient of the denominator shall be 1.
fn controllability_canonical_form<T, U>(tf: &TfGen<T, U>) -> Result<Array2<T>, Error>
where
    T: Clone + Neg<Output = T> + One + PartialEq + Zero,
    U: Time,
{
    // Assert that the denominator is in monic form.
    debug_assert!(tf.den_int().0.leading_coeff().0.is_one());
    match tf.den_int().0.degree() {
        Some(degree) if degree > 0 => {
            let comp = Array2::from_shape_fn((degree, degree), |(i, j)| {
                if i == degree - 1 {
                    -tf.den_int().0[j].0.clone()
                } else if j == i + 1 {
                    T::one()
                } else {
                    T::zero()
                }
            });
            debug_assert!(comp.is_square());
            Ok(comp)
        }
        _ => Err(Error::new_internal(ErrorKind::NoPolesDenominator)),
    }
}

/// Implementation of state-space representation
impl<T, U: Time> Display for SsGen<T, U>
where
    T: Display,
{
    fn fmt(&self, f: &mut Formatter) -> fmt::Result {
        write!(
            f,
            "A: {}\nB: {}\nC: {}\nD: {}",
            self.a, self.b, self.c, self.d
        )
    }
}

/// Struct describing an equilibrium point
#[derive(Clone, Debug)]
pub struct Equilibrium<T> {
    /// State equilibrium
    x: Vec<T>,
    /// Output equilibrium
    y: Vec<T>,
}

/// Implement methods for equilibrium
impl<T> Equilibrium<T> {
    /// Create a new equilibrium given the state and the output vectors
    ///
    /// # Arguments
    ///
    /// * `x` - State equilibrium
    /// * `y` - Output equilibrium
    fn new(x: Vec<T>, y: Vec<T>) -> Self {
        Self { x, y }
    }

    /// Retrieve state coordinates for equilibrium
    #[must_use]
    pub fn x(&self) -> &[T] {
        self.x.as_slice()
    }

    /// Retrieve output coordinates for equilibrium
    #[must_use]
    pub fn y(&self) -> &[T] {
        self.y.as_slice()
    }
}

/// Implementation of printing of equilibrium point
impl<T: Display> Display for Equilibrium<T> {
    fn fmt(&self, f: &mut Formatter) -> fmt::Result {
        write!(f, "x: [")?;
        let mut first_x = true;
        for i in &self.x {
            if !first_x {
                write!(f, ", ")?;
            }
            i.fmt(f)?;
            first_x = false;
        }
        write!(f, "]\ny: [")?;
        let mut first_y = true;
        for i in &self.y {
            if !first_y {
                write!(f, ", ")?;
            }
            i.fmt(f)?;
            first_y = false;
        }
        write!(f, "]")
    }
}

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

    use crate::{polynomial_matrix::MatrixOfPoly, Continuous, Discrete};

    use super::*;

    proptest! {
    #[test]
        fn qc_dimensions(states: usize, inputs: usize, outputs: usize) {
            let d = Dim {
                states,
                inputs,
                outputs,
            };
            assert_eq!(states, d.states());
            assert_eq!(inputs, d.inputs());
            assert_eq!(outputs, d.outputs());
        }
    }

    #[test]
    fn system_dimensions() {
        let states = 2;
        let inputs = 1;
        let outputs = 1;
        let sys = SsGen::<f64, Continuous>::new_from_slice(
            states,
            inputs,
            outputs,
            &[-2., 0., 3., -7.],
            &[1., 3.],
            &[-1., 0.5],
            &[0.1],
        );
        assert_eq!(
            Dim {
                states,
                inputs,
                outputs
            },
            sys.dim()
        );
    }

    #[test]
    #[should_panic]
    fn poles_fail() {
        let eig1 = -2.;
        let eig2 = -7.;
        let a = nalgebra::DMatrix::from_row_slice(2, 2, &[eig1, 0., 3., eig2]);
        let schur = nalgebra::Schur::new(a);
        //dbg!(&schur);
        let poles = schur.complex_eigenvalues();
        //dbg!(poles);
        assert_eq!((eig1, eig2), (poles[0].re, poles[1].re));
    }

    #[test]
    fn poles_regression() {
        let eig1 = -2.;
        let eig2 = -7.;
        let sys = SsGen::<f64, Discrete>::new_from_slice(
            2,
            1,
            1,
            &[eig1, 0., 3., eig2],
            &[1., 3.],
            &[-1., 0.5],
            &[0.1],
        );
        let poles = sys.poles();
        assert_eq!((eig1, eig2), (poles[0].re, poles[1].re));
    }

    proptest! {
        #[test]
        fn qc_poles_one(eig: f32) {
            let sys = SsGen::<f32, Continuous>::new_from_slice(1, 1, 1, &[eig], &[1.], &[-1.], &[0.1]);
            let poles = sys.poles();

            let expected = (eig, 0.);
            let actual = (poles[0].re, poles[0].im);
            assert_relative_eq!(expected.0, actual.0, max_relative = 1e-10);
            assert_relative_eq!(expected.1, actual.1, max_relative = 1e-10);
        }
    }

    proptest! {
        #[test]
        fn qc_poles_two(eig1 in (-1e12..1e12), eig2 in (-1e12..1e12)) {
            let sys = SsGen::<f64, Continuous>::new_from_slice(
                2,
                1,
                1,
                &[eig1, 0., 3., eig2],
                &[1., 3.],
                &[-1., 0.5],
                &[0.1],
            );
            let poles = sys.poles();

            let mut expected = [eig1, eig2];
            expected.sort_unstable_by(|a, b| a.partial_cmp(b).unwrap());
            let mut actual = [poles[0].re, poles[1].re];
            actual.sort_unstable_by(|a, b| a.partial_cmp(b).unwrap());
            assert_relative_eq!(expected[0], actual[0], max_relative = 1e-10);
            assert_relative_eq!(expected[1], actual[1], max_relative = 1e-10);
        }
    }

    #[test]
    fn poles_three() {
        let eig1 = -7.;
        let eig2 = -2.;
        let eig3 = 1.25;
        let sys = SsGen::<f64, Discrete>::new_from_slice(
            3,
            1,
            1,
            &[eig1, 0., 0., 3., eig2, 0., 10., 0.8, eig3],
            &[1., 3., -5.5],
            &[-1., 0.5, -4.3],
            &[0.],
        );
        let mut poles = sys.poles();
        poles.sort_unstable_by(|a, b| a.re.partial_cmp(&b.re).unwrap());
        assert_relative_eq!(eig1, poles[0].re, max_relative = 1e-10);
        assert_relative_eq!(eig2, poles[1].re, max_relative = 1e-10);
        assert_relative_eq!(eig3, poles[2].re, max_relative = 1e-10);
    }

    #[test]
    #[allow(clippy::float_cmp)]
    fn leverrier_algorithm_f64() {
        // Example of LeVerrier algorithm (Wikipedia)");
        let t = Array2::from_shape_vec((3, 3), vec![3., 1., 5., 3., 3., 1., 4., 6., 4.]).unwrap();
        let expected_pc = [-40., 4., -10., 1.];
        let expected_degree0 =
            Array2::from_shape_vec((3, 3), vec![6., 26., -14., -8., -8., 12., 6., -14., 6.])
                .unwrap();
        let expected_degree1 =
            Array2::from_shape_vec((3, 3), vec![-7., 1., 5., 3., -7., 1., 4., 6., -6.]).unwrap();
        let expected_degree2 =
            Array2::from_shape_vec((3, 3), vec![1., 0., 0., 0., 1., 0., 0., 0., 1.]).unwrap();

        let (p, poly_matrix) = leverrier(&t).unwrap();

        println!("T: {}\np: {}\n", &t, &p);
        println!("B: {}", &poly_matrix);

        assert_eq!(expected_pc, p.to_unwrapped_vec().as_slice());
        assert_eq!(expected_degree0, poly_matrix[0]);
        assert_eq!(expected_degree1, poly_matrix[1]);
        assert_eq!(expected_degree2, poly_matrix[2]);

        let mp = MatrixOfPoly::from(poly_matrix);
        println!("mp {}", &mp);
        let expected_result = "[[6 -7s +1s^2, 26 +1s, -14 +5s],\n \
                               [-8 +3s, -8 -7s +1s^2, 12 +1s],\n \
                               [6 +4s, -14 +6s, 6 -6s +1s^2]]";
        assert_eq!(expected_result, format!("{}", &mp));
    }

    #[test]
    #[allow(clippy::float_cmp)]
    fn leverrier_algorithm_f32() {
        // Example of LeVerrier algorithm (Wikipedia)");
        let t =
            Array2::from_shape_vec((3, 3), vec![3.0_f32, 1., 5., 3., 3., 1., 4., 6., 4.]).unwrap();
        let expected_pc = [-40., 4., -10., 1.];
        let expected_degree0 =
            Array2::from_shape_vec((3, 3), vec![6., 26., -14., -8., -8., 12., 6., -14., 6.])
                .unwrap();
        let expected_degree1 =
            Array2::from_shape_vec((3, 3), vec![-7., 1., 5., 3., -7., 1., 4., 6., -6.]).unwrap();
        let expected_degree2 =
            Array2::from_shape_vec((3, 3), vec![1., 0., 0., 0., 1., 0., 0., 0., 1.]).unwrap();

        let (p, poly_matrix) = leverrier(&t).unwrap();

        println!("T: {}\np: {}\n", &t, &p);
        println!("B: {}", &poly_matrix);

        assert_eq!(expected_pc, p.to_unwrapped_vec().as_slice());
        assert_eq!(expected_degree0, poly_matrix[0]);
        assert_eq!(expected_degree1, poly_matrix[1]);
        assert_eq!(expected_degree2, poly_matrix[2]);

        let mp = MatrixOfPoly::from(poly_matrix);
        println!("mp {}", &mp);
        let expected_result = "[[6 -7s +1s^2, 26 +1s, -14 +5s],\n \
                               [-8 +3s, -8 -7s +1s^2, 12 +1s],\n \
                               [6 +4s, -14 +6s, 6 -6s +1s^2]]";
        assert_eq!(expected_result, format!("{}", &mp));
    }

    #[test]
    #[allow(clippy::float_cmp)]
    fn leverrier_1x1_matrix_f32() {
        let t = Array2::from_elem((1, 1), 3.0_f32);
        let expected_pc = [-3., 1.];
        let expected_degree0 = Array2::from_elem((1, 1), 1.);

        let (p, poly_matrix) = leverrier(&t).unwrap();
        assert_eq!(expected_pc, p.to_unwrapped_vec().as_slice());
        assert_eq!(expected_degree0, poly_matrix[0]);

        let mp = MatrixOfPoly::from(poly_matrix);
        let expected_result = "[[1]]";
        assert_eq!(expected_result, format!("{}", &mp));
    }

    #[test]
    #[allow(clippy::float_cmp)]
    fn leverrier_1x1_matrix_f64() {
        let t = Array2::from_elem((1, 1), 3.);
        let expected_pc = [-3., 1.];
        let expected_degree0 = Array2::from_elem((1, 1), 1.);

        let (p, poly_matrix) = leverrier(&t).unwrap();
        assert_eq!(expected_pc, p.to_unwrapped_vec().as_slice());
        assert_eq!(expected_degree0, poly_matrix[0]);

        let mp = MatrixOfPoly::from(poly_matrix);
        let expected_result = "[[1]]";
        assert_eq!(expected_result, format!("{}", &mp));
    }

    #[test]
    fn convert_to_ss_continuous() {
        use crate::transfer_function::continuous::Tf;
        let tf = Tf::new([1.], [1., 1., 1.]);

        let ss = SsGen::new_controllability_realization(&tf).unwrap();

        assert_eq!(
            Array2::from_shape_vec((2, 2), vec![0., 1., -1., -1.]).unwrap(),
            ss.a
        );
        assert_eq!(Array2::from_shape_vec((2, 1), vec![0., 1.]).unwrap(), ss.b);
        assert_eq!(Array2::from_shape_vec((1, 2), vec![1., 0.]).unwrap(), ss.c);
        assert_eq!(Array2::from_shape_vec((1, 1), vec![0.]).unwrap(), ss.d);
    }

    #[test]
    fn convert_to_ss_discrete() {
        use crate::transfer_function::discrete::Tfz;
        let tf = Tfz::new([1., 0., 1.], [3., 4., 1.]);

        let ss = SsGen::new_observability_realization(&tf).unwrap();

        assert_eq!(
            Array2::from_shape_vec((2, 2), vec![0., -3., 1., -4.]).unwrap(),
            ss.a
        );
        assert_eq!(
            Array2::from_shape_vec((2, 1), vec![-2., -4.]).unwrap(),
            ss.b
        );
        assert_eq!(Array2::from_shape_vec((1, 2), vec![0., 1.]).unwrap(), ss.c);
        assert_eq!(Array2::from_shape_vec((1, 1), vec![1.]).unwrap(), ss.d);
    }

    #[test]
    fn failed_observability_realization() {
        use crate::transfer_function::discrete::Tfz;
        let tf = Tfz::new([1.], [0.]);
        let ss = SsGen::new_observability_realization(&tf);
        assert!(ss.is_err());
    }

    #[test]
    fn failed_observability_canonical_form() {
        use crate::transfer_function::discrete::Tfz;
        let tf = Tfz::new([1.], [1.]);
        let matrix = observability_canonical_form(&tf);
        assert!(matrix.is_err());
    }

    #[test]
    fn failed_controllability_realization() {
        use crate::transfer_function::discrete::Tfz;
        let tf = Tfz::new([1.], [0.]);
        let ss = SsGen::new_controllability_realization(&tf);
        assert!(ss.is_err());
    }

    #[test]
    fn failed_controllability_canonical_form() {
        use crate::transfer_function::discrete::Tfz;
        let tf = Tfz::new([1.], [1.]);
        let matrix = controllability_canonical_form(&tf);
        assert!(matrix.is_err());
    }

    #[test]
    fn controllability() {
        let a = [-1., 3., 0., 2.];
        let b = [1., 2.];
        let c = [1., 1.];
        let d = [0.];

        let sys = SsGen::<f64, Discrete>::new_from_slice(2, 1, 1, &a, &b, &c, &d);
        let mr = sys.controllability();
        assert_eq!((2, 2, vec![1., 5., 2., 4.]), mr);
    }

    #[test]
    fn observability() {
        let a = [-1., 3., 0., 2.];
        let b = [1., 2.];
        let c = [1., 1.];
        let d = [0.];

        let sys = SsGen::<f64, Continuous>::new_from_slice(2, 1, 1, &a, &b, &c, &d);
        let mo = sys.observability();
        assert_eq!((2, 2, vec![1., -1., 1., 5.]), mo);
    }

    #[test]
    fn linear_system_display() {
        let a = [-1., 3., 0., 2.];
        let b = [1., 2.];
        let c = [1., 1.];
        let d = [0.];

        let sys = SsGen::<f64, Continuous>::new_from_slice(2, 1, 1, &a, &b, &c, &d);
        let string = format!("{}", &sys);
        assert!(!string.is_empty());
    }
}