"""
object_detector.py — LIDAR-based moving object detector.

Algorithm:
  1. Convert each LaserScan to a 2-D occupancy grid (robot-centred, fixed size).
  2. Maintain a background model via exponential moving average (EMA):
       bg_t = α * current + (1-α) * bg_{t-1}   (only for non-moving cells)
  3. Foreground = cells whose occupancy significantly exceeds the background.
  4. Cluster foreground cells with scipy connected-components → Detection list.

The grid is robot-relative (origin at robot centre) so it naturally tracks
the robot's motion without needing TF at this stage.  The caller is responsible
for transforming detections into a stable frame (odom) before passing to the
tracker.
"""

from __future__ import annotations

from dataclasses import dataclass, field
from typing import List, Optional

import numpy as np
from scipy import ndimage


@dataclass
class Detection:
    """A clustered moving foreground blob from one scan."""
    x: float          # centroid x in sensor frame (m)
    y: float          # centroid y in sensor frame (m)
    size_m2: float    # approximate area of the cluster (m²)
    range_m: float    # distance from robot (m)


class ObjectDetector:
    """
    Detects moving objects in consecutive 2-D LIDAR scans.

    Parameters
    ----------
    grid_radius_m : half-size of the occupancy grid (grid covers ±radius)
    resolution    : metres per cell
    bg_alpha      : EMA update rate for background (small = slow forgetting)
    motion_thr    : occupancy delta above background to count as moving
    min_cluster   : minimum cells to keep a cluster
    max_cluster   : maximum cells before a cluster is considered static wall
    """

    def __init__(
        self,
        grid_radius_m: float = 10.0,
        resolution: float = 0.10,
        bg_alpha: float = 0.04,
        motion_thr: float = 0.45,
        min_cluster: int = 3,
        max_cluster: int = 200,
    ):
        cells = int(2 * grid_radius_m / resolution)
        self._cells     = cells
        self._res       = resolution
        self._origin    = -grid_radius_m        # world x/y at grid index 0
        self._bg_alpha  = bg_alpha
        self._motion_thr = motion_thr
        self._min_clust = min_cluster
        self._max_clust = max_cluster

        self._bg = np.zeros((cells, cells), dtype=np.float32)
        self._initialized = False

    # ── Public API ────────────────────────────────────────────────────────────

    def update(
        self,
        ranges: np.ndarray,
        angle_min: float,
        angle_increment: float,
        range_max: float,
    ) -> List[Detection]:
        """
        Process one LaserScan and return detected moving blobs.

        Parameters
        ----------
        ranges          : 1-D array of range readings (metres)
        angle_min       : angle of first beam (radians)
        angle_increment : angular step between beams (radians)
        range_max       : maximum valid range (metres)
        """
        curr = self._scan_to_grid(ranges, angle_min, angle_increment, range_max)

        if not self._initialized:
            self._bg = curr.copy()
            self._initialized = True
            return []

        # Foreground mask
        motion_mask = (curr - self._bg) > self._motion_thr

        # Update background only on non-moving cells
        static = ~motion_mask
        self._bg[static] = (
            self._bg[static] * (1.0 - self._bg_alpha)
            + curr[static] * self._bg_alpha
        )

        return self._cluster(motion_mask)

    def reset(self) -> None:
        self._bg[:] = 0.0
        self._initialized = False

    # ── Internals ─────────────────────────────────────────────────────────────

    def _scan_to_grid(
        self,
        ranges: np.ndarray,
        angle_min: float,
        angle_increment: float,
        range_max: float,
    ) -> np.ndarray:
        grid = np.zeros((self._cells, self._cells), dtype=np.float32)
        n = len(ranges)
        angles = angle_min + np.arange(n) * angle_increment
        r = np.asarray(ranges, dtype=np.float32)

        valid = (r > 0.05) & (r < range_max) & np.isfinite(r)
        r, a = r[valid], angles[valid]

        x = r * np.cos(a)
        y = r * np.sin(a)

        ix = np.clip(
            ((x - self._origin) / self._res).astype(np.int32), 0, self._cells - 1
        )
        iy = np.clip(
            ((y - self._origin) / self._res).astype(np.int32), 0, self._cells - 1
        )
        grid[iy, ix] = 1.0
        return grid

    def _cluster(self, mask: np.ndarray) -> List[Detection]:
        # Dilate slightly to connect nearby hit cells into one blob
        struct = ndimage.generate_binary_structure(2, 2)
        dilated = ndimage.binary_dilation(mask, structure=struct, iterations=1)

        labeled, n_labels = ndimage.label(dilated)
        detections: List[Detection] = []

        for label_id in range(1, n_labels + 1):
            coords = np.argwhere(labeled == label_id)
            n_cells = len(coords)
            if n_cells < self._min_clust or n_cells > self._max_clust:
                continue

            ys, xs = coords[:, 0], coords[:, 1]
            cx_grid = float(np.mean(xs))
            cy_grid = float(np.mean(ys))
            cx = cx_grid * self._res + self._origin
            cy = cy_grid * self._res + self._origin

            detections.append(Detection(
                x=cx,
                y=cy,
                size_m2=n_cells * self._res ** 2,
                range_m=float(np.hypot(cx, cy)),
            ))

        return detections
