//! Direct scripted sensor trajectories exercise the core flight phases.
mod support;
use kaptan_flight_runtime::*;
use kaptan_navigation::{AccelerationFrd, GRAVITY_MPS2, PositionNed, SensorRecord, VelocityNed};
use support::*;

fn climb(t: u64, start: u64, duration: f64) -> (f64, f64, f64) {
    let s = (t.saturating_sub(start) as f64 * 1e-6 / duration).clamp(0., 1.);
    (
        2. * (10. * s.powi(3) - 15. * s.powi(4) + 6. * s.powi(5)),
        2. / duration * (30. * s * s - 60. * s.powi(3) + 30. * s.powi(4)),
        2. / (duration * duration) * (60. * s - 180. * s * s + 120. * s.powi(3)),
    )
}
fn fly(ep: &mut FlightEndpoint, out: &mut Recording, t: u64, start: u64, land: Option<u64>) {
    fly_with_bounce(ep, out, t, start, land, None);
}

fn fly_with_bounce(
    ep: &mut FlightEndpoint,
    out: &mut Recording,
    t: u64,
    start: u64,
    land: Option<u64>,
    bounce: Option<u64>,
) {
    let motion = |measured: u64| {
        let (mut h, mut v, _) = climb(measured, start, 5.);
        if let Some(land) = land
            && measured >= land
        {
            let (down, dv, _) = climb(measured, land, 6.);
            h = 2. - down;
            v = -dv;
        }
        if let Some(bounce) = bounce
            && measured >= bounce
            && measured <= bounce + 1_000_000
        {
            let phase = (measured - bounce) as f64 * 1e-6 * core::f64::consts::TAU;
            h += 0.05 * (1.0 - phase.cos());
            v += 0.05 * core::f64::consts::TAU * phase.sin();
        }
        (h, v)
    };
    for mut record in records(t) {
        let measured = record.context().measurement_time.as_microseconds();
        let (h, v) = motion(measured);
        match &mut record {
            SensorRecord::Imu(r) => {
                // Complete declared window, not endpoint acceleration.
                let duration = u64::from(r.acquisition_duration_us);
                let a = (v - motion(measured - duration).1) / (duration as f64 * 1e-6);
                r.acceleration =
                    AccelerationFrd::metres_per_second_squared(0., 0., -GRAVITY_MPS2 - a as f32)
            }
            SensorRecord::LocalPositionVelocity(r) => {
                r.position = PositionNed::metres(12., -7., 4. - h as f32);
                r.velocity = VelocityNed::metres_per_second(0., 0., -v as f32);
            }
            SensorRecord::Pressure(r) => {
                r.pascals =
                    (101325. * (-h / (287.05 * 288.15 / f64::from(GRAVITY_MPS2))).exp()) as f32
            }
            SensorRecord::Range(r) => r.metres = 0.04 + h as f32,
            SensorRecord::Magnetic(_) => {}
        }
        assert_eq!(
            ep.observe(ep.status().epoch, record, time(t), out).unwrap(),
            ObservationAdmission::Queued
        );
    }
    ep.tick(time(t), out).unwrap();
}

#[test]
fn onboard_hover_lands_and_stops() {
    let (mut ep, mut out, start) = armed();
    let mission = request(
        &ep,
        2,
        FlightCommand::TakeoffHoverLand {
            height_above_launch_metres: 2.,
        },
        start,
    );
    assert_eq!(
        ep.command(mission, time(start), &mut out)
            .unwrap()
            .lifecycle,
        CommandLifecycle::Accepted
    );
    let flight_deadline = ep.status().flight_deadline;
    for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    let hold = ep.status();
    assert_eq!(hold.phase, VehiclePhase::Hold);
    let land = hold.phase_since.as_microseconds() + 30_000_000;
    assert_eq!(hold.phase_deadline, Some(time(land)));
    assert_eq!(hold.active_command.unwrap().sequence, RequestSequence(2));
    for t in (start + 8_010_000..land).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    assert_eq!(ep.status().phase_since, hold.phase_since);
    assert_eq!(ep.status().phase_deadline, Some(time(land)));
    assert!(!out.commands.iter().any(|command| {
        command.sequence == RequestSequence(2) && command.lifecycle == CommandLifecycle::Completed
    }));
    let output = *out.outputs.last().unwrap();
    ep.supervise(time(land), &mut out).unwrap();
    assert_eq!(ep.status().phase, VehiclePhase::Land);
    assert_eq!(ep.status().flight_deadline, flight_deadline);
    assert_eq!(
        ep.status().phase_deadline,
        Some(time(land + LANDING_BUDGET_US))
    );
    assert_eq!(*out.outputs.last().unwrap(), output);
    fly(&mut ep, &mut out, land, start, Some(land));
    for t in (land + 10_000..=land + 9_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, Some(land));
    }
    assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
    assert_eq!(ep.status().arm, None);
    assert_eq!(ep.status().active_command, None);
    assert_eq!(ep.status().flight_deadline, None);
    assert!(!out.outputs.last().unwrap().permitted());
    let completed = ep.status().last_terminal_command.unwrap();
    assert_eq!(completed.kind, CommandKind::TakeoffHoverLand);
    assert_eq!(completed.lifecycle, CommandLifecycle::Completed);
    assert_eq!(completed.arm, None);
    assert!(completed.at > time(land));
}

#[test]
fn takeoff_hold_offboard_loss_latches_lands_and_requires_new_arm() {
    let (mut ep, mut out, start) = armed();
    ep.command(
        request(
            &ep,
            2,
            FlightCommand::Takeoff {
                height_above_launch_metres: 2.,
            },
            start,
        ),
        time(start),
        &mut out,
    )
    .unwrap();
    for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    assert_eq!(ep.status().phase, VehiclePhase::Hold);
    assert_eq!(ep.status().phase_deadline, None);
    let held_since = ep.status().phase_since;
    // Autonomous phase outlives the finite command TTL and settling deadline.
    for t in (start + 8_010_000..=start + 38_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    assert_eq!(ep.status().phase_since, held_since);
    assert!(ep.status().flight_deadline.is_some());
    let enter = start + 38_000_000;
    let origin = ep.status().estimate.unwrap().origin.id;
    let initial = LocalSetpoint::PositionYaw {
        position: PositionNed::metres(0., 0., -2.),
        yaw_radians: 0.,
    };
    let mut entry = request(
        &ep,
        3,
        FlightCommand::EnterOffboard { origin, initial },
        enter - 3_000,
    );
    entry.context.time_uncertainty_microseconds = 2_000;
    assert_eq!(
        ep.command(entry, time(enter), &mut out).unwrap().lifecycle,
        CommandLifecycle::Completed
    );
    let sp = SetpointRequest {
        epoch: ep.status().epoch,
        owner: ep.status().owner,
        arm: ep.status().arm.unwrap(),
        origin,
        offboard: RequestSequence(3),
        sequence: RequestSequence(0),
        issued_at: time(enter - 3_000),
        time_uncertainty_microseconds: 2_000,
        setpoint: initial,
    };
    assert_eq!(
        ep.setpoint(sp, time(enter), &mut out).unwrap().reason,
        CommandReason::Replay
    );
    assert!(
        ep.setpoint(
            SetpointRequest {
                sequence: RequestSequence(1),
                ..sp
            },
            time(enter),
            &mut out
        )
        .unwrap()
        .accepted
    );
    for t in (enter + 10_000..=enter + 300_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
        if t == enter + 290_000 {
            assert!(out.outputs.last().unwrap().valid_until() > time(enter + 300_000));
        }
    }
    assert_eq!(ep.status().phase, VehiclePhase::FallbackHold);
    assert_eq!(ep.status().phase_deadline, Some(time(enter + 1_295_000)));
    let retry = ep
        .command(
            request(
                &ep,
                4,
                FlightCommand::EnterOffboard { origin, initial },
                enter + 300_000,
            ),
            time(enter + 300_000),
            &mut out,
        )
        .unwrap();
    assert_eq!(retry.reason, CommandReason::FallbackLatched);
    assert!(
        !ep.setpoint(
            SetpointRequest {
                sequence: RequestSequence(2),
                issued_at: time(enter + 298_000),
                ..sp
            },
            time(enter + 300_000),
            &mut out
        )
        .unwrap()
        .accepted
    );
    let land = enter + 1_300_000;
    for t in (enter + 310_000..=land).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
        if t == land - 10_000 {
            assert!(out.outputs.last().unwrap().valid_until() > time(land));
        }
    }
    assert_eq!(ep.status().phase, VehiclePhase::Land);
    let deadline = ep.status().phase_deadline;
    assert_eq!(deadline, Some(time(enter + 21_295_000)));
    for sequence in [5, 6] {
        let repeated = ep
            .command(
                request(&ep, sequence, FlightCommand::Land, land),
                time(land),
                &mut out,
            )
            .unwrap();
        assert_eq!(repeated.reason, CommandReason::AlreadyActive);
        assert_eq!(repeated.lifecycle, CommandLifecycle::Rejected);
        assert_eq!(ep.status().phase_deadline, deadline);
    }
    for t in (land + 10_000..=land + 9_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, Some(land));
    }
    assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
    assert!(ep.status().fallback_latched);
    assert_eq!(ep.status().ground_state, GroundState::Grounded);
    let inhibit = out.outputs.last().unwrap();
    assert!(!inhibit.permitted());
    assert_eq!(inhibit.arm(), None);
    let now = land + 9_000_000;
    let s = ep
        .command(
            request(&ep, 7, FlightCommand::Arm, now),
            time(now),
            &mut out,
        )
        .unwrap();
    assert_eq!(s.arm, Some(ArmId(7)));
    assert!(ep.status().fallback_latched);
    for t in (now + 10_000..=now + 600_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, Some(land));
    }
    assert_eq!(ep.status().phase, VehiclePhase::ArmedIdle);
    assert!(!ep.status().fallback_latched);
}

#[test]
fn failed_inhibition_and_terminal_notification_are_both_retained() {
    let (mut ep, mut out, now) = armed();
    out.fail_inhibit = true;
    out.fail_notification = true;
    let err = ep
        .command(
            request(&ep, 2, FlightCommand::Terminate, now),
            time(now),
            &mut out,
        )
        .unwrap_err();
    assert_eq!(err.primary, EndpointFailure::Notification("notification"));
    assert_eq!(err.inhibition, Some("inhibit"));
    assert_eq!(ep.status().phase, VehiclePhase::Terminated);
    assert!(!out.outputs.last().unwrap().permitted());
    assert_eq!(out.commands.last().unwrap().kind, CommandKind::Terminate);
}

#[test]
fn supervision_latches_stream_expiry_and_land_without_consuming_control_release() {
    let (mut ep, mut out, start) = armed();
    ep.command(
        request(
            &ep,
            2,
            FlightCommand::Takeoff {
                height_above_launch_metres: 2.0,
            },
            start,
        ),
        time(start),
        &mut out,
    )
    .unwrap();
    for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    assert_eq!(ep.status().phase, VehiclePhase::Hold);
    let enter = start + 8_000_000;
    let origin = ep.status().estimate.unwrap().origin.id;
    ep.command(
        request(
            &ep,
            3,
            FlightCommand::EnterOffboard {
                origin,
                initial: LocalSetpoint::PositionYaw {
                    position: PositionNed::metres(0.0, 0.0, -2.0),
                    yaw_radians: 0.0,
                },
            },
            enter,
        ),
        time(enter),
        &mut out,
    )
    .unwrap();
    let expiry = enter + 300_000;
    for t in (enter + 10_000..expiry).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    let output = *out.outputs.last().unwrap();
    let cached = ep.status().estimate;
    ep.supervise(time(expiry), &mut out).unwrap();
    assert_eq!(ep.status().phase, VehiclePhase::FallbackHold);
    assert!(ep.status().fallback_latched);
    assert_eq!(ep.status().phase_deadline, Some(time(expiry + 1_000_000)));
    assert_eq!(ep.status().estimate, cached);
    assert_eq!(*out.outputs.last().unwrap(), output);
    ep.supervise(time(expiry), &mut out).unwrap();
    assert_eq!(*out.outputs.last().unwrap(), output);
    // The post-ingress tick must still release on the due grid point.
    fly(&mut ep, &mut out, expiry, start, None);
    assert_eq!(
        out.outputs.last().unwrap().sequence(),
        output.sequence() + 1
    );
    assert!(out.outputs.last().unwrap().permitted());
    for t in (expiry + 10_000..expiry + 1_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    let output = *out.outputs.last().unwrap();
    let cached = ep.status().estimate;
    ep.supervise(time(expiry + 1_000_000), &mut out).unwrap();
    assert_eq!(ep.status().phase, VehiclePhase::Land);
    assert_eq!(ep.status().phase_deadline, Some(time(expiry + 21_000_000)));
    assert_eq!(ep.status().estimate, cached);
    assert_eq!(*out.outputs.last().unwrap(), output);
}

#[test]
fn unsuccessful_hold_enters_land_without_resetting_flight_deadline() {
    let (mut ep, mut out, start) = armed();
    ep.command(
        request(
            &ep,
            2,
            FlightCommand::Takeoff {
                height_above_launch_metres: 2.,
            },
            start,
        ),
        time(start),
        &mut out,
    )
    .unwrap();
    for t in (start + 10_000..=start + 2_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    let at = start + 2_000_000;
    ep.command(request(&ep, 3, FlightCommand::Hold, at), time(at), &mut out)
        .unwrap();
    let flight_deadline = ep.status().flight_deadline;
    for t in (at + 10_000..=at + 10_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    assert_eq!(ep.status().phase, VehiclePhase::Land);
    assert_eq!(ep.status().flight_deadline, flight_deadline);
    let result = ep.status().last_terminal_command.unwrap();
    assert_eq!(result.kind, CommandKind::Hold);
    assert_eq!(result.lifecycle, CommandLifecycle::Failed);
    assert_eq!(result.reason, CommandReason::PhaseDeadline);
}

#[test]
fn streamed_velocity_and_yaw_reach_the_actual_controller() {
    let (mut ep, mut out, start) = armed();
    ep.command(
        request(
            &ep,
            2,
            FlightCommand::Takeoff {
                height_above_launch_metres: 2.0,
            },
            start,
        ),
        time(start),
        &mut out,
    )
    .unwrap();
    for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    let now = start + 8_000_000;
    let status = ep.status();
    let origin = status.estimate.unwrap().origin.id;
    let initial = LocalSetpoint::VelocityYaw {
        velocity: VelocityNed::metres_per_second(0.3, 0.0, 0.0),
        yaw_radians: 0.5,
    };
    ep.command(
        request(
            &ep,
            3,
            FlightCommand::EnterOffboard { origin, initial },
            now,
        ),
        time(now),
        &mut out,
    )
    .unwrap();
    for t in (now + 10_000..=now + 50_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    let u = out.outputs.last().unwrap().motors();
    assert!(u[0] - u[1] + u[2] - u[3] > 0.0001); // positive yaw, mixer reaction signs
    assert!(u[0] - u[1] - u[2] + u[3] < -0.001); // nose-down pitch for north velocity
    let sp = SetpointRequest {
        epoch: status.epoch,
        owner: status.owner,
        arm: status.arm.unwrap(),
        origin,
        offboard: RequestSequence(3),
        sequence: RequestSequence(1),
        issued_at: time(now + 50_000),
        time_uncertainty_microseconds: 0,
        setpoint: LocalSetpoint::VelocityYaw {
            velocity: VelocityNed::metres_per_second(0.3, 0.0, 0.0),
            yaw_radians: -0.5,
        },
    };
    assert!(
        ep.setpoint(sp, time(now + 50_000), &mut out)
            .unwrap()
            .accepted
    );
    let expires = ep.status().setpoint_expires_at;
    assert_eq!(
        ep.setpoint(sp, time(now + 50_000), &mut out)
            .unwrap()
            .reason,
        CommandReason::Replay
    );
    assert_eq!(ep.status().setpoint_expires_at, expires);
    for t in (now + 60_000..=now + 150_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    let u = out.outputs.last().unwrap().motors();
    assert!(u[0] - u[1] + u[2] - u[3] < -0.0001);
}

#[test]
fn sensed_bounce_restarts_contact_dwell_without_extending_landing() {
    let (mut ep, mut out, start) = armed();
    ep.command(
        request(
            &ep,
            2,
            FlightCommand::Takeoff {
                height_above_launch_metres: 2.0,
            },
            start,
        ),
        time(start),
        &mut out,
    )
    .unwrap();
    for t in (start + 10_000..=start + 8_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    let land = start + 8_000_000;
    ep.command(
        request(&ep, 3, FlightCommand::Land, land),
        time(land),
        &mut out,
    )
    .unwrap();
    let deadline = ep.status().phase_deadline;
    let mut bounce = None;
    let mut observed_motion = false;
    for t in (land + 10_000..=land + 11_000_000).step_by(10_000) {
        fly_with_bounce(&mut ep, &mut out, t, start, Some(land), bounce);
        if bounce.is_none() && ep.status().phase == VehiclePhase::TouchdownConfirm {
            bounce = Some(t + 100_000);
        }
        if let Some(at) = bounce {
            if t >= at + 200_000 && t <= at + 800_000 {
                assert_ne!(ep.status().ground_state, GroundState::Grounded);
                assert_eq!(ep.status().phase_deadline, deadline);
                observed_motion = true;
            }
            if t >= at && t < at + 2_000_000 {
                assert_ne!(ep.status().phase, VehiclePhase::Disarmed);
            }
        }
    }
    assert!(observed_motion);
    assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
    assert_eq!(
        ep.status().last_terminal_command.unwrap().kind,
        CommandKind::Land
    );
}

#[test]
fn flight_reserve_fails_pending_hold_before_autonomous_landing() {
    let (mut ep, mut out, start) = armed();
    ep.command(
        request(
            &ep,
            2,
            FlightCommand::Takeoff {
                height_above_launch_metres: 2.0,
            },
            start,
        ),
        time(start),
        &mut out,
    )
    .unwrap();
    for t in (start + 10_000..=start + 99_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    let hold = start + 99_000_000;
    ep.command(
        request(&ep, 3, FlightCommand::Hold, hold),
        time(hold),
        &mut out,
    )
    .unwrap();
    let land = start + 100_000_000;
    for t in (hold + 10_000..=land).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, None);
    }
    assert_eq!(ep.status().phase, VehiclePhase::Land);
    assert_eq!(ep.status().active_command, None);
    let failure = ep.command_outcomes()[0].unwrap().status;
    assert_eq!(failure.sequence, RequestSequence(3));
    assert_eq!(failure.lifecycle, CommandLifecycle::Failed);
    assert_eq!(failure.reason, CommandReason::PhaseDeadline);
    for t in (land + 10_000..=land + 9_000_000).step_by(10_000) {
        fly(&mut ep, &mut out, t, start, Some(land));
    }
    assert_eq!(ep.status().phase, VehiclePhase::Disarmed);
    assert!(!out.commands.iter().any(|c|c.sequence==RequestSequence(3) && c.lifecycle==CommandLifecycle::Completed));
    assert_eq!(ep.status().last_terminal_command, Some(failure));
}