"""Unit tests for obstacle_clusterer.py — 2D grid BFS clustering."""

import numpy as np
import pytest

from saltybot_obstacle_detect.obstacle_clusterer import cluster_obstacles, ObstacleCluster


def _make_cluster_pts(cx, cz, n=30, spread=0.15, rng=None):
    """Generate points around a centroid in camera frame (x=lateral, z=forward)."""
    if rng is None:
        rng = np.random.default_rng(0)
    x = rng.uniform(cx - spread, cx + spread, n)
    y = rng.uniform(-0.3, 0.0, n)   # above ground (height ~0.3m)
    z = rng.uniform(cz - spread, cz + spread, n)
    return np.stack([x, y, z], axis=1)


def _make_heights(pts, base_h=0.3, noise=0.05, rng=None):
    if rng is None:
        rng = np.random.default_rng(1)
    return np.full(len(pts), base_h) + rng.normal(0, noise, len(pts))


# ── Basic clustering ──────────────────────────────────────────────────────────

def test_single_cluster_detected():
    pts     = _make_cluster_pts(cx=0.0, cz=2.0, n=40)
    heights = _make_heights(pts)
    clusters = cluster_obstacles(pts, heights, cell_m=0.30, min_pts=5)
    assert len(clusters) == 1


def test_two_clusters_separated():
    rng  = np.random.default_rng(42)
    pts1 = _make_cluster_pts(cx=-1.0, cz=2.0, n=40, rng=rng)
    pts2 = _make_cluster_pts(cx=+1.5, cz=3.5, n=40, rng=rng)
    pts  = np.vstack([pts1, pts2])
    h    = _make_heights(pts, rng=rng)
    clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5)
    assert len(clusters) == 2


def test_clusters_sorted_by_distance():
    rng  = np.random.default_rng(7)
    pts_near = _make_cluster_pts(cx=0.0, cz=1.5, n=40, rng=rng)
    pts_far  = _make_cluster_pts(cx=0.0, cz=4.0, n=40, rng=rng)
    pts = np.vstack([pts_far, pts_near])   # far first
    h   = _make_heights(pts, rng=rng)
    clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5)
    assert len(clusters) == 2
    assert clusters[0].distance_m < clusters[1].distance_m


def test_empty_input():
    clusters = cluster_obstacles(
        np.zeros((0, 3), np.float64),
        np.zeros(0, np.float64),
    )
    assert clusters == []


def test_min_pts_filters_small_cluster():
    rng      = np.random.default_rng(9)
    pts_big  = _make_cluster_pts(cx=0.0, cz=2.0, n=50, rng=rng)
    pts_tiny = _make_cluster_pts(cx=2.0, cz=2.0, n=2, rng=rng)
    pts      = np.vstack([pts_big, pts_tiny])
    h        = _make_heights(pts, rng=rng)
    clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5)
    # Only the big cluster should pass
    assert len(clusters) == 1
    assert clusters[0].n_pts >= 40


def test_centroid_near_true_center():
    rng      = np.random.default_rng(5)
    true_cx, true_cz = 0.0, 2.5
    pts      = _make_cluster_pts(cx=true_cx, cz=true_cz, n=100, spread=0.1, rng=rng)
    h        = _make_heights(pts, rng=rng)
    clusters = cluster_obstacles(pts, h, cell_m=0.20, min_pts=5)
    assert len(clusters) >= 1
    c = clusters[0]
    assert abs(c.centroid[0] - true_cx) < 0.3
    assert abs(c.centroid[2] - true_cz) < 0.3


def test_out_of_range_points_ignored():
    rng     = np.random.default_rng(3)
    pts_ok  = _make_cluster_pts(cx=0.0, cz=2.0, n=30, rng=rng)
    # Points beyond max_obstacle_dist_m=5.0
    pts_far = _make_cluster_pts(cx=0.0, cz=7.0, n=30, rng=rng)
    pts     = np.vstack([pts_ok, pts_far])
    h       = _make_heights(pts, rng=rng)
    clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5, z_range=(0.15, 5.0))
    # Only the near cluster should survive
    assert all(c.distance_m <= 5.0 for c in clusters)


# ── ObstacleCluster accessors ─────────────────────────────────────────────────

def test_cluster_distance_and_lateral():
    centroid = np.array([1.5, -0.3, 3.0])
    c = ObstacleCluster(centroid=centroid, radius_m=0.2, height_m=0.3, n_pts=30)
    assert abs(c.distance_m - 3.0) < 1e-9
    assert abs(c.lateral_m  - 1.5) < 1e-9
