automatica 1.0.0

//! # Bode plot
//!
//! Bode plot returns the angular frequency, the magnitude and the phase.
//!
//! Functions use angular frequencies as default inputs and output, being the
//! inverse of the poles and zeros time constants.

use std::ops::{Add, Div, Sub};

use crate::{
    plots::Plotter,
    units::{Hertz, RadiansPerSecond, ToDecibel},
    Atan2, Const, Degree, Floor, Hypot, Log, MulAdd, One, Pow, Zero,
};

/// Struct for the calculation of Bode plots
#[derive(Clone, Debug)]
pub struct Bode<T, U: Plotter<T>> {
    /// Transfer function
    tf: U,
    /// Minimum angular frequency of the plot
    min_freq: RadiansPerSecond<T>,
    /// Maximum angular frequency of the plot
    max_freq: RadiansPerSecond<T>,
    /// Step between frequencies
    step: T,
}

impl<T, U> Bode<T, U>
where
    T: Const + PartialOrd + Zero,
    U: Plotter<T>,
{
    /// Create a `Bode` plot struct
    ///
    /// # Arguments
    ///
    /// * `tf` - Transfer function to plot
    /// * `min_freq` - Minimum angular frequency of the plot
    /// * `max_freq` - Maximum angular frequency of the plot
    /// * `step` - Step between frequencies
    ///
    /// `step` shall be in logarithmic scale. Use 0.1 to have 10 point per decade
    ///
    /// # Panics
    ///
    /// Panics if the step is not strictly positive and the minimum frequency
    /// is not lower than the maximum frequency
    pub fn new(
        tf: U,
        min_freq: RadiansPerSecond<T>,
        max_freq: RadiansPerSecond<T>,
        step: T,
    ) -> Self {
        assert!(step > T::zero());
        assert!(min_freq < max_freq);

        Self {
            tf,
            min_freq,
            max_freq,
            step,
        }
    }
}

impl<T, U> Bode<T, U>
where
    T: Const + PartialOrd + Zero,
    U: Plotter<T>,
{
    /// Create a `Bode` plot struct for discrete time systems.
    ///
    /// # Arguments
    ///
    /// * `tf` - Transfer function to plot
    /// * `min_freq` - Minimum angular frequency of the plot
    /// * `step` - Step between frequencies
    ///
    /// `step` shall be in logarithmic scale. Use 0.1 to have 10 point per decade
    ///
    /// # Panics
    ///
    /// Panics if the step is not strictly positive and the minimum frequency
    /// is not lower than pi.
    pub fn new_discrete(tf: U, min_freq: RadiansPerSecond<T>, step: T) -> Self {
        let pi = RadiansPerSecond(T::pi());
        assert!(step > T::zero());
        assert!(min_freq < pi);

        Self {
            tf,
            min_freq,
            max_freq: pi,
            step,
        }
    }
}

impl<T, U> IntoIterator for Bode<T, U>
where
    T: Add<Output = T>
        + Atan2
        + Clone
        + Div<Output = T>
        + Floor
        + From<f32>
        + Hypot
        + Log
        + MulAdd<Output = T>
        + One
        + PartialOrd
        + Pow<T>
        + Sub<Output = T>
        + Zero,
    U: Plotter<T>,
{
    type Item = Data<T>;
    type IntoIter = IntoIter<T, U>;

    fn into_iter(self) -> Self::IntoIter {
        let min = self.min_freq.0.log10();
        let max = self.max_freq.0.log10();
        let intervals = ((max - min.clone()) / self.step.clone()).floor();
        Self::IntoIter {
            tf: self.tf,
            intervals,
            step: self.step,
            base_freq: RadiansPerSecond(min),
            index: T::zero(),
        }
    }
}

/// Struct for the Polar plot data point iteration.
#[derive(Clone, Debug)]
pub struct IntoIter<T, U>
where
    U: Plotter<T>,
{
    /// Transfer function
    tf: U,
    /// Number of intervals of the plot
    intervals: T,
    /// Step between frequencies
    step: T,
    /// Start frequency
    base_freq: RadiansPerSecond<T>,
    /// Current data index
    index: T,
}

impl<T, U> IntoIter<T, U>
where
    T: Add<Output = T>
        + Atan2
        + Clone
        + Degree
        + From<f32>
        + Hypot
        + MulAdd<Output = T>
        + One
        + PartialOrd
        + Pow<T>
        + ToDecibel,
    U: Plotter<T>,
{
    /// Convert `Bode` into decibels and degrees
    pub fn into_db_deg(self) -> impl Iterator<Item = Data<T>> {
        self.map(|g| Data {
            magnitude: g.magnitude.to_db(),
            phase: g.phase.to_degrees(),
            ..g
        })
    }
}

/// Struct to hold the data returned by the Bode iterator
#[derive(Debug, PartialEq)]
pub struct Data<T> {
    /// Angular frequency (rad/s)
    angular_frequency: RadiansPerSecond<T>,
    /// Magnitude (absolute value or dB)
    magnitude: T,
    /// Phase (rad or degrees)
    phase: T,
}

impl<T> Data<T>
where
    T: Clone + Const + Div<Output = T>,
{
    /// Get the angular frequency
    pub fn angular_frequency(&self) -> RadiansPerSecond<T> {
        self.angular_frequency.clone()
    }

    /// Get the frequency
    pub fn frequency(&self) -> Hertz<T> {
        self.angular_frequency.clone().into()
    }

    /// Get the magnitude
    pub fn magnitude(&self) -> T {
        self.magnitude.clone()
    }

    /// Get the phase
    pub fn phase(&self) -> T {
        self.phase.clone()
    }
}

/// Implementation of the Iterator trait for `Bode` struct
impl<T, U> Iterator for IntoIter<T, U>
where
    T: Add<Output = T>
        + Atan2
        + Clone
        + From<f32>
        + Hypot
        + MulAdd<Output = T>
        + One
        + PartialOrd
        + Pow<T>,
    U: Plotter<T>,
{
    type Item = Data<T>;

    fn next(&mut self) -> Option<Self::Item> {
        if self.index > self.intervals {
            None
        } else {
            let freq_exponent = MulAdd::mul_add(
                self.step.clone(),
                self.index.clone(),
                self.base_freq.0.clone(),
            );
            // Casting is safe for both f32 and f64, representation is exact.
            let omega = T::from(10.0_f32).powf(freq_exponent);
            let g = self.tf.eval_point(omega.clone());
            self.index = self.index.clone() + T::one();
            Some(Data {
                angular_frequency: RadiansPerSecond(omega),
                magnitude: g.norm(),
                phase: g.arg(),
            })
        }
    }
}

#[cfg(test)]
mod tests {
    use crate::transfer_function::{continuous::Tf, discrete::Tfz};

    use super::*;

    #[test]
    fn create_iterator() {
        let tf = Tf::new([2., 3.], [1., 1., 1.]);
        let iter = Bode::new(tf, RadiansPerSecond(10.), RadiansPerSecond(1000.), 0.1).into_iter();
        assert_relative_eq!(20., iter.intervals);
        assert_eq!(RadiansPerSecond(1.), iter.base_freq);
        assert_relative_eq!(0., iter.index);
    }

    #[test]
    fn create_discrete() {
        let tf = Tfz::new([2., 3.], [1., 1., 1.]);
        let iter = Bode::new_discrete(tf, RadiansPerSecond(0.01), 0.1).into_iter();
        assert!(iter.last().unwrap().angular_frequency().0 < std::f32::consts::PI);
    }

    #[test]
    fn create_iterator_db_deg() {
        let tf = Tf::new([2., 3.], [1., 1., 1.]);
        let iter = Bode::new(tf, RadiansPerSecond(10.), RadiansPerSecond(1000.), 0.1).into_iter();
        let iter2 = iter.into_db_deg();
        let res = iter2.last().unwrap();
        assert_eq!(RadiansPerSecond(1000.), res.angular_frequency());
        assert_relative_eq!(-90.0, res.phase(), max_relative = 0.001);
    }

    #[test]
    fn data_struct() {
        let f = RadiansPerSecond(120.);
        let mag = 3.;
        let ph = std::f64::consts::PI;
        let p = Data {
            angular_frequency: f,
            magnitude: mag,
            phase: ph,
        };
        assert_eq!(f, p.angular_frequency());
        assert_relative_eq!(19.0986, p.frequency().0, max_relative = 0.00001);
        assert_relative_eq!(mag, p.magnitude());
        assert_relative_eq!(ph, p.phase());
    }

    #[test]
    fn iterator() {
        let tf = Tf::new([2., 3.], [1., 1., 1.]);
        let iter = Bode::new(tf, RadiansPerSecond(10.), RadiansPerSecond(1000.), 0.1).into_iter();
        // 20 steps -> 21 iteration
        assert_eq!(21, iter.count());
    }
}