"""Pure geofence geometry tests (no GPS hardware, no threads)."""

from __future__ import annotations

import pytest

from config import GeoFenceConfig
from gowelcome.geo.geofence import GeoFence, bearing_deg, haversine_m, normalize_deg

_M_PER_DEG = 111_194.0  # ~metres per degree of latitude


def test_haversine_known_distances():
    assert haversine_m(0, 0, 0, 0) == pytest.approx(0.0, abs=1e-6)
    assert haversine_m(0, 0, 1, 0) == pytest.approx(_M_PER_DEG, rel=0.01)
    assert haversine_m(0, 0, 0, 1) == pytest.approx(_M_PER_DEG, rel=0.01)


def test_bearing_cardinals():
    assert bearing_deg(0, 0, 1, 0) == pytest.approx(0.0, abs=0.5)     # north
    assert bearing_deg(0, 0, 0, 1) == pytest.approx(90.0, abs=0.5)    # east
    assert bearing_deg(0, 0, -1, 0) == pytest.approx(180.0, abs=0.5)  # south
    assert bearing_deg(0, 0, 0, -1) == pytest.approx(270.0, abs=0.5)  # west


def test_normalize_deg():
    assert normalize_deg(270) == pytest.approx(-90.0)
    assert normalize_deg(-270) == pytest.approx(90.0)
    assert normalize_deg(0) == pytest.approx(0.0)
    assert normalize_deg(190) == pytest.approx(-170.0)


def _fence(**over) -> GeoFence:
    cfg = GeoFenceConfig(radius_m=15.0, margin_m=3.0, release_m=2.0)
    for k, v in over.items():
        setattr(cfg, k, v)
    return GeoFence(cfg)


def test_center_capture_onstart():
    f = _fence(center_mode="onstart")
    assert not f.has_center
    f.maybe_capture_center(10.0, 20.0)
    assert f.has_center and f.center == (10.0, 20.0)
    f.maybe_capture_center(11.0, 21.0)   # ignored once set
    assert f.center == (10.0, 20.0)


def test_center_fixed_mode():
    f = _fence(center_mode="fixed", center_lat=1.0, center_lon=2.0)
    assert f.has_center and f.center == (1.0, 2.0)


def test_breached_and_cleared_hysteresis():
    f = _fence(center_mode="onstart")
    f.maybe_capture_center(0.0, 0.0)
    # radius 15, margin 3 -> breached at >=12 m; cleared at <=10 m.
    near = 9.0 / _M_PER_DEG     # ~9 m north
    edge = 13.0 / _M_PER_DEG    # ~13 m north
    assert not f.breached(near, 0.0)
    assert f.cleared(near, 0.0)
    assert f.breached(edge, 0.0)
    assert not f.cleared(edge, 0.0)


def test_homing_yaw_sign_steers_toward_center():
    f = _fence(center_mode="onstart")
    f.maybe_capture_center(0.0, 0.0)
    east = 0.001          # ~111 m east of centre -> centre is to the WEST
    west = -0.001         # west of centre -> centre is to the EAST
    # Heading north (course 0). East of centre -> turn LEFT (vyaw>0) to face west.
    assert f.homing_yaw(0.0, east, course_deg=0.0) > 0.0
    # West of centre -> turn RIGHT (vyaw<0) to face east.
    assert f.homing_yaw(0.0, west, course_deg=0.0) < 0.0


def test_homing_yaw_no_course_gentle_turn():
    f = _fence(center_mode="onstart")
    f.maybe_capture_center(0.0, 0.0)
    v = f.homing_yaw(0.0, 0.001, course_deg=None)
    assert 0.0 < v <= f.cfg.max_homing_yaw


def test_homing_yaw_clamped():
    f = _fence(center_mode="onstart", homing_kp=100.0, max_homing_yaw=0.9)
    f.maybe_capture_center(0.0, 0.0)
    v = f.homing_yaw(0.0, 0.001, course_deg=90.0)
    assert abs(v) <= 0.9 + 1e-9


def test_no_center_returns_none():
    f = _fence(center_mode="onstart")
    assert f.distance_from_center(0, 0) is None
    assert f.bearing_to_center(0, 0) is None
    assert not f.breached(0, 0)
    assert f.cleared(0, 0)   # nothing to enforce -> treated as clear