import math

import pytest

from hitl import geo


GGA_SAMPLE = "$GNGGA,080112.000,3949.8105069,N,11616.6876082,E,4,44,0.42,48.502,M,-6.684,M,1.0,0409*73"


def test_parse_origin_from_gga():
    origin = geo.parse_origin(GGA_SAMPLE)
    assert origin.latitude_deg == pytest.approx(39.830175115)
    assert origin.longitude_deg == pytest.approx(116.278126803)
    assert origin.altitude_m == pytest.approx(48.502)


def test_parse_origin_short_format():
    origin = geo.parse_origin("3949.8105,N,11616.6876,E,50.0")
    assert origin.latitude_deg == pytest.approx(39.830175)
    assert origin.longitude_deg == pytest.approx(116.278126)
    assert origin.altitude_m == pytest.approx(50.0)


def test_enu_roundtrip_accuracy():
    origin = geo.parse_origin(GGA_SAMPLE)
    east, north, up = 5.0, -3.0, 1.2
    lat, lon, alt = geo.enu_to_lla(east, north, up, origin)
    x, y, z = geo.geo_to_ecef(lat, lon, alt)
    dx = x - origin.ecef[0]
    dy = y - origin.ecef[1]
    dz = z - origin.ecef[2]
    east_rt, north_rt, up_rt = geo.ecef_to_enu(dx, dy, dz, origin.latitude_rad, origin.longitude_rad)

    assert east_rt == pytest.approx(east, abs=0.01)
    assert north_rt == pytest.approx(north, abs=0.01)
    assert up_rt == pytest.approx(up, abs=0.01)


def test_heading_math_to_nav():
    assert geo.heading_math_to_nav(math.radians(0.0)) == pytest.approx(90.0)
    assert geo.heading_math_to_nav(math.radians(90.0)) == pytest.approx(0.0)
    assert geo.heading_math_to_nav(math.radians(-90.0)) == pytest.approx(180.0)