from __future__ import annotations

import heapq
import logging
import math
from dataclasses import dataclass, field
from typing import Any, Dict, Iterable, List, Optional, Tuple

import numpy as np

_log = logging.getLogger("SLAM_NavRuntime")


def _safe_float(v: Any, default: float) -> float:
    try:
        return float(v)
    except Exception:
        return float(default)


def _safe_int(v: Any, default: int) -> int:
    try:
        return int(v)
    except Exception:
        return int(default)


def _as_bool(v: Any, default: bool) -> bool:
    if v is None:
        return default
    if isinstance(v, bool):
        return v
    if isinstance(v, (int, float)):
        return bool(v)
    return str(v).strip().lower() in ("1", "true", "yes", "on")


# Disc-offset cache for `_inflate_binary`. Keyed on radius_cells; the disc
# of `(dy, dx)` offsets within `r` is identical across calls for the same
# radius. With `inflation_radius_m / res` typically rounding to 2-3 cells,
# this cache holds a tiny number of entries and pays back its allocation
# every tick after the first.
_INFLATE_OFFSETS_CACHE: Dict[int, List[Tuple[int, int]]] = {}


def _world_to_grid_strict(
    x: float, y: float, origin: np.ndarray, res: float, w: int, h: int,
) -> Optional[Tuple[int, int]]:
    """Convert world (x, y) to grid (gx, gy); returns None if outside [0, w) x [0, h).
    Centralised so the bounds-check semantics stay identical across world_to_grid,
    A* to_grid, and any future cell-locator.
    """
    gx = int(math.floor((float(x) - float(origin[0])) / float(res)))
    gy = int(math.floor((float(y) - float(origin[1])) / float(res)))
    if gx < 0 or gy < 0 or gx >= int(w) or gy >= int(h):
        return None
    return gx, gy


def _inflate_offsets(radius_cells: int) -> List[Tuple[int, int]]:
    cached = _INFLATE_OFFSETS_CACHE.get(int(radius_cells))
    if cached is not None:
        return cached
    rr = int(radius_cells) * int(radius_cells)
    offsets: List[Tuple[int, int]] = []
    for dy in range(-radius_cells, radius_cells + 1):
        for dx in range(-radius_cells, radius_cells + 1):
            if (dy * dy + dx * dx) <= rr:
                offsets.append((dy, dx))
    _INFLATE_OFFSETS_CACHE[int(radius_cells)] = offsets
    return offsets


def _inflate_binary(mask: np.ndarray, radius_cells: int) -> np.ndarray:
    if radius_cells <= 0:
        return mask.copy()
    h, w = mask.shape
    out = mask.copy()
    for dy, dx in _inflate_offsets(int(radius_cells)):
        ys = max(0, -dy)
        ye = min(h, h - dy)
        xs = max(0, -dx)
        xe = min(w, w - dx)
        yd = max(0, dy)
        xd = max(0, dx)
        out[yd : yd + (ye - ys), xd : xd + (xe - xs)] |= mask[ys:ye, xs:xe]
    return out


@dataclass
class LiveCostmapConfig:
    enabled: bool = True
    resolution_m: float = 0.10
    z_min_m: float = -0.40
    z_max_m: float = 1.20
    padding_m: float = 0.80
    inflation_radius_m: float = 0.25
    dynamic_decay_sec: float = 1.5
    dynamic_min_hits: int = 2
    blocked_cost: int = 220
    max_width_cells: int = 900
    max_height_cells: int = 900

    @staticmethod
    def from_dict(d: Dict[str, Any] | None) -> "LiveCostmapConfig":
        src = d or {}
        return LiveCostmapConfig(
            enabled=_as_bool(src.get("enabled", True), True),
            resolution_m=max(0.02, _safe_float(src.get("resolution_m", 0.10), 0.10)),
            z_min_m=_safe_float(src.get("z_min_m", -0.40), -0.40),
            z_max_m=_safe_float(src.get("z_max_m", 1.20), 1.20),
            padding_m=max(0.0, _safe_float(src.get("padding_m", 0.80), 0.80)),
            inflation_radius_m=max(0.0, _safe_float(src.get("inflation_radius_m", 0.25), 0.25)),
            dynamic_decay_sec=max(0.2, _safe_float(src.get("dynamic_decay_sec", 1.5), 1.5)),
            dynamic_min_hits=max(1, _safe_int(src.get("dynamic_min_hits", 2), 2)),
            blocked_cost=int(np.clip(_safe_int(src.get("blocked_cost", 220), 220), 100, 255)),
            max_width_cells=max(100, _safe_int(src.get("max_width_cells", 900), 900)),
            max_height_cells=max(100, _safe_int(src.get("max_height_cells", 900), 900)),
        )

    def patched(self, patch: Dict[str, Any] | None) -> "LiveCostmapConfig":
        src = patch or {}
        return LiveCostmapConfig.from_dict(
            {
                "enabled": src.get("enabled", self.enabled),
                "resolution_m": src.get("resolution_m", self.resolution_m),
                "z_min_m": src.get("z_min_m", self.z_min_m),
                "z_max_m": src.get("z_max_m", self.z_max_m),
                "padding_m": src.get("padding_m", self.padding_m),
                "inflation_radius_m": src.get("inflation_radius_m", self.inflation_radius_m),
                "dynamic_decay_sec": src.get("dynamic_decay_sec", self.dynamic_decay_sec),
                "dynamic_min_hits": src.get("dynamic_min_hits", self.dynamic_min_hits),
                "blocked_cost": src.get("blocked_cost", self.blocked_cost),
                "max_width_cells": src.get("max_width_cells", self.max_width_cells),
                "max_height_cells": src.get("max_height_cells", self.max_height_cells),
            }
        )


class LiveCostmapRuntime:
    """
    Maintains real-time static + dynamic + inflated obstacle layers.
    Static layer comes from SLAM stable points; dynamic layer comes from live points.
    """

    def __init__(self, cfg: LiveCostmapConfig):
        self.cfg = cfg
        self._dynamic_cells: Dict[Tuple[int, int], Tuple[int, float, float, float]] = {}
        self._grid: Optional[Dict[str, Any]] = None

    def reset(self) -> None:
        self._dynamic_cells.clear()
        self._grid = None

    def _clip_nav_band(self, points: Optional[np.ndarray]) -> np.ndarray:
        if points is None:
            return np.zeros((0, 3), dtype=np.float32)
        pts = np.asarray(points, dtype=np.float32)
        if pts.ndim != 2 or pts.shape[1] < 3 or len(pts) == 0:
            return np.zeros((0, 3), dtype=np.float32)
        zmin = float(self.cfg.z_min_m)
        zmax = float(self.cfg.z_max_m)
        m = (pts[:, 2] >= zmin) & (pts[:, 2] <= zmax)
        return pts[m, :3]

    @staticmethod
    def _grid_indices(xy: np.ndarray, min_xy: np.ndarray, res: float, w: int, h: int) -> Tuple[np.ndarray, np.ndarray]:
        gx = np.floor((xy[:, 0] - min_xy[0]) / res).astype(np.int32)
        gy = np.floor((xy[:, 1] - min_xy[1]) / res).astype(np.int32)
        gx = np.clip(gx, 0, w - 1)
        gy = np.clip(gy, 0, h - 1)
        return gx, gy

    def world_to_grid(self, x: float, y: float) -> Optional[Tuple[int, int]]:
        if self._grid is None:
            return None
        origin = np.asarray(self._grid["origin_xy"], dtype=np.float64)
        res = float(self._grid["resolution_m"])
        shape = tuple(self._grid["shape_hw"])
        h, w = int(shape[0]), int(shape[1])
        return _world_to_grid_strict(x, y, origin, res, w, h)

    def cost_at_world(self, x: float, y: float) -> int:
        if self._grid is None:
            return 255
        idx = self.world_to_grid(x, y)
        if idx is None:
            return 255
        gx, gy = idx
        cost = np.asarray(self._grid["costmap"], dtype=np.uint8)
        return int(cost[gy, gx])

    def occupied_near(self, x: float, y: float, radius_m: float, cost_thresh: Optional[int] = None) -> bool:
        if self._grid is None:
            return True
        thresh = int(self.cfg.blocked_cost if cost_thresh is None else cost_thresh)
        idx = self.world_to_grid(x, y)
        if idx is None:
            return True
        gx, gy = idx
        cost = np.asarray(self._grid["costmap"], dtype=np.uint8)
        h, w = cost.shape
        rc = int(math.ceil(max(0.0, float(radius_m)) / float(self._grid["resolution_m"])))
        ys = max(0, gy - rc)
        ye = min(h, gy + rc + 1)
        xs = max(0, gx - rc)
        xe = min(w, gx + rc + 1)
        return bool(np.any(cost[ys:ye, xs:xe] >= thresh))

    def _compute_bounds(self, static_pts: np.ndarray, live_pts: np.ndarray) -> Optional[Tuple[np.ndarray, np.ndarray]]:
        has_static = len(static_pts) > 0
        has_live = len(live_pts) > 0
        if not has_static and not has_live:
            return None
        if has_static and has_live:
            all_xy = np.vstack((static_pts[:, :2], live_pts[:, :2]))
        elif has_static:
            all_xy = static_pts[:, :2]
        else:
            all_xy = live_pts[:, :2]
        pad = float(self.cfg.padding_m)
        mn = all_xy.min(axis=0) - pad
        mx = all_xy.max(axis=0) + pad
        return mn.astype(np.float64), mx.astype(np.float64)

    def _clamp_grid_size(self, mn: np.ndarray, mx: np.ndarray, res: float) -> Tuple[np.ndarray, np.ndarray, float, int, int]:
        span = np.maximum(mx - mn, res)
        w = int(np.ceil(span[0] / res)) + 1
        h = int(np.ceil(span[1] / res)) + 1
        res_out = float(res)
        if w > int(self.cfg.max_width_cells) or h > int(self.cfg.max_height_cells):
            sx = float(w) / float(self.cfg.max_width_cells)
            sy = float(h) / float(self.cfg.max_height_cells)
            scale = max(sx, sy)
            res_out = res_out * scale
            span = np.maximum(mx - mn, res_out)
            w = int(np.ceil(span[0] / res_out)) + 1
            h = int(np.ceil(span[1] / res_out)) + 1
        return mn, mx, float(res_out), int(w), int(h)

    def update(
        self,
        stable_points_world: Optional[np.ndarray],
        live_points_world: Optional[np.ndarray],
        now: float,
    ) -> Optional[Dict[str, Any]]:
        if not self.cfg.enabled:
            return None

        static_pts = self._clip_nav_band(stable_points_world)
        live_pts = self._clip_nav_band(live_points_world)

        bounds = self._compute_bounds(static_pts, live_pts)
        if bounds is None:
            # decay dynamic memory even when there is no input
            self._dynamic_cells = {
                k: v
                for k, v in self._dynamic_cells.items()
                if (float(now) - float(v[1])) <= float(self.cfg.dynamic_decay_sec)
            }
            return None

        mn, mx = bounds
        mn, mx, res, w, h = self._clamp_grid_size(mn, mx, float(self.cfg.resolution_m))

        static_occ = np.zeros((h, w), dtype=bool)
        if len(static_pts) > 0:
            gx, gy = self._grid_indices(static_pts[:, :2], mn, res, w, h)
            static_occ[gy, gx] = True

        # age out dynamic memory first
        max_age = float(self.cfg.dynamic_decay_sec)
        new_dyn: Dict[Tuple[int, int], Tuple[int, float, float, float]] = {}
        dropped_shape = 0
        for key, value in self._dynamic_cells.items():
            try:
                hits, ts, wx, wy = value
            except (ValueError, TypeError):
                # Tuple shape mismatch (legacy 2-tuple from a prior format,
                # or a corrupted entry). Loud-fail rather than silently
                # erasing dynamic obstacle memory.
                dropped_shape += 1
                continue
            if (float(now) - float(ts)) <= max_age:
                new_dyn[key] = (int(hits), float(ts), float(wx), float(wy))
        if dropped_shape:
            _log.warning(
                "_dynamic_cells dropped %d entries with unexpected tuple shape "
                "— check version drift between writers and readers",
                dropped_shape,
            )
        self._dynamic_cells = new_dyn

        if len(live_pts) > 0:
            # Vectorised hit accumulation per dynamic-key cell. Aggregate
            # via np.unique on stacked (kx, ky) pairs — the previous
            # `(kx<<32) | (ky & 0xFFFFFFFF)` int-packing collided sign-bits
            # at large grids (`ky=-1` aliased to `ky=4294967295`).
            res_key = max(0.02, float(self.cfg.resolution_m))
            kx_all = np.floor(live_pts[:, 0].astype(np.float64) / res_key).astype(np.int64)
            ky_all = np.floor(live_pts[:, 1].astype(np.float64) / res_key).astype(np.int64)
            gx_l, gy_l = self._grid_indices(live_pts[:, :2], mn, res, w, h)
            in_grid = ~static_occ[gy_l, gx_l]
            if np.any(in_grid):
                kx_in = kx_all[in_grid]
                ky_in = ky_all[in_grid]
                wx_in = live_pts[in_grid, 0].astype(np.float64)
                wy_in = live_pts[in_grid, 1].astype(np.float64)
                # `idx` is into the *filtered* arrays — read kx_in / ky_in
                # / wx_in / wy_in at those indices to stay self-consistent
                # without relying on operation ordering.
                pairs = np.column_stack([kx_in, ky_in])
                _, idx, counts = np.unique(
                    pairs, axis=0, return_index=True, return_counts=True,
                )
                for u_idx, u_count in zip(idx.tolist(), counts.tolist()):
                    cell = (int(kx_in[u_idx]), int(ky_in[u_idx]))
                    wx = float(wx_in[u_idx])
                    wy = float(wy_in[u_idx])
                    old = self._dynamic_cells.get(cell)
                    old_hits = int(old[0]) if old is not None else 0
                    self._dynamic_cells[cell] = (
                        old_hits + int(u_count), float(now), wx, wy,
                    )

        dynamic_occ = np.zeros((h, w), dtype=bool)
        min_hits = int(self.cfg.dynamic_min_hits)
        for _key, (hits, ts, wx, wy) in self._dynamic_cells.items():
            if hits >= min_hits and (float(now) - float(ts)) <= max_age:
                cell = _world_to_grid_strict(wx, wy, mn, res, w, h)
                if cell is not None:
                    dynamic_occ[cell[1], cell[0]] = True

        occ = static_occ | dynamic_occ
        inf_cells = int(np.ceil(float(self.cfg.inflation_radius_m) / res))
        inflated = _inflate_binary(occ, inf_cells)

        # Cost layering: inflated >= blocked_cost so the local planner +
        # safety supervisor refuse to drive through the inflation buffer.
        # Previously inflation was 180 (advisory) while blocked_cost
        # defaulted to 220 — the planner happily routed through inflated
        # cells, defeating the configured 25 cm inflation radius.
        cost = np.zeros((h, w), dtype=np.uint8)
        blocked = int(self.cfg.blocked_cost)
        cost[inflated] = blocked
        cost[dynamic_occ] = max(blocked, 230)
        cost[static_occ] = 255

        out = {
            "costmap": cost,
            "static_mask": static_occ,
            "dynamic_mask": dynamic_occ,
            "inflated_mask": inflated,
            "origin_xy": mn.astype(np.float32),
            "resolution_m": float(res),
            "shape_hw": [int(h), int(w)],
            "static_cells": int(np.count_nonzero(static_occ)),
            "dynamic_cells": int(np.count_nonzero(dynamic_occ)),
            "inflated_cells": int(np.count_nonzero(inflated)),
        }
        self._grid = out
        return {
            "shape": [int(h), int(w)],
            "resolution_m": float(res),
            "origin_xy": [float(mn[0]), float(mn[1])],
            "static_cells": int(out["static_cells"]),
            "dynamic_cells": int(out["dynamic_cells"]),
            "inflated_cells": int(out["inflated_cells"]),
        }

    @property
    def grid(self) -> Optional[Dict[str, Any]]:
        return self._grid


class GlobalAStarPlanner:
    """Grid A* with corner-cut prevention and a cost-aware admissible
    heuristic.

    Heuristic admissibility: edge cost is `step + (cost/255)*2.0`, so the
    minimum possible per-step cost is `step` (when cost=0). The Euclidean
    heuristic on grid distance with diagonal step `sqrt(2)` is a lower
    bound on the sum-of-steps any reachable path must traverse, hence
    `h(n) <= true_cost(n→goal)`. The previous implementation kept the
    same heuristic but allowed the cost penalty to push `g_score` above
    the heuristic — still admissible because the heuristic ignored that
    penalty entirely. Documented here so a future tweak doesn't break it.
    """

    def __init__(self, blocked_cost: int = 220):
        self.blocked_cost = int(np.clip(int(blocked_cost), 100, 255))
        # Corner-cut threshold separate from `blocked_cost` so a future
        # change to graduated inflation (cells set below blocked_cost)
        # cannot reintroduce diagonal slip through the inflation buffer.
        # Today inflation = blocked_cost so the values coincide.
        self.corner_cut_cost = int(self.blocked_cost)
        # Cost-weight applied per step. Penalty is step-proportional so
        # diagonals through clutter cost more than cardinals through the
        # same clutter — keeps planner preference stable with path length.
        self.cost_weight = 2.0

    @staticmethod
    def _heur(a: Tuple[int, int], b: Tuple[int, int]) -> float:
        return float(math.hypot(float(a[0] - b[0]), float(a[1] - b[1])))

    @staticmethod
    def _find_nearest_safe(
        cost: np.ndarray,
        goal: Tuple[int, int],
        max_radius_cells: int,
        blocked_cost: int,
    ) -> Optional[Tuple[int, int]]:
        """Spiral outward from ``goal`` to find the nearest cell whose cost
        is below ``blocked_cost``. Used when the operator clicked a goal
        that falls inside a wall, table, or other obstacle — instead of
        refusing the request, we snap to the nearest cell the robot can
        actually occupy, so the robot approaches the obstacle's boundary
        and stops there rather than pushing into it indefinitely.

        Returns ``None`` if no safe cell exists within the search radius.
        """
        h, w = cost.shape
        gx, gy = goal
        for r in range(1, int(max_radius_cells) + 1):
            for dy in range(-r, r + 1):
                for dx in range(-r, r + 1):
                    # Only check cells on the perimeter of the current
                    # ring; interior cells were already checked in prior
                    # iterations.
                    if max(abs(dx), abs(dy)) != r:
                        continue
                    nx, ny = gx + dx, gy + dy
                    if 0 <= nx < w and 0 <= ny < h:
                        if int(cost[ny, nx]) < blocked_cost:
                            return (nx, ny)
        return None

    @staticmethod
    def _neighbors(x: int, y: int, w: int, h: int) -> Iterable[Tuple[int, int, float]]:
        for dy in (-1, 0, 1):
            for dx in (-1, 0, 1):
                if dx == 0 and dy == 0:
                    continue
                nx = x + dx
                ny = y + dy
                if 0 <= nx < w and 0 <= ny < h:
                    step = 1.41421356 if (dx != 0 and dy != 0) else 1.0
                    yield nx, ny, step

    def plan(
        self,
        costmap: np.ndarray,
        origin_xy: np.ndarray,
        resolution_m: float,
        start_xy: Tuple[float, float],
        goal_xy: Tuple[float, float],
        max_expansions: int = 120000,
    ) -> List[Tuple[float, float]]:
        cost = np.asarray(costmap, dtype=np.uint8)
        if cost.ndim != 2:
            return []
        h, w = cost.shape
        origin = np.asarray(origin_xy, dtype=np.float64)
        res = float(resolution_m)
        blocked = int(self.blocked_cost)

        def to_grid(x: float, y: float) -> Optional[Tuple[int, int]]:
            return _world_to_grid_strict(x, y, origin, res, w, h)

        def to_world(gx: int, gy: int) -> Tuple[float, float]:
            return (
                float(origin[0] + (float(gx) + 0.5) * res),
                float(origin[1] + (float(gy) + 0.5) * res),
            )

        s = to_grid(float(start_xy[0]), float(start_xy[1]))
        g = to_grid(float(goal_xy[0]), float(goal_xy[1]))
        if s is None or g is None:
            return []
        if int(cost[s[1], s[0]]) >= blocked:
            # Robot's own cell is blocked — can't plan from inside an
            # obstacle. Caller should re-localize before retrying.
            return []
        if int(cost[g[1], g[0]]) >= blocked:
            # Operator clicked on a wall / table / chair. Snap to the
            # nearest reachable cell within ~2 m so the robot approaches
            # the obstacle's edge and stops, instead of refusing the
            # request entirely or trying to push through.
            snap_radius_cells = max(int(round(2.0 / max(res, 1e-3))), 4)
            snapped = self._find_nearest_safe(cost, g, snap_radius_cells, blocked)
            if snapped is None:
                _log.warning(
                    "A* goal in obstacle and no safe cell within %.1f m; refusing",
                    snap_radius_cells * res,
                )
                return []
            _log.info(
                "A* goal snapped from blocked cell %s to safe cell %s "
                "(distance: %.2f m)",
                g, snapped,
                math.hypot((snapped[0] - g[0]) * res, (snapped[1] - g[1]) * res),
            )
            g = snapped

        open_heap: List[Tuple[float, float, Tuple[int, int]]] = []
        heapq.heappush(open_heap, (self._heur(s, g), 0.0, s))
        came_from: Dict[Tuple[int, int], Tuple[int, int]] = {}
        g_score: Dict[Tuple[int, int], float] = {s: 0.0}
        closed: Dict[Tuple[int, int], bool] = {}

        expansions = 0
        while open_heap:
            _, cur_g, cur = heapq.heappop(open_heap)
            if closed.get(cur, False):
                continue
            closed[cur] = True
            if cur == g:
                break
            expansions += 1
            if expansions >= int(max_expansions):
                # Route through the module logger so this lands in
                # logs/lidar.log alongside every other SLAM diagnostic.
                # The previous `print(..., file=sys.stderr)` was filtered
                # out of the SDK-pipe so operators never saw it.
                _log.warning(
                    "A* expansion limit %d reached; no path found from %s to %s",
                    max_expansions, start_xy, goal_xy,
                )
                return []

            cx, cy = cur
            for nx, ny, step in self._neighbors(cx, cy, w, h):
                cell_cost = int(cost[ny, nx])
                if cell_cost >= blocked:
                    continue
                # Corner-cut prevention: a diagonal move from (cx, cy) to
                # (nx, ny) cannot pass through a blocked cardinal cell.
                # Threshold is `corner_cut_cost`, decoupled from `blocked`
                # so a future graduated-inflation refactor cannot
                # reintroduce diagonal slip through the inflation buffer.
                if nx != cx and ny != cy:
                    if (
                        int(cost[cy, nx]) >= self.corner_cut_cost
                        or int(cost[ny, cx]) >= self.corner_cut_cost
                    ):
                        continue
                # Step-proportional cost penalty: a high-cost cell costs
                # ~3× a clear cell to traverse, regardless of cardinal vs
                # diagonal direction. Admissibility holds because penalty
                # ≥ 0 and Euclidean heuristic ignores it.
                ng = float(
                    cur_g + step * (1.0 + self.cost_weight * float(cell_cost) / 255.0)
                )
                node = (nx, ny)
                if ng < float(g_score.get(node, 1e18)):
                    g_score[node] = ng
                    came_from[node] = cur
                    f = ng + self._heur(node, g)
                    heapq.heappush(open_heap, (f, ng, node))

        if g not in came_from and g != s:
            return []

        path_cells: List[Tuple[int, int]] = [g]
        cur = g
        visited_back: set = {g}
        while cur != s:
            nxt = came_from.get(cur)
            if nxt is None or nxt in visited_back:
                # Cycle / missing predecessor implies a corrupted came_from
                # dict (concurrent costmap reset, partial mutation) — surface
                # it loudly so the failure mode is visible in logs rather
                # than silently surfacing as "goal unreachable" upstream.
                _log.warning(
                    "A* path reconstruction aborted: %s at cell %s "
                    "(start=%s, goal=%s)",
                    "cycle in came_from" if nxt in visited_back else "missing predecessor",
                    cur, s, g,
                )
                return []
            visited_back.add(nxt)
            cur = nxt
            path_cells.append(cur)
        path_cells.reverse()
        return [to_world(px, py) for (px, py) in path_cells]


@dataclass
class LocalPlannerConfig:
    lookahead_m: float = 0.8
    max_linear_mps: float = 0.6
    max_angular_rps: float = 1.3
    goal_tolerance_m: float = 0.30
    collision_probe_m: float = 0.6
    # Holonomic evasion — pick from forward / strafe / backward / rotate
    # candidates when the planned forward path is blocked. Capped well
    # below max_linear_mps because G1's RL gait was trained on a forward-
    # heavy velocity envelope; pushing lateral/backward too hard risks an
    # ungainly stumble.
    max_lateral_mps: float = 0.30
    max_backward_mps: float = 0.30
    evasion_enabled: bool = True
    # Whitelist of workflow profile names where holonomic evasion is
    # allowed to fire. An empty list = NO workflow allows evasion (only
    # the autonomous_active override can enable it). To kill evasion
    # entirely, set evasion_enabled=false. Mapping workflows (MAP_NEW /
    # EXTEND_MAP) are intentionally absent from the default so operator
    # teleop isn't fought by auto-avoidance.
    evasion_workflows: List[str] = field(default_factory=lambda: [
        "LOCALIZE_MAP", "LIVE_NAV_MAP", "LIVE_NAV_NO_MAP",
    ])
    evasion_lookahead_s: float = 0.5
    # Stuck recovery — fires only after holonomic evasion has produced
    # zero progress for this many seconds. BACKUP then ROTATE then retry,
    # up to N attempts before disarming the drive entirely.
    stuck_trigger_seconds: float = 2.0
    stuck_backup_seconds: float = 1.0
    stuck_rotate_seconds: float = 1.5
    stuck_max_attempts: int = 3

    @staticmethod
    def from_dict(d: Dict[str, Any] | None) -> "LocalPlannerConfig":
        src = d or {}
        wf_raw = src.get("evasion_workflows")
        if wf_raw is None:
            wf = ["LOCALIZE_MAP", "LIVE_NAV_MAP", "LIVE_NAV_NO_MAP"]
        else:
            try:
                wf = [str(w).upper().strip() for w in wf_raw if str(w).strip()]
            except Exception:
                wf = ["LOCALIZE_MAP", "LIVE_NAV_MAP", "LIVE_NAV_NO_MAP"]
        return LocalPlannerConfig(
            lookahead_m=max(0.1, _safe_float(src.get("lookahead_m", 0.8), 0.8)),
            max_linear_mps=max(0.05, _safe_float(src.get("max_linear_mps", 0.6), 0.6)),
            max_angular_rps=max(0.1, _safe_float(src.get("max_angular_rps", 1.3), 1.3)),
            goal_tolerance_m=max(0.05, _safe_float(src.get("goal_tolerance_m", 0.30), 0.30)),
            collision_probe_m=max(0.1, _safe_float(src.get("collision_probe_m", 0.6), 0.6)),
            max_lateral_mps=max(0.0, _safe_float(src.get("max_lateral_mps", 0.30), 0.30)),
            max_backward_mps=max(0.0, _safe_float(src.get("max_backward_mps", 0.30), 0.30)),
            evasion_enabled=_as_bool(src.get("evasion_enabled", True), True),
            evasion_workflows=wf,
            evasion_lookahead_s=max(0.1, _safe_float(src.get("evasion_lookahead_s", 0.5), 0.5)),
            stuck_trigger_seconds=max(0.5, _safe_float(src.get("stuck_trigger_seconds", 2.0), 2.0)),
            stuck_backup_seconds=max(0.2, _safe_float(src.get("stuck_backup_seconds", 1.0), 1.0)),
            stuck_rotate_seconds=max(0.2, _safe_float(src.get("stuck_rotate_seconds", 1.5), 1.5)),
            stuck_max_attempts=max(0, _safe_int(src.get("stuck_max_attempts", 3), 3)),
        )


class LocalReactivePlanner:
    def __init__(self, cfg: LocalPlannerConfig):
        self.cfg = cfg
        # Stuck-recovery state. ``_stuck_start_time`` is the wall-clock
        # time at which every evasion candidate first started failing;
        # reset to None whenever the robot makes progress. The phase
        # field is the FSM mode ("none" / "backup" / "rotate"). The
        # attempt counter is the only piece that survives a successful
        # FSM cycle — so three full BACKUP→ROTATE cycles with no real
        # progress trip the give-up branch.
        self._stuck_start_time: Optional[float] = None
        self._recovery_phase: str = "none"
        self._recovery_phase_start: float = 0.0
        self._recovery_attempts: int = 0
        self._recovery_rotate_dir: int = 1  # +1 = left, -1 = right

    @staticmethod
    def _yaw_from_pose(pose: np.ndarray) -> float:
        # Canonical extraction lives in SLAM_Transforms; thin wrapper kept
        # for backward-compat with callers that already use this name.
        from SLAM_Transforms import yaw_rad_from_tf
        return yaw_rad_from_tf(pose)

    @staticmethod
    def _wrap_pi(a: float) -> float:
        # Constant-time wrap; the previous `while` form was O(|a|) for
        # unwrapped accumulating angles.
        return (float(a) + math.pi) % (2.0 * math.pi) - math.pi

    def compute_command(
        self,
        pose_world: np.ndarray,
        path_world: List[Tuple[float, float]],
        runtime: LiveCostmapRuntime,
        workflow: str = "",
        autonomous_active: bool = False,
    ) -> Dict[str, Any]:
        """Produce a robot velocity command given pose, path, and costmap.

        Layered behaviour:
          1. Standard pure pursuit on ``path_world`` → desired (vx, vyaw).
             Decelerates as we approach the goal.
          2. Forward arc-probe: simulate the planned (vx, vyaw) ahead and
             check the costmap. If clear → emit it (today's path).
          3. **Holonomic evasion** (only in workflows listed in
             ``cfg.evasion_workflows``): if forward is blocked, sample
             5 candidate motions — forward, strafe-L, strafe-R, backward,
             rotate-only — and pick the one that's clear AND makes the
             most goal progress.
          4. **Stuck recovery** (same workflow gate): if every candidate
             is blocked for ``stuck_trigger_seconds``, fire a deterministic
             BACKUP→ROTATE→retry FSM, up to ``stuck_max_attempts`` times.
             After that, disarm via ``blocked=True`` + ``motion="give_up"``.

        Outside the evasion workflows (MAP_NEW / EXTEND_MAP) only steps
        1 + 2 run — operator teleop is never overridden.
        """
        import time as _time
        now = float(_time.time())

        cmd = {
            "linear_mps": 0.0,
            "lateral_mps": 0.0,
            "angular_rps": 0.0,
            "goal_reached": False,
            "blocked": False,
            "motion": "idle",
        }
        if pose_world is None or np.asarray(pose_world).shape != (4, 4):
            return cmd
        if not path_world:
            return cmd

        p = np.asarray(pose_world, dtype=np.float64)
        x = float(p[0, 3])
        y = float(p[1, 3])
        yaw = self._yaw_from_pose(p)

        goal_x, goal_y = float(path_world[-1][0]), float(path_world[-1][1])
        dist_goal = float(math.hypot(goal_x - x, goal_y - y))
        if dist_goal <= float(self.cfg.goal_tolerance_m):
            cmd["goal_reached"] = True
            cmd["motion"] = "arrived"
            self._reset_recovery_state()
            return cmd

        # ── Evasion / recovery gate ──
        # Holonomic evasion + stuck-recovery are normally restricted to
        # nav workflows. The `autonomous_active` flag is an override —
        # when the autonomous wander system is driving the robot in a
        # mapping workflow (MAP_NEW / EXTEND_MAP), there's no operator
        # teleop to fight, so evasion + recovery should fire. This is
        # the explicit "robot is in charge" gate, separate from the
        # workflow-based gate.
        wf_upper = str(workflow).upper().strip()
        evasion_workflows = set(self.cfg.evasion_workflows or [])
        # An empty `evasion_workflows` list means no workflow opts in to
        # evasion (operator must rely on autonomous_active to override).
        # To disable evasion entirely use evasion_enabled=false.
        evasion_active = (
            bool(self.cfg.evasion_enabled)
            and (
                bool(autonomous_active)
                or (wf_upper in evasion_workflows)
            )
        )

        # If evasion just gated off (workflow change, wander disarmed,
        # localization lost) while a recovery FSM was in flight, drop
        # the FSM state — otherwise the phase + attempt counter survive
        # into the next evasion-allowed session and bias it toward
        # premature give-up. The state is only meaningful as a continuous
        # sequence within a single evasion-enabled window.
        if not evasion_active and self._recovery_phase != "none":
            self._reset_recovery_state()

        # If we're already in active recovery and evasion is allowed,
        # the FSM takes priority over normal navigation (it owns the
        # actuator until it completes or finds a clear path).
        if evasion_active and self._recovery_phase != "none":
            rec = self._tick_recovery(now, x, y, yaw, runtime)
            if rec is not None:
                cmd.update(rec)
                return cmd
            # FSM finished its sequence — fall through to normal nav and
            # see if the world has opened up.

        # ── Pure pursuit → desired (lin, ang) ──
        target = path_world[-1]
        for wx, wy in path_world:
            d = float(math.hypot(float(wx) - x, float(wy) - y))
            if d >= float(self.cfg.lookahead_m):
                target = (float(wx), float(wy))
                break

        dx = float(target[0] - x)
        dy = float(target[1] - y)
        target_yaw = float(math.atan2(dy, dx))
        yaw_err = self._wrap_pi(target_yaw - yaw)

        ang = float(np.clip(1.5 * yaw_err, -float(self.cfg.max_angular_rps), float(self.cfg.max_angular_rps)))
        lin_scale = max(0.0, 1.0 - min(1.0, abs(yaw_err) / math.pi))
        lin = float(self.cfg.max_linear_mps) * lin_scale

        # Smooth deceleration as we approach the goal — same reason as
        # before: avoids overshoot when pure pursuit snaps to zero.
        slow_radius = max(2.0 * float(self.cfg.goal_tolerance_m), 0.7)
        if dist_goal < slow_radius:
            approach_scale = max(0.20, dist_goal / slow_radius)
            lin *= approach_scale

        # ── Forward probe ──
        forward_blocked = self._is_motion_blocked(
            x, y, yaw, lin, 0.0, ang, runtime,
            horizon_s=float(self.cfg.collision_probe_m) / max(0.1, float(self.cfg.max_linear_mps)),
        )

        if not forward_blocked:
            cmd["linear_mps"] = float(lin)
            cmd["angular_rps"] = float(ang)
            cmd["motion"] = "forward"
            # Making progress → reset any pending recovery state.
            self._reset_recovery_state()
            return cmd

        # Forward is blocked. If evasion is gated off (mapping workflows),
        # fall back to today's behaviour: emit zero, blocked=True.
        if not evasion_active:
            cmd["blocked"] = True
            cmd["motion"] = "blocked_no_evasion"
            return cmd

        # ── Holonomic evasion: sample candidates ──
        best = self._best_evasion_candidate(x, y, yaw, runtime, target)
        if best is not None:
            cmd["linear_mps"] = float(best["vx"])
            cmd["lateral_mps"] = float(best["vy"])
            cmd["angular_rps"] = float(best["vyaw"])
            cmd["motion"] = str(best["name"])
            # Robot is moving (any direction) → not stuck. Reset the full
            # recovery state — including the attempt counter — so a later
            # genuine stuck event gets a fresh quota of attempts. Matches
            # the contract documented on _reset_recovery_state.
            self._reset_recovery_state()
            return cmd

        # ── No candidate clear → track stuck time + maybe enter recovery ──
        if self._stuck_start_time is None:
            self._stuck_start_time = now
        stuck_duration = now - float(self._stuck_start_time)

        if stuck_duration >= float(self.cfg.stuck_trigger_seconds):
            if self._recovery_attempts < int(self.cfg.stuck_max_attempts):
                self._start_recovery(now, x, y, yaw, runtime)
                rec = self._tick_recovery(now, x, y, yaw, runtime)
                if rec is not None:
                    cmd.update(rec)
                    return cmd
            else:
                # Three attempts, still stuck — disarm.
                _log.warning(
                    "stuck recovery exhausted (%d attempts); disarming",
                    self._recovery_attempts,
                )
                cmd["blocked"] = True
                cmd["motion"] = "give_up"
                return cmd

        # Still inside the trigger window — emit zero, wait it out.
        cmd["blocked"] = True
        cmd["motion"] = "blocked"
        return cmd

    # ─────────────────────────────────────────────────────────────────
    #  Helpers: motion simulation + candidate scoring
    # ─────────────────────────────────────────────────────────────────
    def _is_motion_blocked(
        self,
        x: float, y: float, yaw: float,
        vx_body: float, vy_body: float, vyaw: float,
        runtime: LiveCostmapRuntime,
        horizon_s: float = 0.5,
        n_probe: int = 6,
    ) -> bool:
        """Simulate (vx_body, vy_body, vyaw) ahead through the costmap.

        Returns True if any probe step lands on a blocked cell. Treats
        vx_body/vy_body in the robot's body frame (so vy is "strafe left
        when positive", vx is "forward when positive"). Yaw is in world
        frame.
        """
        if runtime is None or getattr(runtime, "grid", None) is None:
            return False
        dt = float(max(0.05, horizon_s)) / float(max(1, n_probe))
        blocked = int(runtime.cfg.blocked_cost)
        lateral_r = float(runtime.cfg.inflation_radius_m) + 0.10
        # The cardinal-point check (footprint sample at ±lateral_r along
        # body axes) is needed whenever the robot's footprint could clip
        # a wall the centerline doesn't see. Two scenarios qualify:
        #  1. Near-stationary pivot (vyaw spinning the body in place)
        #  2. Real lateral motion (strafe) where the side of the robot
        #     leads into the inflation buffer ahead of the centerline.
        # Original code only fired in case 1, which let strafe candidates
        # at 0.2 m/s drive a hip into a wall the inflation kept clear of
        # the centerline. Fire when either condition holds.
        max_lin = max(0.1, float(self.cfg.max_linear_mps))
        max_lat = max(0.1, float(self.cfg.max_lateral_mps))
        near_pivot = (abs(vx_body) < 0.05 * max_lin
                      and abs(vy_body) < 0.05 * max_lat
                      and abs(vyaw) > 1e-3)
        strafing = abs(vy_body) > 0.20 * max_lat
        do_cardinal = near_pivot or strafing
        sx, sy, syaw = float(x), float(y), float(yaw)
        for _ in range(int(n_probe)):
            syaw += vyaw * dt
            # Body-frame velocities → world-frame displacements via 2D
            # rotation: world_x = vx*cos(yaw) - vy*sin(yaw).
            sx += (vx_body * math.cos(syaw) - vy_body * math.sin(syaw)) * dt
            sy += (vx_body * math.sin(syaw) + vy_body * math.cos(syaw)) * dt
            if runtime.cost_at_world(sx, sy) >= blocked:
                return True
            if do_cardinal:
                for ka_off in (0.0, 0.5 * math.pi, math.pi, 1.5 * math.pi):
                    ka = syaw + ka_off
                    if runtime.cost_at_world(
                        sx + lateral_r * math.cos(ka),
                        sy + lateral_r * math.sin(ka),
                    ) >= blocked:
                        return True
        return False

    def _best_evasion_candidate(
        self,
        x: float, y: float, yaw: float,
        runtime: LiveCostmapRuntime,
        target: Tuple[float, float],
    ) -> Optional[Dict[str, Any]]:
        """Sample five candidate body-frame motions, simulate each
        forward by `evasion_lookahead_s`, keep only those that don't hit
        a blocked cell, and pick the one that gets us closest to the
        path's lookahead target.

        Returns the winning candidate dict, or ``None`` if every option
        is blocked.
        """
        lookahead = float(self.cfg.evasion_lookahead_s)
        max_lin = float(self.cfg.max_linear_mps)
        max_lat = float(self.cfg.max_lateral_mps)
        max_bwd = float(self.cfg.max_backward_mps)
        max_ang = float(self.cfg.max_angular_rps)

        # Body-frame candidates. Forward gets a slight score bonus so it
        # wins ties against lateral motion (which looks "spooky" on a
        # humanoid and should be a fallback, not preferred).
        candidates: List[Dict[str, Any]] = [
            {"name": "forward",     "vx": +max_lin * 0.7, "vy":  0.0,           "vyaw": 0.0, "bias": 0.30},
            {"name": "strafe_left", "vx":  0.0,           "vy": +max_lat,       "vyaw": 0.0, "bias": 0.0},
            {"name": "strafe_right","vx":  0.0,           "vy": -max_lat,       "vyaw": 0.0, "bias": 0.0},
            {"name": "backward",    "vx": -max_bwd,       "vy":  0.0,           "vyaw": 0.0, "bias": -0.30},
            {"name": "rotate_left", "vx":  0.0,           "vy":  0.0,           "vyaw": +max_ang * 0.5, "bias": -0.20},
            {"name": "rotate_right","vx":  0.0,           "vy":  0.0,           "vyaw": -max_ang * 0.5, "bias": -0.20},
        ]

        best: Optional[Dict[str, Any]] = None
        best_score = -1e9
        tx, ty = float(target[0]), float(target[1])

        for c in candidates:
            blocked = self._is_motion_blocked(
                x, y, yaw, c["vx"], c["vy"], c["vyaw"], runtime,
                horizon_s=lookahead,
            )
            if blocked:
                continue
            # Simulate end pose to score by goal progress. Use same
            # body→world conversion as the probe.
            n = 4
            dt = lookahead / float(n)
            sx, sy, syaw = x, y, yaw
            for _ in range(n):
                syaw += c["vyaw"] * dt
                sx += (c["vx"] * math.cos(syaw) - c["vy"] * math.sin(syaw)) * dt
                sy += (c["vx"] * math.sin(syaw) + c["vy"] * math.cos(syaw)) * dt
            end_dist = math.hypot(tx - sx, ty - sy)
            start_dist = math.hypot(tx - x, ty - y)
            progress = start_dist - end_dist  # positive = closer to target
            score = progress + c["bias"]
            if score > best_score:
                best_score = score
                best = c
        return best

    # ─────────────────────────────────────────────────────────────────
    #  Stuck-recovery FSM
    # ─────────────────────────────────────────────────────────────────
    def _reset_recovery_state(self) -> None:
        """Called whenever the robot is making real progress (forward or
        successful evasion). Clears all stuck/recovery bookkeeping."""
        self._stuck_start_time = None
        self._recovery_phase = "none"
        self._recovery_phase_start = 0.0
        self._recovery_attempts = 0

    def _start_recovery(
        self, now: float, x: float, y: float, yaw: float,
        runtime: LiveCostmapRuntime,
    ) -> None:
        """Pick the rotate direction by sampling the costmap at ±90°
        relative to current heading, then enter BACKUP phase. Increments
        the attempt counter so the FSM can give up after N tries."""
        # Sample 1.0 m to the left and right; whichever side has a lower
        # cost wins (cheaper neighbourhood = more room to rotate into).
        try:
            r = 1.0
            cost_l = runtime.cost_at_world(
                x + r * math.cos(yaw + math.pi / 2),
                y + r * math.sin(yaw + math.pi / 2),
            )
            cost_r = runtime.cost_at_world(
                x + r * math.cos(yaw - math.pi / 2),
                y + r * math.sin(yaw - math.pi / 2),
            )
            self._recovery_rotate_dir = +1 if cost_l <= cost_r else -1
        except Exception:
            self._recovery_rotate_dir = +1
        self._recovery_phase = "backup"
        self._recovery_phase_start = float(now)
        self._recovery_attempts = int(self._recovery_attempts) + 1
        _log.info(
            "stuck recovery start (attempt %d/%d) — backup then rotate %s",
            self._recovery_attempts,
            int(self.cfg.stuck_max_attempts),
            "left" if self._recovery_rotate_dir > 0 else "right",
        )

    def _tick_recovery(
        self, now: float, x: float, y: float, yaw: float,
        runtime: LiveCostmapRuntime,
    ) -> Optional[Dict[str, Any]]:
        """Advance the FSM one tick. Returns the velocity dict to emit, or
        None when the FSM has finished its sequence (caller should retry
        normal nav).
        """
        if self._recovery_phase == "none":
            return None
        elapsed = now - float(self._recovery_phase_start)
        if self._recovery_phase == "backup":
            if elapsed < float(self.cfg.stuck_backup_seconds):
                # Only continue backing up if the path behind is clear.
                if self._is_motion_blocked(
                    x, y, yaw,
                    -float(self.cfg.max_backward_mps), 0.0, 0.0, runtime,
                    horizon_s=0.5,
                ):
                    # Even backward is blocked — skip straight to rotate.
                    self._recovery_phase = "rotate"
                    self._recovery_phase_start = float(now)
                    return {
                        "linear_mps": 0.0,
                        "lateral_mps": 0.0,
                        "angular_rps": float(self._recovery_rotate_dir)
                                       * float(self.cfg.max_angular_rps) * 0.5,
                        "blocked": False,
                        "motion": "recover_rotate",
                    }
                return {
                    "linear_mps": -float(self.cfg.max_backward_mps) * 0.8,
                    "lateral_mps": 0.0,
                    "angular_rps": 0.0,
                    "blocked": False,
                    "motion": "recover_backup",
                }
            # Backup window elapsed → rotate.
            self._recovery_phase = "rotate"
            self._recovery_phase_start = float(now)
            elapsed = 0.0
        if self._recovery_phase == "rotate":
            if elapsed < float(self.cfg.stuck_rotate_seconds):
                return {
                    "linear_mps": 0.0,
                    "lateral_mps": 0.0,
                    "angular_rps": float(self._recovery_rotate_dir)
                                   * float(self.cfg.max_angular_rps) * 0.5,
                    "blocked": False,
                    "motion": "recover_rotate",
                }
            # Sequence complete — exit FSM, retry normal navigation. The
            # attempt counter is preserved so subsequent failures trip
            # the give-up threshold.
            self._recovery_phase = "none"
            self._recovery_phase_start = 0.0
            self._stuck_start_time = None
            _log.info("stuck recovery sequence complete; retrying navigation")
            return None
        return None
