"""
test_lidar_clustering.py — Unit tests for LIDAR clustering helpers (no ROS2 required).

Covers:
  scan_to_cartesian:
    - principal ray (angle=0) maps to (+range, 0)
    - 90° ray maps to (0, +range)
    - inf / NaN ranges are discarded
    - ranges outside [range_min, range_max] are discarded
    - all-invalid scan returns empty array
    - output shape and dtype

  cluster_points:
    - two well-separated groups → two clusters
    - one tight group → one cluster
    - consecutive points just within threshold stay together
    - consecutive points just over threshold split
    - fewer than min_points dropped
    - empty input returns empty list
    - clusters sorted nearest-first
    - cluster centroid is mean of points
    - cluster bbox correct
"""

import sys
import os
import math

import numpy as np
import pytest

sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))

from saltybot_bringup._lidar_clustering import (
    scan_to_cartesian,
    cluster_points,
    Cluster,
)


# ── scan_to_cartesian ─────────────────────────────────────────────────────────

class TestScanToCartesian:

    def test_forward_ray(self):
        """angle=0 → (range, 0)"""
        pts = scan_to_cartesian([2.0], angle_min=0.0, angle_increment=0.0)
        assert pts.shape == (1, 2)
        assert pts[0, 0] == pytest.approx(2.0, abs=1e-6)
        assert pts[0, 1] == pytest.approx(0.0, abs=1e-6)

    def test_left_ray(self):
        """angle=π/2 → (0, range)"""
        pts = scan_to_cartesian([3.0], angle_min=math.pi / 2, angle_increment=0.0)
        assert pts[0, 0] == pytest.approx(0.0,  abs=1e-6)
        assert pts[0, 1] == pytest.approx(3.0,  abs=1e-6)

    def test_inf_discarded(self):
        pts = scan_to_cartesian([float('inf'), 1.0], angle_min=0.0, angle_increment=0.1)
        assert pts.shape == (1, 2)   # only the valid reading

    def test_nan_discarded(self):
        pts = scan_to_cartesian([float('nan'), 1.0], angle_min=0.0, angle_increment=0.1)
        assert pts.shape == (1, 2)

    def test_below_range_min_discarded(self):
        pts = scan_to_cartesian([0.01, 1.0], angle_min=0.0,
                                angle_increment=0.1, range_min=0.05)
        assert pts.shape == (1, 2)

    def test_above_range_max_discarded(self):
        pts = scan_to_cartesian([1.0, 50.0], angle_min=0.0,
                                angle_increment=0.1, range_max=12.0)
        assert pts.shape == (1, 2)

    def test_all_invalid_returns_empty(self):
        pts = scan_to_cartesian([float('inf')] * 10, angle_min=0.0, angle_increment=0.1)
        assert pts.shape[0] == 0

    def test_output_dtype_float64(self):
        pts = scan_to_cartesian([1.0, 2.0], angle_min=0.0, angle_increment=0.1)
        assert pts.dtype == np.float64

    def test_output_shape_n_by_2(self):
        pts = scan_to_cartesian([1.0, 2.0, 3.0], angle_min=0.0, angle_increment=0.1)
        assert pts.shape == (3, 2)

    def test_negative_angle(self):
        """-π/2 → (0, -range)"""
        pts = scan_to_cartesian([2.5], angle_min=-math.pi / 2, angle_increment=0.0)
        assert pts[0, 0] == pytest.approx(0.0,  abs=1e-6)
        assert pts[0, 1] == pytest.approx(-2.5, abs=1e-6)


# ── cluster_points ────────────────────────────────────────────────────────────

def _pts(*xy_pairs) -> np.ndarray:
    """Convenience: _pts(0,0, 1,0, 2,0) → (3,2) array."""
    return np.array(xy_pairs, dtype=np.float64).reshape(-1, 2)


class TestClusterPoints:

    def test_empty_input_returns_empty_list(self):
        result = cluster_points(np.empty((0, 2)))
        assert result == []

    def test_two_separated_groups(self):
        """Points at x≈0 and x≈10 should form two clusters."""
        group_a = _pts(0.0, 0.0,  0.1, 0.0,  0.2, 0.0)  # 3 pts near origin
        group_b = _pts(10.0, 0.0, 10.1, 0.0, 10.2, 0.0) # 3 pts far away
        pts = np.vstack([group_a, group_b])
        clusters = cluster_points(pts, distance_threshold_m=0.5, min_points=2)
        assert len(clusters) == 2

    def test_one_tight_group(self):
        pts = _pts(0.0, 0.0,  0.05, 0.0,  0.10, 0.0,  0.15, 0.0)
        clusters = cluster_points(pts, distance_threshold_m=0.20, min_points=3)
        assert len(clusters) == 1

    def test_gap_just_within_threshold_stays_together(self):
        # gap = exactly threshold → same cluster
        pts = _pts(0.0, 0.0,  0.20, 0.0,  0.40, 0.0)
        clusters = cluster_points(pts, distance_threshold_m=0.20, min_points=2)
        assert len(clusters) == 1

    def test_gap_just_over_threshold_splits(self):
        # gap = threshold + epsilon → different clusters
        pts = _pts(0.0, 0.0,  0.21, 0.0,  0.42, 0.0)
        clusters = cluster_points(pts, distance_threshold_m=0.20, min_points=1)
        assert len(clusters) == 3   # every point is its own cluster

    def test_fewer_than_min_points_dropped(self):
        pts = _pts(0.0, 0.0,  0.05, 0.0)   # only 2 pts
        clusters = cluster_points(pts, distance_threshold_m=0.20, min_points=3)
        assert clusters == []

    def test_sorted_nearest_first(self):
        far_pts  = _pts(8.0, 0.0,  8.1, 0.0,  8.2, 0.0)
        near_pts = _pts(1.0, 0.0,  1.1, 0.0,  1.2, 0.0)
        pts = np.vstack([far_pts, near_pts])  # far group listed first
        clusters = cluster_points(pts, distance_threshold_m=0.5, min_points=2)
        assert len(clusters) == 2
        dist0 = np.linalg.norm(clusters[0].centroid)
        dist1 = np.linalg.norm(clusters[1].centroid)
        assert dist0 < dist1

    def test_centroid_is_mean(self):
        pts = _pts(0.0, 0.0,  2.0, 0.0)   # gap=2 > threshold, but with min_pts=1
        clusters = cluster_points(pts, distance_threshold_m=0.5, min_points=1)
        # two separate clusters; first cluster centroid = (0,0)
        c0 = [c for c in clusters if np.linalg.norm(c.centroid) < 0.5][0]
        np.testing.assert_allclose(c0.centroid, [0.0, 0.0])

    def test_bbox_min_max_correct(self):
        pts = _pts(1.0, 2.0,  3.0, 4.0,  2.0, 3.0)
        clusters = cluster_points(pts, distance_threshold_m=5.0, min_points=2)
        assert len(clusters) == 1
        c = clusters[0]
        np.testing.assert_allclose(c.bbox_min, [1.0, 2.0])
        np.testing.assert_allclose(c.bbox_max, [3.0, 4.0])

    def test_width_depth_correct(self):
        pts = _pts(0.0, 0.0,  4.0, 0.0,  0.0, 2.0,  4.0, 2.0)
        clusters = cluster_points(pts, distance_threshold_m=10.0, min_points=4)
        assert len(clusters) == 1
        c = clusters[0]
        assert c.width_m == pytest.approx(4.0, abs=1e-9)
        assert c.depth_m == pytest.approx(2.0, abs=1e-9)

    def test_single_point_cluster_with_min_pts_1(self):
        pts = _pts(5.0, 5.0)
        clusters = cluster_points(pts, distance_threshold_m=0.5, min_points=1)
        assert len(clusters) == 1
        np.testing.assert_allclose(clusters[0].centroid, [5.0, 5.0])

    def test_cluster_fields_are_namedtuple(self):
        pts = _pts(0.0, 0.0,  0.1, 0.0,  0.2, 0.0)
        clusters = cluster_points(pts, distance_threshold_m=0.5, min_points=3)
        assert len(clusters) == 1
        c = clusters[0]
        assert hasattr(c, 'points')
        assert hasattr(c, 'centroid')
        assert hasattr(c, 'bbox_min')
        assert hasattr(c, 'bbox_max')
        assert hasattr(c, 'width_m')
        assert hasattr(c, 'depth_m')


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