automatica 1.0.0

//! # Transfer functions for discrete time systems.
//!
//! Specialized struct and methods for discrete time transfer functions
//! * time delay
//! * initial value
//! * static gain
//! * ARMA (autoregressive moving average) time evaluation method
//!
//! This module contains the discretization struct of a continuous time
//! transfer function
//! * forward Euler mehtod
//! * backward Euler method
//! * Tustin (trapezoidal) method

use std::ops::Neg;
use std::{
    cmp::Ordering,
    collections::VecDeque,
    fmt::Debug,
    iter::Sum,
    ops::{Add, Div, Mul, Sub},
};

use crate::{
    enums::Discrete, plots::Plotter, transfer_function::TfGen, Abs, Atan2, Complex, Cos, Hypot,
    Infinity, Inv, One, Pow, Sin, Zero,
};

/// Discrete transfer function
pub type Tfz<T> = TfGen<T, Discrete>;

impl<T> Tfz<T>
where
    T: Abs
        + Add<Output = T>
        + Atan2
        + Clone
        + Cos
        + Div<Output = T>
        + Hypot
        + Infinity
        + Inv<Output = T>
        + Mul<Output = T>
        + Neg<Output = T>
        + One
        + PartialEq
        + PartialOrd
        + Pow<T>
        + Sin
        + Sub<Output = T>
        + Zero,
{
    /// Time delay for discrete time transfer function.
    /// `y(k) = u(k - h)`
    /// `G(z) = z^(-h)`
    ///
    /// # Arguments
    ///
    /// * `h` - Time delay
    ///
    /// # Example
    /// ```
    /// use automatica::{Complex, units::Seconds, Tfz};
    /// let d = Tfz::delay(2);
    /// assert_eq!(0.010000001, d(Complex::new(0., 10.0_f32)).norm());
    /// ```
    pub fn delay(k: i32) -> impl Fn(Complex<T>) -> Complex<T> {
        move |z| z.powi(-k)
    }

    /// System inital value response to step input.
    /// `y(0) = G(z->infinity)`
    ///
    /// # Example
    /// ```
    /// use automatica::Tfz;
    /// let tf = Tfz::new([4.], [1., 5.]);
    /// assert_eq!(0., tf.init_value());
    /// ```
    #[must_use]
    pub fn init_value(&self) -> T {
        let n = self.num_int().0.degree();
        let d = self.den_int().0.degree();
        match n.cmp(&d) {
            Ordering::Less => T::zero(),
            Ordering::Equal => {
                self.num_int().0.leading_coeff().0 / self.den_int().0.leading_coeff().0
            }
            Ordering::Greater => T::infinity(),
        }
    }
}

impl<'a, T> Tfz<T>
where
    T: 'a + Add<&'a T, Output = T> + Clone + Div<Output = T> + PartialEq + Zero,
{
    /// Static gain `G(1)`.
    /// Ratio between constant output and constant input.
    /// Static gain is defined only for transfer functions of 0 type.
    ///
    /// Example
    ///
    /// ```
    /// use automatica::Tfz;
    /// let tf = Tfz::new([5., -3.],[2., 5., -6.]);
    /// assert_eq!(2., tf.static_gain());
    /// ```
    #[must_use]
    pub fn static_gain(&'a self) -> T {
        let n = self
            .num_int()
            .0
            .as_slice()
            .iter()
            .fold(T::zero(), |acc, c| acc + &c.0);
        let d = self
            .den_int()
            .0
            .as_slice()
            .iter()
            .fold(T::zero(), |acc, c| acc + &c.0);
        n / d
    }
}

macro_rules! tfz_stable {
    ($ty:ty) => {
        impl Tfz<$ty> {
            #[doc = "System stability. Checks if all poles are inside the unit circle.\n"]
            #[doc = "# Example\n```\nuse polynomen::Poly;\nuse automatica::Tfz;"]
            #[doc = concat!("let tfz = Tfz::new([1.0_",stringify!($ty), "], Poly::new_from_roots(&[0.5, 1.5]));")]
            #[doc = "assert!(!tfz.is_stable());\n```"]
            #[must_use]
            pub fn is_stable(&self) -> bool {
                self.complex_poles().iter().all(|p| p.norm() < <$ty>::one())
            }
        }
    };
}
tfz_stable!(f32);
tfz_stable!(f64);

/// Macro defining the common behaviour when creating the arma iterator.
///
/// # Arguments
///
/// * `self` - `self` parameter keyword
/// * `y_coeffs` - vector containing the coefficients of the output
/// * `u_coeffs` - vector containing the coefficients of the input
/// * `y` - queue containing the calculated outputs
/// * `u` - queue containing the supplied inputs
macro_rules! arma {
    ($self:ident, $y_coeffs:ident, $u_coeffs:ident, $y:ident, $u:ident) => {{
        let g = $self.normalize();
        let n_n = g.num_int().0.degree().unwrap_or(0);
        let n_d = g.den_int().0.degree().unwrap_or(0);
        let n = n_n.max(n_d);

        // The front is the lowest order coefficient.
        // The back is the higher order coefficient.
        // The higher degree terms are the more recent.
        // The last coefficient is always 1, because g is normalized.
        // [a0, a1, a2, ..., a(n-1), 1]
        let mut output_coefficients: Vec<_> =
            g.den_int().0.coeffs().iter().map(|x| x.0.clone()).collect();
        // Remove the last coefficient by truncating the vector by one.
        // This is done because the last coefficient of the denominator corresponds
        // to the currently calculated output.
        output_coefficients.truncate(n_d);
        // [a0, a1, a2, ..., a(n-1)]
        $y_coeffs = output_coefficients;
        // [b0, b1, b2, ..., bm]
        $u_coeffs = g.num_int().0.coeffs().iter().map(|x| x.0.clone()).collect();

        // The coefficients do not need to be extended with zeros,
        // when the coffiecients are 'zipped' with the VecDeque, the zip stops at the
        // shortest iterator.

        let length = n + 1;
        // The front is the oldest calculated output.
        // [y(k-n), y(k-n+1), ..., y(k-1), y(k)]
        $y = VecDeque::from(vec![T::zero(); length]);
        // The front is the oldest input.
        // [u(k-n), u(k-n+1), ..., u(k-1), u(k)]
        $u = VecDeque::from(vec![T::zero(); length]);
    }};
}

impl<T> Tfz<T>
where
    T: Add<Output = T>
        + Clone
        + Div<Output = T>
        + Mul<Output = T>
        + One
        + PartialEq
        + Sub<Output = T>
        + Sum
        + Zero,
{
    /// Autoregressive moving average representation of a discrete transfer function
    /// It transforms the transfer function into time domain input-output
    /// difference equation.
    /// ```text
    ///                   b_n*z^n + b_(n-1)*z^(n-1) + ... + b_1*z + b_0
    /// Y(z) = G(z)U(z) = --------------------------------------------- U(z)
    ///                     z^n + a_(n-1)*z^(n-1) + ... + a_1*z + a_0
    ///
    /// y(k) = - a_(n-1)*y(k-1) - ... - a_1*y(k-n+1) - a_0*y(k-n) +
    ///        + b_n*u(k) + b_(n-1)*u(k-1) + ... + b_1*u(k-n+1) + b_0*u(k-n)
    /// ```
    ///
    /// # Arguments
    ///
    /// * `input` - Input function
    ///
    /// # Example
    /// ```
    /// use automatica::{signals::discrete, Tfz};
    /// let tfz = Tfz::new([1., 2., 3.], [0., 0., 0., 1.]);
    /// let mut iter = tfz.arma_fn(discrete::step(1., 0));
    /// assert_eq!(Some(0.), iter.next());
    /// assert_eq!(Some(3.), iter.next());
    /// assert_eq!(Some(5.), iter.next());
    /// assert_eq!(Some(6.), iter.next());
    /// ```
    pub fn arma_fn<F>(&self, input: F) -> ArmaFn<F, T>
    where
        F: Fn(usize) -> T,
    {
        let y_coeffs: Vec<_>;
        let u_coeffs: Vec<_>;
        let y: VecDeque<_>;
        let u: VecDeque<_>;
        arma!(self, y_coeffs, u_coeffs, y, u);

        ArmaFn {
            y_coeffs,
            u_coeffs,
            y,
            u,
            input,
            k: 0,
        }
    }

    /// Autoregressive moving average representation of a discrete transfer function
    /// It transforms the transfer function into time domain input-output
    /// difference equation.
    /// ```text
    ///                   b_n*z^n + b_(n-1)*z^(n-1) + ... + b_1*z + b_0
    /// Y(z) = G(z)U(z) = --------------------------------------------- U(z)
    ///                     z^n + a_(n-1)*z^(n-1) + ... + a_1*z + a_0
    ///
    /// y(k) = - a_(n-1)*y(k-1) - ... - a_1*y(k-n+1) - a_0*y(k-n) +
    ///        + b_n*u(k) + b_(n-1)*u(k-1) + ... + b_1*u(k-n+1) + b_0*u(k-n)
    /// ```
    ///
    /// # Arguments
    ///
    /// * `iter` - Iterator supplying the input data to the model
    ///
    /// # Example
    /// ```
    /// use automatica::{signals::discrete, Tfz};
    /// let tfz = Tfz::new([1., 2., 3.], [0., 0., 0., 1.]);
    /// let mut iter = tfz.arma_iter(std::iter::repeat(1.));
    /// assert_eq!(Some(0.), iter.next());
    /// assert_eq!(Some(3.), iter.next());
    /// assert_eq!(Some(5.), iter.next());
    /// assert_eq!(Some(6.), iter.next());
    /// ```
    pub fn arma_iter<I, II>(&self, iter: II) -> ArmaIter<I, T>
    where
        II: IntoIterator<Item = T, IntoIter = I>,
        I: Iterator<Item = T>,
    {
        let y_coeffs: Vec<_>;
        let u_coeffs: Vec<_>;
        let y: VecDeque<_>;
        let u: VecDeque<_>;
        arma!(self, y_coeffs, u_coeffs, y, u);

        ArmaIter {
            y_coeffs,
            u_coeffs,
            y,
            u,
            iter: iter.into_iter(),
        }
    }
}

/// Iterator for the autoregressive moving average model of a discrete
/// transfer function.
/// The input is supplied through a function.
#[derive(Debug)]
pub struct ArmaFn<F, T>
where
    F: Fn(usize) -> T,
{
    /// y coefficients
    y_coeffs: Vec<T>,
    /// u coefficients
    u_coeffs: Vec<T>,
    /// y queue buffer
    y: VecDeque<T>,
    /// u queue buffer
    u: VecDeque<T>,
    /// input function
    input: F,
    /// step
    k: usize,
}

/// Macro containing the common iteration steps of the ARMA model
///
/// # Arguments
///
/// * `self` - `self` keyword parameter
macro_rules! arma_iter {
    ($self:ident, $current_input:ident) => {{
        // Push the current input into the most recent position of the input buffer.
        $self.u.push_back($current_input);
        // Discard oldest input.
        $self.u.pop_front();
        let input: T = $self
            .u_coeffs
            .iter()
            .zip(&$self.u)
            .map(|(c, u)| c.clone() * u.clone())
            .sum();

        // Push zero in the last position shifting output values one step back
        // in time, zero suppress last coefficient which shall be the current
        // calculated output value.
        $self.y.push_back(T::zero());
        // Discard oldest output.
        $self.y.pop_front();
        let old_output: T = $self
            .y_coeffs
            .iter()
            .zip(&$self.y)
            .map(|(c, y)| c.clone() * y.clone())
            .sum();

        // Calculate the output.
        let new_y = input - old_output;
        // Put the new calculated value in the last position of the buffer.
        // `back_mut` returns None if the Deque is empty, this should never happen.
        debug_assert!(!$self.y.is_empty());
        *$self.y.back_mut()? = new_y.clone();
        Some(new_y)
    }};
}

impl<F, T> Iterator for ArmaFn<F, T>
where
    F: Fn(usize) -> T,
    T: Clone + Mul<Output = T> + Sub<Output = T> + Sum + Zero,
{
    type Item = T;

    fn next(&mut self) -> Option<Self::Item> {
        let current_input = (self.input)(self.k);
        self.k += 1;
        arma_iter!(self, current_input)
    }
}

/// Iterator for the autoregressive moving average model of a discrete
/// transfer function.
/// The input is supplied through an iterator.
#[derive(Debug)]
pub struct ArmaIter<I, T>
where
    I: Iterator,
{
    /// y coefficients
    y_coeffs: Vec<T>,
    /// u coefficients
    u_coeffs: Vec<T>,
    /// y queue buffer
    y: VecDeque<T>,
    /// u queue buffer
    u: VecDeque<T>,
    /// input iterator
    iter: I,
}

impl<I, T> Iterator for ArmaIter<I, T>
where
    I: Iterator<Item = T>,
    T: Clone + Mul<Output = T> + Sub<Output = T> + Sum + Zero,
{
    type Item = T;

    fn next(&mut self) -> Option<Self::Item> {
        let current_input = self.iter.next()?;
        arma_iter!(self, current_input)
    }
}

impl<T> Plotter<T> for Tfz<T>
where
    T: Abs
        + Add<Output = T>
        + Clone
        + Cos
        + Div<Output = T>
        + Inv<Output = T>
        + Mul<Output = T>
        + Neg<Output = T>
        + One
        + PartialEq
        + PartialOrd
        + Sin
        + Sub<Output = T>
        + Zero,
{
    /// Evaluate the transfer function at the given value.
    ///
    /// # Arguments
    ///
    /// * `theta` - angle at which the function is evaluated.
    /// Evaluation occurs at G(e^(i*theta)).
    fn eval_point(&self, theta: T) -> Complex<T> {
        self.eval(&Complex::from_polar(T::one(), theta))
    }
}

#[cfg(test)]
mod tests {
    use polynomen::Poly;

    use crate::{signals::discrete, units::ToDecibel, Complex};

    use super::*;

    #[test]
    fn tfz() {
        let _tfz = Tfz::new([1.], [1., 2., 3.]);
    }

    #[test]
    fn delay() {
        let d = Tfz::delay(2);
        assert_relative_eq!(0.010_000_001, d(Complex::new(0., 10.0_f32)).norm());
    }

    #[test]
    fn initial_value() {
        let tf = Tfz::new([4.], [1., 5.]);
        assert_relative_eq!(0., tf.init_value());

        let tf = Tfz::new([4., 10.], [1., 5.]);
        assert_relative_eq!(2., tf.init_value());

        let tf = Tfz::new([4., 1.], [5.]);
        assert_relative_eq!(std::f32::INFINITY, tf.init_value());
    }

    #[test]
    fn static_gain() {
        let tf = Tfz::new([5., -3.], [2., 5., -6.]);
        assert_relative_eq!(2., tf.static_gain());
    }

    #[test]
    fn stability() {
        let stable_den = Poly::new_from_roots(&[-0.3_f64, 0.5]);
        let stable_tf = Tfz::new([1., 2.], stable_den);
        assert!(stable_tf.is_stable());

        let unstable_den = Poly::new_from_roots(&[0.0_f64, -2.]);
        let unstable_tf = Tfz::new([1., 2.], unstable_den);
        assert!(!unstable_tf.is_stable());
    }

    #[test]
    fn eval() {
        let tf = Tfz::new([2., 20.], [1., 0.1]);
        let z = Complex::<f64>::new(0., 1.) * 0.5;
        let g = tf.eval(&z);
        assert_relative_eq!(20.159, g.norm().to_db(), max_relative = 1e-4);
        assert_relative_eq!(75.828, g.arg().to_degrees(), max_relative = 1e-4);
    }

    #[test]
    fn arma() {
        let tfz = Tfz::new([0.5_f32], [-0.5, 1.]);
        let mut iter = tfz.arma_fn(discrete::impulse(1., 0)).take(6);
        assert_eq!(Some(0.), iter.next());
        assert_eq!(Some(0.5), iter.next());
        assert_eq!(Some(0.25), iter.next());
        assert_eq!(Some(0.125), iter.next());
        assert_eq!(Some(0.0625), iter.next());
        assert_eq!(Some(0.03125), iter.next());
        assert_eq!(None, iter.next());
    }

    #[test]
    fn arma_iter() {
        use std::iter;
        let tfz = Tfz::new([0.5_f32], [-0.5, 1.]);
        let mut iter = tfz.arma_iter(iter::once(1.).chain(iter::repeat(0.)).take(6));
        assert_eq!(Some(0.), iter.next());
        assert_eq!(Some(0.5), iter.next());
        assert_eq!(Some(0.25), iter.next());
        assert_eq!(Some(0.125), iter.next());
        assert_eq!(Some(0.0625), iter.next());
        assert_eq!(Some(0.03125), iter.next());
        assert_eq!(None, iter.next());
    }
}