use crate::FilterToDifferentiator;
use interface::{Data, Read, Update, Write, UID};
#[derive(Default)]
pub struct Differentiator(f64, f64);
impl Update for Differentiator {}
impl Read<FilterToDifferentiator> for Differentiator {
fn read(&mut self, data: Data<FilterToDifferentiator>) {
self.0 = *data;
}
}
impl Read<IntegratorToDifferentiator> for Differentiator {
fn read(&mut self, data: Data<IntegratorToDifferentiator>) {
self.1 = *data;
}
}
#[derive(UID)]
#[uid(data = f64)]
pub enum DifferentiatorToIntegrator {}
impl Write<DifferentiatorToIntegrator> for Differentiator {
fn write(&mut self) -> Option<Data<DifferentiatorToIntegrator>> {
Some(Data::new(self.0 - self.1))
}
}
#[derive(Default)]
pub struct Integrator {
gain: f64,
mem: Vec<f64>,
}
impl Integrator {
pub fn new(gain: f64, n_data: usize) -> Self {
Self {
gain,
mem: vec![0f64; n_data],
}
}
pub fn last(&self) -> Option<Vec<f64>> {
Some(self.mem.clone())
}
}
impl Update for Integrator {}
impl Read<DifferentiatorToIntegrator> for Integrator {
fn read(&mut self, data: Data<DifferentiatorToIntegrator>) {
self.mem[0] += *data * self.gain;
}
}
#[derive(UID)]
#[uid(data = f64)]
pub enum IntegratorToDifferentiator {}
impl Write<IntegratorToDifferentiator> for Integrator {
fn write(&mut self) -> Option<Data<IntegratorToDifferentiator>> {
self.last().map(|x| Data::new(x[0]))
}
}