embedded-dsp 0.3.0

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 - core::f32::consts::FRAC_1_SQRT_2).abs() < 1e-4);
    assert!((c - core::f32::consts::FRAC_1_SQRT_2).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 - core::f32::consts::LN_2).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, core::f32::consts::FRAC_1_SQRT_2);
    assert_eq!(lp.len(), 5);
    assert!(lp[0] > 0.0);

    let hp = biquad_highpass_coeffs(1000.0, 48000.0, core::f32::consts::FRAC_1_SQRT_2);
    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]);
}

// =========================================================================================
// 20. NON-LINEAR & CONDITIONAL MEDIAN FILTER TESTS
// =========================================================================================

#[test]
fn test_median_filter_1d_conditional() {
    // Signal with an impulse spike at index 3
    let src = [1.0f32, 1.1, 1.0, 100.0, 1.2, 1.1, 1.0];
    let mut dst = [0.0f32; 7];

    // Standard median (threshold = 0.0) -> replaces spike with median ~1.1
    let status = median_filter_1d_f32(&src, &mut dst, 3, 0.0);
    assert_eq!(status, Status::Success);
    assert!((dst[3] - 1.1).abs() < 0.15);

    // Conditional median (threshold = 5.0) -> spike (|100 - 1.1| > 5.0) is filtered, normal small variations preserved
    let mut dst_cond = [0.0f32; 7];
    let status_cond = median_filter_1d_f32(&src, &mut dst_cond, 3, 5.0);
    assert_eq!(status_cond, Status::Success);
    assert!((dst_cond[3] - 1.1).abs() < 0.15);
    assert_eq!(dst_cond[0], src[0]); // Small noise preserved

    // Q15 conditional median
    let src_q15 = [1000i16, 1100, 1050, 30000, 1150, 1100, 1000];
    let mut dst_q15 = [0i16; 7];
    let status_q15 = median_filter_1d_q15(&src_q15, &mut dst_q15, 3, 5000);
    assert_eq!(status_q15, Status::Success);
    assert!(dst_q15[3] < 2000);

    // Q31 conditional median
    let src_q31 = [
        100000i32, 110000, 105000, 2000000000, 115000, 110000, 100000,
    ];
    let mut dst_q31 = [0i32; 7];
    let status_q31 = median_filter_1d_q31(&src_q31, &mut dst_q31, 3, 1000000);
    assert_eq!(status_q31, Status::Success);
    assert!(dst_q31[3] < 200000);
}

// =========================================================================================
// 21. FAST CONVOLUTION & SPECTRAL INTERPOLATION TESTS
// =========================================================================================

#[test]
fn test_fast_convolution_and_spectral_interpolation() {
    let sig = [1.0f32, 2.0, 3.0, 4.0];
    let ker = [0.5f32, 0.5];
    let mut dst_fast = [0.0f32; 5];
    let mut dst_direct = [0.0f32; 5];

    conv_f32(&sig, &ker, &mut dst_direct);
    let status = fast_convolve_f32(&sig, &ker, &mut dst_fast);
    assert_eq!(status, Status::Success);

    for i in 0..5 {
        assert!((dst_fast[i] - dst_direct[i]).abs() < 1e-4);
    }

    // Spectral 2:1 Sinc Interpolation of a sine wave
    let mut sine_16 = [0.0f32; 16];
    for i in 0..16 {
        sine_16[i] = (2.0 * core::f32::consts::PI * (i as f32) / 16.0).sin();
    }
    let mut interp_32 = [0.0f32; 32];
    let status_interp = spectral_interpolate_2x_f32(&sine_16, &mut interp_32);
    assert_eq!(status_interp, Status::Success);

    // Check that original points are preserved at even indices
    for i in 0..16 {
        assert!((interp_32[2 * i] - sine_16[i]).abs() < 1e-3);
    }
}

// =========================================================================================
// 22. BILINEAR TRANSFORM & PRE-WARPING TESTS
// =========================================================================================

#[test]
fn test_bilinear_transform_and_prewarping() {
    let fc = 1000.0f32;
    let fs = 48000.0f32;
    let wp = prewarp_cutoff_f32(fc, fs);
    assert!(wp > 2.0 * core::f32::consts::PI * fc); // Tan(x) > x for x in (0, pi/2)

    // Standard 2nd order analog low-pass prototype: H(s) = 1 / ( (s/wp)^2 + sqrt(2)*(s/wp) + 1 )
    let q = core::f32::consts::FRAC_1_SQRT_2;
    let a0 = wp * wp;
    let a1 = 0.0f32;
    let a2 = 0.0f32;
    let b0 = wp * wp;
    let b1 = wp / q;
    let b2 = 1.0f32;

    let biquad_coeffs = bilinear_transform_biquad(a0, a1, a2, b0, b1, b2, fs);
    assert!(biquad_coeffs[0] > 0.0); // b0
    assert!(biquad_coeffs[1] > 0.0); // b1
    assert!(biquad_coeffs[2] > 0.0); // b2
}

// =========================================================================================
// 23. WEIGHTED POLYNOMIAL LEAST SQUARES REGRESSION TESTS
// =========================================================================================

#[test]
fn test_polynomial_least_squares_fit() {
    // True line: y = 3.0 + 2.0 * x
    let x = [0.0f32, 1.0, 2.0, 3.0, 4.0];
    let y = [3.0f32, 5.0, 7.0, 9.0, 11.0];
    let mut coeffs = [0.0f32; 2];

    let status = polynomial_least_squares_fit(&x, &y, None, 1, &mut coeffs);
    assert_eq!(status, Status::Success);
    assert!((coeffs[0] - 3.0).abs() < 1e-4);
    assert!((coeffs[1] - 2.0).abs() < 1e-4);

    let val = polynomial_eval_f32(&coeffs, 2.5);
    assert!((val - 8.0).abs() < 1e-4);

    // Quadratic fit: y = 1.0 - 0.5 * x + 2.0 * x^2
    let mut y_quad = [0.0f32; 5];
    for i in 0..5 {
        y_quad[i] = 1.0 - 0.5 * x[i] + 2.0 * x[i] * x[i];
    }
    let mut quad_coeffs = [0.0f32; 3];
    let status_quad = polynomial_least_squares_fit(&x, &y_quad, None, 2, &mut quad_coeffs);
    assert_eq!(status_quad, Status::Success);
    assert!((quad_coeffs[0] - 1.0).abs() < 1e-3);
    assert!((quad_coeffs[1] - (-0.5)).abs() < 1e-3);
    assert!((quad_coeffs[2] - 2.0).abs() < 1e-3);
}

// =========================================================================================
// 24. 2D SPATIAL & IMAGE PROCESSING TESTS
// =========================================================================================

#[test]
fn test_spatial_and_image_processing() {
    // 4x4 matrix
    let src = [
        1.0f32, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0, 16.0,
    ];
    let mut dct_out = [0.0f32; 16];
    let mut idct_out = [0.0f32; 16];

    let status_dct = dct2d_f32(&src, &mut dct_out, 4, 4);
    assert_eq!(status_dct, Status::Success);

    let status_idct = idct2d_f32(&dct_out, &mut idct_out, 4, 4);
    assert_eq!(status_idct, Status::Success);

    // Verify reconstruction through 2D IDCT(2D DCT)
    for i in 0..16 {
        assert!((idct_out[i] - src[i]).abs() < 1e-3);
    }

    // 2D Convolution with 3x3 Box Blur
    let kernel_box = [1.0f32, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0];
    let mut conv_out = [0.0f32; 16];
    let status_conv = convolve2d_f32(&src, &mut conv_out, 4, 4, &kernel_box, 3, 3, true);
    assert_eq!(status_conv, Status::Success);
    assert!(conv_out[5] > 0.0);

    // 2D Non-linear Filter (Median, Min, Max)
    let mut nonlin_out = [0.0f32; 16];
    let status_nonlin =
        nonlin2d_filter_f32(&src, &mut nonlin_out, 4, 4, 3, NonlinFilterType::Median);
    assert_eq!(status_nonlin, Status::Success);
    assert!(nonlin_out[5] > 0.0);

    // Sobel Edge Detection
    let mut edges = [0.0f32; 16];
    let status_sobel = sobel_edge_detection_f32(&src, &mut edges, 4, 4, 2.0);
    assert_eq!(status_sobel, Status::Success);

    // 2D Histogram
    let mut bins = [0usize; 4];
    let status_hist = histogram_2d_f32(&src, &mut bins, 1.0, 16.0);
    assert_eq!(status_hist, Status::Success);
    let total_hist: usize = bins.iter().sum();
    assert_eq!(total_hist, 16);

    // MSE and PSNR
    let mse = mse_2d_f32(&src, &idct_out);
    assert!(mse < 1e-4);
    let psnr = psnr_2d_f32(&src, &idct_out, 16.0);
    assert!(psnr > 40.0);
}

// =========================================================================================
// 25. SPECTRAL ESTIMATION & WELCH PSD TESTS
// =========================================================================================

#[test]
fn test_welch_psd_and_periodogram() {
    let fs = 1000.0f32;
    let freq = 100.0f32;
    let n_samples = 256;
    let mut signal = [0.0f32; 256];
    for i in 0..n_samples {
        signal[i] = (2.0 * core::f32::consts::PI * freq * (i as f32) / fs).sin();
    }

    let fft_len = 64;
    let mut psd_linear = [0.0f32; 32];
    let status = welch_psd_f32(
        &signal,
        &mut psd_linear,
        fft_len,
        32,
        fs,
        WelchWindow::Hamming,
        false,
    );
    assert_eq!(status, Status::Success);

    // Frequency bin spacing: fs / fft_len = 1000 / 64 = 15.625 Hz
    // Peak expected around index 100 / 15.625 ~ 6 or 7
    let mut max_idx = 0;
    let mut max_val = 0.0f32;
    for (idx, &val) in psd_linear.iter().enumerate() {
        if val > max_val {
            max_val = val;
            max_idx = idx;
        }
    }
    assert!(max_idx == 6 || max_idx == 7);

    // Periodogram in dB
    let mut psd_db = [0.0f32; 32];
    let status_db = periodogram_f32(
        &signal[..64],
        &mut psd_db,
        fft_len,
        fs,
        WelchWindow::Hanning,
        true,
    );
    assert_eq!(status_db, Status::Success);
    assert!(psd_db[max_idx] > psd_db[0]);
}

// =========================================================================================
// 26. NOISE GENERATION & 4-TERM BLACKMAN-HARRIS TESTS
// =========================================================================================

#[test]
fn test_noise_generation_and_blackman_harris() {
    let mut win_bh = [0.0f32; 64];
    blackman_harris_f32(&mut win_bh);
    assert!((win_bh[0] - 0.00006).abs() < 0.01);
    assert!((win_bh[32] - 1.0).abs() < 0.05);

    let mut seed = 123456789u64;
    let mut uniform_buf = [0.0f32; 100];
    uniform_noise_f32(&mut uniform_buf, -1.0, 1.0, &mut seed);
    for &v in &uniform_buf {
        assert!((-1.0..=1.0).contains(&v));
    }

    let mut gaus_buf = [0.0f32; 200];
    gaussian_noise_f32(&mut gaus_buf, 0.0, 1.0, &mut seed);
    let mut mean = 0.0f32;
    mean_f32(&gaus_buf, &mut mean);
    assert!(mean.abs() < 0.3); // Sample mean close to 0
}

// =========================================================================================
// 27. CIRCULAR BUFFER & DELAY LINE TESTS
// =========================================================================================

#[test]
fn test_circular_buffer_operations() {
    let mut cb = CircularBuffer::<f32, 4>::new(0.0);
    assert!(cb.is_empty());
    assert!(!cb.is_full());
    assert_eq!(cb.len(), 0);

    cb.push(10.0);
    assert_eq!(cb.len(), 1);
    assert_eq!(cb.latest(), Some(10.0));
    assert_eq!(cb.get(0), Some(10.0));
    assert_eq!(cb.get(1), None);

    cb.push(20.0);
    cb.push(30.0);
    cb.push(40.0);
    assert!(cb.is_full());
    assert_eq!(cb.len(), 4);
    assert_eq!(cb.latest(), Some(40.0));
    assert_eq!(cb.oldest(), Some(10.0));
    assert_eq!(cb.get(0), Some(40.0)); // x[n]
    assert_eq!(cb.get(1), Some(30.0)); // x[n-1]
    assert_eq!(cb.get(2), Some(20.0)); // x[n-2]
    assert_eq!(cb.get(3), Some(10.0)); // x[n-3]

    // Push past capacity (overwrite oldest)
    cb.push(50.0);
    assert_eq!(cb.len(), 4);
    assert_eq!(cb.latest(), Some(50.0));
    assert_eq!(cb.oldest(), Some(20.0)); // 10.0 was overwritten
    assert_eq!(cb.get(0), Some(50.0));
    assert_eq!(cb.get(3), Some(20.0));

    cb.clear(0.0);
    assert!(cb.is_empty());
}

// =========================================================================================
// 28. WINDOWED-SINC FIR FILTER DESIGN TESTS
// =========================================================================================

#[test]
fn test_windowed_sinc_fir_design() {
    let mut taps = [0.0f32; 31];

    // Low-pass design
    let status_lp = fir_windowed_sinc_lowpass(0.1, &mut taps);
    assert_eq!(status_lp, Status::Success);
    let sum_lp: f32 = taps.iter().sum();
    assert!((sum_lp - 1.0).abs() < 1e-4); // Unity DC gain

    // High-pass design
    let mut hp_taps = [0.0f32; 31];
    let status_hp = fir_windowed_sinc_highpass(0.2, &mut hp_taps);
    assert_eq!(status_hp, Status::Success);
    let sum_hp: f32 = hp_taps.iter().sum();
    assert!(sum_hp.abs() < 1e-3); // DC gain ~ 0 for high-pass

    // Band-pass design
    let mut bp_taps = [0.0f32; 31];
    let status_bp = fir_windowed_sinc_bandpass(0.1, 0.3, &mut bp_taps);
    assert_eq!(status_bp, Status::Success);

    // Band-stop design
    let mut bs_taps = [0.0f32; 31];
    let status_bs = fir_windowed_sinc_bandstop(0.1, 0.3, &mut bs_taps);
    assert_eq!(status_bs, Status::Success);
    let sum_bs: f32 = bs_taps.iter().sum();
    assert!((sum_bs - 1.0).abs() < 1e-3); // DC gain ~ 1.0 for notch/bandstop
}

// =========================================================================================
// 29. FILTER ANALYSIS TESTS (FREQUENCY RESPONSE, GROUP DELAY, STABILITY)
// =========================================================================================

#[test]
fn test_fir_frequency_response_and_group_delay() {
    // Symmetric linear-phase averaging FIR: constant group delay of (M-1)/2 = 1.0 sample.
    let taps = [0.25f32, 0.5, 0.25];

    // DC gain should be 1.0 (matches the sum of taps).
    let h_dc = fir_frequency_response(&taps, 0.0);
    assert!((response_magnitude(h_dc) - 1.0).abs() < 1e-4);
    assert!(response_phase(h_dc).abs() < 1e-4);

    // Nyquist (freq_norm = 0.5): H = 0.25 - 0.5 + 0.25 = 0.0, a null.
    let h_nyquist = fir_frequency_response(&taps, 0.5);
    assert!(response_magnitude(h_nyquist) < 1e-4);

    for &f in &[0.05f32, 0.15, 0.25, 0.4] {
        let delay = fir_group_delay(&taps, f);
        assert!((delay - 1.0).abs() < 1e-3);
    }
}

#[test]
fn test_biquad_frequency_response_and_stability() {
    let lp = biquad_lowpass_coeffs(1000.0, 48000.0, core::f32::consts::FRAC_1_SQRT_2);

    // A well-formed lowpass biquad must be stable (poles inside the unit circle).
    assert!(biquad_is_stable(&lp));
    assert!(biquad_pole_radius(&lp) < 1.0);

    // DC gain of a properly normalized lowpass biquad should be unity.
    let h_dc = biquad_frequency_response(&lp, 0.0);
    assert!((response_magnitude(h_dc) - 1.0).abs() < 1e-3);

    // An explicitly unstable section (pole outside the unit circle) must be flagged.
    let unstable = [1.0f32, 0.0, 0.0, 1.5, 0.0];
    assert!(!biquad_is_stable(&unstable));
    assert!(biquad_pole_radius(&unstable) > 1.0);

    let mut butter = [0.0f32; 10];
    butterworth_lowpass_biquads(1000.0, 48000.0, 4, &mut butter);
    assert!(biquad_cascade_is_stable(&butter));

    let h_cascade_dc = biquad_cascade_frequency_response(&butter, 0.0);
    assert!((response_magnitude(h_cascade_dc) - 1.0).abs() < 1e-3);
    assert!(response_magnitude_db(h_cascade_dc).abs() < 0.1);
}

// =========================================================================================
// 30. FAST WALSH-HADAMARD TRANSFORM (FWHT) TESTS
// =========================================================================================

#[test]
fn test_fast_walsh_hadamard_transform() {
    let mut data_f32 = [1.0f32, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0];
    let original = data_f32;

    let status_fwht = fwht_f32(&mut data_f32);
    assert_eq!(status_fwht, Status::Success);

    // Hadamard sum property: first element is sum of all elements
    let sum_orig: f32 = original.iter().sum();
    assert_eq!(data_f32[0], sum_orig);

    // Inverse FWHT
    let status_ifwht = ifwht_f32(&mut data_f32);
    assert_eq!(status_ifwht, Status::Success);
    for i in 0..8 {
        assert!((data_f32[i] - original[i]).abs() < 1e-4);
    }

    // 32-bit Integer FWHT
    let mut data_i32 = [1i32, 2, 3, 4];
    let status_i32 = fwht_i32(&mut data_i32);
    assert_eq!(status_i32, Status::Success);
    assert_eq!(data_i32[0], 10); // 1 + 2 + 3 + 4 = 10
}

// =========================================================================================
// 31. CHEBYSHEV, SINGLE-POLE, & RECURSIVE MOVING AVERAGE FILTER TESTS
// =========================================================================================

#[test]
fn test_chebyshev_biquad_stage_matches_book_debug_values() {
    // Data Set 1 from Steven W. Smith, "The Scientist and Engineer's Guide to DSP", Table 20-6
    // (low-pass, no ripple, 4-pole filter, pole-pair 1).
    let stage1 = chebyshev_biquad_stage(0.1, false, 0.0, 4, 1);
    let expected1 = [0.061885f32, 0.123770, 0.061885, 1.048600, -0.296140];
    for (got, want) in stage1.iter().zip(expected1.iter()) {
        assert!(
            (got - want).abs() < 1e-4,
            "got {stage1:?} want {expected1:?}"
        );
    }

    // Data Set 2 from Table 20-6 (high-pass, 10% ripple, 4-pole filter, pole-pair 2).
    let stage2 = chebyshev_biquad_stage(0.1, true, 10.0, 4, 2);
    let expected2 = [0.922920f32, -1.845840, 0.922920, 1.446913, -0.836654];
    for (got, want) in stage2.iter().zip(expected2.iter()) {
        assert!(
            (got - want).abs() < 1e-3,
            "got {stage2:?} want {expected2:?}"
        );
    }
}

#[test]
fn test_chebyshev_lowpass_and_highpass_cascade_design() {
    let mut lp = [0.0f32; 10];
    chebyshev_lowpass_biquads(0.1, 0.5, 4, &mut lp);
    assert!(biquad_cascade_is_stable(&lp));
    let h_dc = biquad_cascade_frequency_response(&lp, 0.0);
    assert!((response_magnitude(h_dc) - 1.0).abs() < 1e-3);

    let mut hp = [0.0f32; 10];
    chebyshev_highpass_biquads(0.1, 0.5, 4, &mut hp);
    assert!(biquad_cascade_is_stable(&hp));
    let h_nyquist = biquad_cascade_frequency_response(&hp, 0.5);
    assert!((response_magnitude(h_nyquist) - 1.0).abs() < 1e-3);
}

#[test]
fn test_single_pole_filters() {
    // A step input through a low-pass single-pole filter should settle to unity gain.
    let decay = single_pole_decay_from_cutoff(0.05);
    let mut lp = SinglePoleFilter::lowpass(decay);
    let mut y = 0.0;
    for _ in 0..500 {
        y = lp.process(1.0);
    }
    assert!((y - 1.0).abs() < 1e-3);

    // A step (DC) input through a high-pass single-pole filter should decay to zero.
    let mut hp = SinglePoleFilter::highpass(decay);
    let mut y_hp = 0.0;
    for _ in 0..500 {
        y_hp = hp.process(1.0);
    }
    assert!(y_hp.abs() < 1e-3);
}

#[test]
fn test_recursive_moving_average_matches_naive_average() {
    let mut rma = RecursiveMovingAverage::<4>::new();
    let input = [1.0f32, 2.0, 3.0, 4.0, 5.0, 6.0];
    let mut outputs = [0.0f32; 6];
    for (i, &x) in input.iter().enumerate() {
        outputs[i] = rma.process(x);
    }

    // Growing window until full, then a proper 4-point moving average thereafter.
    let expected = [1.0f32, 1.5, 2.0, 2.5, 3.5, 4.5];
    for (got, want) in outputs.iter().zip(expected.iter()) {
        assert!(
            (got - want).abs() < 1e-4,
            "got {outputs:?} want {expected:?}"
        );
    }
}

// =========================================================================================
// 32. HAAR & HARTLEY TRANSFORM TESTS
// =========================================================================================

#[test]
fn test_haar_transform_f32_matches_hand_computation_and_round_trips() {
    // Hand-computed via Jörg Arndt, "Matters Computational", Ch. 24, `haar_inplace`.
    let mut data = [1.0f32, 0.0, 0.0, 0.0];
    let status = haar_transform_f32(&mut data);
    assert_eq!(status, Status::Success);
    let expected = [0.5f32, core::f32::consts::FRAC_1_SQRT_2, 0.5, 0.0];
    for (got, want) in data.iter().zip(expected.iter()) {
        assert!((got - want).abs() < 1e-4, "got {data:?} want {expected:?}");
    }

    // Energy (Parseval) is preserved: the transform is orthogonal.
    let energy_out: f32 = data.iter().map(|v| v * v).sum();
    assert!((energy_out - 1.0).abs() < 1e-4);

    // Forward + inverse round-trips to the original signal.
    let original = [1.0f32, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0];
    let mut round_trip = original;
    assert_eq!(haar_transform_f32(&mut round_trip), Status::Success);
    assert_eq!(inverse_haar_transform_f32(&mut round_trip), Status::Success);
    for (got, want) in round_trip.iter().zip(original.iter()) {
        assert!(
            (got - want).abs() < 1e-3,
            "got {round_trip:?} want {original:?}"
        );
    }
}

#[test]
fn test_haar_transform_i32_is_non_normalized_and_forward_only() {
    let mut data = [1i32, 2, 3, 4];
    let status = haar_transform_i32(&mut data);
    assert_eq!(status, Status::Success);
    // DC term after the non-normalized transform is the exact sum of the inputs.
    assert_eq!(data[0], 10);
    assert_eq!(data, [10, -1, -4, -1]);
}

#[test]
fn test_hartley_transform_f32_is_self_inverse() {
    // A unit impulse's Hartley transform is a constant 1/sqrt(n).
    let mut impulse = [1.0f32, 0.0, 0.0, 0.0];
    assert_eq!(hartley_transform_f32(&mut impulse), Status::Success);
    for &v in impulse.iter() {
        assert!((v - 0.5).abs() < 1e-4, "impulse response: {impulse:?}");
    }

    // H[H[a]] == a: applying the transform twice recovers the original signal.
    let original = [1.0f32, 2.0, 3.0, 4.0];
    let mut data = original;
    assert_eq!(hartley_transform_f32(&mut data), Status::Success);
    let expected_once = [5.0f32, -2.0, -1.0, 0.0];
    for (got, want) in data.iter().zip(expected_once.iter()) {
        assert!(
            (got - want).abs() < 1e-3,
            "got {data:?} want {expected_once:?}"
        );
    }
    assert_eq!(hartley_transform_f32(&mut data), Status::Success);
    for (got, want) in data.iter().zip(original.iter()) {
        assert!((got - want).abs() < 1e-3, "got {data:?} want {original:?}");
    }
}

// =========================================================================================
// 33. GENERALIZED WAVELET TRANSFORM TESTS
// =========================================================================================

#[test]
fn test_wavelet_transform_daubechies4_round_trips_and_preserves_energy() {
    let original = [1.0f32, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0];
    let mut data = original;

    assert_eq!(
        wavelet_transform_f32(&mut data, &DAUBECHIES_4),
        Status::Success
    );

    // Orthogonal transform: energy (sum of squares) is preserved.
    let energy_in: f32 = original.iter().map(|v| v * v).sum();
    let energy_out: f32 = data.iter().map(|v| v * v).sum();
    assert!((energy_in - energy_out).abs() < 1e-3);

    assert_eq!(
        inverse_wavelet_transform_f32(&mut data, &DAUBECHIES_4),
        Status::Success
    );
    for (got, want) in data.iter().zip(original.iter()) {
        assert!((got - want).abs() < 1e-3, "got {data:?} want {original:?}");
    }
}

#[test]
fn test_wavelet_step_rejects_bad_arguments() {
    let mut data = [1.0f32, 2.0, 3.0];
    // Not a power of 2.
    assert_eq!(
        wavelet_step_f32(&mut data, 3, &DAUBECHIES_4),
        Status::ArgumentError
    );

    let mut data8 = [0.0f32; 8];
    // Odd-length filter is invalid.
    assert_eq!(
        wavelet_step_f32(&mut data8, 8, &[1.0, 2.0, 3.0]),
        Status::ArgumentError
    );
}

// =========================================================================================
// 34. AUDIO COMPANDING TESTS
// =========================================================================================

#[test]
fn test_mu_law_and_a_law_companding_round_trip_and_expand_small_signals() {
    for &x in &[-0.9f32, -0.3, -0.05, 0.0, 0.05, 0.3, 0.9] {
        let mu_compressed = mu_law_compress_f32(x);
        assert!((-1.0..=1.0).contains(&mu_compressed));
        let mu_round_trip = mu_law_expand_f32(mu_compressed);
        assert!(
            (mu_round_trip - x).abs() < 1e-4,
            "mu-law round trip failed for x={x}: got {mu_round_trip}"
        );

        let a_compressed = a_law_compress_f32(x);
        assert!((-1.0..=1.0).contains(&a_compressed));
        let a_round_trip = a_law_expand_f32(a_compressed);
        assert!(
            (a_round_trip - x).abs() < 1e-4,
            "A-law round trip failed for x={x}: got {a_round_trip}"
        );
    }

    // Companding expands resolution for small signals: a small input should map to a
    // proportionally larger compressed magnitude (the whole point of the nonlinearity).
    let small = 0.01f32;
    assert!(mu_law_compress_f32(small) > small);
    assert!(a_law_compress_f32(small) > small);
}

// =========================================================================================
// 35. CUSTOM FIR FILTER DESIGN (FREQUENCY SAMPLING) TESTS
// =========================================================================================

#[test]
fn test_fir_custom_frequency_sampling_matches_impulse_case() {
    // A flat, zero-phase desired response (magnitude 1 at every bin) corresponds to an
    // impulse in the time domain; after centering and windowing, only the center tap should
    // be nonzero (the Hamming window is exactly 1.0 at its own center).
    let fft_len = 8;
    let half_spec = fft_len / 2 + 1;
    let desired_real = [1.0f32; 5];
    let desired_imag = [0.0f32; 5];
    assert_eq!(desired_real.len(), half_spec);

    let mut taps = [0.0f32; 5];
    let status = fir_custom_frequency_sampling(&desired_real, &desired_imag, fft_len, &mut taps);
    assert_eq!(status, Status::Success);

    let expected = [0.0f32, 0.0, 1.0, 0.0, 0.0];
    for (got, want) in taps.iter().zip(expected.iter()) {
        assert!((got - want).abs() < 1e-4, "got {taps:?} want {expected:?}");
    }
}

#[test]
fn test_fir_custom_frequency_sampling_approximates_lowpass() {
    // Build a desired lowpass magnitude response (1 below cutoff, 0 above), zero phase.
    let fft_len = 64;
    let half_spec = fft_len / 2 + 1;
    let cutoff_bin = half_spec / 4;
    let mut desired_real = [0.0f32; 33];
    let desired_imag = [0.0f32; 33];
    for (k, v) in desired_real.iter_mut().enumerate().take(half_spec) {
        *v = if k <= cutoff_bin { 1.0 } else { 0.0 };
    }

    let mut taps = [0.0f32; 31];
    let status = fir_custom_frequency_sampling(
        &desired_real[..half_spec],
        &desired_imag[..half_spec],
        fft_len,
        &mut taps,
    );
    assert_eq!(status, Status::Success);

    // Passband (DC) gain should be near 1, stopband (near Nyquist) should be attenuated.
    let h_dc = fir_frequency_response(&taps, 0.0);
    let h_stop = fir_frequency_response(&taps, 0.45);
    assert!((response_magnitude(h_dc) - 1.0).abs() < 0.1);
    assert!(response_magnitude(h_stop) < 0.3);
}

// =========================================================================================
// 36. AUDIO & TINYML TESTS (GOERTZEL, ENVELOPE FOLLOWERS, MEL FILTERBANK, MFCC)
// =========================================================================================

#[test]
fn test_goertzel_detects_target_frequency_and_rejects_others() {
    let fs = 8000.0f32;
    let target = 1000.0f32;
    let amplitude = 0.8f32;
    let n = 64;

    let mut on_target = GoertzelDetector::new(target, fs);
    let mut off_target = GoertzelDetector::new(2500.0, fs);
    for i in 0..n {
        let x = amplitude * (2.0 * core::f32::consts::PI * target * i as f32 / fs).sin();
        on_target.process_sample(x);
        off_target.process_sample(x);
    }

    assert!((on_target.magnitude() - amplitude).abs() < 1e-3);
    assert!(off_target.magnitude() < 1e-3);
}

#[test]
fn test_peak_and_rms_envelope_followers_converge_to_constant_input() {
    let mut peak = PeakEnvelopeFollower::new(5.0, 50.0);
    let mut rms = RmsEnvelopeFollower::new(20.0);

    let mut peak_env = 0.0;
    let mut rms_env = 0.0;
    for _ in 0..2000 {
        peak_env = peak.process(0.5);
        rms_env = rms.process(0.5);
    }

    assert!((peak_env - 0.5).abs() < 1e-3);
    assert!((rms_env - 0.5).abs() < 1e-3);

    peak.reset();
    rms.reset();
    assert_eq!(peak.process(0.0), 0.0);
    assert_eq!(rms.process(0.0), 0.0);
}

#[test]
fn test_mel_scale_round_trip() {
    for &hz in &[100.0f32, 440.0, 1000.0, 4000.0] {
        let round_trip = mel_to_hz(hz_to_mel(hz));
        assert!(
            (round_trip - hz).abs() < 1e-2,
            "hz={hz} round_trip={round_trip}"
        );
    }
}

#[test]
fn test_mel_filterbank_impulse_activates_expected_band() {
    let fft_size = 64;
    let sample_rate = 8000.0;
    let num_bins = fft_size / 2 + 1;

    let mut power_spectrum = [0.0f32; 33];
    power_spectrum[10] = 1.0;

    let mut mel_energies = [0.0f32; 8];
    let status = mel_filterbank_f32(
        &power_spectrum[..num_bins],
        fft_size,
        sample_rate,
        0.0,
        sample_rate / 2.0,
        &mut mel_energies,
    );
    assert_eq!(status, Status::Success);

    // Verified against a from-scratch reference implementation: bin 10 sits exactly at the
    // peak of filter index 4, which should receive the full impulse energy.
    let expected = [0.0f32, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0];
    for (got, want) in mel_energies.iter().zip(expected.iter()) {
        assert!(
            (got - want).abs() < 1e-4,
            "got {mel_energies:?} want {expected:?}"
        );
    }
}

#[test]
fn test_mfcc_f32_produces_finite_distinguishable_coefficients() {
    let fs = 8000.0f32;
    let fft_size = 256;

    let make_frame = |freq: f32| {
        let mut frame = [0.0f32; 256];
        for (i, x) in frame.iter_mut().enumerate() {
            *x = (2.0 * core::f32::consts::PI * freq * i as f32 / fs).sin();
        }
        let mut window = [0.0f32; 256];
        hamming_f32(&mut window);
        apply_window_f32(&mut frame, &window);
        frame
    };

    let frame_low = make_frame(300.0);
    let frame_high = make_frame(2500.0);

    let mut mel_scratch_low = [0.0f32; 26];
    let mut mfcc_low = [0.0f32; 13];
    let status_low = mfcc_f32(
        &frame_low,
        fs,
        0.0,
        fs / 2.0,
        &mut mel_scratch_low,
        &mut mfcc_low,
    );
    assert_eq!(status_low, Status::Success);
    assert_eq!(fft_size, frame_low.len());

    let mut mel_scratch_high = [0.0f32; 26];
    let mut mfcc_high = [0.0f32; 13];
    let status_high = mfcc_f32(
        &frame_high,
        fs,
        0.0,
        fs / 2.0,
        &mut mel_scratch_high,
        &mut mfcc_high,
    );
    assert_eq!(status_high, Status::Success);

    for &v in mfcc_low.iter().chain(mfcc_high.iter()) {
        assert!(v.is_finite());
    }

    // Different input frequencies must produce distinguishable feature vectors.
    let diff: f32 = mfcc_low
        .iter()
        .zip(mfcc_high.iter())
        .map(|(a, b)| (a - b).abs())
        .sum();
    assert!(diff > 0.1, "MFCC vectors too similar: diff={diff}");
}