mod support;

use kaptan_config::{ConfigModel, ConfigValue, ConfigViolation, FieldId, ValidationRule};
use kaptan_control::{
    HeadingNed, KinematicLimits, Pid, PidConfig, ThrustEnvelope, Trajectory,
    TranslationalControlConfig, TranslationalController, TranslationalEstimate, force_to_attitude,
};
use kaptan_flight_config::{CandidateProfile, VehicleCapabilities};
use kaptan_navigation::{GRAVITY_MPS2, MAXIMUM_FREE_ACCELERATION_MPS2, PositionNed, VelocityNed};

const CAPABILITIES: VehicleCapabilities = support::capabilities();
const IMU_INTERVAL: &str = "timing.imu_max_interval_us";
const IMU_FRESHNESS: &str = "timing.imu_freshness_us";
const DOWNWARD_ACCELERATION: &str = "control.maximum_downward_acceleration_mps2";

fn id(key: &str) -> FieldId {
    CandidateProfile::descriptors()
        .find(|field| field.display_key == key)
        .unwrap()
        .id
}

fn set(candidate: &mut CandidateProfile, key: &str, value: ConfigValue) {
    candidate.set_field(id(key), value).unwrap();
}

fn with_interval(microseconds: u64) -> CandidateProfile {
    let mut candidate = support::candidate();
    set(&mut candidate, IMU_INTERVAL, ConfigValue::U64(microseconds));
    set(&mut candidate, IMU_FRESHNESS, ConfigValue::U64(20_000));
    candidate
}

fn assert_rejected(candidate: CandidateProfile, issue: ConfigViolation) {
    // Both whole-tree validation and active-profile construction must reject.
    assert_eq!(
        <CandidateProfile as kaptan_config::Config>::validate(&candidate),
        Err(issue)
    );
    assert_eq!(candidate.validate(&CAPABILITIES), Err(issue));
}

#[test]
fn imu_interval_cannot_exceed_the_controller_domain() {
    // The generic primitive still permits 100 ms, but this flight profile's
    // delayed estimator and consumer require 10..20 ms inertial intervals.
    assert_eq!(kaptan_control::MAXIMUM_CONTROL_STEP_MICROSECONDS, 100_000);
    for interval in [10_000, 15_000, 19_999, 20_000] {
        let active = with_interval(interval).validate(&CAPABILITIES).unwrap();
        assert_eq!(active.timing().maximum_imu_interval_microseconds, interval);
    }
    for interval in [
        1, 9_999, 20_001, 50_000, 100_000, 100_001, 200_000, 1_000_000,
    ] {
        let candidate = with_interval(interval);
        assert!(candidate.validate(&CAPABILITIES).is_err());
        assert!(<CandidateProfile as kaptan_config::Config>::validate(&candidate).is_err());
    }
}

#[test]
fn downward_acceleration_has_a_strict_gravity_boundary() {
    let below = f32::from_bits(GRAVITY_MPS2.to_bits() - 1);
    let above = f32::from_bits(GRAVITY_MPS2.to_bits() + 1);
    for acceleration in [0.001, 2.0, below] {
        let mut candidate = support::candidate();
        set(
            &mut candidate,
            DOWNWARD_ACCELERATION,
            ConfigValue::F32(acceleration),
        );
        let active = candidate.validate(&CAPABILITIES).unwrap();
        assert_eq!(
            active.control().maximum_downward_acceleration_mps2,
            acceleration
        );
    }
    for acceleration in [GRAVITY_MPS2, above, 100.0] {
        let mut candidate = support::candidate();
        set(
            &mut candidate,
            DOWNWARD_ACCELERATION,
            ConfigValue::F32(acceleration),
        );
        assert_rejected(
            candidate,
            ConfigViolation::field(id(DOWNWARD_ACCELERATION), ValidationRule::Inconsistent),
        );
    }
}

#[test]
fn thrust_margin_uses_the_same_gravity_contract() {
    for thrust in [f32::from_bits(GRAVITY_MPS2.to_bits() - 1), GRAVITY_MPS2] {
        let mut candidate = support::candidate();
        set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0));
        set(
            &mut candidate,
            "thrust.maximum_collective_thrust_n",
            ConfigValue::F32(thrust),
        );
        assert_rejected(
            candidate,
            ConfigViolation::related(
                id("thrust.maximum_collective_thrust_n"),
                id("thrust.mass_kg"),
                ValidationRule::Inconsistent,
            ),
        );
    }
    let mut candidate = support::candidate();
    set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0));
    set(
        &mut candidate,
        "thrust.maximum_collective_thrust_n",
        ConfigValue::F32(f32::from_bits(GRAVITY_MPS2.to_bits() + 1)),
    );
    assert!(candidate.validate(&CAPABILITIES).is_ok());
}

#[test]
fn thrust_to_mass_certifies_navigation_free_acceleration_without_epsilon() {
    let issue = ConfigViolation::related(
        id("thrust.maximum_collective_thrust_n"),
        id("thrust.mass_kg"),
        ValidationRule::Inconsistent,
    );
    let mut former = support::candidate();
    set(&mut former, "thrust.mass_kg", ConfigValue::F32(0.01));
    assert_rejected(former, issue);
    assert!(support::candidate().validate(&CAPABILITIES).is_ok());

    let acceleration = f64::from(MAXIMUM_FREE_ACCELERATION_MPS2) - f64::from(GRAVITY_MPS2);
    for mass in [0.01_f32, 1.0, 1.2, 100.0] {
        let exact = f64::from(mass) * acceleration;
        let rounded = exact as f32;
        let floor = if f64::from(rounded) > exact {
            rounded.next_down()
        } else {
            rounded
        };
        let mut candidate = support::candidate();
        set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(mass));
        for thrust in [floor.next_down(), floor] {
            set(
                &mut candidate,
                "thrust.maximum_collective_thrust_n",
                ConfigValue::F32(thrust),
            );
            assert!(candidate.validate(&CAPABILITIES).is_ok());
        }
        set(
            &mut candidate,
            "thrust.maximum_collective_thrust_n",
            ConfigValue::F32(floor.next_up()),
        );
        assert_rejected(candidate, issue);
    }
    // At m=1 the upper boundary is exactly representable in binary32.
    let exact_thrust = MAXIMUM_FREE_ACCELERATION_MPS2 - GRAVITY_MPS2;
    assert_eq!(f64::from(exact_thrust), acceleration);
    let mut candidate = support::candidate();
    set(
        &mut candidate,
        "thrust.maximum_collective_thrust_n",
        ConfigValue::F32(exact_thrust),
    );
    set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(1.0));
    assert!(candidate.validate(&CAPABILITIES).is_ok());
    set(
        &mut candidate,
        "thrust.mass_kg",
        ConfigValue::F32(1.0_f32.next_down()),
    );
    assert_rejected(candidate, issue);
    set(
        &mut candidate,
        "thrust.mass_kg",
        ConfigValue::F32(1.0_f32.next_up()),
    );
    assert!(candidate.validate(&CAPABILITIES).is_ok());
}

#[test]
fn accepted_boundary_profiles_operate_the_real_control_primitives() {
    for mass in [0.01, 1.0] {
        let mut candidate = with_interval(20_000);
        let downward_limit = f32::from_bits(GRAVITY_MPS2.to_bits() - 1);
        set(
            &mut candidate,
            DOWNWARD_ACCELERATION,
            ConfigValue::F32(downward_limit),
        );
        set(&mut candidate, "thrust.mass_kg", ConfigValue::F32(mass));
        set(
            &mut candidate,
            "thrust.maximum_collective_thrust_n",
            ConfigValue::F32(15.0 * mass),
        );
        for (key, gain) in [
            ("control.rate_pid.proportional", 0.1),
            ("control.rate_pid.integral", 0.02),
            ("control.rate_pid.derivative", 0.05),
        ] {
            set(&mut candidate, key, ConfigValue::F32(gain));
        }
        let active = candidate.validate(&CAPABILITIES).unwrap();
        assert_eq!(active.timing().maximum_imu_interval_microseconds, 20_000);
        // Preserve the independent 100-ms known answers of the generic
        // primitives, without claiming it is a valid flight-estimator cadence.
        let dt = 0.1;
        let gains = active.rate_pid();
        let mut pid = Pid::new(PidConfig {
            proportional: gains.proportional,
            integral: gains.integral,
            derivative: gains.derivative,
            output_limit: gains.output_limit,
            integral_limit: gains.integral_limit,
        })
        .unwrap();
        // Known answer with all three terms active at the 100-ms boundary.
        assert!((pid.update(0.2, 0.3, dt).unwrap() - 0.0354).abs() < 1.0e-7);

        let mut trajectory = Trajectory::new(
            PositionNed::metres(0.0, 0.0, 0.0).vector(),
            KinematicLimits {
                maximum_horizontal_speed_mps: 1.0,
                maximum_vertical_speed_mps: 1.0,
                maximum_horizontal_acceleration_mps2: 1.0,
                maximum_vertical_acceleration_mps2: 1.0,
            },
        )
        .unwrap();
        trajectory
            .set_goal(PositionNed::metres(0.0, 0.0, 1.0).vector())
            .unwrap();
        let reference = trajectory.update(dt).unwrap();
        assert!((reference.position.z() - 0.01).abs() < 1.0e-8);

        let control = active.control();
        let controller = TranslationalController::new(TranslationalControlConfig {
            position_gain_per_second_squared: control.position_gain_per_second_squared,
            velocity_gain_per_second: control.velocity_gain_per_second,
            maximum_horizontal_acceleration_mps2: control.maximum_horizontal_acceleration_mps2,
            maximum_upward_acceleration_mps2: control.maximum_upward_acceleration_mps2,
            maximum_downward_acceleration_mps2: control.maximum_downward_acceleration_mps2,
        })
        .unwrap();
        let acceleration = controller
            .update(
                reference,
                TranslationalEstimate {
                    position: PositionNed::metres(0.0, 0.0, -100.0).vector(),
                    velocity: VelocityNed::metres_per_second(0.0, 0.0, 0.0).vector(),
                },
            )
            .unwrap();
        assert_eq!(acceleration.z(), downward_limit);

        let thrust = active.thrust();
        let envelope = ThrustEnvelope {
            mass_kg: thrust.mass_kg,
            maximum_collective_thrust_newtons: thrust.maximum_collective_thrust_newtons,
            maximum_tilt_tangent: thrust.maximum_tilt_tangent,
        };
        let (force, saturated) = envelope
            .force_for_acceleration(acceleration, GRAVITY_MPS2)
            .unwrap();
        // Gravity is in [8, 16), so its f32 predecessor is exactly 2^-20 lower.
        assert_eq!(force.vector().z(), -mass / 1_048_576.0);
        assert!(!saturated);
        let attitude = force_to_attitude(force, HeadingNed::new(1.0, 0.0).unwrap()).unwrap();
        assert_eq!(attitude.w(), 1.0);
    }
}