gps-denied-onboard/tests/unit/replay_input/test_az405_auto_sync.py

"""AZ-405 — auto-sync detector + offset-compute + AC-9 validator.

Covers AC-1..AC-10 of ``_docs/02_tasks/todo/AZ-405_replay_auto_sync.md``.

Tests run against the pure compute kernels in
:mod:`gps_denied_onboard.replay_input.auto_sync` (no disk IO, no real
pymavlink, no real OpenCV) so the suite is fast + deterministic.

Style: every test follows the Arrange / Act / Assert pattern.
"""

from __future__ import annotations

import math
from typing import Any

import pytest

from gps_denied_onboard.replay_input.auto_sync import (
    TlogSamples,
    _compute_tlog_takeoff_from_samples,
    _compute_video_onset_from_samples,
    compute_offset,
    validate_offset_or_fail,
)
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
from gps_denied_onboard.replay_input.interface import AutoSyncConfig


# ---------------------------------------------------------------------
# Synthetic-fixture helpers


def _ns(seconds: float) -> int:
    return int(seconds * 1_000_000_000)


def _flat_accel_samples(
    *, start_s: float, end_s: float, hz: int, total_g: float
) -> list[tuple[int, float]]:
    out: list[tuple[int, float]] = []
    period = 1.0 / hz
    t = start_s
    while t < end_s:
        out.append((_ns(t), total_g))
        t += period
    return out


def _flat_attitude_samples(
    *, start_s: float, end_s: float, hz: int, roll: float, pitch: float, yaw: float
) -> list[tuple[int, float, float, float]]:
    out: list[tuple[int, float, float, float]] = []
    period = 1.0 / hz
    t = start_s
    while t < end_s:
        out.append((_ns(t), roll, pitch, yaw))
        t += period
    return out


def _ramp_attitude_samples(
    *,
    start_s: float,
    end_s: float,
    hz: int,
    base_roll: float,
    base_pitch: float,
    base_yaw: float,
    rate_rad_s: float,
) -> list[tuple[int, float, float, float]]:
    """Attitude that ramps in pitch at ``rate_rad_s`` rad/s."""
    out: list[tuple[int, float, float, float]] = []
    period = 1.0 / hz
    t = start_s
    while t < end_s:
        dt = t - start_s
        pitch = base_pitch + rate_rad_s * dt
        out.append((_ns(t), base_roll, pitch, base_yaw))
        t += period
    return out


def _build_takeoff_samples() -> TlogSamples:
    """AC-1 fixture: 2 s flat hover, then 1.5 s sustained 2.2 g + 1.5 rad/s.

    Take-off onset is at t = 2.0 s (the first sample with the
    elevated acceleration). Body-frame accelerometer convention: at
    hover the proper-acceleration magnitude is 1 g (gravity reaction);
    during a 1.2 g thrust climb it is 2.2 g, so the take-off excess
    above 1 g rest is 1.2 g — well above the 0.5 g threshold.
    """
    accel_pre = _flat_accel_samples(start_s=0.0, end_s=2.0, hz=200, total_g=1.0)
    accel_post = _flat_accel_samples(start_s=2.0, end_s=3.5, hz=200, total_g=2.2)
    accel = accel_pre + accel_post

    attitude_pre = _flat_attitude_samples(
        start_s=0.0, end_s=2.0, hz=100, roll=0.0, pitch=0.0, yaw=0.0
    )
    attitude_post = _ramp_attitude_samples(
        start_s=2.0,
        end_s=3.5,
        hz=100,
        base_roll=0.0,
        base_pitch=0.0,
        base_yaw=0.0,
        rate_rad_s=1.5,
    )
    attitude = attitude_pre + attitude_post

    return TlogSamples(
        accel=tuple(accel),
        attitude=tuple(attitude),
        imu_count_by_type={
            "RAW_IMU": len(accel),
            "ATTITUDE": len(attitude),
        },
    )


def _build_low_amplitude_vibration_samples() -> TlogSamples:
    """AC-2 fixture: 5 s of 0.3 g body-frame vibration (no take-off).

    Total proper-acceleration during vibration = 1.3 g (0.3 g excess
    above the 1 g rest baseline) — strictly below the 0.5 g detector
    threshold so the sustained-event search rejects every window.
    """
    accel = _flat_accel_samples(start_s=0.0, end_s=5.0, hz=200, total_g=1.3)
    attitude = _flat_attitude_samples(
        start_s=0.0, end_s=5.0, hz=100, roll=0.0, pitch=0.0, yaw=0.0
    )
    return TlogSamples(
        accel=tuple(accel),
        attitude=tuple(attitude),
        imu_count_by_type={
            "RAW_IMU": len(accel),
            "ATTITUDE": len(attitude),
        },
    )


def _build_hand_launch_samples() -> TlogSamples:
    """AC-3 fixture: 0.8 g impulse for 100 ms; not sustained for 0.5 s.

    Body-frame accelerometer convention (see ``_build_takeoff_samples``):
    a 0.8 g impulse becomes 1.8 g total proper-acceleration during the
    impulse window.
    """
    accel_pre = _flat_accel_samples(start_s=0.0, end_s=2.0, hz=200, total_g=1.0)
    accel_impulse = _flat_accel_samples(
        start_s=2.0, end_s=2.1, hz=200, total_g=1.8
    )
    accel_post = _flat_accel_samples(start_s=2.1, end_s=3.0, hz=200, total_g=1.0)
    accel = accel_pre + accel_impulse + accel_post

    attitude = _flat_attitude_samples(
        start_s=0.0, end_s=3.0, hz=100, roll=0.0, pitch=0.0, yaw=0.0
    )
    return TlogSamples(
        accel=tuple(accel),
        attitude=tuple(attitude),
        imu_count_by_type={
            "RAW_IMU": len(accel),
            "ATTITUDE": len(attitude),
        },
    )


def _flow_samples_from_frames(
    *, n_stationary: int, n_moving: int, fps: int = 30, motion_px: float = 4.0
) -> tuple[tuple[int, float], ...]:
    """Synthesise the flow-magnitude series the video detector consumes.

    The detector emits a ``(ts_ns, mean_magnitude_px)`` tuple for each
    consecutive frame pair (skipping the first frame's pair). For
    AC-4 we pretend frames 1..10 are stationary (mag ≈ 0) and frames
    11..60 are moving (mag = motion_px).
    """
    out: list[tuple[int, float]] = []
    period_ns = int(1_000_000_000 / fps)
    for i in range(1, n_stationary):
        out.append((i * period_ns, 0.05))
    for j in range(n_stationary, n_stationary + n_moving):
        out.append((j * period_ns, motion_px))
    return tuple(out)


# ---------------------------------------------------------------------
# AC-1 — tlog take-off detector (positive)


def test_ac1_tlog_takeoff_detector_positive_within_50ms_and_high_confidence() -> None:
    # Arrange
    config = AutoSyncConfig()
    samples = _build_takeoff_samples()

    # Act
    result = _compute_tlog_takeoff_from_samples(samples, config)

    # Assert
    expected_onset_ns = _ns(2.0)
    assert abs(result.onset_ns - expected_onset_ns) <= _ns(0.05), (
        f"detected onset {result.onset_ns / 1e9}s deviates >50ms from expected 2.0s"
    )
    assert result.confidence >= 0.85, (
        f"confidence {result.confidence} below AC-1 minimum of 0.85"
    )


# ---------------------------------------------------------------------
# AC-2 — tlog take-off detector (ambiguous)


def test_ac2_tlog_takeoff_detector_low_amplitude_vibration_low_confidence() -> None:
    # Arrange
    config = AutoSyncConfig()
    samples = _build_low_amplitude_vibration_samples()

    # Act
    result = _compute_tlog_takeoff_from_samples(samples, config)

    # Assert
    assert result.confidence < 0.50, (
        f"confidence {result.confidence} should be < 0.50 for ambiguous vibration"
    )


# ---------------------------------------------------------------------
# AC-3 — tlog take-off detector (hand launch)


def test_ac3_tlog_takeoff_detector_hand_launch_warn_regime() -> None:
    # Arrange
    config = AutoSyncConfig()
    samples = _build_hand_launch_samples()

    # Act
    result = _compute_tlog_takeoff_from_samples(samples, config)

    # Assert
    assert result.confidence < 0.80, (
        f"confidence {result.confidence} should be < 0.80 for unsustained hand-launch"
    )


# ---------------------------------------------------------------------
# AC-4 — video motion-onset detector


def test_ac4_video_motion_onset_detected_within_one_frame() -> None:
    # Arrange
    config = AutoSyncConfig()
    flow_samples = _flow_samples_from_frames(n_stationary=10, n_moving=50, fps=30)
    period_ns = int(1_000_000_000 / 30)
    expected_onset_ns = 10 * period_ns

    # Act
    result = _compute_video_onset_from_samples(flow_samples, config)

    # Assert
    assert abs(result.onset_ns - expected_onset_ns) <= period_ns, (
        f"detected motion onset {result.onset_ns} ns deviates >1 frame "
        f"from expected {expected_onset_ns} ns"
    )
    assert result.confidence > 0.80, (
        f"confidence {result.confidence} too low for clear motion onset"
    )


# ---------------------------------------------------------------------
# AC-5 — combined offset within ± 200 ms


def test_ac5_combined_offset_within_200ms_of_ground_truth() -> None:
    # Arrange
    config = AutoSyncConfig()
    tlog_samples = _build_takeoff_samples()
    tlog_result = _compute_tlog_takeoff_from_samples(tlog_samples, config)

    flow_samples = _flow_samples_from_frames(n_stationary=10, n_moving=50, fps=30)
    video_result = _compute_video_onset_from_samples(flow_samples, config)

    # Ground-truth offset = tlog take-off (2.0 s) − video onset (10/30 s)
    period_ns = int(1_000_000_000 / 30)
    ground_truth_offset_ms = (_ns(2.0) - 10 * period_ns) // 1_000_000

    # Act
    decision = compute_offset(tlog_result, video_result)

    # Assert
    assert abs(decision.offset_ms - ground_truth_offset_ms) <= 200, (
        f"offset {decision.offset_ms} ms deviates >200 ms from ground truth "
        f"{ground_truth_offset_ms} ms"
    )


# ---------------------------------------------------------------------
# AC-6 — low combined confidence (verified via the coordinator test
# in test_az405_replay_input_adapter.py; here we only verify the
# combined-confidence aggregator picks min())


def test_ac6_combined_confidence_takes_minimum_of_inputs() -> None:
    # Arrange
    from gps_denied_onboard.replay_input.auto_sync import _DetectorResult

    high = _DetectorResult(onset_ns=_ns(1.0), confidence=0.95)
    low = _DetectorResult(onset_ns=_ns(2.0), confidence=0.50)

    # Act
    decision = compute_offset(high, low)

    # Assert
    assert decision.combined_confidence == pytest.approx(0.50)
    assert decision.offset_ms == (_ns(1.0) - _ns(2.0)) // 1_000_000


# ---------------------------------------------------------------------
# AC-7 — AC-9 validator hard-fail (the coordinator-level raise is
# covered in test_az405_replay_input_adapter.py)


def test_ac7_validator_hard_fail_returns_2_for_offset_outside_window() -> None:
    # Arrange
    fps = 30
    period_ns = int(1_000_000_000 / fps)
    video_ts = [i * period_ns for i in range(60)]
    # IMU sampled at 200 Hz from t=0 to t=2 (mismatch deliberate; the
    # bad offset shifts everything outside the window).
    imu_ts = [int(i / 200 * 1_000_000_000) for i in range(400)]
    bad_offset_ms = 60_000

    # Act
    code = validate_offset_or_fail(
        bad_offset_ms,
        imu_ts,
        video_ts,
        threshold_pct=95.0,
        window_ms=100,
    )

    # Assert
    assert code == 2


# ---------------------------------------------------------------------
# AC-9 — frame-window match-percentage validator (positive)


def test_ac9_validator_passes_for_well_matched_offset() -> None:
    # Arrange
    fps = 30
    period_ns = int(1_000_000_000 / fps)
    video_ts = [i * period_ns for i in range(60)]
    # IMU samples densely spanning the same time range — every video
    # frame has an IMU sample within ± 100 ms.
    imu_ts = [int(i / 200 * 1_000_000_000) for i in range(60 * 200 // 30)]

    # Act
    code = validate_offset_or_fail(
        0, imu_ts, video_ts, threshold_pct=95.0, window_ms=100
    )

    # Assert
    assert code == 0


def test_ac9_threshold_configurable() -> None:
    # Arrange — set up a series where exactly 80% of frames match.
    fps = 30
    period_ns = int(1_000_000_000 / fps)
    video_ts = [i * period_ns for i in range(50)]
    # IMU only covers the first 80% of the video timeline; the last
    # 10 frames will be far outside the window.
    imu_ts = [
        int(i / 200 * 1_000_000_000) for i in range(int(40 / 30 * 200))
    ]

    # Act / Assert
    # Default 95% threshold → fail (80% < 95%).
    assert validate_offset_or_fail(
        0, imu_ts, video_ts, threshold_pct=95.0, window_ms=100
    ) == 2
    # Lowered to 75% → pass.
    assert validate_offset_or_fail(
        0, imu_ts, video_ts, threshold_pct=75.0, window_ms=100
    ) == 0


# ---------------------------------------------------------------------
# AC-10 — confidence determinism


def test_ac10_confidence_score_deterministic_across_two_runs() -> None:
    # Arrange
    config = AutoSyncConfig()
    samples = _build_takeoff_samples()

    # Act
    first = _compute_tlog_takeoff_from_samples(samples, config)
    second = _compute_tlog_takeoff_from_samples(samples, config)

    # Assert
    assert first.onset_ns == second.onset_ns
    assert math.isclose(first.confidence, second.confidence, abs_tol=1e-9)


def test_ac10_video_onset_deterministic_across_two_runs() -> None:
    # Arrange
    config = AutoSyncConfig()
    flow_samples = _flow_samples_from_frames(n_stationary=5, n_moving=20, fps=30)

    # Act
    first = _compute_video_onset_from_samples(flow_samples, config)
    second = _compute_video_onset_from_samples(flow_samples, config)

    # Assert
    assert first.onset_ns == second.onset_ns
    assert math.isclose(first.confidence, second.confidence, abs_tol=1e-9)


# ---------------------------------------------------------------------
# R-DEMO-3 fail-fast on the pure compute path


def test_pure_takeoff_kernel_raises_on_no_imu_samples() -> None:
    # Arrange
    config = AutoSyncConfig()
    samples = TlogSamples(
        accel=(),
        attitude=(),
        imu_count_by_type={"ATTITUDE": 100},
    )

    # Act / Assert
    with pytest.raises(ReplayInputAdapterError, match="tlog missing required"):
        _compute_takeoff_or_propagate(samples, config)


def test_pure_takeoff_kernel_raises_on_no_attitude_samples() -> None:
    # Arrange
    config = AutoSyncConfig()
    accel = _flat_accel_samples(start_s=0.0, end_s=1.0, hz=200, total_g=1.0)
    samples = TlogSamples(
        accel=tuple(accel),
        attitude=(),
        imu_count_by_type={"RAW_IMU": len(accel)},
    )

    # Act / Assert
    with pytest.raises(ReplayInputAdapterError, match="tlog missing required"):
        _compute_takeoff_or_propagate(samples, config)


def _compute_takeoff_or_propagate(samples: TlogSamples, config: AutoSyncConfig) -> Any:
    """Local trampoline so the assertions are explicit even if the
    underscore-named helper migrates."""
    return _compute_tlog_takeoff_from_samples(samples, config)


# ---------------------------------------------------------------------
# AC-9 edge cases


def test_validator_returns_2_on_empty_video_or_tlog() -> None:
    # Arrange
    imu_ts = [0, 1_000_000, 2_000_000]
    video_ts: list[int] = []

    # Act / Assert — empty video
    assert (
        validate_offset_or_fail(
            0, imu_ts, video_ts, threshold_pct=95.0, window_ms=100
        )
        == 2
    )
    # Empty tlog
    assert (
        validate_offset_or_fail(
            0, [], [0, 1_000_000], threshold_pct=95.0, window_ms=100
        )
        == 2
    )