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

"""AZ-405 — ``ReplayInputAdapter`` coordinator unit tests.

Covers AC-6 (low-confidence WARN-and-proceed), AC-7 (AC-8 hard-fail),
AC-8 (manual override bypass), AC-11 (open() returns a complete
bundle), AC-12 (idempotent close), and AC-13 (R-DEMO-3 fail-fast on
missing tlog message types).

Synthetic videos use the same OpenCV-driven fixture pattern as
``tests/unit/frame_source/test_protocol_conformance.py``; the tlog
side is faked via the coordinator's ``tlog_source_factory`` injection
point so tests run without a real pymavlink connection.

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

from __future__ import annotations

from pathlib import Path
from types import SimpleNamespace
from typing import Any
from unittest import mock

import cv2
import numpy as np
import pytest

from gps_denied_onboard._types.calibration import CameraCalibration
from gps_denied_onboard._types.fc import FcKind
from gps_denied_onboard.clock.tlog_derived import TlogDerivedClock
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.components.c8_fc_adapter.tlog_replay_adapter import (
    ReplayPace,
    TlogReplayFcAdapter,
)
from gps_denied_onboard.frame_source.video_file import VideoFileFrameSource
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
from gps_denied_onboard.replay_input.interface import (
    AutoSyncConfig,
    AutoSyncDecision,
    ReplayInputBundle,
)
from gps_denied_onboard.replay_input.tlog_video_adapter import ReplayInputAdapter


# ---------------------------------------------------------------------
# Fixtures


@pytest.fixture(autouse=True)
def _enable_build_flags(monkeypatch: pytest.MonkeyPatch) -> None:
    """Both downstream strategies are gated by build flags (AZ-398 / AZ-399)."""
    monkeypatch.setenv("BUILD_VIDEO_FILE_FRAME_SOURCE", "ON")
    monkeypatch.setenv("BUILD_TLOG_REPLAY_ADAPTER", "ON")


@pytest.fixture
def fake_fdr_client() -> mock.MagicMock:
    return mock.MagicMock(name="FdrClient")


@pytest.fixture
def fake_wgs_converter() -> mock.MagicMock:
    return mock.MagicMock(name="WgsConverter")


@pytest.fixture
def camera_calibration() -> CameraCalibration:
    return CameraCalibration(
        camera_id="az405-test",
        intrinsics_3x3=None,
        distortion=None,
        body_to_camera_se3=None,
        acquisition_method="synthetic",
    )


def _make_synthetic_video(path: Path, n_frames: int = 60, fps: int = 30) -> Path:
    """Write a 64×48 BGR MP4V file at ``path`` and return it."""
    fourcc = cv2.VideoWriter_fourcc(*"mp4v")
    writer = cv2.VideoWriter(str(path), fourcc, fps, (64, 48))
    if not writer.isOpened():
        raise RuntimeError(f"OpenCV could not open writer at {path!s}")
    try:
        for i in range(n_frames):
            frame = np.full((48, 64, 3), i % 256, dtype=np.uint8)
            writer.write(frame)
    finally:
        writer.release()
    return path


@pytest.fixture
def synthetic_video(tmp_path: Path) -> Path:
    return _make_synthetic_video(tmp_path / "az405-video.mp4", n_frames=60, fps=30)


@pytest.fixture
def synthetic_tlog_path(tmp_path: Path) -> Path:
    p = tmp_path / "az405.tlog"
    p.write_bytes(b"fake-tlog")
    return p


# ---------------------------------------------------------------------
# Fake pymavlink source


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


def _fake_msg(msg_type: str, *, ts_s: float, **fields: Any) -> SimpleNamespace:
    ns = SimpleNamespace(_timestamp=ts_s, **fields)
    ns.get_type = lambda: msg_type
    return ns


def _build_takeoff_messages(
    *,
    accel_pre_total_g: float = 1.0,
    accel_post_total_g: float = 2.2,
    accel_hz: int = 200,
    pre_seconds: float = 2.0,
    post_seconds: float = 1.5,
) -> list[SimpleNamespace]:
    """A short tlog stream with a clear take-off pattern + GPS + heartbeat."""
    out: list[SimpleNamespace] = []
    accel_period = 1.0 / accel_hz
    # Pre-takeoff: 1 g hover (z-acc = -1g in body, after sign).
    t = 0.0
    while t < pre_seconds:
        out.append(
            _fake_msg(
                "RAW_IMU",
                ts_s=t,
                xacc=0,
                yacc=0,
                zacc=-int(accel_pre_total_g * 1000),
                xgyro=0,
                ygyro=0,
                zgyro=0,
            )
        )
        t += accel_period
    # Post-takeoff: 2.2 g sustained climb thrust.
    while t < pre_seconds + post_seconds:
        out.append(
            _fake_msg(
                "RAW_IMU",
                ts_s=t,
                xacc=0,
                yacc=0,
                zacc=-int(accel_post_total_g * 1000),
                xgyro=0,
                ygyro=0,
                zgyro=0,
            )
        )
        t += accel_period

    # Attitude: flat hover then 1.5 rad/s pitch ramp.
    t = 0.0
    attitude_period = 1.0 / 100.0
    while t < pre_seconds:
        out.append(
            _fake_msg("ATTITUDE", ts_s=t, roll=0.0, pitch=0.0, yaw=0.0)
        )
        t += attitude_period
    pitch_rate = 1.5
    while t < pre_seconds + post_seconds:
        dt = t - pre_seconds
        out.append(
            _fake_msg(
                "ATTITUDE",
                ts_s=t,
                roll=0.0,
                pitch=pitch_rate * dt,
                yaw=0.0,
            )
        )
        t += attitude_period

    # GPS_RAW_INT + HEARTBEAT (required by AZ-399 pre-scan).
    out.append(
        _fake_msg(
            "GPS_RAW_INT",
            ts_s=0.0,
            fix_type=3,
            lat=499910000,
            lon=362210000,
            alt=153_400,
        )
    )
    out.append(_fake_msg("HEARTBEAT", ts_s=0.0, system_status=4, base_mode=0))
    out.sort(key=lambda m: m._timestamp)
    return out


class _FakeTlog:
    """Minimal pymavlink ``mavlink_connection`` stand-in.

    Returns each stored message once on ``recv_match``; ignores the
    ``type=`` filter (the AZ-399 decode loop receives unfiltered
    HEARTBEAT/IMU/ATTITUDE/GPS streams).
    """

    def __init__(self, messages: list[SimpleNamespace]) -> None:
        self._iter = iter(messages)
        self.closed = False

    def recv_match(self, **_kwargs: Any) -> SimpleNamespace | None:
        return next(self._iter, None)

    def close(self) -> None:
        self.closed = True


def _factory_for(messages: list[SimpleNamespace]) -> Any:
    """Return a source factory that yields a fresh ``_FakeTlog`` per call.

    The coordinator opens the tlog twice (once for ``_load_tlog_samples``
    in the auto-sync path, once via the FC adapter's pre-scan + decode
    handles), so the messages have to be re-emittable.
    """

    def _factory(_path: str) -> _FakeTlog:
        return _FakeTlog(list(messages))

    return _factory


def _frames_factory_with_motion(
    *,
    n_stationary: int = 10,
    n_moving: int = 50,
    fps: int = 30,
) -> Any:
    """Return a frames_factory yielding the AC-4 motion-onset shape."""
    period_ns = int(1_000_000_000 / fps)
    rng = np.random.default_rng(seed=0)

    def _factory(_path: Path, _scan_seconds: float) -> Any:
        out: list[tuple[int, np.ndarray]] = []
        # Stationary: identical frames so optical flow ≈ 0.
        base = np.full((48, 64, 3), 128, dtype=np.uint8)
        for i in range(n_stationary):
            out.append((i * period_ns, base.copy()))
        # Moving: each frame replaces a 16×16 patch at a random offset
        # so Farneback returns a clear non-zero magnitude. Determinism
        # is preserved by the seeded RNG.
        for j in range(n_moving):
            frame = base.copy()
            r = rng.integers(0, 32)
            c = rng.integers(0, 48)
            frame[r : r + 16, c : c + 16, :] = 240
            out.append(((n_stationary + j) * period_ns, frame))
        return out

    return _factory


def _video_timestamps_factory(
    *,
    n_frames: int = 60,
    fps: int = 30,
) -> Any:
    """Return a timestamps_factory with deterministic per-frame ts (ns)."""
    period_ns = int(1_000_000_000 / fps)

    def _factory(_path: Path) -> list[int]:
        return [i * period_ns for i in range(n_frames)]

    return _factory


# ---------------------------------------------------------------------
# AC-11 — open() returns a complete bundle


def test_ac11_open_returns_complete_bundle_with_correct_strategies(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
) -> None:
    # Arrange
    messages = _build_takeoff_messages()
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.ASAP,
        manual_time_offset_ms=0,
        auto_sync_config=AutoSyncConfig(),
        tlog_source_factory=_factory_for(messages),
        video_timestamps_factory=_video_timestamps_factory(),
    )

    # Act
    try:
        bundle = adapter.open()

        # Assert
        assert isinstance(bundle, ReplayInputBundle)
        assert isinstance(bundle.frame_source, VideoFileFrameSource)
        assert isinstance(bundle.fc_adapter, TlogReplayFcAdapter)
        assert isinstance(bundle.clock, TlogDerivedClock)
        assert bundle.resolved_time_offset_ms == 0
        # Manual path → no auto-sync result.
        assert bundle.auto_sync_result is None
    finally:
        adapter.close()


def test_ac11_pace_realtime_yields_wall_clock(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
) -> None:
    # Arrange
    messages = _build_takeoff_messages()
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.REALTIME,
        manual_time_offset_ms=0,
        auto_sync_config=AutoSyncConfig(),
        tlog_source_factory=_factory_for(messages),
        video_timestamps_factory=_video_timestamps_factory(),
    )

    # Act
    try:
        bundle = adapter.open()

        # Assert
        assert isinstance(bundle.clock, WallClock)
    finally:
        adapter.close()


def test_ac11_pace_asap_yields_tlog_derived_clock(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
) -> None:
    # Arrange
    messages = _build_takeoff_messages()
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.ASAP,
        manual_time_offset_ms=0,
        auto_sync_config=AutoSyncConfig(),
        tlog_source_factory=_factory_for(messages),
        video_timestamps_factory=_video_timestamps_factory(),
    )

    # Act
    try:
        bundle = adapter.open()

        # Assert
        assert isinstance(bundle.clock, TlogDerivedClock)
    finally:
        adapter.close()


# ---------------------------------------------------------------------
# AC-12 — idempotent close


def test_ac12_close_is_idempotent(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
) -> None:
    # Arrange
    messages = _build_takeoff_messages()
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.ASAP,
        manual_time_offset_ms=0,
        auto_sync_config=AutoSyncConfig(),
        tlog_source_factory=_factory_for(messages),
        video_timestamps_factory=_video_timestamps_factory(),
    )
    adapter.open()

    # Act / Assert — both calls must complete without raising.
    adapter.close()
    adapter.close()


def test_close_without_open_does_not_raise(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
) -> None:
    # Arrange
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.ASAP,
        manual_time_offset_ms=0,
        auto_sync_config=AutoSyncConfig(),
    )

    # Act / Assert
    adapter.close()


# ---------------------------------------------------------------------
# AC-13 — missing tlog messages fail fast


def test_ac13_missing_imu_messages_fails_fast_before_video_read(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
) -> None:
    # Arrange — tlog has only ATTITUDE; no RAW_IMU / SCALED_IMU2.
    attitude_only = [
        _fake_msg("ATTITUDE", ts_s=t * 0.01, roll=0.0, pitch=0.0, yaw=0.0)
        for t in range(100)
    ]
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.ASAP,
        manual_time_offset_ms=0,
        auto_sync_config=AutoSyncConfig(),
        tlog_source_factory=_factory_for(attitude_only),
    )

    # Act / Assert
    with pytest.raises(
        ReplayInputAdapterError, match="tlog missing required message types"
    ):
        adapter.open()


def test_ac13_missing_attitude_messages_fails_fast(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
) -> None:
    # Arrange — tlog has only RAW_IMU; no ATTITUDE.
    imu_only = [
        _fake_msg(
            "RAW_IMU",
            ts_s=t * 0.005,
            xacc=0,
            yacc=0,
            zacc=-1000,
            xgyro=0,
            ygyro=0,
            zgyro=0,
        )
        for t in range(100)
    ]
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.ASAP,
        manual_time_offset_ms=0,
        auto_sync_config=AutoSyncConfig(),
        tlog_source_factory=_factory_for(imu_only),
    )

    # Act / Assert
    with pytest.raises(
        ReplayInputAdapterError, match=r"tlog missing required message types.*ATTITUDE"
    ):
        adapter.open()


# ---------------------------------------------------------------------
# AC-8 — manual override bypasses auto-detect


def test_ac8_manual_override_bypasses_auto_detect(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
    monkeypatch: pytest.MonkeyPatch,
) -> None:
    # Arrange
    detect_calls: list[Any] = []

    def _explode_if_called(*args: Any, **kwargs: Any) -> Any:
        detect_calls.append((args, kwargs))
        raise AssertionError(
            "auto-sync detector called even though manual_time_offset_ms was set"
        )

    monkeypatch.setattr(
        "gps_denied_onboard.replay_input.tlog_video_adapter.detect_video_motion_onset",
        _explode_if_called,
    )

    # Patch the take-off compute kernel referenced via the helper; the
    # coordinator's manual path must skip it entirely.
    monkeypatch.setattr(
        "gps_denied_onboard.replay_input.auto_sync._compute_tlog_takeoff_from_samples",
        _explode_if_called,
    )

    messages = _build_takeoff_messages()
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.ASAP,
        manual_time_offset_ms=500,
        auto_sync_config=AutoSyncConfig(),
        tlog_source_factory=_factory_for(messages),
        video_timestamps_factory=_video_timestamps_factory(),
    )

    # Act
    try:
        bundle = adapter.open()

        # Assert — detector helpers were NOT invoked.
        assert detect_calls == []
        assert bundle.resolved_time_offset_ms == 500
        assert bundle.auto_sync_result is None
    finally:
        adapter.close()


# ---------------------------------------------------------------------
# AC-7 — AC-8 hard-fail raises


def test_ac7_ac8_validator_hard_fail_raises_on_open(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
) -> None:
    # Arrange — manual offset of 60 s will push every video frame
    # outside the IMU coverage window (the fake tlog only carries
    # ~3.5 s of samples).
    messages = _build_takeoff_messages()
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.ASAP,
        manual_time_offset_ms=60_000,
        auto_sync_config=AutoSyncConfig(),
        tlog_source_factory=_factory_for(messages),
        video_timestamps_factory=_video_timestamps_factory(),
    )

    # Act / Assert
    with pytest.raises(ReplayInputAdapterError, match="auto-sync hard-fail"):
        adapter.open()


# ---------------------------------------------------------------------
# AC-6 — low combined confidence WARN-and-proceed


def test_ac6_low_confidence_warn_and_proceed_does_not_raise(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
    monkeypatch: pytest.MonkeyPatch,
    caplog: pytest.LogCaptureFixture,
) -> None:
    # Arrange — stub the detectors to return low-confidence results.
    from gps_denied_onboard.replay_input.auto_sync import _DetectorResult

    low_conf = _DetectorResult(onset_ns=_ns(2.0), confidence=0.40)

    def _stub_take_off(*args: Any, **kwargs: Any) -> _DetectorResult:
        return low_conf

    def _stub_motion_onset(*args: Any, **kwargs: Any) -> _DetectorResult:
        return _DetectorResult(onset_ns=_ns(2.0), confidence=0.40)

    monkeypatch.setattr(
        "gps_denied_onboard.replay_input.auto_sync._compute_tlog_takeoff_from_samples",
        _stub_take_off,
    )
    monkeypatch.setattr(
        "gps_denied_onboard.replay_input.tlog_video_adapter.detect_video_motion_onset",
        _stub_motion_onset,
    )

    messages = _build_takeoff_messages()
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.ASAP,
        manual_time_offset_ms=None,
        auto_sync_config=AutoSyncConfig(),
        tlog_source_factory=_factory_for(messages),
        video_timestamps_factory=_video_timestamps_factory(),
    )

    # Act
    caplog.set_level("WARNING", logger="replay_input.tlog_video_adapter")
    try:
        bundle = adapter.open()

        # Assert — open() returned the bundle (didn't raise) and the
        # WARN log fired.
        assert bundle.auto_sync_result is not None
        assert bundle.auto_sync_result.combined_confidence == pytest.approx(0.40)
        warn_kinds = [
            r.kind for r in caplog.records if hasattr(r, "kind")
        ]
        assert "replay.auto_sync.low_confidence" in warn_kinds
    finally:
        adapter.close()


def test_ac11_resolved_offset_matches_auto_sync_result(
    synthetic_video: Path,
    synthetic_tlog_path: Path,
    camera_calibration: CameraCalibration,
    fake_wgs_converter: mock.MagicMock,
    fake_fdr_client: mock.MagicMock,
    monkeypatch: pytest.MonkeyPatch,
) -> None:
    # Arrange — high-confidence stubs so AC-6 WARN does not fire.
    from gps_denied_onboard.replay_input.auto_sync import _DetectorResult

    def _stub_take_off(*args: Any, **kwargs: Any) -> _DetectorResult:
        return _DetectorResult(onset_ns=_ns(2.0), confidence=0.95)

    def _stub_motion_onset(*args: Any, **kwargs: Any) -> _DetectorResult:
        return _DetectorResult(onset_ns=_ns(0.333), confidence=0.95)

    monkeypatch.setattr(
        "gps_denied_onboard.replay_input.auto_sync._compute_tlog_takeoff_from_samples",
        _stub_take_off,
    )
    monkeypatch.setattr(
        "gps_denied_onboard.replay_input.tlog_video_adapter.detect_video_motion_onset",
        _stub_motion_onset,
    )

    messages = _build_takeoff_messages()
    adapter = ReplayInputAdapter(
        video_path=synthetic_video,
        tlog_path=synthetic_tlog_path,
        camera_calibration=camera_calibration,
        target_fc_dialect=FcKind.ARDUPILOT_PLANE,
        wgs_converter=fake_wgs_converter,
        fdr_client=fake_fdr_client,
        pace=ReplayPace.ASAP,
        manual_time_offset_ms=None,
        auto_sync_config=AutoSyncConfig(),
        tlog_source_factory=_factory_for(messages),
        video_timestamps_factory=_video_timestamps_factory(),
    )

    # Act
    try:
        bundle = adapter.open()

        # Assert
        expected_offset_ms = (_ns(2.0) - _ns(0.333)) // 1_000_000
        assert bundle.resolved_time_offset_ms == expected_offset_ms
        assert bundle.auto_sync_result is not None
        assert bundle.auto_sync_result.offset_ms == expected_offset_ms
    finally:
        adapter.close()