gps-denied-onboard/src/gps_denied_onboard/replay_input/tlog_ground_truth.py

"""Direct binary-tlog GPS-truth extractor (AZ-697 / E-DEMO-REPLAY).

AUDIT-ONLY (AZ-895): the production replay pipeline now consumes
ground truth through :class:`CsvGroundTruth` (AZ-894) driven by the
operator's IMU+GPS CSV. This helper is retained for one-off audits and
the AZ-699 / AZ-701 validation paths that still operate against legacy
``.tlog`` archives; it is not part of the main replay composition root.

Streams ``GLOBAL_POSITION_INT`` (preferred) or ``GPS_RAW_INT`` (fallback)
from an ArduPilot binary tlog into a typed :class:`TlogGroundTruth` DTO.

Design notes:

* Lazy ``pymavlink.mavutil`` import — missing dependency raises
  :class:`ReplayInputAdapterError` rather than crashing the import.
* Optional ``source_factory`` injection point so unit tests can swap in
  a synthetic source (mirrors the AZ-399 pattern).
* Placed under ``replay_input/`` because the GPS extraction is
  intrinsically tied to the tlog input pipeline; the comparison kernels
  themselves live in :mod:`helpers.gps_compare`.
"""

from __future__ import annotations

import logging
import math
from collections.abc import Callable
from dataclasses import dataclass
from pathlib import Path
from typing import Any

from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError

__all__ = [
    "TlogGpsFix",
    "TlogGroundTruth",
    "load_tlog_ground_truth",
]


_LOGGER = logging.getLogger("gps_denied_onboard.replay_input.tlog_ground_truth")

# MAVLink GLOBAL_POSITION_INT / GPS_RAW_INT integer encodings.
# lat/lon are deg × 1e7; alt is mm above MSL; vx/vy/vz are cm/s;
# hdg/cog are cdeg (0..36000).
_LATLON_SCALE: float = 1.0e-7
_MM_PER_M: float = 1000.0
_CM_PER_M_S: float = 100.0
_CDEG_PER_DEG: float = 100.0

# Source-label constants returned in :attr:`TlogGroundTruth.source`.
_SOURCE_GLOBAL_POSITION_INT: str = "GLOBAL_POSITION_INT"
_SOURCE_GPS_RAW_INT: str = "GPS_RAW_INT"
_SOURCE_NONE: str = ""


@dataclass(frozen=True, slots=True)
class TlogGpsFix:
    """One time-aligned GPS-truth row extracted from a tlog.

    Attributes:
        ts_ns: Absolute timestamp (ns) sourced from pymavlink's
            ``_timestamp`` field (Unix time × 1e9). Comparable to the
            airborne runtime clock during replay.
        lat_deg, lon_deg: Latitude / longitude in degrees (WGS84).
        alt_m: Altitude above MSL in metres (MAVLink ``alt`` field).
        hdg_deg: Aircraft heading in degrees [0, 360). When sourced
            from ``GPS_RAW_INT``, this is course over ground (cog),
            not the IMU-derived heading.
        vx_m_s, vy_m_s, vz_m_s: North / east / down velocity in m/s.
            For ``GPS_RAW_INT``-sourced rows, ``vx`` / ``vy`` are
            derived from the ground velocity + course over ground;
            ``vz`` is 0.0 because the message does not expose vertical
            velocity.
    """

    ts_ns: int
    lat_deg: float
    lon_deg: float
    alt_m: float
    hdg_deg: float
    vx_m_s: float
    vy_m_s: float
    vz_m_s: float


@dataclass(frozen=True, slots=True)
class TlogGroundTruth:
    """Ground-truth GPS series extracted from a tlog.

    Attributes:
        records: Time-ordered fixes. Empty when no GPS messages were
            present in the tlog.
        source: MAVLink message type the records were sourced from —
            ``"GLOBAL_POSITION_INT"`` (preferred), ``"GPS_RAW_INT"``
            (fallback), or ``""`` (no GPS messages found).
    """

    records: tuple[TlogGpsFix, ...]
    source: str


def load_tlog_ground_truth(
    tlog_path: Path,
    *,
    source_factory: Callable[[str], Any] | None = None,
) -> TlogGroundTruth:
    """Stream GPS-truth records from a tlog.

    Args:
        tlog_path: Path to the binary tlog. Existence is checked at
            entry.
        source_factory: Test-only injection — when provided, replaces
            the pymavlink open call with the factory's return value.
            The factory must yield an object with ``recv_match`` and
            ``close`` semantics matching pymavlink's
            ``mavutil.mavlink_connection``.

    Returns:
        A :class:`TlogGroundTruth` whose ``records`` contain
        ``GLOBAL_POSITION_INT`` rows when any are present; otherwise
        ``GPS_RAW_INT`` rows; otherwise an empty tuple (with a WARN log).

    Raises:
        ReplayInputAdapterError: When the tlog file is missing or
            pymavlink cannot be imported.
    """
    if not tlog_path.is_file():
        raise ReplayInputAdapterError(f"tlog file not found: {tlog_path}")
    source = _open_tlog(tlog_path, source_factory=source_factory)
    gpi_records: list[TlogGpsFix] = []
    raw_records: list[TlogGpsFix] = []
    try:
        while True:
            try:
                msg = source.recv_match(
                    type=[_SOURCE_GLOBAL_POSITION_INT, _SOURCE_GPS_RAW_INT],
                    blocking=False,
                )
            except Exception as exc:  # pragma: no cover — defensive.
                raise ReplayInputAdapterError(
                    f"tlog scan failed on {tlog_path}: {exc!r}"
                ) from exc
            if msg is None:
                break
            msg_type = _safe_msg_type(msg)
            if not msg_type:
                continue
            ts_ns = _msg_timestamp_ns(msg)
            if msg_type == _SOURCE_GLOBAL_POSITION_INT:
                gpi_records.append(_from_global_position_int(msg, ts_ns))
            elif msg_type == _SOURCE_GPS_RAW_INT:
                raw_records.append(_from_gps_raw_int(msg, ts_ns))
    finally:
        if hasattr(source, "close"):
            try:
                source.close()
            except Exception:  # pragma: no cover — defensive.
                pass
    if gpi_records:
        return TlogGroundTruth(
            records=tuple(gpi_records),
            source=_SOURCE_GLOBAL_POSITION_INT,
        )
    if raw_records:
        return TlogGroundTruth(
            records=tuple(raw_records),
            source=_SOURCE_GPS_RAW_INT,
        )
    _LOGGER.warning(
        "tlog %s contains no GLOBAL_POSITION_INT or GPS_RAW_INT messages",
        tlog_path,
    )
    return TlogGroundTruth(records=(), source=_SOURCE_NONE)


def _from_global_position_int(msg: Any, ts_ns: int) -> TlogGpsFix:
    return TlogGpsFix(
        ts_ns=ts_ns,
        lat_deg=int(getattr(msg, "lat", 0)) * _LATLON_SCALE,
        lon_deg=int(getattr(msg, "lon", 0)) * _LATLON_SCALE,
        alt_m=int(getattr(msg, "alt", 0)) / _MM_PER_M,
        hdg_deg=int(getattr(msg, "hdg", 0)) / _CDEG_PER_DEG,
        vx_m_s=int(getattr(msg, "vx", 0)) / _CM_PER_M_S,
        vy_m_s=int(getattr(msg, "vy", 0)) / _CM_PER_M_S,
        vz_m_s=int(getattr(msg, "vz", 0)) / _CM_PER_M_S,
    )


def _from_gps_raw_int(msg: Any, ts_ns: int) -> TlogGpsFix:
    # GPS_RAW_INT exposes ground velocity + course over ground rather
    # than NED components. Derive horizontal components; leave vertical
    # at 0.0 because the message lacks a vz field. Callers that need
    # vertical velocity from GPS_RAW_INT must source it elsewhere
    # (e.g., VFR_HUD.climb).
    vel_cm_s = int(getattr(msg, "vel", 0))
    cog_cdeg = int(getattr(msg, "cog", 0))
    cog_rad = math.radians(cog_cdeg / _CDEG_PER_DEG)
    vel_m_s = vel_cm_s / _CM_PER_M_S
    vx_m_s = vel_m_s * math.cos(cog_rad)
    vy_m_s = vel_m_s * math.sin(cog_rad)
    return TlogGpsFix(
        ts_ns=ts_ns,
        lat_deg=int(getattr(msg, "lat", 0)) * _LATLON_SCALE,
        lon_deg=int(getattr(msg, "lon", 0)) * _LATLON_SCALE,
        alt_m=int(getattr(msg, "alt", 0)) / _MM_PER_M,
        hdg_deg=cog_cdeg / _CDEG_PER_DEG,
        vx_m_s=vx_m_s,
        vy_m_s=vy_m_s,
        vz_m_s=0.0,
    )


def _open_tlog(
    tlog_path: Path,
    *,
    source_factory: Callable[[str], Any] | None,
) -> Any:
    if source_factory is not None:
        return source_factory(str(tlog_path))
    try:
        from pymavlink import mavutil
    except ImportError as exc:
        raise ReplayInputAdapterError(
            "pymavlink is required for replay tlog ground-truth "
            "extraction but is not importable in this binary"
        ) from exc
    return mavutil.mavlink_connection(
        str(tlog_path),
        dialect="ardupilotmega",
        mavlink_version="2.0",
    )


def _safe_msg_type(msg: Any) -> str:
    try:
        if hasattr(msg, "get_type"):
            return str(msg.get_type())
    except Exception:
        return ""
    return type(msg).__name__


def _msg_timestamp_ns(msg: Any) -> int:
    raw = getattr(msg, "_timestamp", None)
    if raw is None:
        raise ReplayInputAdapterError(
            "tlog message missing _timestamp attribute; pymavlink "
            "mavlogfile should populate it on every recv_match() return"
        )
    return int(float(raw) * 1_000_000_000)