gps-denied-onboard/tests/test_gps_input_encoding.py

"""Unit tests for GPS_INPUT (MAVLink #232) field encoding.

Pure unit tests — no SITL dependency. Verifies that _eskf_to_gps_input
produces field values per MAVLink spec:
  - lat/lon: int × 1e7 (degE7)
  - alt: float metres MSL
  - vn/ve/vd: float m/s (ArduPilot pymavlink encodes to int cm/s on wire)
  - satellites_visible: 10 synthetic (prevents ArduPilot satellite-count failsafe)
  - fix_type: 0=no fix, 2=2D, 3=3D per ConfidenceTier mapping

Ref: MAVProxy/modules/mavproxy_GPSInput.py, solution.md §GPS_INPUT Population
Ref: docs/superpowers/specs/2026-04-18-oss-stack-tech-audit-design.md §6
"""

import time

import numpy as np
import pytest

from gps_denied.core.mavlink import _confidence_to_fix_type, _eskf_to_gps_input
from gps_denied.schemas import GPSPoint
from gps_denied.schemas.eskf import ConfidenceTier, ESKFState


_ORIGIN = GPSPoint(lat=49.0, lon=32.0)


def _make_state(
    pos=(0.0, 0.0, 0.0),
    vel=(0.0, 0.0, 0.0),
    confidence: ConfidenceTier = ConfidenceTier.HIGH,
    cov_scale: float = 1.0,
) -> ESKFState:
    return ESKFState(
        position=np.array(pos, dtype=float),
        velocity=np.array(vel, dtype=float),
        quaternion=np.array([1.0, 0.0, 0.0, 0.0]),
        accel_bias=np.zeros(3),
        gyro_bias=np.zeros(3),
        covariance=np.eye(15) * cov_scale,
        timestamp=time.time(),
        confidence=confidence,
    )


def test_gps_input_lat_lon_encoded_as_deg_e7():
    """lat/lon must be int × 1e7 (degE7) per MAVLink #232 spec."""
    state = _make_state(pos=(0.0, 0.0, 0.0))
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)

    # At origin with zero position, lat/lon should match origin exactly.
    assert msg.lat == int(49.0 * 1e7)
    assert msg.lon == int(32.0 * 1e7)
    assert isinstance(msg.lat, int)
    assert isinstance(msg.lon, int)


def test_gps_input_lat_lon_offset_from_enu_position():
    """ENU position is correctly converted to lat/lon offset."""
    # 111.319.5 m northward = ~0.001 deg latitude
    state = _make_state(pos=(0.0, 111_319.5, 0.0))
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)

    # 1 deg north
    assert msg.lat == pytest.approx(int((49.0 + 1.0) * 1e7), abs=100)
    # lon unchanged
    assert msg.lon == int(32.0 * 1e7)


def test_gps_input_alt_in_meters_msl():
    """alt field is float metres MSL (not mm as in some MAVLink variants)."""
    state = _make_state()
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.5)
    assert msg.alt == pytest.approx(600.5)
    assert isinstance(msg.alt, float)


def test_gps_input_velocity_enu_to_ned_conversion():
    """ENU velocity must be converted to NED before populating vn/ve/vd."""
    # ENU velocity: East=+5, North=+10, Up=+2
    state = _make_state(vel=(5.0, 10.0, 2.0))
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)

    # NED: vn=North=+10, ve=East=+5, vd=-Up=-2
    assert msg.vn == pytest.approx(10.0, abs=0.01)
    assert msg.ve == pytest.approx(5.0, abs=0.01)
    assert msg.vd == pytest.approx(-2.0, abs=0.01)


def test_gps_input_satellites_visible_synthetic_10():
    """satellites_visible=10 prevents ArduPilot satellite-count failsafe."""
    state = _make_state()
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
    assert msg.satellites_visible == 10


def test_gps_input_fix_type_high_confidence_is_3d():
    """HIGH confidence (satellite-anchored) → fix_type=3 (3D fix)."""
    state = _make_state(confidence=ConfidenceTier.HIGH)
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
    assert msg.fix_type == 3


def test_gps_input_fix_type_medium_confidence_is_3d():
    """MEDIUM confidence (VO tracking valid) → fix_type=3 per solution.md."""
    state = _make_state(confidence=ConfidenceTier.MEDIUM)
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
    assert msg.fix_type == 3


def test_gps_input_fix_type_low_confidence_no_fix():
    """LOW confidence (IMU-only) → fix_type=0 (no fix, honest signal)."""
    state = _make_state(confidence=ConfidenceTier.LOW)
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
    assert msg.fix_type == 0


def test_gps_input_fix_type_failed_no_fix():
    """FAILED confidence → fix_type=0 (no fix)."""
    state = _make_state(confidence=ConfidenceTier.FAILED)
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
    assert msg.fix_type == 0


def test_gps_input_accuracy_from_covariance():
    """horiz_accuracy derived from covariance[0:2, 0:2]; vert from [2,2]."""
    state = _make_state(cov_scale=4.0)  # σ_h² = 4+4=8, σ_v² = 4
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)

    expected_h = (8.0) ** 0.5       # sqrt(var_x + var_y)
    expected_v = (4.0) ** 0.5
    assert msg.horiz_accuracy == pytest.approx(expected_h, abs=0.01)
    assert msg.vert_accuracy == pytest.approx(expected_v, abs=0.01)


def test_gps_input_hdop_vdop_clamped_to_min():
    """hdop/vdop have a floor of 0.1 to prevent ArduPilot divide-by-zero."""
    state = _make_state(cov_scale=0.0)  # σ = 0 → hdop would be 0 without clamp
    msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
    assert msg.hdop >= 0.1
    assert msg.vdop >= 0.1


# ---------------------------------------------------------------------------
# _confidence_to_fix_type direct mapping
# ---------------------------------------------------------------------------


def test_confidence_tier_mapping_complete():
    """All four ConfidenceTier values map to valid fix_type values."""
    assert _confidence_to_fix_type(ConfidenceTier.HIGH) == 3
    assert _confidence_to_fix_type(ConfidenceTier.MEDIUM) == 3
    assert _confidence_to_fix_type(ConfidenceTier.LOW) == 0
    assert _confidence_to_fix_type(ConfidenceTier.FAILED) == 0