"""Unit tests for saltybot_uwb_logger.accuracy_stats (Issue #634). No ROS2 needed."""

import math
import sys
import os

import numpy as np
import pytest

sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
from saltybot_uwb_logger.accuracy_stats import compute_stats, RangeAccum


def _circle(n, r, cx=0.0, cy=0.0):
    a = np.linspace(0, 2 * math.pi, n, endpoint=False)
    return cx + r * np.cos(a), cy + r * np.sin(a)


class TestComputeStats:
    def test_zero_error(self):
        xs = np.full(5, 2.0); ys = np.full(5, 3.0)
        s = compute_stats(xs, ys, 2.0, 3.0)
        assert abs(s.cep50) < 1e-9
        assert abs(s.rmse) < 1e-9
        assert abs(s.max_error) < 1e-9

    def test_known_bias(self):
        rng = np.random.default_rng(0)
        xs = rng.normal(4.0, 0.01, 100); ys = rng.normal(2.0, 0.01, 100)
        s = compute_stats(xs, ys, 3.0, 2.0)
        assert abs(s.bias_x - 1.0) < 0.05
        assert abs(s.bias_y) < 0.05

    def test_cep50_below_cep95(self):
        rng = np.random.default_rng(1)
        xs = rng.normal(0, 0.1, 500); ys = rng.normal(0, 0.1, 500)
        s = compute_stats(xs, ys, 0.0, 0.0)
        assert s.cep50 < s.cep95

    def test_cep50_is_median(self):
        rng = np.random.default_rng(2)
        xs = rng.normal(0, 0.15, 1000); ys = rng.normal(0, 0.15, 1000)
        errors = np.sqrt(xs**2 + ys**2)
        s = compute_stats(xs, ys, 0.0, 0.0)
        assert abs(s.cep50 - float(np.median(errors))) < 1e-9

    def test_rmse(self):
        xs = np.array([0.1, -0.1, 0.2, -0.2]); ys = np.zeros(4)
        s = compute_stats(xs, ys, 0.0, 0.0)
        assert abs(s.rmse - math.sqrt(np.mean(xs**2))) < 1e-9

    def test_max_error(self):
        xs = np.array([0.0, 0.0, 1.0]); ys = np.zeros(3)
        s = compute_stats(xs, ys, 0.0, 0.0)
        assert abs(s.max_error - 1.0) < 1e-9

    def test_circle_symmetry(self):
        xs, ys = _circle(360, 0.12)
        s = compute_stats(xs, ys, 0.0, 0.0)
        assert abs(s.bias_x) < 1e-6
        assert abs(s.cep50 - 0.12) < 1e-6

    def test_std_2d_pythagorean(self):
        rng = np.random.default_rng(3)
        xs = rng.normal(0, 0.1, 200); ys = rng.normal(0, 0.2, 200)
        s = compute_stats(xs, ys, 0.0, 0.0)
        assert abs(s.std_2d - math.sqrt(s.std_x**2 + s.std_y**2)) < 1e-9

    def test_n_samples(self):
        xs = np.ones(42); ys = np.zeros(42)
        s = compute_stats(xs, ys, 0.0, 0.0)
        assert s.n_samples == 42

    def test_too_few_raises(self):
        with pytest.raises(ValueError):
            compute_stats(np.array([1.0]), np.array([1.0]), 0.0, 0.0)

    def test_length_mismatch_raises(self):
        with pytest.raises(ValueError):
            compute_stats(np.array([1.0, 2.0]), np.array([1.0]), 0.0, 0.0)


class TestRealistic:
    def test_10cm_cep50(self):
        rng = np.random.default_rng(42)
        xs = rng.normal(3.0, 0.07, 500); ys = rng.normal(2.0, 0.07, 500)
        s = compute_stats(xs, ys, 3.0, 2.0)
        assert 0.05 < s.cep50 < 0.20

    def test_biased_uwb(self):
        rng = np.random.default_rng(7)
        xs = rng.normal(3.15, 0.05, 300); ys = rng.normal(2.0, 0.05, 300)
        s = compute_stats(xs, ys, 3.0, 2.0)
        assert abs(s.bias_x - 0.15) < 0.02


class TestRangeAccum:
    def test_empty(self):
        a = RangeAccum(0)
        assert a.mean == 0.0 and a.std == 0.0 and a.count == 0

    def test_single_std_zero(self):
        a = RangeAccum(0); a.add(2.0)
        assert a.std == 0.0

    def test_mean_std(self):
        a = RangeAccum(0)
        for v in [1.0, 2.0, 3.0, 4.0, 5.0]:
            a.add(v)
        assert abs(a.mean - 3.0) < 1e-9
        assert abs(a.std - float(np.std([1,2,3,4,5], ddof=1))) < 1e-9
        assert a.count == 5


if __name__ == "__main__":
    pytest.main([__file__, "-v"])
