use std::{
fmt::Debug,
marker::PhantomData,
ops::{Add, Div, Mul, Sub},
};
use ndarray::Array1;
use crate::{
enums::{Discrete, Discretization},
linear_system::{continuous::Ss, Equilibrium, SsGen},
matrix::{identity, inverse, lup, MatrixMul, MatrixMulSca, ScalarMul},
Abs, One, Zero,
};
pub type Ssd<T> = SsGen<T, Discrete>;
impl<T> Ssd<T>
where
T: Abs
+ Add<Output = T>
+ Clone
+ Div<Output = T>
+ Mul<Output = T>
+ One
+ PartialOrd
+ Sub<Output = T>
+ Zero,
{
#[must_use]
pub fn equilibrium(&self, u: &[T]) -> Option<Equilibrium<T>> {
if u.len() != self.dim.inputs() {
eprintln!("Wrong number of inputs.");
return None;
}
let bu = self.b.mmul(u);
let lu = lup(identity(self.a.nrows()) - &self.a);
let x = lu?.solve(&bu);
let y = self.c.mmul(&x) + self.d.mmul(u);
Some(Equilibrium::new(
x.iter().cloned().collect(),
y.iter().cloned().collect(),
))
}
}
impl<T> Ssd<T>
where
T: Clone,
{
pub fn evolution_fn<F>(&self, steps: usize, input: F, x0: &[T]) -> EvolutionFn<T, F>
where
F: Fn(usize) -> Vec<T>,
{
let state = Array1::from(x0.to_vec());
let next_state = Array1::from(x0.to_vec());
EvolutionFn {
sys: self,
time: 0,
steps,
input,
state,
next_state,
}
}
pub fn evolution_iter<I, II>(&self, iter: II, x0: &[T]) -> EvolutionIter<T, I>
where
II: IntoIterator<Item = Vec<T>, IntoIter = I>,
I: Iterator<Item = Vec<T>>,
{
let state = Array1::from(x0.to_vec());
let next_state = Array1::from(x0.to_vec());
EvolutionIter {
sys: self,
state,
next_state,
iter: iter.into_iter(),
}
}
}
impl Ssd<f64> {
#[must_use]
pub fn is_stable(&self) -> bool {
self.poles().iter().all(|p| p.norm() < f64::one())
}
}
impl<T> Ss<T>
where
T: Abs
+ Add<Output = T>
+ Clone
+ Div<Output = T>
+ From<f32>
+ Mul<Output = T>
+ One
+ PartialOrd
+ Sub<Output = T>
+ Zero,
{
#[must_use]
pub fn discretize(&self, st: T, method: Discretization) -> Option<Ssd<T>> {
match method {
Discretization::ForwardEuler => Some(self.forward_euler(st)),
Discretization::BackwardEuler => self.backward_euler(st),
Discretization::Tustin => self.tustin(st),
}
}
fn forward_euler(&self, st: T) -> Ssd<T> {
let identity = identity(self.dim.states);
Ssd {
a: identity + (&self.a).smul(st.clone()),
b: (&self.b).smul(st),
c: self.c.clone(),
d: self.d.clone(),
dim: self.dim,
time: PhantomData,
}
}
fn backward_euler(&self, st: T) -> Option<Ssd<T>> {
let identity = identity(self.dim.states);
let a = inverse(identity - (&self.a).smul(st.clone()))?;
let a_b_st = a.mmul_scale(&self.b, st);
Some(Ssd {
d: self.c.mmul(&a_b_st) + &self.d,
b: a_b_st,
c: self.c.mmul(&a),
a,
dim: self.dim,
time: PhantomData,
})
}
fn tustin(&self, st: T) -> Option<Ssd<T>> {
let identity = identity(self.dim.states);
let n_05 = T::from(0.5_f32);
let a_05_st = (&self.a).smul(n_05.clone() * st.clone());
let k = inverse(&identity - &a_05_st)?;
let b = k.mmul_scale(&self.b, st);
Some(Ssd {
a: k.mmul(&(identity + a_05_st)),
c: self.c.mmul(&k),
d: self.c.mmul_scale(&b, n_05) + &self.d,
b,
dim: self.dim,
time: PhantomData,
})
}
}
#[derive(Debug)]
pub struct EvolutionFn<'a, T, F>
where
F: Fn(usize) -> Vec<T>,
{
sys: &'a Ssd<T>,
time: usize,
steps: usize,
input: F,
state: Array1<T>,
next_state: Array1<T>,
}
impl<'a, T, F> Iterator for EvolutionFn<'a, T, F>
where
T: Add<Output = T> + Clone + Mul<Output = T> + Zero,
F: Fn(usize) -> Vec<T>,
{
type Item = TimeEvolution<T>;
fn next(&mut self) -> Option<Self::Item> {
if self.time > self.steps {
None
} else {
let current_time = self.time;
let u = Array1::from((self.input)(current_time));
std::mem::swap(&mut self.state, &mut self.next_state);
self.next_state = self.sys.a.mmul(&self.state) + self.sys.b.mmul(&u);
let output = self.sys.c.mmul(&self.state) + self.sys.d.mmul(u);
self.time += 1;
Some(TimeEvolution {
time: current_time,
state: self.state.iter().cloned().collect(),
output: output.iter().cloned().collect(),
})
}
}
}
#[derive(Debug)]
pub struct EvolutionIter<'a, T, I>
where
I: Iterator<Item = Vec<T>>,
{
sys: &'a Ssd<T>,
state: Array1<T>,
next_state: Array1<T>,
iter: I,
}
impl<'a, T, I> Iterator for EvolutionIter<'a, T, I>
where
T: Add<Output = T> + Clone + Mul<Output = T> + Zero,
I: Iterator<Item = Vec<T>>,
{
type Item = Vec<T>;
fn next(&mut self) -> Option<Self::Item> {
let u_vec = self.iter.next()?;
let u = Array1::from(u_vec);
std::mem::swap(&mut self.state, &mut self.next_state);
self.next_state = self.sys.a.mmul(&self.state) + self.sys.b.mmul(&u);
let output = self.sys.c.mmul(&self.state) + self.sys.d.mmul(u);
Some(output.iter().cloned().collect())
}
}
#[derive(Debug)]
pub struct TimeEvolution<T> {
time: usize,
state: Vec<T>,
output: Vec<T>,
}
impl<T> TimeEvolution<T> {
#[must_use]
pub fn time(&self) -> usize {
self.time
}
#[must_use]
pub fn state(&self) -> &Vec<T> {
&self.state
}
#[must_use]
pub fn output(&self) -> &Vec<T> {
&self.output
}
}
#[cfg(test)]
mod tests {
use super::*;
#[allow(clippy::many_single_char_names)]
#[test]
fn equilibrium() {
let a = &[0., 0.8, 0.4, 1., 0., 0., 0., 1., 0.7];
let b = &[0., 1., 0., 0., -1., 0.];
let c = &[1., 1.8, 1.1];
let d = &[-1., 1.];
let u = &[170., 0.];
let sys = Ssd::new_from_slice(3, 2, 1, a, b, c, d);
let eq = sys.equilibrium(u).unwrap();
dbg!(&eq);
assert_relative_eq!(200.0, eq.x()[0]);
assert_relative_eq!(200.0, eq.x()[1]);
assert_relative_eq!(100.0, eq.x()[2], max_relative = 1e-10);
assert_relative_eq!(500.0, eq.y()[0]);
assert!(sys.equilibrium(&[0., 0., 0.]).is_none());
}
#[test]
fn stability() {
let a = &[0., 0.8, 0.4, 1., 0., 0., 0., 1., 0.7];
let b = &[0., 1., 0., 0., -1., 0.];
let c = &[1., 1.8, 1.1];
let d = &[-1., 1.];
let sys = Ssd::new_from_slice(3, 2, 1, a, b, c, d);
assert!(!sys.is_stable());
}
#[test]
fn time_evolution() {
let disc_sys =
Ssd::new_from_slice(2, 1, 1, &[0.6, 0., 0., 0.4], &[1., 5.], &[1., 3.], &[0.]);
let impulse = |t| if t == 0 { vec![1.] } else { vec![0.] };
let evo = disc_sys.evolution_fn(20, impulse, &[0., 0.]);
let last = evo.last().unwrap();
assert_eq!(20, last.time());
assert_abs_diff_eq!(0., last.state()[1], epsilon = 0.001);
assert_abs_diff_eq!(0., last.output()[0], epsilon = 0.001);
}
#[test]
fn time_evolution_iter() {
use std::iter;
let disc_sys =
Ssd::new_from_slice(2, 1, 1, &[0.6, 0., 0., 0.4], &[1., 5.], &[1., 3.], &[0.]);
let impulse = iter::once(vec![1.]).chain(iter::repeat(vec![0.])).take(20);
let evo = disc_sys.evolution_iter(impulse, &[0., 0.]);
let last = evo.last().unwrap();
assert!(last[0] < 0.001);
}
#[test]
fn discretization_tustin() {
let sys = Ss::new_from_slice(2, 1, 1, &[-3., 0., -4., -4.], &[0., 1.], &[1., 1.], &[0.]);
let disc_sys = sys.discretize(0.1, Discretization::Tustin).unwrap();
let evo = disc_sys.evolution_fn(20, |_| vec![1.], &[0., 0.]);
let last = evo.last().unwrap();
assert_relative_eq!(0.25, last.state()[1], max_relative = 0.01);
}
#[test]
fn discretization_tustin_fail() {
let sys = Ss::new_from_slice(2, 1, 1, &[-3., 5., 4., -4.], &[0., 1.], &[1., 1.], &[0.]);
let disc_sys = sys.discretize(2., Discretization::Tustin);
assert!(disc_sys.is_none());
}
#[test]
fn discretization_euler_backward() {
let sys = Ss::new_from_slice(2, 1, 1, &[-3., 0., -4., -4.], &[0., 1.], &[1., 1.], &[0.]);
let disc_sys = sys.discretize(0.1, Discretization::BackwardEuler).unwrap();
let evo = disc_sys.evolution_fn(50, |_| vec![1.], &[0., 0.]);
let last = evo.last().unwrap();
assert_relative_eq!(0.25, last.state()[1], max_relative = 0.01);
}
#[test]
fn discretization_euler_backward_fail() {
let sys = Ss::new_from_slice(2, 1, 1, &[-3., 5., 4., -4.], &[0., 1.], &[1., 1.], &[0.]);
let disc_sys = sys.discretize(1., Discretization::BackwardEuler);
assert!(disc_sys.is_none());
}
#[test]
fn discretization_euler_forward() {
let sys = Ss::new_from_slice(2, 1, 1, &[-3., 0., -4., -4.], &[0., 1.], &[1., 1.], &[0.]);
let disc_sys = sys.discretize(0.1, Discretization::ForwardEuler).unwrap();
let evo = disc_sys.evolution_fn(20, |_| vec![1.], &[0., 0.]);
let last = evo.last().unwrap();
assert_relative_eq!(0.25, last.state()[1], max_relative = 0.01);
}
}