"""
_lidar_clustering.py — Euclidean clustering of 2-D LIDAR scan points (no ROS2 deps).

Algorithm
---------
1. Convert LaserScan ranges to Cartesian (x, y) points, dropping inf / NaN.
2. Walk through the points in angular order.  When the Euclidean distance
   between consecutive scan points exceeds `distance_threshold_m`, a new
   cluster begins (gap-based segmentation — O(N), standard for dense 360°
   LIDAR scans at short range).
3. Discard clusters with fewer than `min_points` points.

Each cluster is returned as a Cluster namedtuple with:
  points      — (N, 2) float64 ndarray of (x, y) in metres
  centroid    — (2,)   float64 ndarray, mean of points
  bbox_min    — (2,)   min (x, y) corner of axis-aligned bounding box
  bbox_max    — (2,)   max (x, y) corner of axis-aligned bounding box
  width_m     — bbox X extent (metres)
  depth_m     — bbox Y extent (metres)

Coordinate frame: 2-D projection of the LIDAR optical frame (+X forward, +Y left).
"""

from __future__ import annotations

import math
from typing import List, NamedTuple

import numpy as np


class Cluster(NamedTuple):
    points:   np.ndarray   # (N, 2) float64
    centroid: np.ndarray   # (2,)   float64
    bbox_min: np.ndarray   # (2,)   float64
    bbox_max: np.ndarray   # (2,)   float64
    width_m:  float
    depth_m:  float


def scan_to_cartesian(
    ranges:          List[float],
    angle_min:       float,
    angle_increment: float,
    range_min:       float = 0.05,
    range_max:       float = 12.0,
) -> np.ndarray:
    """
    Convert LaserScan ranges to a (M, 2) float64 array of valid (x, y) points.

    Parameters
    ----------
    ranges          : raw range values from LaserScan.ranges
    angle_min       : start angle of the scan (radians)
    angle_increment : angular step between consecutive readings (radians)
    range_min       : readings below this are discarded (m)
    range_max       : readings above this are discarded (m)

    Returns
    -------
    (M, 2) ndarray of valid Cartesian points, in scan order.
    """
    angles = angle_min + np.arange(len(ranges)) * angle_increment
    r      = np.asarray(ranges, dtype=np.float64)

    # Mask out invalid readings
    valid  = np.isfinite(r) & (r >= range_min) & (r <= range_max)
    r      = r[valid]
    a      = angles[valid]

    x = r * np.cos(a)
    y = r * np.sin(a)
    return np.column_stack((x, y))   # (M, 2)


def cluster_points(
    points:              np.ndarray,
    distance_threshold_m: float = 0.20,
    min_points:          int   = 3,
) -> List[Cluster]:
    """
    Cluster a (M, 2) array of scan-order Cartesian points.

    Parameters
    ----------
    points               : output of scan_to_cartesian()
    distance_threshold_m : max distance between consecutive points in the same cluster
    min_points           : discard clusters with fewer than this many points

    Returns
    -------
    List of Cluster objects, sorted by distance from the origin (nearest first).
    """
    if len(points) == 0:
        return []

    clusters: List[Cluster] = []
    current: List[np.ndarray] = [points[0]]

    for i in range(1, len(points)):
        gap = np.linalg.norm(points[i] - points[i - 1])
        if gap <= distance_threshold_m:
            current.append(points[i])
        else:
            _flush(current, clusters, min_points)
            current = [points[i]]

    _flush(current, clusters, min_points)

    # Sort by distance of centroid from origin (nearest first)
    clusters.sort(key=lambda c: float(np.linalg.norm(c.centroid)))
    return clusters


def _flush(pts: List[np.ndarray], out: List[Cluster], min_pts: int) -> None:
    """Convert accumulated points list into a Cluster and append to out."""
    if len(pts) < min_pts:
        return
    arr     = np.stack(pts)              # (N, 2)
    centroid = arr.mean(axis=0)
    bmin     = arr.min(axis=0)
    bmax     = arr.max(axis=0)
    span     = bmax - bmin
    out.append(Cluster(
        points   = arr,
        centroid = centroid,
        bbox_min = bmin,
        bbox_max = bmax,
        width_m  = float(span[0]),
        depth_m  = float(span[1]),
    ))
