"""
test_ranging_math.py — Unit tests for UWB triangulation and Kalman filter.

No ROS2 / serial / GPU dependencies — runs with plain pytest.
"""

import math
import pytest

from saltybot_uwb.ranging_math import triangulate_2anchor, KalmanFilter2D, bearing_from_pos


# ── triangulate_2anchor ───────────────────────────────────────────────────────

class TestTriangulate2Anchor:

    def test_person_directly_ahead(self):
        """Person 2m ahead, centred: x≈2, y≈0."""
        sep = 0.25
        # A0 at y=+0.125, A1 at y=-0.125 → symmetric → y_t ≈ 0
        r0 = math.sqrt(2.0**2 + 0.125**2)
        r1 = r0  # symmetric
        x, y = triangulate_2anchor(r0, r1, sep)
        assert abs(x - 2.0) < 0.02
        assert abs(y) < 0.01

    def test_person_offset_left(self):
        """Person 2m ahead, 0.5m to the left: y_t ≈ +0.5."""
        sep = 0.25
        # A0 at y=+0.125, A1 at y=-0.125
        px, py = 2.0, 0.5
        r0 = math.sqrt(px**2 + (py - 0.125)**2)
        r1 = math.sqrt(px**2 + (py + 0.125)**2)
        x, y = triangulate_2anchor(r0, r1, sep)
        assert abs(x - px) < 0.05
        assert abs(y - py) < 0.05

    def test_person_offset_right(self):
        """Person 2m ahead, 0.5m to the right: y_t ≈ -0.5."""
        sep = 0.25
        px, py = 2.0, -0.5
        r0 = math.sqrt(px**2 + (py - 0.125)**2)
        r1 = math.sqrt(px**2 + (py + 0.125)**2)
        x, y = triangulate_2anchor(r0, r1, sep)
        assert abs(x - px) < 0.05
        assert abs(y - py) < 0.05

    def test_height_compensation_reduces_horizontal_range(self):
        """With height difference, horizontal range < slant range → x smaller."""
        sep = 0.25
        # No height: x ≈ 2.0
        x_flat, _ = triangulate_2anchor(2.0, 2.0, sep, anchor_z=0.0, tag_z=0.0)
        # Same slant range but 0.5m height difference
        slant = math.sqrt(2.0**2 + 0.5**2)
        x_tall, _ = triangulate_2anchor(slant, slant, sep, anchor_z=0.8, tag_z=0.3)
        # x_tall should be close to 2.0 (height-compensated back to horizontal)
        assert abs(x_tall - x_flat) < 0.05

    def test_returns_nonnegative_x(self):
        """x_t is always ≥ 0 (person can't be geometrically behind anchors)."""
        sep = 0.25
        # Pathological: extremely mismatched ranges → sqrt clamped to 0
        x, _ = triangulate_2anchor(0.1, 0.1, sep)
        assert x >= 0.0

    def test_invalid_sep_raises(self):
        with pytest.raises(ValueError):
            triangulate_2anchor(1.0, 1.0, sep=0.0)

    def test_negative_range_raises(self):
        with pytest.raises(ValueError):
            triangulate_2anchor(-1.0, 1.0, sep=0.25)

    def test_close_range(self):
        """Person very close (0.3m) — should not crash."""
        sep = 0.25
        px, py = 0.3, 0.0
        r0 = math.sqrt(px**2 + 0.125**2)
        r1 = r0
        x, y = triangulate_2anchor(r0, r1, sep)
        assert x >= 0.0

    def test_large_range(self):
        """Person 7m ahead — within DW3000 practical range."""
        sep = 0.25
        r0 = math.sqrt(7.0**2 + 0.125**2)
        r1 = r0
        x, y = triangulate_2anchor(r0, r1, sep)
        assert abs(x - 7.0) < 0.1
        assert abs(y) < 0.05


# ── KalmanFilter2D ────────────────────────────────────────────────────────────

class TestKalmanFilter2D:

    def test_initial_position_after_first_update(self):
        """First update initialises position to measurement."""
        kf = KalmanFilter2D()
        kf.predict()
        kf.update(3.0, 1.0)
        x, y = kf.position()
        assert abs(x - 3.0) < 0.01
        assert abs(y - 1.0) < 0.01

    def test_converges_to_stationary_target(self):
        """Repeated updates to same position → Kalman converges to it."""
        kf = KalmanFilter2D(process_noise=0.1, measurement_noise=0.3)
        for _ in range(50):
            kf.predict(dt=0.05)
            kf.update(2.0, 0.5)
        x, y = kf.position()
        assert abs(x - 2.0) < 0.1
        assert abs(y - 0.5) < 0.1

    def test_smooths_noisy_measurements(self):
        """Filter output should be smoother than raw measurements."""
        import random
        rng = random.Random(42)
        kf = KalmanFilter2D(process_noise=0.05, measurement_noise=0.5)
        raw_errors = []
        kf_errors  = []
        true_x, true_y = 2.0, 1.0
        for _ in range(30):
            noisy_x = true_x + rng.gauss(0, 0.3)
            noisy_y = true_y + rng.gauss(0, 0.3)
            kf.predict(dt=0.05)
            kf.update(noisy_x, noisy_y)
            kx, ky = kf.position()
            raw_errors.append((noisy_x - true_x)**2 + (noisy_y - true_y)**2)
            kf_errors.append((kx - true_x)**2 + (ky - true_y)**2)
        avg_raw = sum(raw_errors[-10:]) / 10
        avg_kf  = sum(kf_errors[-10:]) / 10
        assert avg_kf < avg_raw  # filter is smoother

    def test_reset_clears_state(self):
        """After reset(), position returns to ~0."""
        kf = KalmanFilter2D()
        for _ in range(10):
            kf.predict()
            kf.update(5.0, 3.0)
        kf.reset()
        kf.predict()
        kf.update(0.0, 0.0)
        x, y = kf.position()
        assert abs(x) < 0.1
        assert abs(y) < 0.1

    def test_velocity_nonzero_for_moving_target(self):
        """For a target moving at constant velocity, vx should be detected."""
        kf = KalmanFilter2D(process_noise=1.0, measurement_noise=0.1, dt=0.1)
        # Target moving at 0.5 m/s in X
        for i in range(40):
            kf.predict(dt=0.1)
            kf.update(0.5 * i * 0.1, 0.0)
        vx, vy = kf.velocity()
        assert abs(vx - 0.5) < 0.15

    def test_custom_dt(self):
        """predict() accepts custom dt without raising."""
        kf = KalmanFilter2D()
        kf.predict(dt=0.02)
        kf.update(1.0, 1.0)
        x, y = kf.position()
        assert abs(x - 1.0) < 0.01

    def test_no_nan_on_zero_measurement(self):
        """Zero measurement should not produce NaN."""
        kf = KalmanFilter2D()
        kf.predict()
        kf.update(0.0, 0.0)
        x, y = kf.position()
        assert not math.isnan(x)
        assert not math.isnan(y)


# ── bearing_from_pos ──────────────────────────────────────────────────────────

class TestBearingFromPos:

    def test_directly_ahead_zero_bearing(self):
        """Person directly ahead: x=2, y=0 → bearing ≈ 0."""
        b = bearing_from_pos(2.0, 0.0)
        assert abs(b) < 0.001

    def test_left_gives_positive_bearing(self):
        """Person to the left (y>0): bearing should be positive."""
        b = bearing_from_pos(1.0, 1.0)
        assert b > 0.0
        assert abs(b - math.pi / 4.0) < 0.001

    def test_right_gives_negative_bearing(self):
        """Person to the right (y<0): bearing should be negative."""
        b = bearing_from_pos(1.0, -1.0)
        assert b < 0.0
        assert abs(b + math.pi / 4.0) < 0.001

    def test_directly_left_ninety_degrees(self):
        """Person directly to the left: x=0, y=1 → bearing = π/2."""
        b = bearing_from_pos(0.0, 1.0)
        assert abs(b - math.pi / 2.0) < 0.001

    def test_directly_right_minus_ninety_degrees(self):
        """Person directly to the right: x=0, y=-1 → bearing = -π/2."""
        b = bearing_from_pos(0.0, -1.0)
        assert abs(b + math.pi / 2.0) < 0.001

    def test_range_pi_to_minus_pi(self):
        """Bearing is always in -π..+π."""
        for x in [-2.0, -0.1, 0.1, 2.0]:
            for y in [-2.0, -0.1, 0.1, 2.0]:
                b = bearing_from_pos(x, y)
                assert -math.pi <= b <= math.pi

    def test_no_nan_for_tiny_distance(self):
        """Very close target should not produce NaN."""
        b = bearing_from_pos(0.001, 0.001)
        assert not math.isnan(b)
