//! Flight observation, uncertainty and bounded execution contracts.

use crate::*;
use kaptan_math::{Frd, Ned, UnitQuaternion, Vector3};
use kaptan_time::Timestamp;

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct OriginId(pub u32);
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct SensorReferenceId(pub u32);

/// Mean calibrated force/rate over the declared preceding acquisition window.
/// Interior propagation reconstructs that window with its constant average.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct FlightImuObservation {
    pub context: ObservationContext,
    pub acceleration: AccelerationFrd,
    pub angular_rate: AngularRateFrd,
    /// Positive duration in the endpoint clock, bounded by the configured IMU
    /// interval. Both window endpoints must share the same timing model.
    pub acquisition_duration_us: u32,
}
impl FlightImuObservation {
    pub(crate) fn acquisition_start(self) -> Option<u64> {
        if self.acquisition_duration_us == 0 {
            return None;
        }
        self.context
            .measurement_time
            .as_microseconds()
            .checked_sub(u64::from(self.acquisition_duration_us))
    }
}
/// Timing of the latest valid admitted IMU window, possibly not yet propagated.
/// This historical snapshot does not establish freshness, estimator health or
/// output authority. It may remain available after a numerical failure.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct ImuCoverage {
    pub end: Timestamp,
    pub acquisition_duration_us: u32,
    pub time_uncertainty_microseconds: u32,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct MagneticObservation {
    pub context: ObservationContext,
    pub microtesla_frd: Vector3<Frd>,
    pub variance_microtesla_squared: [ScalarVariance; 3],
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PressureObservation {
    pub context: ObservationContext,
    pub pascals: f32,
    pub variance_pascals_squared: ScalarVariance,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum SensorRecord {
    Imu(FlightImuObservation),
    LocalPositionVelocity(LocalPositionVelocityObservation),
    Magnetic(MagneticObservation),
    Pressure(PressureObservation),
    Range(RangeObservation),
}
impl SensorRecord {
    pub const fn context(self) -> ObservationContext {
        match self {
            Self::Imu(v) => v.context,
            Self::LocalPositionVelocity(v) => v.context,
            Self::Magnetic(v) => v.context,
            Self::Pressure(v) => v.context,
            Self::Range(v) => v.context,
        }
    }
    pub const fn kind(self) -> u8 {
        match self {
            Self::Imu(_) => 1,
            Self::LocalPositionVelocity(_) => 2,
            Self::Magnetic(_) => 3,
            Self::Pressure(_) => 4,
            Self::Range(_) => 5,
        }
    }
}

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u16)]
pub enum ObservationRejection {
    WrongEpoch = 1,
    InvalidTime = 2,
    TooOld = 3,
    Replay = 4,
    SourceMismatch = 5,
    FrameMismatch = 6,
    CalibrationMismatch = 7,
    InvalidValue = 8,
    InvalidQuality = 9,
    QueueFull = 10,
    RateExceeded = 11,
    BehindFusionHorizon = 12,
    ReferenceMismatch = 13,
    Terminated = 14,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum ObservationAdmission {
    Queued,
    Rejected(ObservationRejection),
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum NavigationFailure {
    ImuGap,
    QueueOverrun,
    WorkBudget,
    Numerical,
    RequiredCapabilityLost,
}
impl From<kaptan_math::MathError> for NavigationFailure {
    fn from(_: kaptan_math::MathError) -> Self {
        Self::Numerical
    }
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum SourceStatus {
    Unknown,
    Healthy,
    Stale,
    Isolated,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct SensorStatus {
    pub kind: u8,
    pub source: SourceId,
    pub integrity: SourceStatus,
    pub last_received_measurement: Option<Timestamp>,
    pub last_fused_measurement: Option<Timestamp>,
    pub queued: u16,
    pub rejected: u32,
    pub fused: u32,
    pub dropped: u32,
}

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct LocalOrigin {
    pub id: OriginId,
    pub established_at: Timestamp,
    pub reference: SensorReferenceId,
    pub launch_in_reference: PositionNed,
    pub launch_variance_m2: [f32; 3],
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct CapabilityEvidence {
    pub available: bool,
    pub last_measurement_at: Option<Timestamp>,
    pub valid_until: Option<Timestamp>,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct FlightCapabilities {
    pub tilt: CapabilityEvidence,
    pub heading: CapabilityEvidence,
    pub horizontal_position: CapabilityEvidence,
    pub horizontal_velocity: CapabilityEvidence,
    pub height: CapabilityEvidence,
    pub vertical_velocity: CapabilityEvidence,
    pub ground_clearance: CapabilityEvidence,
}
impl FlightCapabilities {
    pub fn flight_ready(self) -> bool {
        self.tilt.available
            && self.heading.available
            && self.horizontal_position.available
            && self.horizontal_velocity.available
            && self.height.available
            && self.vertical_velocity.available
            && self.ground_clearance.available
    }
    pub fn valid_until(self) -> Option<Timestamp> {
        if !self.flight_ready() {
            return None;
        }
        [
            self.tilt,
            self.heading,
            self.horizontal_position,
            self.horizontal_velocity,
            self.height,
            self.vertical_velocity,
            self.ground_clearance,
        ]
        .into_iter()
        .map(|v| v.valid_until)
        .try_fold(Timestamp::from_microseconds(u64::MAX), |a, b| {
            Some(a.min(b?))
        })
    }
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct GroundClearance {
    pub measurement_at: Timestamp,
    pub valid_until: Timestamp,
    pub source: SourceId,
    pub body_origin_height_metres: f32,
    pub contact_clearance_metres: f32,
    pub variance_m2: f32,
    pub measurement_time_uncertainty_microseconds: u32,
    pub upper_error_metres: f32,
}
impl GroundClearance {
    /// Bound current clearance using current foot orientation and velocity
    /// confidence. `upper_error_metres` bounds measured COM height, not a foot
    /// projection frozen at the range timestamp.
    pub fn upper_at(
        self,
        now: Timestamp,
        estimate: &FlightEstimate,
        contact: ContactGeometry,
    ) -> Option<f32> {
        let speed_bound =
            estimate.velocity_error_bound().norm() + estimate.velocity.vector().norm();
        let angle_bound = estimate.attitude_error_bound().norm();
        if now >= self.valid_until
            || estimate.at != now
            || !speed_bound.is_finite()
            || !angle_bound.is_finite()
            || !contact.validate()
        {
            return None;
        }
        let earliest = self
            .measurement_at
            .as_microseconds()
            .checked_sub(u64::from(self.measurement_time_uncertainty_microseconds))?;
        let age = now.as_microseconds().checked_sub(earliest)? as f32 * 1e-6;
        // Configuration checks the vehicle's free-flight acceleration bound.
        let feet = contact.depth(estimate.attitude);
        let upper = self.body_origin_height_metres
            + self.upper_error_metres
            + speed_bound * age
            + 0.5 * MAXIMUM_FREE_ACCELERATION_MPS2 * age * age
            - feet
            + contact.rotation_radius_bound() * angle_bound;
        upper.is_finite().then_some(upper)
    }
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct FlightEstimate {
    pub at: Timestamp,
    pub fused_through: Timestamp,
    /// Latest complete IMU acquisition-window end, not its start or receipt.
    pub last_imu_at: Timestamp,
    pub origin: LocalOrigin,
    pub attitude: UnitQuaternion<Frd, Ned>,
    pub position: PositionNed,
    pub velocity: VelocityNed,
    /// Bias-corrected average of the latest complete declared window. This is not
    /// an instantaneous angular-rate measurement at `at`.
    pub angular_rate: AngularRateFrd,
    pub position_variance_m2: [f32; 3],
    pub velocity_variance_m2ps2: [f32; 3],
    pub attitude_variance_rad2: [f32; 3],
    pub timing_error_bound: TimingErrorBounds,
    pub capabilities: FlightCapabilities,
    pub ground_clearance: Option<GroundClearance>,
}
/// Deterministic absolute error radii from timing, separate from Gaussian P.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct TimingErrorBounds {
    pub position_metres: [f32; 3],
    pub velocity_mps: [f32; 3],
    pub attitude_radians: [f32; 3],
}
impl TimingErrorBounds {
    pub const ZERO: Self = Self {
        position_metres: [0.0; 3],
        velocity_mps: [0.0; 3],
        attitude_radians: [0.0; 3],
    };
}
impl FlightEstimate {
    pub fn velocity_error_bound(&self) -> Vector3<Ned> {
        let e: [f32; 3] = core::array::from_fn(|i| {
            3.0 * kaptan_math::sqrt(self.velocity_variance_m2ps2[i])
                + self.timing_error_bound.velocity_mps[i]
        });
        Vector3::new(e[0], e[1], e[2])
    }
    pub fn attitude_error_bound(&self) -> Vector3<Ned> {
        let e: [f32; 3] = core::array::from_fn(|i| {
            3.0 * kaptan_math::sqrt(self.attitude_variance_rad2[i])
                + self.timing_error_bound.attitude_radians[i]
        });
        Vector3::new(e[0], e[1], e[2])
    }
}
/// Four equal contact spheres at the corners of a body-FRD rectangle.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ContactGeometry {
    pub half_span_x_metres: f32,
    pub half_span_y_metres: f32,
    pub centre_z_metres: f32,
    pub radius_metres: f32,
}
impl ContactGeometry {
    pub(crate) fn validate(self) -> bool {
        self.half_span_x_metres.is_finite()
            && self.half_span_x_metres > 0.0
            && self.half_span_y_metres.is_finite()
            && self.half_span_y_metres > 0.0
            && self.centre_z_metres.is_finite()
            && self.radius_metres.is_finite()
            && self.radius_metres >= 0.0
            && (self.rotation_radius_bound() + self.radius_metres).is_finite()
    }
    fn rotation_radius_bound(self) -> f32 {
        l1_bound(Vector3::new(
            self.half_span_x_metres,
            self.half_span_y_metres,
            self.centre_z_metres,
        ))
    }
    pub(crate) fn depth(self, q: UnitQuaternion<Frd, Ned>) -> f32 {
        let mut depth = f32::NEG_INFINITY;
        for x in [-self.half_span_x_metres, self.half_span_x_metres] {
            for y in [-self.half_span_y_metres, self.half_span_y_metres] {
                let foot = q.body_to_ned(Vector3::new(x, y, self.centre_z_metres)).z()
                    + self.radius_metres;
                if !foot.is_finite() {
                    return f32::NAN;
                }
                depth = depth.max(foot);
            }
        }
        depth
    }
}

/// Selected source for each supported observation kind.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct NavigationSources {
    pub imu: SourceId,
    pub local_position_velocity: SourceId,
    pub magnetic: SourceId,
    pub pressure: SourceId,
    pub range: SourceId,
}
impl NavigationSources {
    pub(crate) const fn ordered(self) -> [SourceId; 5] {
        [
            self.imu,
            self.local_position_velocity,
            self.magnetic,
            self.pressure,
            self.range,
        ]
    }
}

/// Range-sensor extrinsics and deterministic quantization error.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct RangeInstallation {
    pub frame: FrameId,
    pub offset_frd_metres: Vector3<Frd>,
    /// Finite nonzero beam direction, normalized once by navigation construction.
    pub direction_frd: Vector3<Frd>,
    pub quantization_error_metres: f32,
}
impl RangeInstallation {
    fn validate(self) -> bool {
        self.offset_frd_metres.is_finite()
            && l1_bound(self.offset_frd_metres).is_finite()
            && self.direction_frd.normalized().is_some()
            && self.quantization_error_metres.is_finite()
            && self.quantization_error_metres >= 0.0
    }
    pub(crate) fn lever_bound(self) -> f32 {
        l1_bound(self.offset_frd_metres)
    }
}

// The L1 norm bounds Euclidean radius. Round both additions outward.
fn l1_bound(v: Vector3<Frd>) -> f32 {
    ((v.x().abs() + v.y().abs()).next_up() + v.z().abs()).next_up()
}

/// Local isothermal pressure reference. The conversion accepts 0.95..1.05 of it.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct BarometricReference {
    pub pressure_pascals: f32,
    pub temperature_kelvin: f32,
}
impl BarometricReference {
    pub(crate) fn height_scale(self) -> f32 {
        287.05 * self.temperature_kelvin / GRAVITY_MPS2
    }
    fn validate(self) -> bool {
        let lower = 0.95 * self.pressure_pascals;
        let upper = 1.05 * self.pressure_pascals;
        let scale = self.height_scale();
        self.pressure_pascals.is_finite()
            && self.pressure_pascals > 0.0
            && self.temperature_kelvin.is_finite()
            && self.temperature_kelvin > 0.0
            && (self.pressure_pascals + upper).is_finite()
            && scale.is_finite()
            && (scale / lower).is_finite()
            && scale / upper > 0.0
    }
}

/// Required compiled installation. Calibration identifies one coherent set of
/// mounting, calibration and uncertainty data, changed only by reinitialization.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct NavigationInstallation {
    pub sources: NavigationSources,
    pub range: RangeInstallation,
    pub contact: ContactGeometry,
    pub atmosphere: BarometricReference,
    pub magnetic_field_ned_microtesla: Vector3<Ned>,
    pub calibration: CalibrationRevision,
    pub reference: SensorReferenceId,
}
impl NavigationInstallation {
    fn validate(self) -> bool {
        self.range.validate()
            && self.contact.validate()
            && self.atmosphere.validate()
            && self.magnetic_field_ned_microtesla.is_finite()
            && Vector3::<Ned>::new(
                self.magnetic_field_ned_microtesla.x(),
                self.magnetic_field_ned_microtesla.y(),
                0.0,
            )
            .norm()
                > 10.0
            && self.magnetic_field_ned_microtesla.norm() < 100.0
            && self.reference.0 != 0
    }
}

/// Continuous process-noise densities in SI units.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct NoiseDensities {
    pub gyro: f32,              // rad²/s
    pub acceleration: f32,      // m²/s³
    pub gyro_bias: f32,         // rad²/s³
    pub acceleration_bias: f32, // m²/s⁵
    pub barometric_datum: f32,  // m²/s
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct NavigationConfig {
    pub noise: NoiseDensities,
    /// Acquisition-window, numerical-step and current-extrapolation ceiling.
    pub maximum_imu_interval_microseconds: u64,
    pub imu_freshness_microseconds: u64,
    pub aiding_freshness_microseconds: u64,
    pub maximum_time_uncertainty_microseconds: u32,
    pub lag_microseconds: u64,
    pub installation: NavigationInstallation,
}
impl NavigationConfig {
    #[cfg(test)]
    pub const fn engineering_fixture() -> Self {
        Self {
            noise: NoiseDensities {
                gyro: 2.5e-7,
                acceleration: 2.5e-5,
                gyro_bias: 1e-10,
                acceleration_bias: 1e-8,
                barometric_datum: 1e-5,
            },
            maximum_imu_interval_microseconds: 20_000,
            imu_freshness_microseconds: 20_000,
            aiding_freshness_microseconds: 250_000,
            maximum_time_uncertainty_microseconds: 2_000,
            lag_microseconds: 100_000,
            installation: NavigationInstallation {
                sources: NavigationSources {
                    imu: SourceId::new(1),
                    local_position_velocity: SourceId::new(2),
                    magnetic: SourceId::new(3),
                    pressure: SourceId::new(4),
                    range: SourceId::new(5),
                },
                range: RangeInstallation {
                    frame: FrameId::new(3),
                    offset_frd_metres: Vector3::new(0.0, 0.0, 0.04),
                    direction_frd: Vector3::new(0.0, 0.0, 1.0),
                    quantization_error_metres: 0.0005,
                },
                contact: ContactGeometry {
                    half_span_x_metres: 0.12,
                    half_span_y_metres: 0.12,
                    centre_z_metres: 0.065,
                    radius_metres: 0.015,
                },
                atmosphere: BarometricReference {
                    pressure_pascals: 101_325.0,
                    temperature_kelvin: 288.15,
                },
                magnetic_field_ned_microtesla: Vector3::new(20.0, 0.0, 45.0),
                calibration: CalibrationRevision::new(1),
                reference: SensorReferenceId(1),
            },
        }
    }
    pub fn validate(self) -> bool {
        let n = self.noise;
        [
            n.gyro,
            n.acceleration,
            n.gyro_bias,
            n.acceleration_bias,
            n.barometric_datum,
        ]
        .iter()
        .all(|v| v.is_finite() && *v > 0.0 && *v <= 1.0)
            && (10_000..=20_000).contains(&self.maximum_imu_interval_microseconds)
            && self.imu_freshness_microseconds >= self.maximum_imu_interval_microseconds
            && self.imu_freshness_microseconds <= 20_000
            && self.lag_microseconds == 100_000
            && (200_000..=500_000).contains(&self.aiding_freshness_microseconds)
            && self.maximum_time_uncertainty_microseconds <= 2_000
            && self.installation.validate()
    }
}

pub(crate) fn earliest(c: ObservationContext) -> Option<u64> {
    c.measurement_time
        .as_microseconds()
        .checked_sub(u64::from(c.time_uncertainty_microseconds))
}
pub(crate) fn expiry(c: ObservationContext, age: u64) -> Option<Timestamp> {
    Some(Timestamp::from_microseconds(earliest(c)?.checked_add(age)?))
}

#[cfg(test)]
mod tests {
    use super::*;
    fn contact() -> ContactGeometry {
        NavigationConfig::engineering_fixture().installation.contact
    }
    fn time(v: u64) -> Timestamp {
        Timestamp::from_microseconds(v)
    }
    fn estimate(at: Timestamp) -> FlightEstimate {
        let capability = CapabilityEvidence {
            available: true,
            last_measurement_at: Some(at),
            valid_until: Some(time(u64::MAX)),
        };
        FlightEstimate {
            at,
            fused_through: at,
            last_imu_at: at,
            origin: LocalOrigin {
                id: OriginId(1),
                established_at: at,
                reference: SensorReferenceId(1),
                launch_in_reference: PositionNed::metres(0.0, 0.0, 0.0),
                launch_variance_m2: [0.0; 3],
            },
            attitude: UnitQuaternion::identity(),
            position: PositionNed::metres(0.0, 0.0, 0.0),
            velocity: VelocityNed::metres_per_second(0.1, 0.0, 0.0),
            angular_rate: AngularRateFrd::radians_per_second(0.0, 0.0, 0.0),
            position_variance_m2: [0.0; 3],
            velocity_variance_m2ps2: [0.0; 3],
            attitude_variance_rad2: [0.0; 3],
            timing_error_bound: TimingErrorBounds::ZERO,
            capabilities: FlightCapabilities {
                tilt: capability,
                heading: capability,
                horizontal_position: capability,
                horizontal_velocity: capability,
                height: capability,
                vertical_velocity: capability,
                ground_clearance: capability,
            },
            ground_clearance: None,
        }
    }
    #[test]
    fn capability_expiry_requires_every_requested_capability() {
        let evidence = CapabilityEvidence {
            available: true,
            last_measurement_at: Some(time(10)),
            valid_until: Some(time(100)),
        };
        let healthy = FlightCapabilities {
            tilt: evidence,
            heading: evidence,
            horizontal_position: evidence,
            horizontal_velocity: evidence,
            height: evidence,
            vertical_velocity: evidence,
            ground_clearance: evidence,
        };
        assert_eq!(healthy.valid_until(), Some(time(100)));
        for index in 0..7 {
            let mut c = healthy;
            [
                &mut c.tilt,
                &mut c.heading,
                &mut c.horizontal_position,
                &mut c.horizontal_velocity,
                &mut c.height,
                &mut c.vertical_velocity,
                &mut c.ground_clearance,
            ]
            .into_iter()
            .nth(index)
            .unwrap()
            .available = false;
            assert!(!c.flight_ready());
            assert_eq!(c.valid_until(), None);
        }
        let mut c = healthy;
        c.ground_clearance.valid_until = None;
        assert_eq!(c.valid_until(), None);
        c = healthy;
        c.heading.valid_until = Some(time(50));
        assert_eq!(c.valid_until(), Some(time(50)));
    }
    #[test]
    fn clearance_bound_uses_earliest_capture_and_rejects_invalid_arithmetic() {
        let c = GroundClearance {
            measurement_at: time(10_000),
            valid_until: time(60_000),
            source: SourceId::new(5),
            body_origin_height_metres: 0.08,
            contact_clearance_metres: 0.0,
            variance_m2: 4e-6,
            measurement_time_uncertainty_microseconds: 1_000,
            upper_error_metres: 0.01,
        };
        let e = estimate(time(20_000));
        assert!((c.upper_at(time(20_000), &e, contact()).unwrap() - 0.012_612_5).abs() < 1e-7);
        assert_eq!(
            c.upper_at(time(60_000), &estimate(time(60_000)), contact()),
            None
        );
        for speed in [f32::NAN, f32::INFINITY] {
            let mut bad = e;
            bad.velocity = VelocityNed::metres_per_second(speed, 0.0, 0.0);
            assert_eq!(c.upper_at(time(20_000), &bad, contact()), None);
        }
        assert_eq!(
            c.upper_at(time(8_000), &estimate(time(8_000)), contact()),
            None
        );
        assert_eq!(c.upper_at(time(20_001), &e, contact()), None);
        let mut bad_angle = e;
        bad_angle.attitude_variance_rad2[0] = f32::NAN;
        assert_eq!(c.upper_at(time(20_000), &bad_angle, contact()), None);
        let mut invalid_contact = contact();
        invalid_contact.radius_metres = -0.01;
        assert_eq!(c.upper_at(time(20_000), &e, invalid_contact), None);
        assert_eq!(
            GroundClearance {
                measurement_time_uncertainty_microseconds: 20_000,
                ..c
            }
            .upper_at(time(20_000), &e, contact()),
            None
        );
        assert_eq!(
            GroundClearance {
                valid_until: time(3_000_000),
                ..c
            }
            .upper_at(
                time(2_000_000),
                &FlightEstimate {
                    at: time(2_000_000),
                    velocity: VelocityNed::metres_per_second(f32::MAX, 0.0, 0.0),
                    ..e
                },
                contact(),
            ),
            None
        );
    }
    #[test]
    fn current_foot_geometry_and_velocity_confidence_bound_landing_clearance() {
        // Independent rigid geometry: fixed COM 0.132 m; foot-sphere centres
        // (+/-0.12,+/-0.12,0.065), radius 0.015. Roll changes 0.2 -> 0.
        let old_depth = 0.12_f64 * 0.2_f64.sin() + 0.065 * 0.2_f64.cos() + 0.015;
        let measured_contact = 0.132 - old_depth;
        let old_bound = measured_contact + 0.0075 + 0.2 * 0.020 + 0.5 * 25.0 * 0.020_f64.powi(2);
        assert!(old_bound < 0.05);
        let actual_now = 0.132 - (0.065 + 0.015);
        let c = GroundClearance {
            measurement_at: time(10_000),
            valid_until: time(70_000),
            source: SourceId::new(5),
            body_origin_height_metres: 0.132,
            contact_clearance_metres: measured_contact as f32,
            variance_m2: 4e-6,
            measurement_time_uncertainty_microseconds: 0,
            upper_error_metres: 0.0075,
        };
        let mut e = estimate(time(30_000));
        e.velocity = VelocityNed::metres_per_second(0.0, 0.0, 0.0);
        let upper = c.upper_at(e.at, &e, contact()).unwrap();
        assert!(f64::from(upper) >= actual_now);
        assert!(upper > 0.05); // neither cap entry nor contact dwell may use the old bound
        e.velocity_variance_m2ps2 = [0.04; 3];
        let uncertain = c.upper_at(e.at, &e, contact()).unwrap();
        assert!(uncertain > upper + 0.020); // 3*sqrt(trace(Pv))*20 ms, not fixed +0.2 m/s
    }
    #[test]
    fn contact_geometry() {
        for geometry in [
            contact(),
            ContactGeometry {
                half_span_x_metres: 0.3,
                half_span_y_metres: 0.2,
                centre_z_metres: 0.15,
                radius_metres: 0.025,
            },
        ] {
            let x = f64::from(geometry.half_span_x_metres);
            let y = f64::from(geometry.half_span_y_metres);
            let z = f64::from(geometry.centre_z_metres);
            assert!(f64::from(geometry.rotation_radius_bound()) >= (x * x + y * y + z * z).sqrt());
            for (roll, pitch) in [(-0.5_f64, 0.2_f64), (0.0, 0.0), (0.4, -0.3)] {
                let (sr, cr) = ((roll / 2.0).sin(), (roll / 2.0).cos());
                let (sp, cp) = ((pitch / 2.0).sin(), (pitch / 2.0).cos());
                let q = UnitQuaternion::from_components(
                    (cp * cr) as f32,
                    (cp * sr) as f32,
                    (sp * cr) as f32,
                    (-sp * sr) as f32,
                )
                .unwrap();
                let expected = x * pitch.sin().abs()
                    + y * (pitch.cos() * roll.sin()).abs()
                    + z * pitch.cos() * roll.cos()
                    + f64::from(geometry.radius_metres);
                assert!((f64::from(geometry.depth(q)) - expected).abs() < 1e-7);
                let e = FlightEstimate {
                    attitude: q,
                    ..estimate(time(10_000))
                };
                let c = GroundClearance {
                    measurement_at: e.at,
                    valid_until: time(70_000),
                    source: SourceId::new(90),
                    body_origin_height_metres: 1.0,
                    contact_clearance_metres: 0.0,
                    variance_m2: 1e-6,
                    measurement_time_uncertainty_microseconds: 0,
                    upper_error_metres: 0.001,
                };
                assert!(f64::from(c.upper_at(e.at, &e, geometry).unwrap()) >= 1.0 - expected);
            }
        }
    }
    #[test]
    fn installation_validation() {
        let fixture = NavigationConfig::engineering_fixture();
        for field in 0..9 {
            for bad in [f32::NAN, f32::INFINITY, -1.0] {
                let mut config = fixture;
                let i = &mut config.installation;
                match field {
                    0 => i.contact.half_span_x_metres = bad,
                    1 => i.contact.half_span_y_metres = bad,
                    2 => i.contact.radius_metres = bad,
                    3 => i.range.quantization_error_metres = bad,
                    4 => i.atmosphere.pressure_pascals = bad,
                    5 => i.atmosphere.temperature_kelvin = bad,
                    6 if !bad.is_finite() => i.contact.centre_z_metres = bad,
                    7 if !bad.is_finite() => {
                        i.range.offset_frd_metres = Vector3::new(bad, 0.0, 0.0)
                    }
                    8 if !bad.is_finite() => i.range.direction_frd = Vector3::new(0.0, 0.0, bad),
                    _ => continue,
                }
                assert!(Navigation::new(config).is_err());
            }
        }
        for field in 0..6 {
            let mut config = fixture;
            let i = &mut config.installation;
            match field {
                0 => i.range.direction_frd = Vector3::zero(),
                1 => i.contact.half_span_x_metres = 0.0,
                2 => i.contact.half_span_y_metres = 0.0,
                3 => i.atmosphere.pressure_pascals = 0.0,
                4 => i.atmosphere.temperature_kelvin = 0.0,
                _ => i.atmosphere.pressure_pascals = f32::MAX,
            }
            assert!(Navigation::new(config).is_err());
        }
        let mut config = fixture;
        config.installation.contact.half_span_x_metres = f32::MAX;
        config.installation.contact.half_span_y_metres = f32::MAX;
        assert!(Navigation::new(config).is_err());
        config = fixture;
        config.installation.range.offset_frd_metres = Vector3::new(f32::MAX, f32::MAX, 0.0);
        assert!(Navigation::new(config).is_err());
        for (pressure_pascals, temperature_kelvin) in [
            (101_325.0, f32::MAX),
            (f32::MIN_POSITIVE, 288.15),
            (1e20, f32::from_bits(1)),
        ] {
            config = fixture;
            config.installation.atmosphere = BarometricReference {
                pressure_pascals,
                temperature_kelvin,
            };
            assert!(Navigation::new(config).is_err());
        }
    }
    #[test]
    fn navigation_configuration_rejects_each_invalid_domain() {
        let factory = NavigationConfig::engineering_fixture();
        assert!(factory.validate());
        for index in 0..5 {
            for bad in [f32::NAN, 0.0, -1.0, 1.01] {
                let mut c = factory;
                *[
                    &mut c.noise.gyro,
                    &mut c.noise.acceleration,
                    &mut c.noise.gyro_bias,
                    &mut c.noise.acceleration_bias,
                    &mut c.noise.barometric_datum,
                ]
                .into_iter()
                .nth(index)
                .unwrap() = bad;
                assert!(!c.validate());
                assert!(crate::Navigation::new(c).is_err());
            }
        }
        let mut c = factory;
        c.maximum_imu_interval_microseconds = 9_999;
        assert!(!c.validate());
        c = factory;
        c.imu_freshness_microseconds = 19_999;
        assert!(!c.validate());
        c = factory;
        c.imu_freshness_microseconds = 20_001;
        assert!(!c.validate());
        c = factory;
        c.lag_microseconds = 0;
        assert!(!c.validate());
        c = factory;
        c.aiding_freshness_microseconds = 100_000;
        assert!(!c.validate());
        c = factory;
        c.maximum_time_uncertainty_microseconds = 2_001;
        assert!(!c.validate());
        c = factory;
        c.installation.magnetic_field_ned_microtesla = Vector3::new(f32::NAN, 0.0, 45.0);
        assert!(!c.validate());
        c = factory;
        c.installation.magnetic_field_ned_microtesla = Vector3::new(10.0, 0.0, 45.0);
        assert!(!c.validate());
        c = factory;
        c.installation.magnetic_field_ned_microtesla = Vector3::new(20.0, f32::MAX, 45.0);
        assert!(!c.validate());
        c = factory;
        c.installation.reference = SensorReferenceId(0);
        assert!(!c.validate());
        c = factory;
        c.installation.magnetic_field_ned_microtesla = Vector3::new(0.0, 20.0, 45.0);
        assert!(c.validate());
    }
}