deimos 0.16.2

//! A second-order Butterworth low-pass filter

#[cfg(feature = "python")]
use pyo3::prelude::*;

use super::*;
use crate::{calc_config, calc_input_names, calc_output_names, py_json_methods};
use flaw::{
    SisoIirFilter, butter2,
    generated::butter::butter2::{MAX_CUTOFF_RATIO, MIN_CUTOFF_RATIO},
};

/// Single-input, single-output Butterworth low-pass filter implemented with `flaw::butter2`
#[cfg_attr(feature = "python", pyclass)]
#[derive(Default, Serialize, Deserialize)]
pub struct Butter2 {
    // User inputs
    input_name: String,
    cutoff_hz: f64,
    save_outputs: bool,

    // Values provided by calc orchestrator during init
    #[serde(skip)]
    input_index: usize,

    #[serde(skip)]
    output_index: usize,

    // Internal state
    #[serde(skip)]
    filt: SisoIirFilter<2>,

    #[serde(skip)]
    initialized: bool,
}

impl core::fmt::Debug for Butter2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        f.debug_struct("Butter")
            .field("input_name", &self.input_name)
            .field("cutoff_hz", &self.cutoff_hz)
            .field("save_outputs", &self.save_outputs)
            .finish()
    }
}

impl Butter2 {
    pub fn new(input_name: String, cutoff_hz: f64, save_outputs: bool) -> Box<Self> {
        let input_index = usize::MAX;
        let output_index = usize::MAX;

        Box::new(Self {
            input_name,
            cutoff_hz,
            save_outputs,
            input_index,
            output_index,
            filt: SisoIirFilter::default(),
            initialized: false,
        })
    }
}

py_json_methods!(
    Butter2,
    Calc,
    #[new]
    fn py_new(input_name: String, cutoff_hz: f64, save_outputs: bool) -> Self {
        *Self::new(input_name, cutoff_hz, save_outputs)
    }
);

#[typetag::serde]
impl Calc for Butter2 {
    fn init(
        &mut self,
        ctx: ControllerCtx,
        input_indices: Vec<usize>,
        output_range: Range<usize>,
    ) -> Result<(), String> {
        assert!(
            ctx.dt_ns > 0,
            "dt_ns value of {} provided. dt_ns must be > 0",
            ctx.dt_ns
        );

        self.input_index = input_indices[0];
        self.output_index = output_range.clone().next().unwrap();

        let sample_rate_hz = 1e9f64 / f64::from(ctx.dt_ns);
        let cutoff_ratio =
            (self.cutoff_hz / sample_rate_hz).clamp(MIN_CUTOFF_RATIO, MAX_CUTOFF_RATIO);

        let filter = butter2(cutoff_ratio).unwrap_or_else(|err| {
            panic!("Failed to construct butter2 filter for ratio {cutoff_ratio}: {err}")
        });

        self.filt = filter;
        self.initialized = false;
        Ok(())
    }

    fn terminate(&mut self) -> Result<(), String> {
        self.input_index = usize::MAX;
        self.output_index = usize::MAX;
        self.filt = SisoIirFilter::default();
        self.initialized = false;
        Ok(())
    }

    fn eval(&mut self, tape: &mut [f64]) -> Result<(), String> {
        let x = tape[self.input_index];
        let y = if branches::unlikely(!self.initialized) {
            // Pass through the first value to avoid excessive timing
            // on first cycle due to initialization
            self.filt.set_steady_state(x as f32);
            self.initialized = true;
            x
        } else {
            self.filt.update(x as f32) as f64
        };
        tape[self.output_index] = y;
        Ok(())
    }

    fn get_input_map(&self) -> BTreeMap<CalcInputName, FieldName> {
        let mut map = BTreeMap::new();
        map.insert("x".to_owned(), self.input_name.clone());
        map
    }

    fn update_input_map(&mut self, field: &str, source: &str) -> Result<(), String> {
        if field == "x" {
            self.input_name = source.to_owned();
            Ok(())
        } else {
            Err(format!("Unrecognized field {field}"))
        }
    }

    calc_config!(cutoff_hz);
    calc_input_names!(x);
    calc_output_names!(y);

    // FUTURE: passthrough — a filtered voltage is still a voltage. Resolving to the input
    // channel's unit requires `CalcOrchestrator` to pass channel units into `init`.
    fn get_output_units(&self) -> Vec<Option<String>> {
        vec![None]
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::controller::context::ControllerCtx;

    /// Running `terminate()` then `init()` must reset `Butter2` state to the same baseline,
    /// so that two back-to-back sessions fed the same input sequence produce identical output.
    #[test]
    fn butter2_state_resets_across_terminate_init() {
        let ctx = ControllerCtx {
            dt_ns: 50_000_000, // 20 Hz sample rate
            ..Default::default()
        };

        let mut calc = Butter2::new("ignored".to_owned(), 5.0, true);

        // Input sits at tape[0], output at tape[1].
        let inputs: [f64; 8] = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0];
        let mut tape = [0.0f64; 2];

        let mut run = || -> Vec<f64> {
            calc.init(ctx.clone(), vec![0], 1..2).unwrap();
            let mut out = Vec::with_capacity(inputs.len());
            for &x in &inputs {
                tape[0] = x;
                calc.eval(&mut tape).unwrap();
                out.push(tape[1]);
            }
            calc.terminate().unwrap();
            out
        };

        let run1 = run();
        let run2 = run();

        assert_eq!(
            run1, run2,
            "Butter2 output must match bit-for-bit across terminate+init; \
             run1={run1:?} run2={run2:?}"
        );

        // Sanity check: the filter must actually do work (beyond just passing through the
        // first sample), otherwise the equality above is trivial.
        assert!(
            run1.iter().any(|&y| y != inputs[0]),
            "Butter2 output never deviated from the first input — filter appears inert"
        );
    }
}