"""Cluster a 2D LiDAR scan into world-frame sheep position estimates.

Pipeline:

    ranges (N,) → hit mask → world-frame points
                                │
                                ▼
                         adjacency clustering (gap > GAP_THRESHOLD
                         starts a new cluster, walking rays in
                         angular order)
                                │
                                ▼
                         centroid + span + region + structure filters
                                │
                                ▼
                         list of (x, y) detections

The downstream tracker handles association across frames.
"""

from __future__ import annotations

import math

import numpy as np

from herding.world.geometry import FIELD_X, FIELD_Y, GATE_Y, PEN_X, PEN_Y
from herding.perception.lidar_sim import (
    LIDAR_FOV, LIDAR_MAX_RANGE, LIDAR_N_RAYS, SHEEP_RADIUS, ray_angles,
)


GAP_THRESHOLD = 0.6      # m — adjacent ray-points farther apart start a new cluster
MAX_CLUSTER_SPAN = 1.5   # m — wider clusters are walls / structures
RANGE_HIT_EPS = 0.05     # m — hit if range < max_range - eps
WALL_REJECT = 0.5        # m — drop detections this close to a known wall line

# Sheep-sized static features (gate posts, corner pillars). A cluster
# centred within STATIC_REJECT of any of these is never a sheep.
_STATIC_FEATURES = (
    ( 10.0, -15.0), ( 13.0, -15.0),                   # gate posts
    ( 15.0,  15.0), ( 15.0, -15.0),
    (-15.0,  15.0), (-15.0, -15.0),                   # field corners
)
STATIC_REJECT = 0.8


def detections_from_scan(
    ranges: np.ndarray,
    dog_x: float, dog_y: float, dog_heading: float,
    max_range: float = LIDAR_MAX_RANGE,
) -> list[tuple[float, float]]:
    """Return list of (x, y) world-frame sheep position estimates."""
    ranges = np.asarray(ranges, dtype=np.float32)
    n_rays = ranges.shape[0]
    if n_rays == 0:
        return []
    angles = ray_angles(n_rays, LIDAR_FOV)
    hit = ranges < max_range - RANGE_HIT_EPS

    world_a = dog_heading + angles
    px = dog_x + ranges * np.cos(world_a)
    py = dog_y + ranges * np.sin(world_a)

    # Walk rays in angular order; a large jump between consecutive
    # world-frame hit points closes the current cluster.
    clusters: list[list[tuple[float, float]]] = []
    current: list[tuple[float, float]] = []
    prev: tuple[float, float] | None = None
    for i in range(n_rays):
        if not bool(hit[i]):
            if current:
                clusters.append(current)
                current = []
            prev = None
            continue
        pt = (float(px[i]), float(py[i]))
        if prev is not None and math.hypot(pt[0] - prev[0], pt[1] - prev[1]) > GAP_THRESHOLD:
            clusters.append(current)
            current = []
        current.append(pt)
        prev = pt
    if current:
        clusters.append(current)

    detections: list[tuple[float, float]] = []
    for cluster in clusters:
        xs = [p[0] for p in cluster]
        ys = [p[1] for p in cluster]
        cx, cy = sum(xs) / len(xs), sum(ys) / len(ys)
        span = math.hypot(max(xs) - min(xs), max(ys) - min(ys))
        if span > MAX_CLUSTER_SPAN:
            continue
        # Rays hit the front edge of the sheep; offset outward by
        # SHEEP_RADIUS along the dog→cluster direction to estimate the
        # centre.
        dx, dy = cx - dog_x, cy - dog_y
        d = math.hypot(dx, dy)
        if d > 1e-3:
            cx += SHEEP_RADIUS * dx / d
            cy += SHEEP_RADIUS * dy / d
        # Region filter: in-field clusters, plus a narrow strip south of
        # the gate so sheep mid-crossing get latched penned. Detections
        # deeper into the pen are dropped — pen posts and rails would
        # otherwise generate phantom penned tracks.
        in_main = (FIELD_X[0] - 0.2 < cx < FIELD_X[1] + 0.2 and
                   FIELD_Y[0] - 0.2 < cy < FIELD_Y[1] + 0.2)
        in_gate_strip = (PEN_X[0] - 0.2 < cx < PEN_X[1] + 0.2 and
                         GATE_Y - 1.0 < cy < GATE_Y + 0.2)
        if not (in_main or in_gate_strip):
            continue
        # Known sheep-sized static features.
        if any(math.hypot(cx - fx, cy - fy) < STATIC_REJECT
               for fx, fy in _STATIC_FEATURES):
            continue
        # Wall-proximity filter — sheep can't get this close to a wall,
        # so detections right at the wall line are structure noise.
        near_field_wall = (
            cx > FIELD_X[1] - WALL_REJECT or cx < FIELD_X[0] + WALL_REJECT or
            cy > FIELD_Y[1] - WALL_REJECT or
            (cy < FIELD_Y[0] + WALL_REJECT and not (PEN_X[0] <= cx <= PEN_X[1]))
        )
        if near_field_wall:
            continue
        detections.append((cx, cy))
    return detections