autopilot/crates/gimbal_controller/tests/batch_11_integration.rs

//! AZ-654 / AZ-655 / AZ-656 integration tests.
//!
//! Each test exercises one batch-11 primitive against the production
//! `GimbalControllerHandle` surface (set_pose / zoom / state_stream),
//! catching wiring bugs that the per-primitive unit tests can't see
//! (e.g. `ts_monotonic_ns` plumbing, transport interaction).

use std::net::{Ipv4Addr, SocketAddr};
use std::sync::Arc;
use std::time::{Duration, Instant};

use tokio::net::UdpSocket;

use gimbal_controller::{
    encode_frame, A40Transport, CentreOnTarget, CentreOnTargetConfig, FrameId, GimbalCommand,
    GimbalController, NextStep, PlanExecutor, SweepConfig, SweepEngine, SweepPattern,
};
use shared::models::frame::BoundingBox;
use shared::models::gimbal::{GimbalState, PanGoal, PanPlan};

fn loopback(port: u16) -> SocketAddr {
    SocketAddr::new(Ipv4Addr::new(127, 0, 0, 1).into(), port)
}

fn initial_state() -> GimbalState {
    GimbalState {
        yaw: 0.0,
        pitch: 0.0,
        zoom: 1.0,
        ts_monotonic_ns: 0,
        command_in_flight: false,
    }
}

/// AZ-656 AC-2 — every `set_pose` publishes a `GimbalState` with a
/// strictly-monotonic `ts_monotonic_ns`. Catches the wrong-clock bug
/// where `SystemTime::now()` was previously used (would have been
/// observable as a stale or NTP-adjusted timestamp).
#[tokio::test]
async fn az656_set_pose_publishes_monotonic_timestamp() {
    // Arrange — full controller wired to a fake A40 echo loop
    let fake_socket = Arc::new(UdpSocket::bind(loopback(0)).await.expect("fake bind"));
    let fake_addr = fake_socket.local_addr().expect("fake addr");
    let test_socket = Arc::new(UdpSocket::bind(loopback(0)).await.expect("test bind"));
    test_socket.connect(fake_addr).await.expect("connect");

    let fake_socket_clone = fake_socket.clone();
    tokio::spawn(async move {
        loop {
            let mut buf = [0u8; 128];
            let Ok((_, from)) = fake_socket_clone.recv_from(&mut buf).await else {
                return;
            };
            let reply = encode_frame(FrameId::T1F1B1D1, &[0; 12], 0).expect("encode");
            let _ = fake_socket_clone.send_to(&reply, from).await;
        }
    });

    let (transport, _recv_task) =
        A40Transport::from_socket(test_socket, fake_addr).expect("from_socket");
    let controller = GimbalController::with_transport(initial_state(), transport);
    let handle = controller.handle();
    let state_rx = handle.state_stream();

    // Act — three sequential set_pose calls; capture the stamps each
    // call publishes onto the watch channel.
    let mut timestamps = Vec::with_capacity(3);
    for i in 0..3 {
        handle
            .set_pose(GimbalCommand {
                yaw_deg: i as f32 * 5.0,
                pitch_deg: 0.0,
            })
            .await
            .expect("set_pose");
        timestamps.push(state_rx.borrow().ts_monotonic_ns);
        tokio::time::sleep(Duration::from_millis(2)).await;
    }

    // Assert
    assert!(
        timestamps[0] > 0,
        "initial stamp should be > 0 after first set_pose"
    );
    assert!(
        timestamps[1] > timestamps[0],
        "ts not monotonic: {} → {}",
        timestamps[0],
        timestamps[1]
    );
    assert!(
        timestamps[2] > timestamps[1],
        "ts not monotonic: {} → {}",
        timestamps[1],
        timestamps[2]
    );
}

/// AZ-655 integration — load a plan and exercise the executor against
/// a real wall-clock-driven tick loop; verify the throttle counter
/// matches the emission ratio.
#[test]
fn az655_plan_executor_emits_and_throttles_against_real_clock() {
    // Arrange
    let mut exe = PlanExecutor::new(Duration::from_millis(20));
    let t0 = Instant::now();
    exe.load(
        PanPlan {
            goals: vec![
                PanGoal {
                    yaw_deg: -10.0,
                    pitch_deg: 0.0,
                    zoom: 1.0,
                    at_ns: 0,
                },
                PanGoal {
                    yaw_deg: 10.0,
                    pitch_deg: 0.0,
                    zoom: 1.0,
                    at_ns: 200_000_000,
                },
            ],
        },
        t0,
    )
    .expect("load plan");

    // Act — 100 ticks at 5 ms cadence over 500 ms
    let mut emits = 0_u64;
    let mut throttled = 0_u64;
    for i in 0..100 {
        match exe.next_step(t0 + Duration::from_millis(i * 5)).unwrap() {
            NextStep::Emit(_) => emits += 1,
            NextStep::Throttled => throttled += 1,
        }
    }

    // Assert — 20 ms throttle over 500 ms ≈ 25 emissions
    assert!((23..=27).contains(&emits), "emits = {emits}, want ~25");
    assert_eq!(emits + throttled, 100);
    assert_eq!(exe.stats().commands_emitted_total, emits);
    assert_eq!(exe.stats().commands_dropped_to_throttle_total, throttled);
}

/// AZ-654 integration — pendulum sweep produces commands the
/// controller can accept (matches the `GimbalCommand` contract used by
/// `set_pose`). No transport wiring needed; this is a contract test.
#[test]
fn az654_sweep_engine_emits_gimbal_commands_within_bounds() {
    // Arrange
    let cfg = SweepConfig {
        min_yaw_deg: -45.0,
        max_yaw_deg: 45.0,
        pitch_deg: -15.0,
        step_deg: 3.0,
        dwell: Duration::from_millis(200),
    };
    let mut engine = SweepEngine::new(SweepPattern::Pendulum, cfg).expect("new sweep");

    // Act + Assert — every emitted command stays inside the envelope
    let mut t = Instant::now();
    for _ in 0..200 {
        let cmd = engine.next_step(t).expect("pendulum tick");
        assert!(cmd.yaw_deg >= -45.0 && cmd.yaw_deg <= 45.0);
        assert!((cmd.pitch_deg - (-15.0)).abs() < 0.001);
        t += Duration::from_millis(50);
    }
}

/// AZ-656 integration — closed-loop convergence smoke against the
/// public `CentreOnTarget` surface (mirrors the unit-test kinematic
/// model but uses only the public API; catches re-export drift).
#[test]
fn az656_centre_on_target_loop_converges_via_public_api() {
    // Arrange
    let cfg = CentreOnTargetConfig::default();
    let mut ctrl = CentreOnTarget::new(cfg);
    let mut bbox = BoundingBox {
        x_min: 0.70,
        y_min: 0.50,
        x_max: 0.80,
        y_max: 0.60,
    };
    let mut yaw = 0.0_f32;
    let mut pitch = 0.0_f32;
    let zoom = 1.0_f32;
    let fov = cfg.fov_deg_at_zoom1 / zoom;

    // Act
    for _ in 0..3 {
        let out = ctrl.tick(Some(bbox), yaw, pitch, zoom);
        let cmd = out.command.expect("emit");
        let dy = cmd.yaw_deg - yaw;
        let dp = cmd.pitch_deg - pitch;
        yaw = cmd.yaw_deg;
        pitch = cmd.pitch_deg;
        let cx = (bbox.x_min + bbox.x_max) * 0.5 - dy / fov;
        let cy = (bbox.y_min + bbox.y_max) * 0.5 + dp / fov;
        bbox = BoundingBox {
            x_min: cx - 0.05,
            y_min: cy - 0.05,
            x_max: cx + 0.05,
            y_max: cy + 0.05,
        };
    }
    let final_cx = (bbox.x_min + bbox.x_max) * 0.5;
    let final_cy = (bbox.y_min + bbox.y_max) * 0.5;

    // Assert
    assert!(
        (0.375..=0.625).contains(&final_cx),
        "x = {final_cx} outside centre 25%"
    );
    assert!(
        (0.375..=0.625).contains(&final_cy),
        "y = {final_cy} outside centre 25%"
    );
}