gps-denied-onboard/src/gps_denied_onboard/runtime_root/_replay_branch.py

"""Replay-mode branch of :func:`compose_root` (AZ-401 / E-DEMO-REPLAY).

Internal module. Owns the wiring that turns a ``config.mode == "replay"``
:class:`Config` into a :class:`RuntimeRoot` whose components dict carries
the replay-only strategies (``frame_source``, ``fc_adapter``, ``clock``,
``mavlink_transport``, ``replay_sink``) plus whatever C1-C7+C13 strategies
the binary's bootstrap registered against
:data:`gps_denied_onboard.runtime_root._STRATEGY_REGISTRY`.

Per replay protocol v2.0.0 (ADR-011): replay is a configuration of the
single airborne composition root, not a sibling root. The branch lives
in this module to keep ``runtime_root/__init__.py`` focused on the
shared composition spine while still exposing exactly one
``compose_root(config)`` entrypoint.

Build-flag gates (per replay protocol Invariant 9):

- ``BUILD_VIDEO_FILE_FRAME_SOURCE`` — required for the
  :class:`VideoFileFrameSource` instance returned by the coordinator.
- ``BUILD_TLOG_REPLAY_ADAPTER`` — required for the
  :class:`TlogReplayFcAdapter` instance returned by the coordinator.
- ``BUILD_REPLAY_SINK_JSONL`` — shared by the JSONL sink and the noop
  outbound transport.

All three default ON in the airborne binary (per ADR-011); flipping any
OFF disables replay mode without affecting live mode.
"""

from __future__ import annotations

import json
import os
from collections.abc import Mapping
from pathlib import Path
from typing import TYPE_CHECKING, Any, Final

from gps_denied_onboard._types.calibration import CameraCalibration
from gps_denied_onboard._types.fc import FcKind
from gps_denied_onboard.components.c8_fc_adapter.noop_mavlink_transport import (
    NoopMavlinkTransport,
)
from gps_denied_onboard.components.c8_fc_adapter.replay_sink import (
    JsonlReplaySink,
)
from gps_denied_onboard.config import Config
from gps_denied_onboard.fdr_client import make_fdr_client
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
from gps_denied_onboard.logging import get_logger
from gps_denied_onboard.replay_input import (
    AutoSyncConfig,
    ReplayInputAdapter,
    ReplayInputBundle,
)
from gps_denied_onboard.replay_input.tlog_video_adapter import ReplayPace

if TYPE_CHECKING:
    from gps_denied_onboard.fdr_client.client import FdrClient

__all__ = [
    "REPLAY_BUILD_FLAGS",
    "REPLAY_COMPONENT_KEYS",
    "CompositionError",
    "build_replay_components",
]


_LOG_KIND_READY: Final[str] = "replay.compose_root.ready"


REPLAY_BUILD_FLAGS: Final[tuple[str, ...]] = (
    "BUILD_VIDEO_FILE_FRAME_SOURCE",
    "BUILD_TLOG_REPLAY_ADAPTER",
    "BUILD_REPLAY_SINK_JSONL",
)


REPLAY_COMPONENT_KEYS: Final[tuple[str, ...]] = (
    "frame_source",
    "fc_adapter",
    "clock",
    "mavlink_transport",
    "replay_sink",
)


class CompositionError(RuntimeError):
    """Raised when the replay-mode branch refuses to compose a runtime.

    Carries the human-readable reason (build-flag OFF, missing path,
    contradictory config) so the caller can surface it in the structured
    log + on stderr without a second introspection pass.
    """


def build_replay_components(
    config: Config,
    *,
    fdr_client_factory: Any | None = None,
    replay_input_adapter_factory: Any | None = None,
    sink_factory: Any | None = None,
    transport_factory: Any | None = None,
) -> tuple[dict[str, Any], tuple[str, ...]]:
    """Construct the replay-mode component dict + construction order.

    The factories are test-only injection points. Production callers
    (just ``compose_root``) leave them ``None`` so the real constructors
    run; unit tests pass fakes so they don't have to satisfy the full
    OpenCV / pymavlink / FDR side-effects of the real strategies.

    Returns:
        ``(components, construction_order)`` — the same shape
        :func:`gps_denied_onboard.runtime_root._compose` returns. The
        keys are the entries of :data:`REPLAY_COMPONENT_KEYS`; the
        values are typed strategy instances.
    """
    if config.mode != "replay":
        raise CompositionError(
            "build_replay_components called with non-replay config "
            f"(mode={config.mode!r})"
        )
    _validate_build_flags()
    _validate_replay_paths(config)

    fdr_factory = fdr_client_factory or make_fdr_client
    fdr_client = fdr_factory("replay_input", config)

    sink_fdr_client = fdr_factory("c8_fc_adapter.replay_sink", config)

    # AZ-558: build the outbound MAVLink transport BEFORE the FC adapter
    # so it can be threaded through `ReplayInputAdapter` and into
    # `TlogReplayFcAdapter`. The same instance is exposed as the
    # ``mavlink_transport`` slot in ``components`` (replay protocol
    # Invariant 5: encoders write through the seam in both modes;
    # replay drops the bytes via NoopMavlinkTransport).
    if transport_factory is not None:
        transport = transport_factory(config)
    else:
        transport = NoopMavlinkTransport()

    bundle = _build_replay_input_bundle(
        config,
        fdr_client=fdr_client,
        adapter_factory=replay_input_adapter_factory,
        mavlink_transport=transport,
    )

    if sink_factory is not None:
        sink = sink_factory(config, sink_fdr_client)
    else:
        sink = JsonlReplaySink(
            output_path=Path(config.replay.output_path),
            fdr_client=sink_fdr_client,
        )

    components: dict[str, Any] = {
        "frame_source": bundle.frame_source,
        "fc_adapter": bundle.fc_adapter,
        "clock": bundle.clock,
        "mavlink_transport": transport,
        "replay_sink": sink,
    }

    _log_ready(config, bundle)
    return components, REPLAY_COMPONENT_KEYS


def _validate_build_flags() -> None:
    """Refuse construction when any replay-mode ``BUILD_*`` flag is OFF."""
    for flag_name in REPLAY_BUILD_FLAGS:
        raw = os.environ.get(flag_name, "ON").strip().upper()
        if raw == "OFF":
            raise CompositionError(
                f"{flag_name} is OFF; replay mode requires it"
            )


def _validate_replay_paths(config: Config) -> None:
    """Reject empty / missing replay paths early with a precise message."""
    if not config.replay.video_path:
        raise CompositionError(
            "config.replay.video_path is empty; replay mode requires a video path"
        )
    if not config.replay.tlog_path:
        raise CompositionError(
            "config.replay.tlog_path is empty; replay mode requires a tlog path"
        )
    if not config.replay.output_path:
        raise CompositionError(
            "config.replay.output_path is empty; replay mode requires an output path"
        )


def _build_replay_input_bundle(
    config: Config,
    *,
    fdr_client: "FdrClient",
    adapter_factory: Any | None,
    mavlink_transport: Any | None = None,
) -> ReplayInputBundle:
    """Build the :class:`ReplayInputAdapter` and call ``open()``."""
    pace = _resolve_pace(config.replay.pace)
    target_fc_dialect = _resolve_fc_kind(config.replay.target_fc_dialect)
    auto_sync = _build_auto_sync_config(config)
    camera_calibration = _load_camera_calibration(config)
    wgs_converter = WgsConverter()

    if adapter_factory is not None:
        adapter = adapter_factory(
            config=config,
            camera_calibration=camera_calibration,
            target_fc_dialect=target_fc_dialect,
            wgs_converter=wgs_converter,
            fdr_client=fdr_client,
            pace=pace,
            auto_sync_config=auto_sync,
            mavlink_transport=mavlink_transport,
        )
    else:
        adapter = ReplayInputAdapter(
            video_path=Path(config.replay.video_path),
            tlog_path=Path(config.replay.tlog_path),
            camera_calibration=camera_calibration,
            target_fc_dialect=target_fc_dialect,
            wgs_converter=wgs_converter,
            fdr_client=fdr_client,
            pace=pace,
            manual_time_offset_ms=config.replay.time_offset_ms,
            skip_auto_sync_validation=config.replay.skip_auto_sync_validation,
            auto_trim=config.replay.auto_trim,
            auto_sync_config=auto_sync,
            mavlink_transport=mavlink_transport,
        )
    return adapter.open()


def _resolve_pace(raw: str) -> ReplayPace:
    if raw == "asap":
        return ReplayPace.ASAP
    if raw == "realtime":
        return ReplayPace.REALTIME
    raise CompositionError(
        f"config.replay.pace={raw!r} not in ('asap', 'realtime')"
    )


def _resolve_fc_kind(raw: str) -> FcKind:
    if raw == "ardupilot_plane":
        return FcKind.ARDUPILOT_PLANE
    if raw == "inav":
        return FcKind.INAV
    raise CompositionError(
        f"config.replay.target_fc_dialect={raw!r} not in "
        "('ardupilot_plane', 'inav')"
    )


def _build_auto_sync_config(config: Config) -> AutoSyncConfig:
    block = config.replay.auto_sync
    return AutoSyncConfig(
        takeoff_accel_threshold_g=block.takeoff_accel_threshold_g,
        takeoff_attitude_rate_threshold_rad_s=(
            block.takeoff_attitude_rate_threshold_rad_s
        ),
        sustained_seconds=block.sustained_seconds,
        prescan_max_messages=block.prescan_max_messages,
        video_motion_threshold=block.video_motion_threshold,
        video_motion_scan_seconds=block.video_motion_scan_seconds,
        match_threshold_pct=block.match_threshold_pct,
        match_window_ms=block.match_window_ms,
        low_confidence_threshold=block.low_confidence_threshold,
        alignment_resample_hz=block.alignment_resample_hz,
        alignment_video_scan_seconds=block.alignment_video_scan_seconds,
        alignment_low_confidence_threshold=block.alignment_low_confidence_threshold,
        alignment_segment_motion_threshold_g=(
            block.alignment_segment_motion_threshold_g
        ),
        alignment_segment_min_flight_duration_seconds=(
            block.alignment_segment_min_flight_duration_seconds
        ),
        alignment_segment_max_internal_gap_seconds=(
            block.alignment_segment_max_internal_gap_seconds
        ),
    )


def _load_camera_calibration(config: Config) -> CameraCalibration:
    """Read the camera calibration JSON into a :class:`CameraCalibration` DTO.

    The replay binary uses the SAME calibration file the live binary
    loads; AZ-401 does not introduce a new on-disk format.
    """
    import numpy as np

    path = config.runtime.camera_calibration_path
    if not path:
        raise CompositionError(
            "config.runtime.camera_calibration_path is empty; replay mode "
            "requires a camera calibration JSON"
        )
    try:
        blob = json.loads(Path(path).read_text(encoding="utf-8"))
    except OSError as exc:
        raise CompositionError(
            f"failed to read camera calibration from {path!r}: {exc!r}"
        ) from exc
    except json.JSONDecodeError as exc:
        raise CompositionError(
            f"camera calibration {path!r} is not valid JSON: {exc!r}"
        ) from exc
    if not isinstance(blob, Mapping):
        raise CompositionError(
            f"camera calibration {path!r} must decode to a mapping; "
            f"got {type(blob).__name__}"
        )
    intrinsics = np.asarray(blob.get("intrinsics_3x3"), dtype=np.float64)
    if intrinsics.shape != (3, 3):
        raise CompositionError(
            f"camera calibration {path!r} 'intrinsics_3x3' must be 3x3; "
            f"got shape {intrinsics.shape}"
        )
    distortion = np.asarray(blob.get("distortion", []), dtype=np.float64)
    body_to_camera = np.asarray(
        blob.get("body_to_camera_se3", np.eye(4).tolist()),
        dtype=np.float64,
    )
    return CameraCalibration(
        camera_id=str(blob.get("camera_id", "replay-camera")),
        intrinsics_3x3=intrinsics,
        distortion=distortion,
        body_to_camera_se3=body_to_camera,
        acquisition_method=str(blob.get("acquisition_method", "operator")),
        metadata=dict(blob.get("metadata", {})),
    )


def _log_ready(config: Config, bundle: ReplayInputBundle) -> None:
    log = get_logger("runtime_root.replay_branch")
    log.info(
        f"{_LOG_KIND_READY}: pace={config.replay.pace} "
        f"resolved_offset_ms={bundle.resolved_time_offset_ms}",
        extra={
            "kind": _LOG_KIND_READY,
            "kv": {
                "video_path": config.replay.video_path,
                "tlog_path": config.replay.tlog_path,
                "output_path": config.replay.output_path,
                "pace": config.replay.pace,
                "resolved_offset_ms": bundle.resolved_time_offset_ms,
                "calib_path": config.runtime.camera_calibration_path,
                "auto_sync_used": bundle.auto_sync_result is not None,
            },
        },
    )