embedded-dsp 0.2.2

use embedded_dsp::*;

// =========================================================================================
// 1. BASIC MATH & BITWISE TESTS
// =========================================================================================

#[test]
fn test_basic_math_float() {
    let a = [1.0f32, 2.0, -3.0, 4.0];
    let b = [5.0f32, 6.0, 7.0, 8.0];
    let mut out = [0.0f32; 4];

    add_f32(&a, &b, &mut out);
    assert_eq!(out, [6.0, 8.0, 4.0, 12.0]);

    sub_f32(&b, &a, &mut out);
    assert_eq!(out, [4.0, 4.0, 10.0, 4.0]);

    mult_f32(&a, &b, &mut out);
    assert_eq!(out, [5.0, 12.0, -21.0, 32.0]);

    negate_f32(&a, &mut out);
    assert_eq!(out, [-1.0, -2.0, 3.0, -4.0]);

    abs_f32(&a, &mut out);
    assert_eq!(out, [1.0, 2.0, 3.0, 4.0]);

    offset_f32(&a, 10.0, &mut out);
    assert_eq!(out, [11.0, 12.0, 7.0, 14.0]);

    scale_f32(&a, 2.0, &mut out);
    assert_eq!(out, [2.0, 4.0, -6.0, 8.0]);

    clip_f32(&a, 0.0, 3.0, &mut out);
    assert_eq!(out, [1.0, 2.0, 0.0, 3.0]);

    let dot = dot_prod_f32(&a, &b);
    assert_eq!(dot, 5.0 + 12.0 - 21.0 + 32.0);
}

#[test]
fn test_fixed_point_basic_math_saturating() {
    let a = [1000i16, 2000, 3000, 30000];
    let b = [5000i16, 6000, 7000, 10000];
    let mut out = [0i16; 4];

    add_q15(&a, &b, &mut out);
    assert_eq!(out[0], 6000);
    assert_eq!(out[3], 32767); // Saturating max i16

    let a_neg = [-30000i16];
    let b_neg = [-10000i16];
    let mut out_neg = [0i16; 1];
    add_q15(&a_neg, &b_neg, &mut out_neg);
    assert_eq!(out_neg[0], -32768); // Saturating min i16

    let a_q31 = [2000000000i32];
    let b_q31 = [1000000000i32];
    let mut out_q31 = [0i32; 1];
    add_q31(&a_q31, &b_q31, &mut out_q31);
    assert_eq!(out_q31[0], 2147483647); // Saturating max i32
}

#[test]
fn test_bitwise_operations() {
    let a = [0b1100u32, 0b1010];
    let b = [0b1010u32, 0b0110];
    let mut out = [0u32; 2];

    and_u32(&a, &b, &mut out);
    assert_eq!(out, [0b1000, 0b0010]);

    or_u32(&a, &b, &mut out);
    assert_eq!(out, [0b1110, 0b1110]);

    xor_u32(&a, &b, &mut out);
    assert_eq!(out, [0b0110, 0b1100]);

    not_u32(&a, &mut out);
    assert_eq!(out, [!0b1100u32, !0b1010u32]);
}

// =========================================================================================
// 2. COMPLEX MATH TESTS
// =========================================================================================

#[test]
fn test_complex_math_comprehensive() {
    let a = [1.0f32, 2.0, 3.0, 4.0];
    let b = [5.0f32, 6.0, 7.0, 8.0];
    let mut out = [0.0f32; 4];

    cmplx_add_f32(&a, &b, &mut out);
    assert_eq!(out, [6.0, 8.0, 10.0, 12.0]);

    cmplx_sub_f32(&b, &a, &mut out);
    assert_eq!(out, [4.0, 4.0, 4.0, 4.0]);

    cmplx_mult_cmplx_f32(&a, &b, &mut out);
    assert_eq!(out, [-7.0, 16.0, -11.0, 52.0]);

    cmplx_mult_real_f32(&a, &[2.0, 3.0], &mut out);
    assert_eq!(out, [2.0, 4.0, 9.0, 12.0]);

    let mut mag = [0.0f32; 2];
    cmplx_mag_f32(&a, &mut mag);
    assert!((mag[0] - (1.0f32 + 4.0).sqrt()).abs() < 1e-4);
    assert!((mag[1] - (9.0f32 + 16.0).sqrt()).abs() < 1e-4);

    let mut mag_sq = [0.0f32; 2];
    cmplx_mag_squared_f32(&a, &mut mag_sq);
    assert_eq!(mag_sq, [5.0, 25.0]);

    cmplx_conj_f32(&a, &mut out);
    assert_eq!(out, [1.0, -2.0, 3.0, -4.0]);

    let dot = cmplx_dot_prod_f32(&a, &b);
    assert_eq!(dot.real, -18.0);
    assert_eq!(dot.imag, 68.0);
}

// =========================================================================================
// 3. FAST MATH & TRIGONOMETRY TESTS
// =========================================================================================

#[test]
fn test_fast_trig_and_roots() {
    let pi = core::f32::consts::PI;

    assert!((sin_f32(0.0) - 0.0).abs() < 1e-5);
    assert!((sin_f32(pi / 2.0) - 1.0).abs() < 1e-4);
    assert!((cos_f32(0.0) - 1.0).abs() < 1e-5);
    assert!((cos_f32(pi) - (-1.0)).abs() < 1e-4);

    let mut s = 0.0f32;
    let mut c = 0.0f32;
    sin_cos_f32(45.0, &mut s, &mut c);
    assert!((s - 0.70710678).abs() < 1e-4);
    assert!((c - 0.70710678).abs() < 1e-4);

    let mut res = 0.0f32;
    assert_eq!(sqrt_f32(16.0, &mut res), Status::Success);
    assert_eq!(res, 4.0);

    let src = [4.0f32, 9.0, 16.0, 25.0];
    let mut dst = [0.0f32; 4];
    vsqrt_f32(&src, &mut dst);
    assert_eq!(dst, [2.0, 3.0, 4.0, 5.0]);

    assert!((log_f32(1.0) - 0.0).abs() < 1e-5);
    assert!((exp_f32(0.0) - 1.0).abs() < 1e-5);

    let mut atan_res = 0.0f32;
    assert_eq!(atan2_f32(1.0, 1.0, &mut atan_res), Status::Success);
    assert!((atan_res - (pi / 4.0)).abs() < 1e-4);

    let mut out_q31 = 0i32;
    assert_eq!(sqrt_q31(1073741824, &mut out_q31), Status::Success);
    assert!((out_q31 - 1518500249).abs() < 1000);
}

// =========================================================================================
// 4. FILTERING & CONVOLUTION TESTS
// =========================================================================================

#[test]
fn test_fir_filter_impulse_response() {
    let coeffs = [0.25f32, 0.5, 0.25];
    let mut state = [0.0f32; 3];
    let mut fir = FirInstanceF32::init(3, &coeffs, &mut state);

    let src = [1.0f32, 0.0, 0.0, 0.0];
    let mut dst = [0.0f32; 4];
    fir_f32(&mut fir, &src, &mut dst);

    assert_eq!(dst, [0.25, 0.5, 0.25, 0.0]);
}

#[test]
fn test_biquad_cascade_iir() {
    let coeffs = [1.0f32, 0.0, 0.0, 0.0, 0.0];
    let mut state = [0.0f32; 4];
    let mut iir = BiquadCascadeInstanceF32::init(1, &coeffs, &mut state);

    let src = [1.0f32, 2.0, 3.0, 4.0];
    let mut dst = [0.0f32; 4];
    biquad_cascade_df1_f32(&mut iir, &src, &mut dst);

    assert_eq!(dst, [1.0, 2.0, 3.0, 4.0]);
}

#[test]
fn test_lms_adaptive_filter() {
    let mut coeffs = [0.0f32; 2];
    let mut state = [0.0f32; 2];
    let mut lms = LmsInstanceF32::init(2, &mut coeffs, &mut state, 0.01);

    let src = [1.0f32, 2.0];
    let ref_sig = [1.0f32, 2.0];
    let mut out = [0.0f32; 2];
    let mut err = [0.0f32; 2];

    lms_f32(&mut lms, &src, &ref_sig, &mut out, &mut err);
    assert_eq!(out.len(), 2);
}

#[test]
fn test_convolution_and_correlation() {
    let a = [1.0f32, 2.0, 3.0];
    let b = [4.0f32, 5.0];
    let mut conv_out = [0.0f32; 4];

    conv_f32(&a, &b, &mut conv_out);
    assert_eq!(conv_out, [4.0, 13.0, 22.0, 15.0]);

    let mut corr_out = [0.0f32; 4];
    correlate_f32(&a, &b, &mut corr_out);
}

// =========================================================================================
// 5. TRANSFORMS & SPECTRAL ANALYSIS TESTS
// =========================================================================================

#[test]
fn test_cfft_and_ifft() {
    let mut data = [1.0f32, 0.0, 1.0f32, 0.0, 1.0f32, 0.0, 1.0f32, 0.0];
    cfft_f32(&mut data, 4, 0, 1);
    assert!((data[0] - 4.0).abs() < 1e-4);
    assert!((data[1] - 0.0).abs() < 1e-4);

    cfft_f32(&mut data, 4, 1, 1);
    assert!((data[0] - 1.0).abs() < 1e-4);
    assert!((data[2] - 1.0).abs() < 1e-4);
}

#[test]
fn test_rfft_and_dct4() {
    let src = [1.0f32, 2.0, 3.0, 4.0];
    let mut dst = [0.0f32; 4];

    rfft_f32(&src, &mut dst, 4, 0);
    dct4_f32(&src, &mut dst, 4);
}

// =========================================================================================
// 6. MATRIX OPERATIONS TESTS
// =========================================================================================

#[test]
fn test_matrix_operations_comprehensive() {
    let a_data = [1.0f32, 2.0, 3.0, 4.0];
    let b_data = [5.0f32, 6.0, 7.0, 8.0];
    let mut out_data = [0.0f32; 4];

    let mat_a = MatrixInstance::new(2, 2, &a_data);
    let mat_b = MatrixInstance::new(2, 2, &b_data);
    let mut mat_out = MatrixInstanceMut::new(2, 2, &mut out_data);

    assert_eq!(mat_add_f32(&mat_a, &mat_b, &mut mat_out), Status::Success);
    assert_eq!(mat_out.data, &[6.0, 8.0, 10.0, 12.0]);

    assert_eq!(mat_sub_f32(&mat_b, &mat_a, &mut mat_out), Status::Success);
    assert_eq!(mat_out.data, &[4.0, 4.0, 4.0, 4.0]);

    assert_eq!(mat_scale_f32(&mat_a, 2.0, &mut mat_out), Status::Success);
    assert_eq!(mat_out.data, &[2.0, 4.0, 6.0, 8.0]);

    assert_eq!(mat_mult_f32(&mat_a, &mat_b, &mut mat_out), Status::Success);
    assert_eq!(mat_out.data, &[19.0, 22.0, 43.0, 50.0]);

    assert_eq!(mat_trans_f32(&mat_a, &mut mat_out), Status::Success);
    assert_eq!(mat_out.data, &[1.0, 3.0, 2.0, 4.0]);
}

#[test]
fn test_matrix_inverse() {
    let a_data = [4.0f32, 7.0, 2.0, 6.0];
    let mut inv_data = [0.0f32; 4];

    let mat_a = MatrixInstance::new(2, 2, &a_data);
    let mut mat_inv = MatrixInstanceMut::new(2, 2, &mut inv_data);

    assert_eq!(mat_inverse_f32(&mat_a, &mut mat_inv), Status::Success);

    let mut res_data = [0.0f32; 4];
    let mut mat_res = MatrixInstanceMut::new(2, 2, &mut res_data);
    mat_mult_f32(
        &mat_a,
        &MatrixInstance::new(2, 2, mat_inv.data),
        &mut mat_res,
    );

    assert!((mat_res.data[0] - 1.0).abs() < 1e-4);
    assert!((mat_res.data[1] - 0.0).abs() < 1e-4);
    assert!((mat_res.data[2] - 0.0).abs() < 1e-4);
    assert!((mat_res.data[3] - 1.0).abs() < 1e-4);
}

// =========================================================================================
// 7. CONTROLLER & MOTOR TRANSFORMS TESTS
// =========================================================================================

#[test]
fn test_pid_and_clarke_park() {
    let mut pid = PidInstanceF32::new(1.0, 0.1, 0.01);
    let out1 = pid.process(10.0);
    assert!((out1 - 11.1).abs() < 1e-3);

    let mut alpha = 0.0f32;
    let mut beta = 0.0f32;
    clarke_f32(1.0, -0.5, &mut alpha, &mut beta);
    assert_eq!(alpha, 1.0);

    let mut d = 0.0f32;
    let mut q = 0.0f32;
    park_f32(alpha, beta, 0.0, &mut d, &mut q);
    assert_eq!(d, 1.0);
    assert_eq!(q, 0.0);
}

// =========================================================================================
// 8. STATISTICS & INFORMATION THEORY TESTS
// =========================================================================================

#[test]
fn test_statistics_comprehensive() {
    let data = [1.0f32, 2.0, 3.0, 4.0, 5.0];
    let mut m = 0.0f32;
    mean_f32(&data, &mut m);
    assert_eq!(m, 3.0);

    let mut v = 0.0f32;
    var_f32(&data, &mut v);
    assert_eq!(v, 2.5);

    let mut s = 0.0f32;
    std_f32(&data, &mut s);
    assert!((s - 1.5811388).abs() < 1e-4);

    let mut rms_val = 0.0f32;
    rms_f32(&data, &mut rms_val);
    assert!((rms_val - (55.0f32 / 5.0).sqrt()).abs() < 1e-4);

    let mut pwr_val = 0.0f32;
    power_f32(&data, &mut pwr_val);
    assert_eq!(pwr_val, 55.0);

    let mut min_val = 0.0f32;
    let mut min_idx = 0;
    min_f32(&data, &mut min_val, &mut min_idx);
    assert_eq!(min_val, 1.0);
    assert_eq!(min_idx, 0);

    let mut max_val = 0.0f32;
    let mut max_idx = 0;
    max_f32(&data, &mut max_val, &mut max_idx);
    assert_eq!(max_val, 5.0);
    assert_eq!(max_idx, 4);

    let prob = [0.5f32, 0.5];
    let ent = entropy_f32(&prob);
    assert!((ent - 0.693147).abs() < 1e-4);

    let kl = kullback_leibler_f32(&prob, &prob);
    assert!((kl - 0.0).abs() < 1e-5);

    let lse = logsumexp_f32(&[1.0, 2.0]);
    assert!((lse - (1.0f32.exp() + 2.0f32.exp()).ln()).abs() < 1e-4);
}

// =========================================================================================
// 9. SUPPORT & CONVERSIONS TESTS
// =========================================================================================

#[test]
fn test_conversions_and_sorting() {
    let src_q15 = [16384i16, -16384];
    let mut dst_f32 = [0.0f32; 2];
    q15_to_f32(&src_q15, &mut dst_f32);
    assert!((dst_f32[0] - 0.5).abs() < 1e-4);
    assert!((dst_f32[1] - (-0.5)).abs() < 1e-4);

    let mut dst_q15 = [0i16; 2];
    f32_to_q15(&dst_f32, &mut dst_q15);
    assert_eq!(dst_q15[0], 16384);
    assert_eq!(dst_q15[1], -16384);

    let src = [5.0f32, 1.0, 4.0, 2.0, 3.0];
    let mut dst = [0.0f32; 5];
    sort_f32(&src, &mut dst, true); // Ascending
    assert_eq!(dst, [1.0, 2.0, 3.0, 4.0, 5.0]);

    sort_f32(&src, &mut dst, false); // Descending
    assert_eq!(dst, [5.0, 4.0, 3.0, 2.0, 1.0]);
}

// =========================================================================================
// 10. INTERPOLATION TESTS
// =========================================================================================

#[test]
fn test_interpolation() {
    let y_table = [0.0f32, 10.0, 20.0, 30.0];
    let val = linear_interp_f32(&y_table, 1.5, 1.0);
    assert_eq!(val, 15.0);
}

// =========================================================================================
// 11. QUATERNIONS TESTS
// =========================================================================================

#[test]
fn test_quaternion_comprehensive() {
    let q = [1.0f32, 0.0, 0.0, 0.0];
    let norm = quaternion_norm_f32(&q);
    assert_eq!(norm, 1.0);

    let mut rot = [0.0f32; 9];
    quaternion_to_rotmat_f32(&q, &mut rot);
    assert_eq!(rot, [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]);

    let q1 = [1.0f32, 0.0, 0.0, 0.0];
    let q2 = [0.0f32, 1.0, 0.0, 0.0];
    let mut prod = [0.0f32; 4];
    quaternion_product_f32(&q1, &q2, &mut prod);
    assert_eq!(prod, [0.0, 1.0, 0.0, 0.0]);
}

// =========================================================================================
// 12. WINDOW FUNCTIONS TESTS
// =========================================================================================

#[test]
fn test_windows() {
    let mut w = [0.0f32; 5];
    hanning_f32(&mut w);
    assert_eq!(w[0], 0.0);
    assert_eq!(w[2], 1.0);
    assert_eq!(w[4], 0.0);

    hamming_f32(&mut w);
    assert!((w[0] - 0.08).abs() < 1e-4);
    assert!((w[2] - 1.0).abs() < 1e-4);

    let mut sig = [2.0f32; 5];
    apply_window_f32(&mut sig, &w);
    assert!((sig[2] - 2.0).abs() < 1e-4);
}

// =========================================================================================
// 13. DISTANCE METRICS TESTS
// =========================================================================================

#[test]
fn test_distance_metrics_exhaustive() {
    let a = [1.0f32, 2.0, 3.0];
    let b = [4.0f32, 5.0, 6.0];

    assert!((euclidean_distance_f32(&a, &b) - 5.196152).abs() < 1e-4);
    assert!((chebyshev_distance_f32(&a, &b) - 3.0).abs() < 1e-4);
    assert!((manhattan_distance_f32(&a, &b) - 9.0).abs() < 1e-4);
    assert!((minkowski_distance_f32(&a, &b, 1.0) - 9.0).abs() < 1e-4);
    assert!((canberra_distance_f32(&a, &b) - (3.0 / 5.0 + 3.0 / 7.0 + 3.0 / 9.0)).abs() < 1e-4);
}

// =========================================================================================
// 14. MACHINE LEARNING CLASSIFIERS TESTS
// =========================================================================================

// =========================================================================================
// 15. FILTER DESIGN TESTS
// =========================================================================================

#[test]
fn test_filter_design_coeffs() {
    let lp = biquad_lowpass_coeffs(1000.0, 48000.0, 0.7071);
    assert_eq!(lp.len(), 5);
    assert!(lp[0] > 0.0);

    let hp = biquad_highpass_coeffs(1000.0, 48000.0, 0.7071);
    assert_eq!(hp.len(), 5);

    let notch = biquad_notch_coeffs(60.0, 1000.0, 10.0);
    assert_eq!(notch.len(), 5);

    let mut butter = [0.0f32; 10];
    butterworth_lowpass_biquads(1000.0, 48000.0, 4, &mut butter);
    assert_ne!(butter[0], 0.0);
}

// =========================================================================================
// 17. RESAMPLING & MULTI-RATE TESTS
// =========================================================================================

#[test]
fn test_cic_and_resampling() {
    let mut decimator = CicDecimator::<3>::new(4);
    let mut decimated = None;
    for i in 1..=4 {
        decimated = decimator.process_sample(i * 10);
    }
    assert!(decimated.is_some());

    let mut interpolator = CicInterpolator::<2>::new(4);
    let mut out_buf = [0i32; 4];
    interpolator.process_sample(10, &mut out_buf);
    assert_eq!(out_buf.len(), 4);

    let src = [1.0f32, 2.0, 3.0, 4.0];
    let mut dst = [0.0f32; 8];
    resample_linear_f32(&src, &mut dst, 0.5);
    assert!((dst[0] - 1.0).abs() < 1e-4);
}

// =========================================================================================
// 18. KALMAN FILTERING TESTS
// =========================================================================================

#[test]
fn test_kalman_1d_and_2d() {
    let mut kf1d = KalmanFilter1D::new(0.0, 1.0, 0.01, 0.1);
    kf1d.predict(0.0);
    let est = kf1d.update(10.0);
    assert!(est > 0.0);

    let mut kf2d = KalmanFilter2D::new(0.0, 0.0, 0.01, 0.1);
    kf2d.predict(0.1);
    let state = kf2d.update(1.0);
    assert!(state[0] > 0.0);
}

#[test]
fn test_kalman_generic_1x1_smoother() {
    let mut kf = KalmanFilter::<1, 1>::from_variances([0.0], 1.0, 0.01, 0.1);
    let f = [[1.0f32]];
    let h = [[1.0f32]];
    kf.predict(&f);
    assert_eq!(kf.update(&h, &[10.0]), Status::Success);
    assert!(kf.x[0] > 0.0);
    assert!(kf.x[0] < 10.0);
}

#[test]
fn test_kalman_generic_2x1_constant_velocity() {
    let mut kf = KalmanFilter::<2, 1>::from_variances([0.0, 0.0], 1.0, 0.01, 0.1);
    let dt = 0.1f32;
    let f = [[1.0, dt], [0.0, 1.0]];
    let h = [[1.0, 0.0]];

    // True trajectory: position = t, velocity = 1
    for step in 1..=20 {
        kf.predict(&f);
        let z = [step as f32 * dt];
        assert_eq!(kf.update(&h, &z), Status::Success);
    }
    assert!((kf.x[0] - 2.0).abs() < 0.5);
    assert!((kf.x[1] - 1.0).abs() < 0.5);
}

/// Range-only measurement model: state = [x, y], z = sqrt(x² + y²).
#[derive(Debug, Clone, Copy)]
struct RangeOnlyModel;

impl EkfModel<2, 1> for RangeOnlyModel {
    fn f(&self, x: &[f32; 2], _dt: f32, out: &mut [f32; 2]) {
        *out = *x;
    }

    fn h(&self, x: &[f32; 2], out: &mut [f32; 1]) {
        out[0] = (x[0] * x[0] + x[1] * x[1]).sqrt();
    }

    fn jacobian_f(&self, _x: &[f32; 2], _dt: f32, out: &mut [[f32; 2]; 2]) {
        *out = [[1.0, 0.0], [0.0, 1.0]];
    }

    fn jacobian_h(&self, x: &[f32; 2], out: &mut [[f32; 2]; 1]) {
        let r = (x[0] * x[0] + x[1] * x[1]).sqrt().max(1e-6);
        out[0] = [x[0] / r, x[1] / r];
    }
}

#[test]
fn test_ekf_range_measurement() {
    let truth = [3.0f32, 4.0]; // range = 5
    let mut ekf = ExtendedKalmanFilter::<2, 1, _>::from_variances(
        [1.0, 1.0],
        1.0,
        0.001,
        0.05,
        RangeOnlyModel,
    );

    let initial_err = {
        let dx = ekf.x[0] - truth[0];
        let dy = ekf.x[1] - truth[1];
        (dx * dx + dy * dy).sqrt()
    };

    for _ in 0..40 {
        ekf.predict(0.0);
        let z = [(truth[0] * truth[0] + truth[1] * truth[1]).sqrt()];
        assert_eq!(ekf.update(&z), Status::Success);
    }

    let final_err = {
        let dx = ekf.x[0] - truth[0];
        let dy = ekf.x[1] - truth[1];
        (dx * dx + dy * dy).sqrt()
    };
    assert!(final_err < initial_err);
    assert!(((ekf.x[0] * ekf.x[0] + ekf.x[1] * ekf.x[1]).sqrt() - 5.0).abs() < 0.5);
}

/// Constant-acceleration model driven by a commanded acceleration `u = [accel]` that isn't
/// part of the state, and measured through a position sensor with a known offset `u = [bias]`
/// that also isn't part of the state. Exercises `EkfModel::f_with_input`/`h_with_input` and
/// `ExtendedKalmanFilter::predict_with_input`/`update_with_input`.
#[derive(Debug, Clone, Copy)]
struct ControlledPositionModel;

impl EkfModel<2, 1> for ControlledPositionModel {
    fn f(&self, x: &[f32; 2], dt: f32, out: &mut [f32; 2]) {
        out[0] = x[0] + dt * x[1];
        out[1] = x[1];
    }

    fn h(&self, x: &[f32; 2], out: &mut [f32; 1]) {
        out[0] = x[0];
    }

    fn jacobian_f(&self, _x: &[f32; 2], dt: f32, out: &mut [[f32; 2]; 2]) {
        *out = [[1.0, dt], [0.0, 1.0]];
    }

    fn jacobian_h(&self, _x: &[f32; 2], out: &mut [[f32; 2]; 1]) {
        *out = [[1.0, 0.0]];
    }

    fn f_with_input<const U: usize>(
        &self,
        x: &[f32; 2],
        u: &[f32; U],
        dt: f32,
        out: &mut [f32; 2],
    ) {
        let accel = u[0];
        out[0] = x[0] + dt * x[1] + 0.5 * dt * dt * accel;
        out[1] = x[1] + dt * accel;
    }

    // Jacobian w.r.t. x is unchanged by u: accel enters f affinely, so the default
    // `jacobian_f_with_input` (which defers to `jacobian_f`) is already exact here. Implemented
    // explicitly anyway so the test exercises the override path, not just the default.
    fn jacobian_f_with_input<const U: usize>(
        &self,
        x: &[f32; 2],
        _u: &[f32; U],
        dt: f32,
        out: &mut [[f32; 2]; 2],
    ) {
        self.jacobian_f(x, dt, out)
    }

    fn h_with_input<const U: usize>(&self, x: &[f32; 2], u: &[f32; U], out: &mut [f32; 1]) {
        out[0] = x[0] + u[0];
    }

    fn jacobian_h_with_input<const U: usize>(
        &self,
        x: &[f32; 2],
        _u: &[f32; U],
        out: &mut [[f32; 2]; 1],
    ) {
        self.jacobian_h(x, out)
    }
}

#[test]
fn test_ekf_predict_with_input_matches_manual_integration() {
    let mut ekf = ExtendedKalmanFilter::<2, 1, _>::from_variances(
        [0.0, 0.0],
        1.0,
        1e-4,
        0.01,
        ControlledPositionModel,
    );

    let accel = 2.0f32;
    let dt = 0.5f32;
    for _ in 0..10 {
        ekf.predict_with_input(dt, &[accel]);
    }

    let t = 10.0 * dt;
    let expected_velocity = accel * t;
    let expected_position = 0.5 * accel * t * t;
    assert!((ekf.x[1] - expected_velocity).abs() < 1e-3);
    assert!((ekf.x[0] - expected_position).abs() < 1e-2);
}

#[test]
fn test_ekf_update_with_input_compensates_known_bias() {
    let mut ekf = ExtendedKalmanFilter::<2, 1, _>::from_variances(
        [0.0, 0.0],
        4.0,
        0.0,
        0.01,
        ControlledPositionModel,
    );

    let true_position = 10.0f32;
    let sensor_bias = 3.0f32;
    // The raw sensor reading is offset by `sensor_bias`; feeding it through plain `update`
    // (which ignores the bias) would converge to the biased reading instead of the truth.
    let biased_reading = true_position + sensor_bias;

    for _ in 0..20 {
        assert_eq!(
            ekf.update_with_input(&[biased_reading], &[sensor_bias]),
            Status::Success
        );
    }

    assert!((ekf.x[0] - true_position).abs() < 0.5);
}

#[test]
fn test_kalman_update_singular_leaves_state() {
    let mut kf = KalmanFilter::<1, 1>::new([1.0], [[0.0]], [[0.0]], [[0.0]]);
    let x_before = kf.x;
    let p_before = kf.p;
    let status = kf.update(&[[1.0]], &[2.0]);
    assert_eq!(status, Status::Singular);
    assert_eq!(kf.x, x_before);
    assert_eq!(kf.p, p_before);
}

// =========================================================================================
// 19. CONST GENERICS TESTS
// =========================================================================================

#[test]
fn test_const_generics_wrappers() {
    let mut fir = FirFilter::<3>::new([0.25, 0.5, 0.25]);
    let mut dst = [0.0f32; 4];
    fir.process(&[1.0, 0.0, 0.0, 0.0], &mut dst);
    assert_eq!(dst, [0.25, 0.5, 0.25, 0.0]);

    let mut biquad = BiquadCascade::<5, 4>::new([1.0, 0.0, 0.0, 0.0, 0.0]);
    biquad.process(&[1.0, 2.0, 3.0, 4.0], &mut dst);
    assert_eq!(dst, [1.0, 2.0, 3.0, 4.0]);

    let m1 = Matrix::<2, 2, 4>::new([1.0, 2.0, 3.0, 4.0]);
    let m2 = Matrix::<2, 2, 4>::new([5.0, 6.0, 7.0, 8.0]);
    let m3 = m1.add(&m2);
    assert_eq!(m3.data, [6.0, 8.0, 10.0, 12.0]);

    let m_mul: Matrix<2, 2, 4> = m1.mul_mat(&m2);
    assert_eq!(m_mul.data, [19.0, 22.0, 43.0, 50.0]);
}