mod support;
use kaptan_config::ConfigModel;
use kaptan_flight_runtime::*;
use kaptan_math::Vector3;
use kaptan_navigation::{
    AccelerationFrd, AngularRateFrd, NavigationFailure, PositionNed, SensorRecord, VelocityNed,
};
use support::*;

#[test]
fn actual_navigation_fixed_reference_origin_and_delayed_aiding() {
    let (ep, out, now) = ready();
    let estimate = ep.status().estimate.unwrap();
    assert!(
        estimate.capabilities.flight_ready(),
        "{:?}",
        estimate.capabilities
    );
    assert!(estimate.position.vector().norm() < 0.01);
    assert!((estimate.origin.launch_in_reference.vector().x() - 12.).abs() < 0.001);
    assert!(estimate.fused_through.as_microseconds() <= now - 100_000);
    assert_eq!(estimate.at, time(now));
    assert!(
        estimate
            .ground_clearance
            .unwrap()
            .contact_clearance_metres
            .abs()
            < 0.002
    );
    assert!(out.outputs.iter().all(|v| !v.permitted()));
}
#[test]
fn navigation_failures_retain_their_reason() {
    for (delay, observe, failure, reason) in [
        (
            30_000,
            true,
            NavigationFailure::ImuGap,
            TerminalReason::ImuLost,
        ),
        (
            100_000,
            false,
            NavigationFailure::WorkBudget,
            TerminalReason::NavigationLost,
        ),
    ] {
        let (mut ep, mut out, now) = ready();
        let now = now + delay;
        let error = if observe {
            ep.observe(
                ep.status().epoch,
                SensorRecord::Imu(imu(now)),
                time(now),
                &mut out,
            )
            .unwrap_err()
        } else {
            ep.tick(time(now), &mut out).unwrap_err()
        };
        assert_eq!(error.primary, EndpointFailure::Navigation(failure));
        assert_eq!(ep.status().terminal_reason, Some(reason));
        assert_eq!(ep.status().phase, VehiclePhase::Terminated);
        assert!(!out.outputs.last().unwrap().permitted());
    }
}

#[test]
fn healthy_armed_idle_expires_without_rearming() {
    let (mut ep, mut out, now) = armed();
    for t in (now + 10_000..=now + 10_500_000).step_by(10_000) {
        feed(&mut ep, &mut out, t);
    }
    assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
    assert_eq!(ep.status().arm, None);
    assert!(out.outputs.iter().all(|v| !v.permitted()));
}

#[test]
fn declared_noise_realizations_initialize_and_remain_grounded() {
    // Box-Muller Gaussian draws, independently implemented at the sensor boundary.
    // q/T gives IMU per-sample variance; no packet carries an ImuNoise value.
    let mut seed = 0xaca5_8113_78ab_1234_u64;
    let mut uniform = || {
        seed ^= seed << 13;
        seed ^= seed >> 7;
        seed ^= seed << 17;
        ((seed >> 11) as f64 + 0.5) / 9_007_199_254_740_992.
    };
    let mut normal = || (-2. * uniform().ln()).sqrt() * (core::f64::consts::TAU * uniform()).cos();
    let profile = profile_fixture::candidate()
        .validate(&profile_fixture::capabilities())
        .unwrap();
    let mut ep = FlightEndpoint::new(
        profile,
        RunBinding {
            session: TransportSessionId(7),
            owner: CommandOwner(1),
        },
    )
    .unwrap();
    let mut out = Recording::default();
    for m in (10_000..=6_000_000).step_by(10_000) {
        let t = m + 1_000;
        for mut record in records(t) {
            match &mut record {
                SensorRecord::Imu(r) => {
                    let s = (2.5e-5_f64 / 0.01).sqrt();
                    r.acceleration = AccelerationFrd::metres_per_second_squared(
                        (normal() * s) as f32,
                        (normal() * s) as f32,
                        (-f64::from(kaptan_navigation::GRAVITY_MPS2) + normal() * s) as f32,
                    );
                    let s = (2.5e-7_f64 / 0.01).sqrt();
                    r.angular_rate = AngularRateFrd::radians_per_second(
                        (normal() * s) as f32,
                        (normal() * s) as f32,
                        (normal() * s) as f32,
                    );
                }
                SensorRecord::LocalPositionVelocity(r) => {
                    r.position = PositionNed::metres(
                        (12. + normal() * 0.1) as f32,
                        (-7. + normal() * 0.1) as f32,
                        (4. + normal() * 0.1) as f32,
                    );
                    r.velocity = VelocityNed::metres_per_second(
                        (normal() * 0.05) as f32,
                        (normal() * 0.05) as f32,
                        (normal() * 0.05) as f32,
                    );
                }
                SensorRecord::Magnetic(r) => {
                    r.microtesla_frd = Vector3::new(
                        (20. + normal() * 0.02) as f32,
                        (normal() * 0.02) as f32,
                        (45. + normal() * 0.02) as f32,
                    )
                }
                SensorRecord::Pressure(r) => r.pascals = (101_325. + normal() * 0.3) as f32,
                SensorRecord::Range(r) => {
                    r.metres = (((0.04 + normal() * 0.002) / 0.001).round() * 0.001) as f32
                }
            }
            let admitted = ep
                .observe(ep.status().epoch, record, time(t), &mut out)
                .unwrap();
            assert!(
                admitted == ObservationAdmission::Queued
                    || admitted
                        == ObservationAdmission::Rejected(
                            ObservationRejection::BehindFusionHorizon
                        )
            );
        }
        ep.tick(time(t), &mut out).unwrap();
    }
    let e = ep.status().estimate.unwrap();
    assert!(e.capabilities.flight_ready(), "{:?}", e.capabilities);
    assert!(e.position.vector().norm() < 0.1, "{:?}", e.position);
    assert_eq!(ep.status().ground_state, GroundState::Grounded);
    assert!(out.outputs.iter().all(|r| !r.permitted()));
}