"""GPS source tests: NMEA parsing + the simulated MockGpsSource (no hardware)."""

from __future__ import annotations

import time

import pytest

from config import default_config
from gowelcome.geo.gps import (
    MockGpsSource,
    _nmea_deg,
    _parse_gga,
    _parse_rmc,
    build_gps_source,
)


def test_nmea_deg_conversion():
    # 48 deg 07.038' N -> 48.1173
    assert _nmea_deg("4807.038", "N") == pytest.approx(48.1173, abs=1e-4)
    assert _nmea_deg("4807.038", "S") == pytest.approx(-48.1173, abs=1e-4)
    assert _nmea_deg("01131.000", "E") == pytest.approx(11.5167, abs=1e-3)
    assert _nmea_deg("", "N") is None


def test_parse_gga():
    s = "$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47"
    lat, lon, sats, hdop = _parse_gga(s)
    assert lat == pytest.approx(48.1173, abs=1e-4)
    assert lon == pytest.approx(11.5167, abs=1e-3)
    assert sats == 8
    assert hdop == pytest.approx(0.9)
    # fix quality 0 -> no fix
    assert _parse_gga("$GPGGA,123519,4807.038,N,01131.000,E,0,00,,,M,,M,,*47") is None


def test_parse_rmc():
    s = "$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A"
    course, speed = _parse_rmc(s)
    assert course == pytest.approx(84.4)
    assert speed == pytest.approx(22.4 * 0.514444, rel=0.01)  # knots -> m/s
    # status V (void) -> no data
    assert _parse_rmc("$GPRMC,123519,V,,,,,,,230394,,*00") == (None, None)


def test_build_gps_source_mock_and_disabled():
    cfg = default_config()
    cfg.gps.enabled = True          # GPS is opt-in now (default vision-only)
    cfg.gps.source = "mock"
    assert isinstance(build_gps_source(cfg), MockGpsSource)
    cfg.gps.enabled = False
    assert build_gps_source(cfg) is None


def test_mock_gps_integrates_forward_motion():
    cfg = default_config()
    src = MockGpsSource(cfg.gps, rate_hz=50.0)
    src.start()
    try:
        # Drive forward (north, heading 0) for a moment.
        src.on_command(0.5, 0.0, 0.0)
        time.sleep(0.4)
        fix = src.latest()
        assert fix is not None
        # Moved north of the start latitude; longitude ~unchanged.
        assert fix.lat > cfg.gps.mock_start_lat
        assert fix.lon == pytest.approx(cfg.gps.mock_start_lon, abs=1e-6)
        assert fix.course_deg == pytest.approx(0.0, abs=1.0)  # heading north
        assert fix.num_sats and fix.num_sats >= 4
    finally:
        src.stop()