カメラで姿を映さずに、そっと見守る技術：
福祉工学と非接触空間センシング（IPA未踏から続く原点）

#![no_std]
#![forbid(unsafe_code)]

/// Direct Form II Transposed 2nd-Order Biquad IIR Filter State.
/// Minimizes floating-point roundoff noise near the unit circle (Q = 30.0).
#[derive(Clone, Copy, Debug)]
pub struct BiquadNotchFilter {
    pub b0: f32,
    pub b1: f32,
    pub b2: f32,
    pub a1: f32,
    pub a2: f32,
    z1: f32,
    z2: f32,
}

impl BiquadNotchFilter {
    /// Pre-computed exact coefficients for f_s = 1000.0 Hz, f_0 = 60.0 Hz (West Japan / Osaka), Q = 30.0:
    /// omega_0 = 2 * pi * 60 / 1000 = 0.3769911184 rad
    /// sin(omega_0) = 0.3681245527, cos(omega_0) = 0.9297764859
    /// alpha = sin(omega_0) / (2 * 30.0) = 0.0061354092
    pub const fn new_60hz_fs1000_q30() -> Self {
        let inv_a0 = 1.0f32 / 1.0061354092f32;
        let b0 = 1.0f32 * inv_a0;
        let b1 = (-2.0f32 * 0.9297764859f32) * inv_a0;
        let b2 = 1.0f32 * inv_a0;
        let a1 = b1;
        let a2 = (1.0f32 - 0.0061354092f32) * inv_a0;
        Self { b0, b1, b2, a1, a2, z1: 0.0, z2: 0.0 }
    }

    /// Processes a single sEMG sample x[n] in O(1) time via Direct Form II Transposed:
    /// y[n]  = b0 * x[n] + z1[n-1]
    /// z1[n] = b1 * x[n] - a1 * y[n] + z2[n-1]
    /// z2[n] = b2 * x[n] - a2 * y[n]
    #[inline]
    pub fn process_sample(&mut self, x: f32) -> f32 {
        let y = self.b0 * x + self.z1;
        self.z1 = self.b1 * x - self.a1 * y + self.z2;
        self.z2 = self.b2 * x - self.a2 * y;
        y
    }
}

#[derive(Clone, Copy, Debug)]
pub struct SemgHudginsFeatures {
    pub mean_abs_value: f32,
    pub waveform_length: f32,
    pub zero_crossings: u16,
    pub slope_sign_changes: u16,
}

/// Extracts Hudgins time-domain features over a 256-sample filtered sEMG window (256ms @ 1kHz).
pub fn extract_hudgins_features_256(frame: &[f32; 256], deadzone_mv: f32) -> SemgHudginsFeatures {
    let mut mav_sum = 0.0f32;
    let mut wl_sum = 0.0f32;
    let mut zc: u16 = 0;
    let mut ssc: u16 = 0;

    let mut i = 0usize;
    while i < 256 {
        let x = frame[i];
        mav_sum += if x >= 0.0 { x } else { -x };
        if i >= 1 {
            let prev = frame[i - 1];
            let diff = x - prev;
            wl_sum += if diff >= 0.0 { diff } else { -diff };
            // Zero Crossing with noise deadzone threshold
            if ((x > 0.0 && prev < 0.0) || (x < 0.0 && prev > 0.0))
                && (if diff >= 0.0 { diff } else { -diff }) >= deadzone_mv
            {
                zc += 1;
            }
        }
        if i >= 2 {
            let prev = frame[i - 1];
            let prev2 = frame[i - 2];
            let d1 = prev - prev2;
            let d2 = prev - x;
            if (d1 * d2) >= (deadzone_mv * deadzone_mv) {
                ssc += 1;
            }
        }
        i += 1;
    }

    SemgHudginsFeatures {
        mean_abs_value: mav_sum / 256.0,
        waveform_length: wl_sum,
        zero_crossings: zc,
        slope_sign_changes: ssc,
    }
}