"""
Unit tests for saltybot_uwb_position.uwb_position_node (Issue #546).
No ROS2 or hardware required — tests the covariance math only.
"""

import math
import sys
import os

# Make the package importable without a ROS2 install
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))


# ── Covariance helper (extracted from node for unit testing) ──────────────────

def polar_to_cartesian_cov(bearing_rad, range_m, sigma_r, sigma_theta):
    """Compute 2×2 Cartesian covariance from polar uncertainty."""
    cos_b = math.cos(bearing_rad)
    sin_b = math.sin(bearing_rad)
    j00 =  cos_b;   j01 = -range_m * sin_b
    j10 =  sin_b;   j11 =  range_m * cos_b
    sr2 = sigma_r * sigma_r
    st2 = sigma_theta * sigma_theta
    cov_xx = j00 * j00 * sr2 + j01 * j01 * st2
    cov_xy = j00 * j10 * sr2 + j01 * j11 * st2
    cov_yy = j10 * j10 * sr2 + j11 * j11 * st2
    return cov_xx, cov_xy, cov_yy


# ── Tests ─────────────────────────────────────────────────────────────────────

class TestPolarToCartesianCovariance:

    def test_forward_bearing_zero(self):
        """At bearing=0 (directly ahead) covariance aligns with axes."""
        cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov(
            bearing_rad=0.0, range_m=5.0, sigma_r=0.10, sigma_theta=0.087
        )
        assert cov_xx > 0
        assert cov_yy > 0
        # At bearing=0: cov_xx = σ_r², cov_yy = (r·σ_θ)², cov_xy ≈ 0
        assert abs(cov_xx - 0.10 ** 2) < 1e-9
        assert abs(cov_xy) < 1e-9
        expected_yy = (5.0 * 0.087) ** 2
        assert abs(cov_yy - expected_yy) < 1e-6

    def test_sideways_bearing(self):
        """At bearing=90° covariance axes swap."""
        sigma_r = 0.10
        sigma_theta = 0.10
        r = 3.0
        cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov(
            bearing_rad=math.pi / 2, range_m=r,
            sigma_r=sigma_r, sigma_theta=sigma_theta
        )
        # At bearing=90°: cov_xx = (r·σ_θ)², cov_yy = σ_r²
        assert abs(cov_xx - (r * sigma_theta) ** 2) < 1e-9
        assert abs(cov_yy - sigma_r ** 2) < 1e-9

    def test_covariance_positive_definite(self):
        """Matrix must be positive semi-definite (det ≥ 0, diag > 0)."""
        for bearing in [0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0]:
            for r in [1.0, 5.0, 10.0]:
                cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov(
                    bearing, r, sigma_r=0.10, sigma_theta=0.087
                )
                assert cov_xx > 0
                assert cov_yy > 0
                det = cov_xx * cov_yy - cov_xy ** 2
                assert det >= -1e-12, f"Non-PSD at bearing={bearing}, r={r}: det={det}"

    def test_inflation_single_anchor(self):
        """Covariance doubles (variance ×4) when only one anchor active."""
        sigma_r = 0.10
        sigma_theta = 0.087
        bearing = 0.5
        r = 4.0
        cov_xx_full, _, _ = polar_to_cartesian_cov(bearing, r, sigma_r, sigma_theta)
        cov_xx_half, _, _ = polar_to_cartesian_cov(
            bearing, r,
            sigma_r * math.sqrt(4.0),
            sigma_theta * math.sqrt(4.0),
        )
        assert abs(cov_xx_half / cov_xx_full - 4.0) < 1e-9


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