//! Sensor-driven lifecycle and output boundary regressions.
mod support;
use kaptan_flight_runtime::*;
use support::*;

#[test]
fn sensor_startup_arm_binding_and_ground_disarm() {
    let (mut ep, mut out, now) = ready();
    assert_eq!(ep.status().ground_state, GroundState::Grounded);
    let s = ep
        .command(
            request(&ep, 1, FlightCommand::Arm, now),
            time(now),
            &mut out,
        )
        .unwrap();
    assert_eq!(s.lifecycle, CommandLifecycle::Accepted);
    assert_eq!(s.arm, Some(ArmId(1)));
    assert_eq!(ep.status().phase, VehiclePhase::PreArm);
    let binding = out.outputs.last().unwrap();
    assert_eq!(binding.arm(), s.arm);
    assert!(!binding.permitted());
    let s = ep
        .command(
            request(&ep, 2, FlightCommand::GroundDisarm, now),
            time(now),
            &mut out,
        )
        .unwrap();
    assert_eq!(s.lifecycle, CommandLifecycle::Completed);
    assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
    assert_eq!(out.outputs.last().unwrap().arm(), None);
}
#[test]
fn command_ttl_replay_and_epoch_do_not_renew_authority() {
    let (mut ep, mut out, now) = ready();
    let zero = request(&ep, 0, FlightCommand::Arm, now);
    assert_eq!(
        ep.command(zero, time(now), &mut out).unwrap().reason,
        CommandReason::Replay
    );
    let mut wrong = request(&ep, 50, FlightCommand::Arm, now);
    wrong.context.owner = CommandOwner(999);
    assert_eq!(
        ep.command(wrong, time(now), &mut out).unwrap().reason,
        CommandReason::WrongOwner
    );
    assert_eq!(ep.status().command_highwater, None);
    let mut expired = request(&ep, 1, FlightCommand::Arm, now);
    expired.context.expires_at = time(now);
    assert_eq!(
        ep.command(expired, time(now), &mut out).unwrap().lifecycle,
        CommandLifecycle::Rejected
    );
    assert_eq!(ep.status().command_highwater, Some(RequestSequence(1)));
    assert_eq!(
        ep.command(
            request(&ep, 1, FlightCommand::Arm, now),
            time(now),
            &mut out
        )
        .unwrap()
        .reason,
        CommandReason::Replay
    );
    ep.command(
        request(&ep, 2, FlightCommand::Arm, now),
        time(now),
        &mut out,
    )
    .unwrap();
    for t in (now + 10_000..=now + 600_000).step_by(10_000) {
        feed(&mut ep, &mut out, t);
    }
    assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle); // TTL expired, arm completed.
    ep.command(
        request(&ep, 3, FlightCommand::GroundDisarm, now + 600_000),
        time(now + 600_000),
        &mut out,
    )
    .unwrap();
    let old = ep.status().epoch;
    let seq = out.outputs.last().unwrap().sequence();
    let epoch = ep
        .begin_epoch(CommandOwner(8), time(now + 600_000), &mut out)
        .unwrap();
    assert_ne!(old, epoch);
    assert_eq!(out.outputs.last().unwrap().sequence(), seq + 1);
    assert_eq!(ep.status().at, time(now + 600_000));
}
#[test]
fn failed_sink_consumes_sequence_and_attempts_new_inhibition() {
    let (mut ep, mut out, now) = armed();
    ep.command(
        request(
            &ep,
            2,
            FlightCommand::Takeoff {
                height_above_launch_metres: 2.,
            },
            now,
        ),
        time(now),
        &mut out,
    )
    .unwrap();
    out.fail_enabled = true;
    out.fail_inhibit = true;
    let attempts = out.outputs.len();
    let err = feed_result(&mut ep, &mut out, now + 10_000).unwrap_err();
    assert_eq!(out.outputs.len(), attempts + 2);
    assert_eq!(err.primary, EndpointFailure::Output("enabled"));
    assert_eq!(err.inhibition, Some("inhibit"));
    assert_eq!(ep.status().phase, VehiclePhase::Terminated);
    let n = out.outputs.len();
    assert!(out.outputs[n - 2].permitted());
    assert!(!out.outputs[n - 1].permitted());
    assert_eq!(
        out.outputs[n - 1].sequence(),
        out.outputs[n - 2].sequence() + 1
    );
    assert!(
        ep.begin_epoch(CommandOwner(1), time(now + 10_000), &mut out)
            .is_err()
    );
}

#[test]
fn failed_arm_binding_publication_retains_the_failed_transaction() {
    let (mut ep, mut out, now) = ready();
    out.fail_inhibit = true;
    let error = ep
        .command(
            request(&ep, 1, FlightCommand::Arm, now),
            time(now),
            &mut out,
        )
        .unwrap_err();
    assert_eq!(error.primary, EndpointFailure::Output("inhibit"));
    assert_eq!(ep.status().phase, VehiclePhase::Terminated);
    assert_eq!(ep.status().arm, None);
    let failed = ep.status().last_terminal_command.unwrap();
    assert_eq!(failed.sequence, RequestSequence(1));
    assert_eq!(failed.lifecycle, CommandLifecycle::Failed);
    assert_eq!(failed.arm, Some(ArmId(1)));
    assert!(!out.outputs.last().unwrap().permitted());
}
#[test]
fn asynchronous_tick_deadline_and_clock_fault_inhibit() {
    let (mut ep, mut out, now) = armed();
    ep.command(
        request(
            &ep,
            2,
            FlightCommand::Takeoff {
                height_above_launch_metres: 2.,
            },
            now,
        ),
        time(now),
        &mut out,
    )
    .unwrap();
    feed(&mut ep, &mut out, now + 10_000);
    let record = *out.outputs.last().unwrap();
    assert!(record.permitted());
    assert!(
        record.valid_until().as_microseconds() <= record.produced_at().as_microseconds() + 20_000
    );
    ep.tick(time(now + 10_000), &mut out).unwrap();
    assert_eq!(*out.outputs.last().unwrap(), record);
    let error = ep.tick(time(now + 30_001), &mut out).unwrap_err();
    assert_eq!(error.primary, EndpointFailure::ControlDeadline);
    assert!(!out.outputs.last().unwrap().permitted());
    let error = ep.tick(time(now), &mut out).unwrap_err();
    assert_eq!(error.primary, EndpointFailure::ClockRegression);
    assert_eq!(ep.status().phase, VehiclePhase::Terminated);
}
#[test]
fn ordinary_prearm_health_failure_clears_arm_without_terminal_fault() {
    let (mut ep, mut out, now) = ready();
    ep.command(
        request(&ep, 1, FlightCommand::Arm, now),
        time(now),
        &mut out,
    )
    .unwrap();
    ep.tick(time(now + 25_000), &mut out).unwrap();
    assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
    assert_eq!(ep.status().arm, None);
    assert_eq!(
        ep.status().last_terminal_command.unwrap().lifecycle,
        CommandLifecycle::Failed
    );
    assert!(!out.outputs.last().unwrap().permitted());
}
#[test]
fn typed_bad_quality_consumes_sensor_watermark_not_health() {
    let (mut ep, mut out, now) = ready();
    // The previous complete acquisition ended 1 ms before the service tick.
    let mut bad = imu(now + 9_000);
    bad.context.quality = 0;
    assert_eq!(
        ep.observe(
            ep.status().epoch,
            SensorRecord::Imu(bad),
            time(now + 10_000),
            &mut out
        )
        .unwrap(),
        ObservationAdmission::Rejected(ObservationRejection::InvalidQuality)
    );
    bad.context.quality = 255;
    assert_eq!(
        ep.observe(
            ep.status().epoch,
            SensorRecord::Imu(bad),
            time(now + 10_000),
            &mut out
        )
        .unwrap(),
        ObservationAdmission::Rejected(ObservationRejection::Replay)
    );
}

#[test]
fn fresh_terminate_does_not_require_current_navigation() {
    let (mut ep, mut out, now) = armed();
    let now = now + 50_000;
    let result = ep
        .command(
            request(&ep, 2, FlightCommand::Terminate, now),
            time(now),
            &mut out,
        )
        .unwrap();
    assert_eq!(result.lifecycle, CommandLifecycle::Completed);
    assert_eq!(ep.status().phase, VehiclePhase::Terminated);
    assert_eq!(ep.status().terminal_reason, Some(TerminalReason::Requested));
    assert!(!out.outputs.last().unwrap().permitted());
    let sources = ep.status().source_status;
    for t in (now + 10_000..=now + 500_000).step_by(10_000) {
        for record in records(t) {
            assert_eq!(
                ep.observe(ep.status().epoch, record, time(t), &mut out)
                    .unwrap(),
                ObservationAdmission::Rejected(ObservationRejection::Terminated)
            );
        }
        ep.tick(time(t), &mut out).unwrap();
    }
    assert_eq!(ep.status().source_status, sources);
    assert_eq!(ep.status().terminal_reason, Some(TerminalReason::Requested));
}

#[test]
fn new_source_isolation_is_seen_before_the_next_control_tick() {
    let (mut ep, mut out, now) = ready();
    for step in 1..=15 {
        let t = now + step * 10_000;
        for mut record in records(t) {
            let bad = if let SensorRecord::Magnetic(ref mut magnetic) = record {
                magnetic.context.quality = 0;
                true
            } else {
                false
            };
            let outcome = ep
                .observe(ep.status().epoch, record, time(t), &mut out)
                .unwrap();
            assert_eq!(
                outcome,
                if bad {
                    ObservationAdmission::Rejected(ObservationRejection::InvalidQuality)
                } else {
                    ObservationAdmission::Queued
                }
            );
        }
        if step == 15 {
            let arm = ep
                .command(request(&ep, 1, FlightCommand::Arm, t), time(t), &mut out)
                .unwrap();
            assert_eq!(arm.reason, CommandReason::NotReady);
            assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
        }
        ep.tick(time(t), &mut out).unwrap();
    }
    for t in (now + 160_000..=now + 1_400_000).step_by(10_000) {
        feed(&mut ep, &mut out, t);
    }
    assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
    assert_eq!(ep.status().ground_state, GroundState::Grounded);
    assert!(ep.status().estimate.unwrap().capabilities.heading.available);
    let t = now + 1_400_000;
    assert_eq!(
        ep.command(request(&ep, 2, FlightCommand::Arm, t), time(t), &mut out)
            .unwrap()
            .lifecycle,
        CommandLifecycle::Accepted
    );
}

#[test]
fn sensed_motion_during_prearm_consumes_the_arm_transaction() {
    let (mut ep, mut out, now) = ready();
    ep.command(
        request(&ep, 1, FlightCommand::Arm, now),
        time(now),
        &mut out,
    )
    .unwrap();
    for step in 1..=3 {
        let t = now + step * 10_000;
        for mut record in records(t) {
            if let SensorRecord::Imu(ref mut imu) = record {
                imu.acceleration = kaptan_navigation::AccelerationFrd::metres_per_second_squared(
                    0.0,
                    0.0,
                    -kaptan_navigation::GRAVITY_MPS2 - 5.0,
                );
            }
            ep.observe(ep.status().epoch, record, time(t), &mut out)
                .unwrap();
        }
        ep.tick(time(t), &mut out).unwrap();
    }
    assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
    assert_eq!(ep.status().arm, None);
    assert_eq!(ep.status().terminal_reason, None);
    assert_eq!(
        ep.status().last_terminal_command.unwrap().lifecycle,
        CommandLifecycle::Failed
    );
    assert!(out.outputs.iter().all(|record| !record.permitted()));
}

#[test]
fn full_notification_sink_observes_both_cancelled_arm_and_ground_disarm() {
    let (mut ep, mut out, now) = ready();
    ep.command(
        request(&ep, 1, FlightCommand::Arm, now),
        time(now),
        &mut out,
    )
    .unwrap();
    out.fail_notification = true;
    let error = ep
        .command(
            request(&ep, 2, FlightCommand::GroundDisarm, now),
            time(now),
            &mut out,
        )
        .unwrap_err();
    assert_eq!(error.primary, EndpointFailure::Notification("notification"));
    assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
    assert!(!out.outputs.last().unwrap().permitted());
    let outcomes = ep.command_outcomes();
    let cancelled = outcomes[0].unwrap();
    let disarmed = outcomes[1].unwrap();
    assert_eq!(cancelled.status.sequence, RequestSequence(1));
    assert_eq!(cancelled.status.lifecycle, CommandLifecycle::Cancelled);
    assert_eq!(disarmed.status.sequence, RequestSequence(2));
    assert_eq!(disarmed.status.lifecycle, CommandLifecycle::Completed);
    assert!(!cancelled.published);
    assert!(!disarmed.published);
    assert_eq!(
        &out.commands[out.commands.len() - 2..],
        &[cancelled.status, disarmed.status]
    );
    assert_eq!(ep.status().last_terminal_command, Some(disarmed.status));
}

#[test]
fn asynchronous_tick_and_observe_faults_deliver_failed_command_outcomes() {
    for observe_fault in [false, true] {
        let (mut ep, mut out, now) = armed();
        ep.command(
            request(
                &ep,
                2,
                FlightCommand::Takeoff {
                    height_above_launch_metres: 2.0,
                },
                now,
            ),
            time(now),
            &mut out,
        )
        .unwrap();
        out.fail_notification = true;
        let error = if observe_fault {
            let sample = imu(now + 30_000);
            ep.observe(
                ep.status().epoch,
                SensorRecord::Imu(sample),
                time(now + 30_000),
                &mut out,
            )
            .unwrap_err()
        } else {
            ep.tick(time(now + 30_000), &mut out).unwrap_err()
        };
        assert!(!matches!(error.primary, EndpointFailure::Notification(_)));
        let outcome = ep.command_outcomes()[0].unwrap();
        assert_eq!(outcome.status.sequence, RequestSequence(2));
        assert_eq!(outcome.status.lifecycle, CommandLifecycle::Failed);
        assert!(!outcome.published);
        assert_eq!(out.commands.last(), Some(&outcome.status));
        assert_eq!(ep.status().phase, VehiclePhase::Terminated);
        assert!(!out.outputs.last().unwrap().permitted());
    }
}