fusion-altitude 0.1.4

use super::*;

const DT: f32 = 0.01;
const TRUE_BIAS: f32 = 0.12;
const CONVERGENCE_STEPS: usize = 2000;

#[test]
fn first_update_auto_zeroes_reference() {
    let mut est = AltitudeEstimator::new();
    est.update(0.0, 123.4, DT);
    assert_eq!(est.altitude(), 123.4);
    assert_eq!(est.vertical_velocity(), 0.0);
}

#[test]
fn explicit_reset_sets_reference() {
    let mut est = AltitudeEstimator::new();
    est.reset(50.0);
    assert_eq!(est.altitude(), 50.0);
    assert_eq!(est.vertical_velocity(), 0.0);
    // Subsequent first update does not re-zero.
    est.update(0.0, 200.0, DT);
    assert!(est.altitude() < 200.0);
}

#[test]
fn stationary_converges_to_baro() {
    let mut est = AltitudeEstimator::new();
    for _ in 0..2000 {
        est.update(0.0, 100.0, DT);
    }
    assert!((est.altitude() - 100.0).abs() < 1e-3);
    assert!(est.vertical_velocity().abs() < 1e-3);
}

#[test]
fn baro_drift_pulls_altitude() {
    let mut est = AltitudeEstimator::new();
    est.reset(0.0);
    // Constant baro reading of 10 m with no accel — filter must
    // track the baro reference over time.
    for _ in 0..5000 {
        est.update(0.0, 10.0, DT);
    }
    assert!((est.altitude() - 10.0).abs() < 1e-2);
}

#[test]
fn constant_upward_accel_against_truthful_baro() {
    // Truth: h(t) = 0.5 * a * t², v(t) = a * t. Provide matching baro.
    let a = 0.2_f32; // m/s²
    let mut est = AltitudeEstimator::new();
    est.reset(0.0);

    let mut t = 0.0_f32;
    for _ in 0..1000 {
        t += DT;
        let truth_h = 0.5 * a * t * t;
        est.update(a, truth_h, DT);
    }

    let truth_h = 0.5 * a * t * t;
    let truth_v = a * t;
    assert!(
        (est.altitude() - truth_h).abs() < 0.05,
        "h={} truth={}",
        est.altitude(),
        truth_h
    );
    assert!(
        (est.vertical_velocity() - truth_v).abs() < 0.05,
        "v={} truth={}",
        est.vertical_velocity(),
        truth_v
    );
}

#[test]
fn settings_default_is_well_damped() {
    let s = AltitudeSettings::default();
    // Approximate effective damping of the position/velocity subsystem:
    // ζ_eff ≈ K_h / (2 sqrt(K_v)). Exact only when bias_gain = 0, but the
    // cascaded design keeps it close to the design value of 0.7.
    let zeta = s.position_gain / (2.0 * libm_sqrtf(s.velocity_gain));
    assert!(zeta > 0.5 && zeta < 1.0, "ζ={}", zeta);
}

#[test]
fn settings_default_satisfies_routh_hurwitz() {
    // Stability of s³ + K_h s² + K_v s + K_b requires K_h · K_v > K_b.
    let s = AltitudeSettings::default();
    assert!(
        s.position_gain * s.velocity_gain > s.bias_gain,
        "K_h·K_v={} not > K_b={}",
        s.position_gain * s.velocity_gain,
        s.bias_gain
    );
}

#[test]
fn estimates_constant_accel_bias_and_zeroes_altitude_error() {
    // 2-state filter would park at +bias/K_v; 3-state observer must
    // identify the bias and drive altitude error to zero.
    let mut est = AltitudeEstimator::new();

    for _ in 0..CONVERGENCE_STEPS {
        est.update(TRUE_BIAS, 0.0, DT);
    }

    assert!(est.altitude().abs() < 0.01, "h={}", est.altitude());
    assert!(
        est.vertical_velocity().abs() < 0.01,
        "v={}",
        est.vertical_velocity()
    );
    assert!(
        (est.accel_bias() - TRUE_BIAS).abs() < 0.01,
        "b_hat={} true={}",
        est.accel_bias(),
        TRUE_BIAS
    );
}

#[test]
fn reset_preserves_accel_bias() {
    // Bias is a physical sensor property, independent of the altitude
    // reference frame. A converged estimate must survive a reset.
    let mut est = AltitudeEstimator::new();
    for _ in 0..CONVERGENCE_STEPS {
        est.update(TRUE_BIAS, 0.0, DT);
    }

    let converged_bias = est.accel_bias();
    assert!((converged_bias - TRUE_BIAS).abs() < 0.01);

    est.reset(50.0);
    assert_eq!(est.accel_bias(), converged_bias);
    assert_eq!(est.altitude(), 50.0);
    assert_eq!(est.vertical_velocity(), 0.0);
}

#[test]
fn baro_residual_is_zero_before_update_and_after_reset() {
    let mut est = AltitudeEstimator::new();
    assert_eq!(est.baro_residual(), 0.0);

    // Auto-zero on first sample → altitude == baro → residual stays 0.
    est.update(0.0, 50.0, DT);
    assert_eq!(est.baro_residual(), 0.0);

    // Diverge, then reset; residual must clear.
    for _ in 0..50 {
        est.update(0.0, 60.0, DT);
    }
    assert!(est.baro_residual() > 0.0);
    est.reset(0.0);
    assert_eq!(est.baro_residual(), 0.0);
}

#[test]
fn baro_residual_sign_matches_baro_minus_altitude() {
    // Baro suddenly above the converged estimate ⇒ residual > 0.
    let mut est = AltitudeEstimator::new();
    est.reset(0.0);
    est.update(0.0, 10.0, DT);
    assert!(est.baro_residual() > 0.0);
    assert!((est.baro_residual() - (10.0 - 0.0)).abs() < 1e-6);

    // Step baro below estimate ⇒ residual flips negative.
    let h = est.altitude();
    est.update(0.0, h - 5.0, DT);
    assert!(est.baro_residual() < 0.0);
}

#[test]
fn baro_residual_decays_at_convergence() {
    // Once the filter has converged on a constant baro, residual ≈ 0.
    let mut est = AltitudeEstimator::new();
    for _ in 0..CONVERGENCE_STEPS {
        est.update(0.0, 100.0, DT);
    }
    assert!(
        est.baro_residual().abs() < 1e-3,
        "r={}",
        est.baro_residual()
    );
}

#[test]
fn bias_gain_zero_recovers_two_state_filter() {
    // bias_gain = 0 disables the bias loop; a constant accel bias
    // must then park at +bias/K_v steady state, matching the original
    // 2-state behavior.
    let settings = AltitudeSettings {
        bias_gain: 0.0,
        ..AltitudeSettings::default()
    };
    let mut est = AltitudeEstimator::with_settings(settings);

    for _ in 0..CONVERGENCE_STEPS {
        est.update(TRUE_BIAS, 0.0, DT);
    }

    let expected_err = TRUE_BIAS / settings.velocity_gain;
    assert!(
        (est.altitude() - expected_err).abs() < 0.005,
        "h={} expected≈{}",
        est.altitude(),
        expected_err
    );
    assert_eq!(est.accel_bias(), 0.0);
}

// tiny no_std-friendly sqrt for the damping check
fn libm_sqrtf(x: f32) -> f32 {
    // Newton's method, 8 iterations — fine for a test.
    let mut g = x;
    for _ in 0..8 {
        g = 0.5 * (g + x / g);
    }
    g
}