from __future__ import annotations

import numpy as np

from tools.rscap_v2.g90_rtk import (
    parse_bestnava,
    parse_pvtslna,
    unicore_checksum_valid,
)


BESTNAVA = (
    '#BESTNAVA,48,GPS,FINE,2430,552524000,0,0,18,8;SOL_COMPUTED,NARROW_INT,'
    '30.46551864298,114.09274943336,29.6465,-15.1091,WGS84,0.0100,0.0091,'
    '0.0279,"547",1.000,176.000,38,31,31,31,0,01,03,f3,SOL_COMPUTED,'
    'DOPPLER_VELOCITY,0.000,0.000,1.6286,81.665533,-0.0015,0.0320,0.0167'
    '*3e7e4885'
)
PVTSLNA = (
    '#PVTSLNA,49,GPS,FINE,2430,552523000,0,0,18,42;NARROW_INT,29.6427,'
    '30.46551666558,114.09273316982,0.0288,0.0098,0.0094,1.000,PSRDIFF,'
    '29.2291,30.46551517805,114.09273145886,-15.1092,38,31,38,28,0.2276,'
    '1.5569,-0.0014,NARROW_INT,0.8328,350.8610,-1.9697,37,31,31,31,'
    '1.6227,1.3605,0.6398,1.0915,0.8843,5.0,28,10,25*00000000'
)


def test_bestnava_preserves_gnss_time_and_doppler_velocity() -> None:
    assert unicore_checksum_valid(BESTNAVA)
    row = parse_bestnava(BESTNAVA)
    assert row["gnss_week"] == 2430
    assert row["gnss_tow_ms"] == 552524000
    assert row["position_fixed"]
    assert row["doppler_velocity_valid"]
    assert np.isclose(row["horizontal_speed_m_s"], 1.6286)
    assert np.isclose(
        np.hypot(row["velocity_east_m_s"], row["velocity_north_m_s"]), 1.6286
    )


def test_pvtslna_preserves_quality_baseline_and_velocity() -> None:
    row = parse_pvtslna(PVTSLNA)
    assert row["position_fixed"]
    assert row["heading_type"] == "NARROW_INT"
    assert np.isclose(row["baseline_length_m"], 0.8328)
    assert np.isclose(row["heading_deg"], 350.8610)
    assert np.isclose(row["horizontal_speed_m_s"], np.hypot(0.2276, 1.5569))