use super::*;

// Simple 1D position tracking: state = [position, velocity]
struct ConstantVelocity;

impl ekf::ProcessModel<2, 0> for ConstantVelocity {
    fn predict_next(&self, state: &[f64; 2], _control: &[f64; 0], dt: f64) -> [f64; 2] {
        [state[0] + state[1] * dt, state[1]]
    }

    fn process_noise_std(&self) -> [f64; 2] {
        [0.1, 0.01]
    }
}

struct PositionSensor;

impl ekf::MeasurementModel<2, 1> for PositionSensor {
    fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
        [state[0]]
    }

    fn measurement_noise_std(&self) -> [f64; 1] {
        [0.5]
    }
}

#[test]
fn test_ekf_tracking() {
    let mut ekf =
        ekf::ExtendedKalmanFilter::new([0.0, 1.0], [1.0, 0.5], ConstantVelocity, PositionSensor);

    // Simulate for a few steps
    for i in 0..10 {
        ekf.predict(&[], 0.1);

        // Simulate noisy measurement of true position
        let true_pos = (i as f64 + 1.0) * 0.1;
        let measurement = [true_pos + 0.05];
        ekf.update(&measurement).unwrap();
    }

    let state = ekf.estimate();
    // Should be tracking roughly position=1.0, velocity=1.0
    assert!((state[0] - 1.0).abs() < 0.3);
    assert!((state[1] - 1.0).abs() < 0.3);
}

#[test]
fn test_numerical_jacobian() {
    // Test that numerical differentiation works correctly
    fn f(x: &[f64; 2]) -> [f64; 2] {
        [x[0] * x[0], x[0] * x[1]]
    }

    let jac = ekf::numerical_jacobian(f, &[2.0, 3.0]);

    // df1/dx0 = 2*x0 = 4
    assert!((jac[0][0] - 4.0).abs() < 1e-6);
    // df1/dx1 = 0
    assert!(jac[1][0].abs() < 1e-6);
    // df2/dx0 = x1 = 3
    assert!((jac[0][1] - 3.0).abs() < 1e-6);
    // df2/dx1 = x0 = 2
    assert!((jac[1][1] - 2.0).abs() < 1e-6);
}

// Particle filter tests
struct SimpleMotion;

impl particle::ProcessModel<[f64; 2], [f64; 0]> for SimpleMotion {
    fn sample_next<R: rand::Rng>(
        &self,
        state: &[f64; 2],
        _control: &[f64; 0],
        dt: f64,
        rng: &mut R,
    ) -> [f64; 2] {
        use rand_distr::{Distribution, StandardNormal};
        let noise: f64 = StandardNormal.sample(rng);
        [state[0] + state[1] * dt + noise * 0.1 * dt.sqrt(), state[1]]
    }
}

struct SimpleSensor;

impl particle::MeasurementModel<[f64; 2], [f64; 1]> for SimpleSensor {
    fn likelihood(&self, state: &[f64; 2], measurement: &[f64; 1]) -> f64 {
        let error = state[0] - measurement[0];
        (-0.5 * (error / 0.5).powi(2)).exp()
    }
}

struct UniformInitial {
    center: [f64; 2],
    spread: f64,
}

impl rand::distr::Distribution<[f64; 2]> for UniformInitial {
    fn sample<R: rand::Rng + ?Sized>(&self, rng: &mut R) -> [f64; 2] {
        [
            self.center[0] + (rng.random::<f64>() - 0.5) * self.spread,
            self.center[1] + (rng.random::<f64>() - 0.5) * self.spread * 0.2,
        ]
    }
}

#[test]
fn test_particle_filter() {
    use rand::SeedableRng;
    use rand_chacha::ChaCha8Rng;

    let initial = UniformInitial {
        center: [0.0, 1.0],
        spread: 2.0,
    };

    let mut pf = particle::ParticleFilter::new(
        initial,
        particle::Config {
            num_particles: 500,
            resample_threshold: 0.5,
        },
        SimpleMotion,
        SimpleSensor,
        ChaCha8Rng::seed_from_u64(42),
    );

    // Run a few steps
    for i in 0..10 {
        pf.predict(&[], 0.1);
        let measurement = [(i as f64 + 1.0) * 0.1];
        pf.update(&measurement).unwrap();
    }

    let state = pf.estimate();
    // Should be roughly tracking the measurements
    assert!((state[0] - 1.0).abs() < 0.5);
}

#[test]
fn test_uncertainty_decreases() {
    let mut ekf = ekf::ExtendedKalmanFilter::new(
        [0.0, 1.0],
        [10.0, 5.0], // Start very uncertain
        ConstantVelocity,
        PositionSensor,
    );

    let initial_uncertainty = ekf.uncertainty();

    // Give it some measurements
    for _ in 0..20 {
        ekf.predict(&[], 0.1);
        ekf.update(&[0.1]).unwrap();
    }

    let final_uncertainty = ekf.uncertainty();

    // Position uncertainty should decrease
    assert!(final_uncertainty[0] < initial_uncertainty[0]);
}

#[test]
fn test_ekf_singular_covariance_error() {
    // Test that we get a proper error with zero measurement noise AND zero state covariance
    // S = H * P * H^T + R, so both P and R must be zero for S to be singular
    struct ZeroNoiseSensor;

    impl ekf::MeasurementModel<2, 1> for ZeroNoiseSensor {
        fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
            [state[0]]
        }

        fn measurement_noise_std(&self) -> [f64; 1] {
            [0.0] // Zero measurement noise
        }
    }

    let mut ekf = ekf::ExtendedKalmanFilter::new(
        [0.0, 1.0],
        [0.0, 0.0], // Zero initial uncertainty makes P = 0
        ConstantVelocity,
        ZeroNoiseSensor,
    );

    // Skip predict to keep P at zero (predict adds process noise)
    let result = ekf.update(&[0.1]);

    assert!(matches!(
        result,
        Err(EstimatorError::SingularInnovationCovariance)
    ));
}

#[test]
fn test_particle_degeneracy_error() {
    use rand::SeedableRng;
    use rand_chacha::ChaCha8Rng;

    // A sensor that always returns 0 likelihood
    struct ImpossibleSensor;

    impl particle::MeasurementModel<[f64; 2], [f64; 1]> for ImpossibleSensor {
        fn likelihood(&self, _state: &[f64; 2], _measurement: &[f64; 1]) -> f64 {
            0.0 // Nothing ever matches
        }
    }

    let initial = UniformInitial {
        center: [0.0, 1.0],
        spread: 2.0,
    };

    let mut pf = particle::ParticleFilter::new(
        initial,
        particle::Config {
            num_particles: 100,
            resample_threshold: 0.5,
        },
        SimpleMotion,
        ImpossibleSensor,
        ChaCha8Rng::seed_from_u64(42),
    );

    let result = pf.update(&[0.0]);

    assert!(matches!(result, Err(EstimatorError::ParticleDegeneracy)));
}

// ========================================================================
// Comprehensive Tests for Correctness, Accuracy, and Resilience
// ========================================================================

mod linear_system_tests {
    //! For linear systems, the EKF reduces to the standard Kalman Filter
    //! and should be optimal. These tests verify correct behavior.

    use super::*;
    use rand::{Rng, SeedableRng};
    use rand_chacha::ChaCha8Rng;

    /// Linear constant-velocity model with configurable noise
    struct LinearMotion {
        process_noise: [f64; 2],
    }

    impl ekf::ProcessModel<2, 0> for LinearMotion {
        fn predict_next(&self, state: &[f64; 2], _: &[f64; 0], dt: f64) -> [f64; 2] {
            [state[0] + state[1] * dt, state[1]]
        }

        fn process_noise_std(&self) -> [f64; 2] {
            self.process_noise
        }

        // Analytical Jacobian for linear system (should match numerical)
        fn prediction_sensitivity(&self, _: &[f64; 2], _: &[f64; 0], dt: f64) -> [[f64; 2]; 2] {
            [[1.0, 0.0], [dt, 1.0]]
        }
    }

    struct LinearSensor {
        noise_std: f64,
    }

    impl ekf::MeasurementModel<2, 1> for LinearSensor {
        fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
            [state[0]]
        }

        fn measurement_noise_std(&self) -> [f64; 1] {
            [self.noise_std]
        }

        fn measurement_sensitivity(&self, _: &[f64; 2]) -> [[f64; 1]; 2] {
            [[1.0], [0.0]]
        }
    }

    #[test]
    fn test_ekf_tracks_constant_velocity_exactly() {
        // With no noise and perfect initial conditions, EKF should track perfectly
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0], // Start at origin, velocity = 1
            [0.01, 0.01],
            LinearMotion {
                process_noise: [0.001, 0.001],
            },
            LinearSensor { noise_std: 0.01 },
        );

        let dt = 0.1;
        for i in 1..=100 {
            ekf.predict(&[], dt);
            // Perfect measurement of true position
            let true_pos = i as f64 * dt;
            ekf.update(&[true_pos]).unwrap();

            let est = ekf.estimate();
            assert!(
                (est[0] - true_pos).abs() < 0.1,
                "Position error too large at step {}: {} vs {}",
                i,
                est[0],
                true_pos
            );
            assert!(
                (est[1] - 1.0).abs() < 0.1,
                "Velocity error too large at step {}: {}",
                i,
                est[1]
            );
        }
    }

    #[test]
    fn test_ekf_converges_from_wrong_initial_estimate() {
        // Start with completely wrong estimate, should converge
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [100.0, -5.0], // Way off!
            [50.0, 10.0],  // High uncertainty
            LinearMotion {
                process_noise: [0.1, 0.01],
            },
            LinearSensor { noise_std: 0.5 },
        );

        let dt = 0.1;
        let true_velocity = 1.0;

        for i in 1..=200 {
            ekf.predict(&[], dt);
            let true_pos = i as f64 * dt * true_velocity;
            // Add some noise to measurements
            let noise = (i as f64 * 0.1).sin() * 0.3;
            ekf.update(&[true_pos + noise]).unwrap();
        }

        let est = ekf.estimate();
        let true_final_pos = 200.0 * dt * true_velocity;

        // Should have converged to within a few sigma
        assert!(
            (est[0] - true_final_pos).abs() < 2.0,
            "Failed to converge: position {} vs {}",
            est[0],
            true_final_pos
        );
        assert!(
            (est[1] - true_velocity).abs() < 0.5,
            "Failed to converge: velocity {} vs {}",
            est[1],
            true_velocity
        );
    }

    #[test]
    fn test_ekf_uncertainty_is_consistent() {
        // Run Monte Carlo simulations and verify errors match uncertainty
        let mut rng = ChaCha8Rng::seed_from_u64(12345);
        let num_trials = 100;
        let mut position_errors: Vec<f64> = Vec::new();
        let mut normalized_errors: Vec<f64> = Vec::new();

        for _ in 0..num_trials {
            let mut ekf = ekf::ExtendedKalmanFilter::new(
                [0.0, 1.0],
                [1.0, 0.5],
                LinearMotion {
                    process_noise: [0.1, 0.01],
                },
                LinearSensor { noise_std: 0.5 },
            );

            let dt = 0.1;
            let true_velocity = 1.0;

            for i in 1..=50 {
                ekf.predict(&[], dt);
                let true_pos = i as f64 * dt * true_velocity;
                // Gaussian-ish noise using multiple uniform samples
                let noise: f64 = (0..12).map(|_| rng.random::<f64>() - 0.5).sum::<f64>() * 0.5;
                ekf.update(&[true_pos + noise]).unwrap();
            }

            let est = ekf.estimate();
            let unc = ekf.uncertainty();
            let true_final_pos = 50.0 * dt * true_velocity;

            let error = est[0] - true_final_pos;
            position_errors.push(error);
            normalized_errors.push(error / unc[0]);
        }

        // Check that ~68% of errors are within 1 sigma (allowing some slack)
        let within_1_sigma = normalized_errors.iter().filter(|e| e.abs() < 1.0).count();
        let fraction = within_1_sigma as f64 / num_trials as f64;

        assert!(
            fraction > 0.5 && fraction < 0.9,
            "Uncertainty inconsistent: {}% within 1 sigma (expected ~68%)",
            fraction * 100.0
        );

        // Check RMS error is reasonable
        let rms_error =
            (position_errors.iter().map(|e| e * e).sum::<f64>() / num_trials as f64).sqrt();
        assert!(rms_error < 1.0, "RMS error too large: {}", rms_error);
    }

    #[test]
    fn test_pf_converges_from_wrong_initial_estimate() {
        use rand::SeedableRng;
        use rand_chacha::ChaCha8Rng;

        struct PFMotion;
        impl particle::ProcessModel<[f64; 2], [f64; 0]> for PFMotion {
            fn sample_next<R: rand::Rng>(
                &self,
                state: &[f64; 2],
                _: &[f64; 0],
                dt: f64,
                rng: &mut R,
            ) -> [f64; 2] {
                use rand_distr::{Distribution, StandardNormal};
                let n1: f64 = StandardNormal.sample(rng);
                let n2: f64 = StandardNormal.sample(rng);
                // Higher process noise to allow particles to migrate
                let noise_pos = n1 * 0.5 * dt.sqrt();
                let noise_vel = n2 * 0.1 * dt.sqrt();
                [state[0] + state[1] * dt + noise_pos, state[1] + noise_vel]
            }
        }

        struct PFSensor;
        impl particle::MeasurementModel<[f64; 2], [f64; 1]> for PFSensor {
            fn likelihood(&self, state: &[f64; 2], measurement: &[f64; 1]) -> f64 {
                let error = state[0] - measurement[0];
                // Wider likelihood allows recovery from bad initial guess
                (-0.5 * (error / 2.0).powi(2)).exp()
            }
        }

        // Start particles centered at wrong location (but not impossibly far)
        struct WrongInitial;
        impl rand::distr::Distribution<[f64; 2]> for WrongInitial {
            fn sample<R: rand::Rng + ?Sized>(&self, rng: &mut R) -> [f64; 2] {
                let r1: f64 = rng.random();
                let r2: f64 = rng.random();
                [
                    10.0 + (r1 - 0.5) * 5.0, // Centered at 10, spread 5
                    -1.0 + (r2 - 0.5) * 1.0, // Wrong velocity
                ]
            }
        }

        let mut pf = particle::ParticleFilter::new(
            WrongInitial,
            particle::Config {
                num_particles: 2000,
                resample_threshold: 0.5,
            },
            PFMotion,
            PFSensor,
            ChaCha8Rng::seed_from_u64(99),
        );

        let dt = 0.1;
        for i in 1..=300 {
            pf.predict(&[], dt);
            let true_pos = i as f64 * dt;
            pf.update(&[true_pos]).unwrap();
        }

        let est = pf.estimate();
        let true_final = 300.0 * dt;

        assert!(
            (est[0] - true_final).abs() < 3.0,
            "PF failed to converge: {} vs {}",
            est[0],
            true_final
        );
        assert!(
            (est[1] - 1.0).abs() < 1.0,
            "PF velocity estimate wrong: {} vs 1.0",
            est[1]
        );
    }
}

mod nonlinear_system_tests {
    //! Test on genuinely nonlinear systems where EKF approximation matters

    use super::*;
    use rand::SeedableRng;
    use rand_chacha::ChaCha8Rng;

    /// 2D robot with [x, y, heading] state and [velocity, turn_rate] control
    struct UnicycleModel {
        process_noise: [f64; 3],
    }

    impl ekf::ProcessModel<3, 2> for UnicycleModel {
        fn predict_next(&self, state: &[f64; 3], control: &[f64; 2], dt: f64) -> [f64; 3] {
            let [x, y, theta] = *state;
            let [v, omega] = *control;
            [
                x + v * theta.cos() * dt,
                y + v * theta.sin() * dt,
                theta + omega * dt,
            ]
        }

        fn process_noise_std(&self) -> [f64; 3] {
            self.process_noise
        }
    }

    /// Range-bearing sensor measuring distance and angle to a landmark
    struct RangeBearingSensor {
        landmark: [f64; 2],
        range_noise: f64,
        bearing_noise: f64,
    }

    impl ekf::MeasurementModel<3, 2> for RangeBearingSensor {
        fn expected_measurement(&self, state: &[f64; 3]) -> [f64; 2] {
            let dx = self.landmark[0] - state[0];
            let dy = self.landmark[1] - state[1];
            let range = (dx * dx + dy * dy).sqrt();
            let bearing = dy.atan2(dx) - state[2];
            [range, bearing]
        }

        fn measurement_noise_std(&self) -> [f64; 2] {
            [self.range_noise, self.bearing_noise]
        }
    }

    #[test]
    fn test_ekf_nonlinear_circular_motion() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 0.0, 0.0],
            [0.5, 0.5, 0.2],
            UnicycleModel {
                process_noise: [0.05, 0.05, 0.02],
            },
            RangeBearingSensor {
                landmark: [5.0, 5.0],
                range_noise: 0.3,
                bearing_noise: 0.1,
            },
        );

        let dt = 0.05;
        let velocity = 1.0;
        let turn_rate = 0.5; // Circular motion

        let mut true_state: [f64; 3] = [0.0, 0.0, 0.0];

        for _ in 0..200 {
            // True motion
            true_state[0] += velocity * true_state[2].cos() * dt;
            true_state[1] += velocity * true_state[2].sin() * dt;
            true_state[2] += turn_rate * dt;

            // Filter step
            ekf.predict(&[velocity, turn_rate], dt);

            // True measurement
            let dx = 5.0 - true_state[0];
            let dy = 5.0 - true_state[1];
            let range = (dx * dx + dy * dy).sqrt();
            let bearing = dy.atan2(dx) - true_state[2];
            ekf.update(&[range, bearing]).unwrap();
        }

        let est = ekf.estimate();

        // Should track within reasonable bounds for nonlinear system
        assert!(
            (est[0] - true_state[0]).abs() < 1.0,
            "X error: {} vs {}",
            est[0],
            true_state[0]
        );
        assert!(
            (est[1] - true_state[1]).abs() < 1.0,
            "Y error: {} vs {}",
            est[1],
            true_state[1]
        );
    }

    #[test]
    fn test_pf_nonlinear_handles_ambiguity() {
        // Particle filter should handle multimodal distributions
        // where EKF might fail

        struct CircularMotion;
        impl particle::ProcessModel<[f64; 2], ()> for CircularMotion {
            fn sample_next<R: rand::Rng>(
                &self,
                state: &[f64; 2],
                _: &(),
                dt: f64,
                rng: &mut R,
            ) -> [f64; 2] {
                use rand_distr::{Distribution, StandardNormal};
                let angle = state[1] + 0.1 * dt;
                let n: f64 = StandardNormal.sample(rng);
                [state[0] + n * 0.05, angle]
            }
        }

        // Sensor that only measures distance from origin (ambiguous!)
        struct DistanceOnlySensor;
        impl particle::MeasurementModel<[f64; 2], f64> for DistanceOnlySensor {
            fn likelihood(&self, state: &[f64; 2], measurement: &f64) -> f64 {
                // state[0] is radius, state[1] is angle
                let error = state[0] - measurement;
                (-0.5 * (error / 0.2).powi(2)).exp()
            }
        }

        struct RadialInitial;
        impl rand::distr::Distribution<[f64; 2]> for RadialInitial {
            fn sample<R: rand::Rng + ?Sized>(&self, rng: &mut R) -> [f64; 2] {
                let r1: f64 = rng.random();
                let r2: f64 = rng.random();
                [
                    5.0 + (r1 - 0.5) * 2.0,     // radius ~5
                    r2 * std::f64::consts::TAU, // any angle
                ]
            }
        }

        let mut pf = particle::ParticleFilter::new(
            RadialInitial,
            particle::Config {
                num_particles: 500,
                resample_threshold: 0.5,
            },
            CircularMotion,
            DistanceOnlySensor,
            ChaCha8Rng::seed_from_u64(777),
        );

        // Measure distance = 5 repeatedly
        for _ in 0..50 {
            pf.predict(&(), 0.1);
            pf.update(&5.0).unwrap();
        }

        let est = pf.estimate();
        // Radius should converge to 5, angle could be anything
        assert!(
            (est[0] - 5.0).abs() < 0.5,
            "Radius estimate wrong: {}",
            est[0]
        );
    }
}

mod resilience_tests {
    //! Test filter behavior under adverse conditions

    use super::*;

    #[test]
    fn test_ekf_handles_outliers() {
        // Note: Standard EKF has no built-in outlier rejection.
        // This test verifies the filter can RECOVER from outliers,
        // not that it ignores them.
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        let dt = 0.1;

        // Normal tracking
        for i in 1..=20 {
            ekf.predict(&[], dt);
            ekf.update(&[i as f64 * dt]).unwrap();
        }

        let before_outlier = ekf.estimate();

        // Inject massive outlier
        ekf.predict(&[], dt);
        ekf.update(&[1000.0]).unwrap();

        let after_outlier = ekf.estimate();

        // Filter WILL be affected by outlier (this is expected behavior)
        // but it shouldn't completely trust the bad measurement
        assert!(
            after_outlier[0] > before_outlier[0],
            "Filter should be affected by outlier"
        );
        assert!(
            after_outlier[0] < 1000.0,
            "Filter shouldn't jump all the way to outlier"
        );

        // Continue with normal measurements - should recover
        for i in 22..=100 {
            ekf.predict(&[], dt);
            ekf.update(&[i as f64 * dt]).unwrap();
        }

        let recovered = ekf.estimate();
        let expected = 100.0 * dt;
        assert!(
            (recovered[0] - expected).abs() < 4.0,
            "Failed to recover from outlier: {} vs {}",
            recovered[0],
            expected
        );
    }

    #[test]
    fn test_ekf_handles_missing_measurements() {
        // Run two filters: one with every measurement, one with sparse measurements
        let mut ekf_dense = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [0.5, 0.3],
            ConstantVelocity,
            PositionSensor,
        );

        let mut ekf_sparse = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [0.5, 0.3],
            ConstantVelocity,
            PositionSensor,
        );

        let dt = 0.1;

        for i in 1..=100 {
            // Both filters predict
            ekf_dense.predict(&[], dt);
            ekf_sparse.predict(&[], dt);

            // Dense filter gets every measurement
            ekf_dense.update(&[i as f64 * dt]).unwrap();

            // Sparse filter only gets every 5th measurement
            if i % 5 == 0 {
                ekf_sparse.update(&[i as f64 * dt]).unwrap();
            }
        }

        let est_dense = ekf_dense.estimate();
        let est_sparse = ekf_sparse.estimate();
        let unc_dense = ekf_dense.uncertainty();
        let unc_sparse = ekf_sparse.uncertainty();

        // Both should track reasonably well
        assert!(
            (est_dense[0] - 10.0).abs() < 1.0,
            "Dense tracking failed: {}",
            est_dense[0]
        );
        assert!(
            (est_sparse[0] - 10.0).abs() < 2.0,
            "Sparse tracking failed: {}",
            est_sparse[0]
        );

        // Sparse measurements should result in higher uncertainty
        assert!(
            unc_sparse[0] > unc_dense[0],
            "Sparse should have higher uncertainty: {} vs {}",
            unc_sparse[0],
            unc_dense[0]
        );
    }

    #[test]
    fn test_ekf_numerical_stability_long_run() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        let dt = 0.01;

        // Run for a very long time
        for i in 1..=10000 {
            ekf.predict(&[], dt);
            ekf.update(&[i as f64 * dt]).unwrap();

            let est = ekf.estimate();
            let unc = ekf.uncertainty();

            // Check for NaN or Inf
            assert!(est[0].is_finite(), "NaN/Inf in state at step {}", i);
            assert!(est[1].is_finite(), "NaN/Inf in velocity at step {}", i);
            assert!(unc[0].is_finite(), "NaN/Inf in uncertainty at step {}", i);
            assert!(unc[0] > 0.0, "Uncertainty went non-positive at step {}", i);
        }
    }

    #[test]
    fn test_pf_recovers_from_low_diversity() {
        use rand::SeedableRng;
        use rand_chacha::ChaCha8Rng;

        // Use tight likelihood that will cause weight collapse
        struct TightSensor;
        impl particle::MeasurementModel<[f64; 2], [f64; 1]> for TightSensor {
            fn likelihood(&self, state: &[f64; 2], measurement: &[f64; 1]) -> f64 {
                let error = state[0] - measurement[0];
                (-0.5 * (error / 0.1).powi(2)).exp() // Very tight
            }
        }

        let initial = UniformInitial {
            center: [0.0, 1.0],
            spread: 1.0,
        };

        let mut pf = particle::ParticleFilter::new(
            initial,
            particle::Config {
                num_particles: 200,
                resample_threshold: 0.5,
            },
            SimpleMotion,
            TightSensor,
            ChaCha8Rng::seed_from_u64(123),
        );

        // Run many steps - resampling should keep filter healthy
        for i in 1..=100 {
            pf.predict(&[], 0.1);
            pf.update(&[i as f64 * 0.1]).unwrap();

            // Diversity should stay reasonable due to resampling
            let diversity = pf.particle_diversity();
            assert!(
                diversity > 0.1,
                "Diversity too low at step {}: {}",
                i,
                diversity
            );
        }
    }

    #[test]
    fn test_pf_different_seeds_converge_similarly() {
        use rand::SeedableRng;
        use rand_chacha::ChaCha8Rng;

        let mut estimates: Vec<f64> = Vec::new();

        for seed in [1, 42, 123, 999, 314159] {
            let initial = UniformInitial {
                center: [0.0, 1.0],
                spread: 2.0,
            };

            let mut pf = particle::ParticleFilter::new(
                initial,
                particle::Config {
                    num_particles: 500,
                    resample_threshold: 0.5,
                },
                SimpleMotion,
                SimpleSensor,
                ChaCha8Rng::seed_from_u64(seed),
            );

            for i in 1..=50 {
                pf.predict(&[], 0.1);
                pf.update(&[i as f64 * 0.1]).unwrap();
            }

            estimates.push(pf.estimate()[0]);
        }

        // All estimates should be reasonably close
        let mean: f64 = estimates.iter().sum::<f64>() / estimates.len() as f64;
        let max_deviation = estimates
            .iter()
            .map(|e| (e - mean).abs())
            .fold(0.0, f64::max);

        assert!(
            max_deviation < 0.5,
            "Too much variance between seeds: estimates = {:?}",
            estimates
        );
    }
}

mod comparison_tests {
    //! Compare EKF and Particle Filter on the same problems

    use super::*;
    use rand::SeedableRng;
    use rand_chacha::ChaCha8Rng;

    #[test]
    fn test_ekf_and_pf_agree_on_linear_problem() {
        // For linear problems, both should give similar results

        // EKF setup
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        // Matching PF setup
        let initial = UniformInitial {
            center: [0.0, 1.0],
            spread: 2.0, // Roughly matches EKF uncertainty
        };

        let mut pf = particle::ParticleFilter::new(
            initial,
            particle::Config {
                num_particles: 2000,
                resample_threshold: 0.5,
            },
            SimpleMotion,
            SimpleSensor,
            ChaCha8Rng::seed_from_u64(42),
        );

        let dt = 0.1;

        // Run both with identical measurements
        for i in 1..=50 {
            let measurement = i as f64 * dt;

            ekf.predict(&[], dt);
            ekf.update(&[measurement]).unwrap();

            pf.predict(&[], dt);
            pf.update(&[measurement]).unwrap();
        }

        let ekf_est = ekf.estimate();
        let pf_est = pf.estimate();

        // Should agree to within PF sampling error
        assert!(
            (ekf_est[0] - pf_est[0]).abs() < 0.3,
            "Position disagreement: EKF={}, PF={}",
            ekf_est[0],
            pf_est[0]
        );
        assert!(
            (ekf_est[1] - pf_est[1]).abs() < 0.2,
            "Velocity disagreement: EKF={}, PF={}",
            ekf_est[1],
            pf_est[1]
        );
    }
}

mod step_method_tests {
    //! Test the convenience step() method

    use super::*;

    #[test]
    fn test_step_equals_predict_update() {
        let mut ekf1 = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        let mut ekf2 = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        let dt = 0.1;
        let measurement = [0.15];

        // Method 1: step()
        let est1 = ekf1.step(&[], &measurement, dt).unwrap();

        // Method 2: predict() then update()
        ekf2.predict(&[], dt);
        ekf2.update(&measurement).unwrap();
        let est2 = ekf2.estimate();

        assert_eq!(est1, est2);
    }
}

mod edge_case_tests {
    //! Test edge cases and boundary conditions

    use super::*;
    use rand::SeedableRng;
    use rand_chacha::ChaCha8Rng;

    #[test]
    fn test_ekf_zero_dt() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [5.0, 2.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        let before = ekf.estimate();
        ekf.predict(&[], 0.0);
        let after = ekf.estimate();

        // State should be unchanged with dt=0
        assert_eq!(before, after);
    }

    #[test]
    fn test_ekf_very_small_dt() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        let dt = 1e-10;

        for _ in 0..1000 {
            ekf.predict(&[], dt);
            ekf.update(&[0.0]).unwrap();

            let est = ekf.estimate();
            assert!(est[0].is_finite());
            assert!(est[1].is_finite());
        }
    }

    #[test]
    fn test_ekf_large_dt() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        ekf.predict(&[], 1000.0);
        let est = ekf.estimate();

        // Should predict position = 1000 (velocity * dt)
        assert!((est[0] - 1000.0).abs() < 1.0);
        assert!(est[0].is_finite());
    }

    #[test]
    fn test_pf_single_particle() {
        // Degenerate case: just one particle
        struct SingleParticle;
        impl rand::distr::Distribution<[f64; 2]> for SingleParticle {
            fn sample<R: rand::Rng + ?Sized>(&self, _: &mut R) -> [f64; 2] {
                [0.0, 1.0]
            }
        }

        let mut pf = particle::ParticleFilter::new(
            SingleParticle,
            particle::Config {
                num_particles: 1,
                resample_threshold: 0.5,
            },
            SimpleMotion,
            SimpleSensor,
            ChaCha8Rng::seed_from_u64(1),
        );

        assert_eq!(pf.num_particles(), 1);

        // Should still work (though not well)
        pf.predict(&[], 0.1);
        pf.update(&[0.1]).unwrap();

        let est = pf.estimate();
        assert!(est[0].is_finite());
    }

    #[test]
    fn test_pf_many_particles() {
        let initial = UniformInitial {
            center: [0.0, 1.0],
            spread: 1.0,
        };

        let mut pf = particle::ParticleFilter::new(
            initial,
            particle::Config {
                num_particles: 10000,
                resample_threshold: 0.5,
            },
            SimpleMotion,
            SimpleSensor,
            ChaCha8Rng::seed_from_u64(42),
        );

        assert_eq!(pf.num_particles(), 10000);

        for i in 1..=20 {
            pf.predict(&[], 0.1);
            pf.update(&[i as f64 * 0.1]).unwrap();
        }

        let est = pf.estimate();
        // With many particles, should be very accurate
        assert!((est[0] - 2.0).abs() < 0.2);
    }
}

mod control_input_tests {
    //! Test systems with non-trivial control inputs

    use super::*;
    use rand::SeedableRng;
    use rand_chacha::ChaCha8Rng;

    /// Accelerating object: state = [position, velocity], control = [acceleration]
    struct AcceleratingMotion;

    impl ekf::ProcessModel<2, 1> for AcceleratingMotion {
        fn predict_next(&self, state: &[f64; 2], control: &[f64; 1], dt: f64) -> [f64; 2] {
            let [pos, vel] = *state;
            let [acc] = *control;
            [pos + vel * dt + 0.5 * acc * dt * dt, vel + acc * dt]
        }

        fn process_noise_std(&self) -> [f64; 2] {
            [0.05, 0.02]
        }
    }

    #[test]
    fn test_ekf_with_acceleration_control() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 0.0],
            [0.5, 0.3],
            AcceleratingMotion,
            PositionSensor,
        );

        let dt = 0.1;
        let acceleration = 2.0;

        // Simulate accelerating motion: x = 0.5 * a * t^2
        for i in 1..=50 {
            let t = i as f64 * dt;
            let true_pos = 0.5 * acceleration * t * t;
            let true_vel = acceleration * t;

            ekf.predict(&[acceleration], dt);
            ekf.update(&[true_pos]).unwrap();

            let est = ekf.estimate();

            // Allow some error for filter lag
            if i > 10 {
                assert!(
                    (est[0] - true_pos).abs() < 0.5,
                    "Position error at t={}: {} vs {}",
                    t,
                    est[0],
                    true_pos
                );
                assert!(
                    (est[1] - true_vel).abs() < 0.5,
                    "Velocity error at t={}: {} vs {}",
                    t,
                    est[1],
                    true_vel
                );
            }
        }
    }

    #[test]
    fn test_ekf_varying_control_inputs() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 0.0],
            [1.0, 0.5],
            AcceleratingMotion,
            PositionSensor,
        );

        let dt = 0.1;
        let mut true_pos = 0.0;
        let mut true_vel = 0.0;

        // Alternating acceleration pattern
        for i in 1..=100 {
            let acc = if (i / 10) % 2 == 0 { 1.0 } else { -1.0 };

            true_pos += true_vel * dt + 0.5 * acc * dt * dt;
            true_vel += acc * dt;

            ekf.predict(&[acc], dt);
            ekf.update(&[true_pos]).unwrap();
        }

        let est = ekf.estimate();
        assert!(
            (est[0] - true_pos).abs() < 1.0,
            "Position: {} vs {}",
            est[0],
            true_pos
        );
    }

    #[test]
    fn test_pf_with_control_inputs() {
        struct AccelMotionPF;

        impl particle::ProcessModel<[f64; 2], [f64; 1]> for AccelMotionPF {
            fn sample_next<R: rand::Rng>(
                &self,
                state: &[f64; 2],
                control: &[f64; 1],
                dt: f64,
                rng: &mut R,
            ) -> [f64; 2] {
                use rand_distr::{Distribution, StandardNormal};
                let [pos, vel] = *state;
                let [acc] = *control;
                let n1: f64 = StandardNormal.sample(rng);
                let n2: f64 = StandardNormal.sample(rng);
                [
                    pos + vel * dt + 0.5 * acc * dt * dt + n1 * 0.05,
                    vel + acc * dt + n2 * 0.02,
                ]
            }
        }

        let initial = UniformInitial {
            center: [0.0, 0.0],
            spread: 1.0,
        };

        let mut pf = particle::ParticleFilter::new(
            initial,
            particle::Config {
                num_particles: 500,
                resample_threshold: 0.5,
            },
            AccelMotionPF,
            SimpleSensor,
            ChaCha8Rng::seed_from_u64(42),
        );

        let dt = 0.1;
        let acc = 2.0;

        for i in 1..=30 {
            let t = i as f64 * dt;
            let true_pos = 0.5 * acc * t * t;

            pf.predict(&[acc], dt);
            pf.update(&[true_pos]).unwrap();
        }

        let est = pf.estimate();
        let final_true_pos = 0.5 * acc * (30.0 * dt).powi(2);
        assert!(
            (est[0] - final_true_pos).abs() < 0.5,
            "PF position: {} vs {}",
            est[0],
            final_true_pos
        );
    }
}

mod higher_dimensional_tests {
    //! Test with larger state spaces

    use super::*;
    use rand::SeedableRng;
    use rand_chacha::ChaCha8Rng;

    /// 4D state: [x, y, vx, vy] - 2D position and velocity
    struct Motion2D;

    impl ekf::ProcessModel<4, 0> for Motion2D {
        fn predict_next(&self, state: &[f64; 4], _: &[f64; 0], dt: f64) -> [f64; 4] {
            [
                state[0] + state[2] * dt,
                state[1] + state[3] * dt,
                state[2],
                state[3],
            ]
        }

        fn process_noise_std(&self) -> [f64; 4] {
            [0.1, 0.1, 0.05, 0.05]
        }
    }

    struct PositionSensor2D;

    impl ekf::MeasurementModel<4, 2> for PositionSensor2D {
        fn expected_measurement(&self, state: &[f64; 4]) -> [f64; 2] {
            [state[0], state[1]]
        }

        fn measurement_noise_std(&self) -> [f64; 2] {
            [0.5, 0.5]
        }
    }

    #[test]
    fn test_ekf_4d_tracking() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 0.0, 1.0, 0.5], // Moving diagonally
            [1.0, 1.0, 0.5, 0.5],
            Motion2D,
            PositionSensor2D,
        );

        let dt = 0.1;
        let vx = 1.0;
        let vy = 0.5;

        for i in 1..=50 {
            let t = i as f64 * dt;
            ekf.predict(&[], dt);
            ekf.update(&[vx * t, vy * t]).unwrap();
        }

        let est = ekf.estimate();
        let final_t = 50.0 * dt;

        assert!((est[0] - vx * final_t).abs() < 0.5, "X position error");
        assert!((est[1] - vy * final_t).abs() < 0.5, "Y position error");
        assert!((est[2] - vx).abs() < 0.3, "X velocity error");
        assert!((est[3] - vy).abs() < 0.3, "Y velocity error");
    }

    /// 6D state: [x, y, z, vx, vy, vz] - 3D position and velocity
    struct Motion3D;

    impl ekf::ProcessModel<6, 0> for Motion3D {
        fn predict_next(&self, state: &[f64; 6], _: &[f64; 0], dt: f64) -> [f64; 6] {
            [
                state[0] + state[3] * dt,
                state[1] + state[4] * dt,
                state[2] + state[5] * dt,
                state[3],
                state[4],
                state[5],
            ]
        }

        fn process_noise_std(&self) -> [f64; 6] {
            [0.1, 0.1, 0.1, 0.05, 0.05, 0.05]
        }
    }

    struct PositionSensor3D;

    impl ekf::MeasurementModel<6, 3> for PositionSensor3D {
        fn expected_measurement(&self, state: &[f64; 6]) -> [f64; 3] {
            [state[0], state[1], state[2]]
        }

        fn measurement_noise_std(&self) -> [f64; 3] {
            [0.5, 0.5, 0.5]
        }
    }

    #[test]
    fn test_ekf_6d_tracking() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 0.0, 0.0, 1.0, 2.0, -0.5],
            [1.0, 1.0, 1.0, 0.5, 0.5, 0.5],
            Motion3D,
            PositionSensor3D,
        );

        let dt = 0.1;
        let velocity = [1.0, 2.0, -0.5];

        for i in 1..=50 {
            let t = i as f64 * dt;
            ekf.predict(&[], dt);
            ekf.update(&[velocity[0] * t, velocity[1] * t, velocity[2] * t])
                .unwrap();
        }

        let est = ekf.estimate();
        let final_t = 50.0 * dt;

        for dim in 0..3 {
            assert!(
                (est[dim] - velocity[dim] * final_t).abs() < 0.5,
                "Dimension {} position error",
                dim
            );
            assert!(
                (est[dim + 3] - velocity[dim]).abs() < 0.3,
                "Dimension {} velocity error",
                dim
            );
        }
    }

    #[test]
    fn test_pf_4d_tracking() {
        struct Motion2DPF;

        impl particle::ProcessModel<[f64; 4], [f64; 0]> for Motion2DPF {
            fn sample_next<R: rand::Rng>(
                &self,
                state: &[f64; 4],
                _: &[f64; 0],
                dt: f64,
                rng: &mut R,
            ) -> [f64; 4] {
                use rand_distr::{Distribution, StandardNormal};
                let n1: f64 = StandardNormal.sample(rng);
                let n2: f64 = StandardNormal.sample(rng);
                let n3: f64 = StandardNormal.sample(rng);
                let n4: f64 = StandardNormal.sample(rng);
                [
                    state[0] + state[2] * dt + n1 * 0.1,
                    state[1] + state[3] * dt + n2 * 0.1,
                    state[2] + n3 * 0.05,
                    state[3] + n4 * 0.05,
                ]
            }
        }

        struct Sensor2DPF;

        impl particle::MeasurementModel<[f64; 4], [f64; 2]> for Sensor2DPF {
            fn likelihood(&self, state: &[f64; 4], measurement: &[f64; 2]) -> f64 {
                let ex = state[0] - measurement[0];
                let ey = state[1] - measurement[1];
                (-0.5 * (ex * ex + ey * ey) / 0.25).exp()
            }
        }

        struct Initial4D;
        impl rand::distr::Distribution<[f64; 4]> for Initial4D {
            fn sample<R: rand::Rng + ?Sized>(&self, rng: &mut R) -> [f64; 4] {
                let r1: f64 = rng.random();
                let r2: f64 = rng.random();
                let r3: f64 = rng.random();
                let r4: f64 = rng.random();
                [
                    (r1 - 0.5) * 2.0,
                    (r2 - 0.5) * 2.0,
                    1.0 + (r3 - 0.5) * 0.5,
                    0.5 + (r4 - 0.5) * 0.5,
                ]
            }
        }

        let mut pf = particle::ParticleFilter::new(
            Initial4D,
            particle::Config {
                num_particles: 1000,
                resample_threshold: 0.5,
            },
            Motion2DPF,
            Sensor2DPF,
            ChaCha8Rng::seed_from_u64(42),
        );

        let dt = 0.1;
        for i in 1..=30 {
            let t = i as f64 * dt;
            pf.predict(&[], dt);
            pf.update(&[t, 0.5 * t]).unwrap();
        }

        let est = pf.estimate();
        let final_t = 30.0 * dt;

        assert!((est[0] - final_t).abs() < 0.5, "X position");
        assert!((est[1] - 0.5 * final_t).abs() < 0.5, "Y position");
    }
}

mod multiple_update_tests {
    //! Test multiple measurement updates per prediction

    use super::*;

    #[test]
    fn test_ekf_multiple_sensors() {
        // Two sensors measuring the same position
        struct AccurateSensor;
        impl ekf::MeasurementModel<2, 1> for AccurateSensor {
            fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
                [state[0]]
            }
            fn measurement_noise_std(&self) -> [f64; 1] {
                [0.1]
            }
        }

        struct NoisySensor;
        impl ekf::MeasurementModel<2, 1> for NoisySensor {
            fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
                [state[0]]
            }
            fn measurement_noise_std(&self) -> [f64; 1] {
                [1.0]
            }
        }

        let mut ekf_accurate = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            AccurateSensor,
        );

        let mut ekf_noisy =
            ekf::ExtendedKalmanFilter::new([0.0, 1.0], [1.0, 0.5], ConstantVelocity, NoisySensor);

        let dt = 0.1;
        for i in 1..=50 {
            let true_pos = i as f64 * dt;

            ekf_accurate.predict(&[], dt);
            ekf_accurate.update(&[true_pos]).unwrap();

            ekf_noisy.predict(&[], dt);
            ekf_noisy.update(&[true_pos]).unwrap();
        }

        // Accurate sensor should yield lower uncertainty
        let unc_accurate = ekf_accurate.uncertainty();
        let unc_noisy = ekf_noisy.uncertainty();

        assert!(
            unc_accurate[0] < unc_noisy[0],
            "Accurate sensor should give lower uncertainty: {} vs {}",
            unc_accurate[0],
            unc_noisy[0]
        );
    }

    #[test]
    fn test_ekf_multiple_updates_per_predict() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        let dt = 0.1;

        for i in 1..=20 {
            let true_pos = i as f64 * dt;

            ekf.predict(&[], dt);

            // Multiple updates with slightly different measurements
            ekf.update(&[true_pos - 0.1]).unwrap();
            ekf.update(&[true_pos + 0.1]).unwrap();
            ekf.update(&[true_pos]).unwrap();
        }

        let est = ekf.estimate();
        let unc = ekf.uncertainty();

        // Should track well
        assert!((est[0] - 2.0).abs() < 0.3);

        // Multiple updates should reduce uncertainty further
        assert!(
            unc[0] < 0.3,
            "Uncertainty should be low with multiple updates"
        );
    }
}

mod reset_and_initialization_tests {
    //! Test reset() and with_covariance() methods

    use super::*;

    #[test]
    fn test_ekf_reset() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        // Run for a while
        for i in 1..=20 {
            ekf.predict(&[], 0.1);
            ekf.update(&[i as f64 * 0.1]).unwrap();
        }

        let before_reset = ekf.estimate();
        assert!(before_reset[0] > 1.0); // Moved from origin

        // Reset to new state
        ekf.reset([100.0, 5.0], [2.0, 1.0]);

        let after_reset = ekf.estimate();
        let unc_after = ekf.uncertainty();

        assert_eq!(after_reset[0], 100.0);
        assert_eq!(after_reset[1], 5.0);
        assert!((unc_after[0] - 2.0).abs() < 0.01);
        assert!((unc_after[1] - 1.0).abs() < 0.01);
    }

    #[test]
    fn test_ekf_with_covariance() {
        // Non-diagonal initial covariance (correlated uncertainty)
        let initial_cov = [[1.0, 0.5], [0.5, 1.0]];

        let ekf = ekf::ExtendedKalmanFilter::with_covariance(
            [0.0, 1.0],
            initial_cov,
            ConstantVelocity,
            PositionSensor,
        );

        let unc = ekf.uncertainty();
        assert!((unc[0] - 1.0).abs() < 0.01);
        assert!((unc[1] - 1.0).abs() < 0.01);
    }

    #[test]
    fn test_ekf_reset_recovers_tracking() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0],
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        // Get into a bad state by giving wrong measurements
        for _ in 0..20 {
            ekf.predict(&[], 0.1);
            ekf.update(&[1000.0]).unwrap(); // Way off
        }

        // Reset to correct state
        ekf.reset([0.0, 1.0], [0.5, 0.3]);

        // Should track correctly again
        for i in 1..=20 {
            ekf.predict(&[], 0.1);
            ekf.update(&[i as f64 * 0.1]).unwrap();
        }

        let est = ekf.estimate();
        assert!((est[0] - 2.0).abs() < 0.5);
    }
}

mod jacobian_tests {
    //! Test numerical vs analytical Jacobians

    use super::*;
    use ekf::ProcessModel;

    #[test]
    fn test_numerical_jacobian_accuracy() {
        // Test on a known nonlinear function
        fn nonlinear(x: &[f64; 2]) -> [f64; 2] {
            [x[0].powi(2) + x[1].sin(), x[0] * x[1].exp()]
        }

        let point = [1.0, 0.5];
        let jac = ekf::numerical_jacobian(nonlinear, &point);

        // Analytical derivatives:
        // df1/dx0 = 2*x0 = 2.0
        // df1/dx1 = cos(x1) ≈ 0.8776
        // df2/dx0 = exp(x1) ≈ 1.6487
        // df2/dx1 = x0 * exp(x1) ≈ 1.6487

        assert!((jac[0][0] - 2.0).abs() < 1e-5, "df1/dx0");
        assert!((jac[1][0] - 0.5_f64.cos()).abs() < 1e-5, "df1/dx1");
        assert!((jac[0][1] - 0.5_f64.exp()).abs() < 1e-5, "df2/dx0");
        assert!((jac[1][1] - 0.5_f64.exp()).abs() < 1e-5, "df2/dx1");
    }

    #[test]
    fn test_analytical_matches_numerical() {
        struct AnalyticalModel;

        impl ekf::ProcessModel<2, 0> for AnalyticalModel {
            fn predict_next(&self, state: &[f64; 2], _: &[f64; 0], dt: f64) -> [f64; 2] {
                // Nonlinear: x' = x + v*dt, v' = v * cos(x)
                [state[0] + state[1] * dt, state[1] * state[0].cos()]
            }

            fn process_noise_std(&self) -> [f64; 2] {
                [0.1, 0.1]
            }

            fn prediction_sensitivity(
                &self,
                state: &[f64; 2],
                _: &[f64; 0],
                dt: f64,
            ) -> [[f64; 2]; 2] {
                // Analytical Jacobian
                [
                    [1.0, -state[1] * state[0].sin()], // d/dx0 of [x', v']
                    [dt, state[0].cos()],              // d/dx1 of [x', v']
                ]
            }
        }

        let state = [0.5, 1.0];
        let dt = 0.1;

        // Compute numerical Jacobian
        let numerical =
            ekf::numerical_jacobian(|s| AnalyticalModel.predict_next(s, &[], dt), &state);

        // Get analytical
        let analytical = AnalyticalModel.prediction_sensitivity(&state, &[], dt);

        for i in 0..2 {
            for j in 0..2 {
                assert!(
                    (numerical[i][j] - analytical[i][j]).abs() < 1e-5,
                    "Mismatch at [{},{}]: numerical={}, analytical={}",
                    i,
                    j,
                    numerical[i][j],
                    analytical[i][j]
                );
            }
        }
    }
}

mod particle_filter_advanced_tests {
    //! Additional particle filter tests

    use super::*;
    use rand::SeedableRng;
    use rand_chacha::ChaCha8Rng;

    #[test]
    fn test_pf_best_particle() {
        let initial = UniformInitial {
            center: [0.0, 1.0],
            spread: 0.5,
        };

        let mut pf = particle::ParticleFilter::new(
            initial,
            particle::Config {
                num_particles: 100,
                resample_threshold: 0.5,
            },
            SimpleMotion,
            SimpleSensor,
            ChaCha8Rng::seed_from_u64(42),
        );

        // After measurement, best particle should be close to measurement
        pf.predict(&[], 0.1);
        pf.update(&[0.1]).unwrap();

        let best = pf.best_particle().unwrap();
        let estimate = pf.estimate();

        // Best particle should be reasonable
        assert!(best[0].is_finite());

        // Estimate should be close to best particle (though not necessarily identical)
        assert!((best[0] - estimate[0]).abs() < 0.5);
    }

    #[test]
    fn test_pf_from_particles() {
        // Initialize from explicit particle list
        let particles = vec![[0.0, 1.0], [0.1, 1.0], [-0.1, 1.0], [0.0, 1.1], [0.0, 0.9]];

        let mut pf = particle::ParticleFilter::from_particles(
            particles.clone(),
            particle::Config {
                num_particles: 5,
                resample_threshold: 0.5,
            },
            SimpleMotion,
            SimpleSensor,
            ChaCha8Rng::seed_from_u64(42),
        );

        assert_eq!(pf.num_particles(), 5);

        // Initial estimate should be mean of particles
        let est = pf.estimate();
        let mean_x: f64 = particles.iter().map(|p| p[0]).sum::<f64>() / 5.0;
        let mean_v: f64 = particles.iter().map(|p| p[1]).sum::<f64>() / 5.0;

        assert!((est[0] - mean_x).abs() < 0.01);
        assert!((est[1] - mean_v).abs() < 0.01);

        // Should still work after update
        pf.predict(&[], 0.1);
        pf.update(&[0.1]).unwrap();

        let new_est = pf.estimate();
        assert!(new_est[0].is_finite());
    }

    #[test]
    fn test_pf_resampling_thresholds() {
        // Test with different resampling thresholds
        for threshold in [0.1, 0.5, 0.9] {
            let initial = UniformInitial {
                center: [0.0, 1.0],
                spread: 1.0,
            };

            let mut pf = particle::ParticleFilter::new(
                initial,
                particle::Config {
                    num_particles: 200,
                    resample_threshold: threshold,
                },
                SimpleMotion,
                SimpleSensor,
                ChaCha8Rng::seed_from_u64(42),
            );

            for i in 1..=30 {
                pf.predict(&[], 0.1);
                pf.update(&[i as f64 * 0.1]).unwrap();
            }

            let est = pf.estimate();
            assert!(
                (est[0] - 3.0).abs() < 0.5,
                "Threshold {} failed: estimate = {}",
                threshold,
                est[0]
            );
        }
    }

    #[test]
    fn test_pf_particle_diversity_reporting() {
        let initial = UniformInitial {
            center: [0.0, 1.0],
            spread: 2.0,
        };

        let mut pf = particle::ParticleFilter::new(
            initial,
            particle::Config {
                num_particles: 500,
                resample_threshold: 0.5,
            },
            SimpleMotion,
            SimpleSensor,
            ChaCha8Rng::seed_from_u64(42),
        );

        // Initial diversity should be high (all weights equal)
        let initial_diversity = pf.particle_diversity();
        assert!(initial_diversity > 0.9, "Initial diversity should be ~1.0");

        // After update, diversity typically drops
        pf.predict(&[], 0.1);
        pf.update(&[0.1]).unwrap();

        let diversity = pf.particle_diversity();
        assert!(
            diversity > 0.0 && diversity <= 1.0,
            "Diversity should be in (0, 1]: {}",
            diversity
        );
    }
}

mod stationary_tracking_tests {
    //! Test tracking of stationary or slow-moving objects

    use super::*;

    #[test]
    fn test_ekf_stationary_object() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [5.0, 0.0], // Stationary at position 5
            [1.0, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        let dt = 0.1;

        // Object stays at position 5
        for _ in 0..50 {
            ekf.predict(&[], dt);
            ekf.update(&[5.0]).unwrap();
        }

        let est = ekf.estimate();
        assert!((est[0] - 5.0).abs() < 0.2, "Position should stay at 5");
        assert!(est[1].abs() < 0.2, "Velocity should be ~0");
    }

    #[test]
    fn test_ekf_object_stops() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 1.0], // Moving
            [0.5, 0.5],
            ConstantVelocity,
            PositionSensor,
        );

        let dt = 0.1;

        // First phase: moving
        for i in 1..=20 {
            ekf.predict(&[], dt);
            ekf.update(&[i as f64 * dt]).unwrap();
        }

        // Second phase: stopped at position 2
        for _ in 0..50 {
            ekf.predict(&[], dt);
            ekf.update(&[2.0]).unwrap();
        }

        let est = ekf.estimate();
        assert!((est[0] - 2.0).abs() < 0.3, "Position should converge to 2");
        assert!(est[1].abs() < 0.3, "Velocity should converge to 0");
    }
}

mod observability_tests {
    //! Test behavior when state is not fully observable

    use super::*;

    /// Sensor that only measures position, not velocity
    struct PositionOnlySensor;

    impl ekf::MeasurementModel<2, 1> for PositionOnlySensor {
        fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
            [state[0]]
        }
        fn measurement_noise_std(&self) -> [f64; 1] {
            [0.5]
        }
    }

    #[test]
    fn test_velocity_inferred_from_position_changes() {
        let mut ekf = ekf::ExtendedKalmanFilter::new(
            [0.0, 0.0], // Unknown velocity initially
            [0.5, 2.0], // High velocity uncertainty
            ConstantVelocity,
            PositionOnlySensor,
        );

        let dt = 0.1;
        let true_velocity = 1.5;

        // Feed position measurements and velocity should be inferred
        for i in 1..=50 {
            ekf.predict(&[], dt);
            ekf.update(&[true_velocity * i as f64 * dt]).unwrap();
        }

        let est = ekf.estimate();
        assert!(
            (est[1] - true_velocity).abs() < 0.3,
            "Velocity should be inferred: {} vs {}",
            est[1],
            true_velocity
        );
    }

    #[test]
    fn test_unobservable_state_maintains_uncertainty() {
        // If we only measure sum of states, individual components are unobservable

        struct SumSensor;
        impl ekf::MeasurementModel<2, 1> for SumSensor {
            fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
                [state[0] + state[1]] // Only measures sum
            }
            fn measurement_noise_std(&self) -> [f64; 1] {
                [0.1]
            }
        }

        struct Identity;
        impl ekf::ProcessModel<2, 0> for Identity {
            fn predict_next(&self, state: &[f64; 2], _: &[f64; 0], _dt: f64) -> [f64; 2] {
                *state // State doesn't change
            }
            fn process_noise_std(&self) -> [f64; 2] {
                [0.01, 0.01]
            }
        }

        let mut ekf = ekf::ExtendedKalmanFilter::new([0.0, 0.0], [1.0, 1.0], Identity, SumSensor);

        // Measure sum = 1.0 repeatedly
        for _ in 0..100 {
            ekf.predict(&[], 0.1);
            ekf.update(&[1.0]).unwrap();
        }

        let est = ekf.estimate();

        // Sum should converge to 1.0
        assert!(
            (est[0] + est[1] - 1.0).abs() < 0.1,
            "Sum should be 1.0: {} + {} = {}",
            est[0],
            est[1],
            est[0] + est[1]
        );

        // But individual uncertainties won't collapse as much as if
        // we could observe them directly
        let unc = ekf.uncertainty();
        // Note: Due to process noise and observability, individual uncertainties
        // should still be reduced somewhat but remain meaningful
        assert!(unc[0] > 0.01 && unc[1] > 0.01);
    }
}