"""
frontier_detector.py — Detect exploration frontiers in an OccupancyGrid.

A frontier cell is a FREE cell (0) that has at least one UNKNOWN neighbour (-1 / 255).
Connected frontier cells are clustered; each cluster becomes a Frontier message with
a centroid and area estimate.

Algorithm:
  1. Convert OccupancyGrid.data to a 2-D numpy array.
  2. Find all free cells adjacent to unknown cells (4-connectivity for border check).
  3. Label connected components of frontier cells (8-connectivity for clustering).
  4. Compute centroid and area per cluster; filter by minimum area.
  5. Score each frontier: utility = area_m2 / (distance_to_robot + 1.0)

Typical call rate: ~1 Hz on the merged fleet map.
"""

from __future__ import annotations
import math
from typing import List, Optional, Tuple

import numpy as np

from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Point

from saltybot_fleet_msgs.msg import Frontier


_FREE     =  0
_UNKNOWN  = -1   # stored as 255 in uint8 arrays; we use int8 semantics


def _grid_to_array(grid: OccupancyGrid) -> np.ndarray:
    """Return (height, width) int8 array from OccupancyGrid.data."""
    arr = np.array(grid.data, dtype=np.int8).reshape(grid.info.height, grid.info.width)
    return arr


def detect_frontiers(
    grid: OccupancyGrid,
    robot_xy: Optional[Tuple[float, float]] = None,
    min_area_cells: int = 10,
    robot_id: str = '',
    seq: int = 0,
) -> List[Frontier]:
    """
    Detect frontier clusters in *grid* and return a list of Frontier messages.

    Args:
        grid:           Source OccupancyGrid (merged or local).
        robot_xy:       (x, y) in map frame for distance-weighted utility scoring.
        min_area_cells: Discard clusters smaller than this (noise filter).
        robot_id:       Used to build frontier_id strings.
        seq:            Monotonic sequence counter for stable IDs.

    Returns:
        List of Frontier messages sorted by utility descending.
    """
    arr = _grid_to_array(grid)
    h, w = arr.shape
    res = grid.info.resolution
    ox = grid.info.origin.position.x
    oy = grid.info.origin.position.y

    # ── Find frontier cells ────────────────────────────────────────────────
    # A cell is a frontier if it is FREE and has at least one UNKNOWN neighbour.
    free_mask    = (arr == _FREE)
    unknown_mask = (arr == _UNKNOWN)

    # Shift unknown_mask in 4 directions to find free cells adjacent to unknown
    has_unknown_n = np.zeros_like(unknown_mask)
    has_unknown_s = np.zeros_like(unknown_mask)
    has_unknown_e = np.zeros_like(unknown_mask)
    has_unknown_w = np.zeros_like(unknown_mask)

    has_unknown_n[1:, :]  = unknown_mask[:-1, :]
    has_unknown_s[:-1, :] = unknown_mask[1:,  :]
    has_unknown_e[:, :-1] = unknown_mask[:, 1:]
    has_unknown_w[:, 1:]  = unknown_mask[:, :-1]

    frontier_mask = free_mask & (
        has_unknown_n | has_unknown_s | has_unknown_e | has_unknown_w
    )

    if not frontier_mask.any():
        return []

    # ── Connected-component labelling (8-connectivity) ─────────────────────
    from scipy.ndimage import label as scipy_label
    struct = np.ones((3, 3), dtype=int)
    labelled, num_features = scipy_label(frontier_mask, structure=struct)

    frontiers: List[Frontier] = []
    for cluster_id in range(1, num_features + 1):
        cells = np.argwhere(labelled == cluster_id)   # (row, col) pairs
        n_cells = len(cells)
        if n_cells < min_area_cells:
            continue

        # centroid in map frame
        rows = cells[:, 0].astype(float)
        cols = cells[:, 1].astype(float)
        cx = ox + (cols.mean() + 0.5) * res
        cy = oy + (rows.mean() + 0.5) * res

        area_m2 = n_cells * res * res

        # utility: larger + closer = better
        if robot_xy is not None:
            dist = math.hypot(cx - robot_xy[0], cy - robot_xy[1]) + 1.0
        else:
            dist = 1.0
        utility = area_m2 / dist

        f = Frontier()
        f.frontier_id   = f'{robot_id}_{seq}_{cluster_id}'
        f.centroid      = Point(x=cx, y=cy, z=0.0)
        f.area_m2       = float(area_m2)
        f.utility       = float(utility)
        f.assigned_to   = ''
        frontiers.append(f)

    frontiers.sort(key=lambda x: x.utility, reverse=True)
    return frontiers
