Sanadv3/dashboard/routes/navigation.py

"""Navigation tab — proxy to the web_nav3 Nav2 stack.

Routes live under /api/nav (the prefix is applied centrally in dashboard/app.py,
NOT here). This router is a thin HTTP proxy: it forwards dashboard requests to a
single module-level WebNav3Client, which itself talks to the standalone web_nav3
FastAPI service (default http://127.0.0.1:8765 + rosbridge on :9090).

Fault isolation, two layers:
  1. The `from ...navigation import WebNav3Client` import is GUARDED. If the
     navigation package can't be imported (missing dep, syntax error), this
     module still imports cleanly — `_CLIENT` is None and every handler degrades
     (GET /status returns {"available": False}; actions raise 503). This mirrors
     how app.py loads each router in isolation.
  2. WebNav3Client never raises into us by contract — every method returns a
     clean dict / NavStatus even when web_nav3 is unreachable — so handlers just
     forward the result. Blocking HTTP calls run off the event loop.
"""

from __future__ import annotations

import asyncio

from fastapi import APIRouter, HTTPException, Query
from pydantic import BaseModel

from Project.Sanad.core.logger import get_logger

from Project.Sanad.dashboard.routes import _arbiter

log = get_logger("navigation_route")

# Module-level router with NO prefix and NO tags — those are supplied by
# app.include_router(prefix="/api/nav", tags=["navigation"]) at registration time.
router = APIRouter()


# ── guarded optional import ─────────────────────────────────
# A broken navigation package must NOT stop this route module from importing —
# app.py would otherwise log the whole router as failed. Guard it and degrade.
try:
    from Project.Sanad.navigation import WebNav3Client  # type: ignore
    _IMPORT_ERROR: str | None = None
except Exception as exc:  # noqa: BLE001
    WebNav3Client = None  # type: ignore[assignment,misc]
    _IMPORT_ERROR = f"{type(exc).__name__}: {exc}"
    log.warning("navigation client unavailable — nav routes degraded: %s", _IMPORT_ERROR)


# ── config (env var -> dashboard config section -> default) ──
def _nav_config() -> dict:
    """Resolve nav connection config. Precedence: env var -> config -> default."""
    import os

    from Project.Sanad.core.config_loader import section as _cfg_section

    cfg = _cfg_section("dashboard", "navigation")
    web_nav3_url = (
        os.environ.get("WEB_NAV3_URL")
        or cfg.get("web_nav3_url")
        or "http://127.0.0.1:8765"
    )
    rosbridge_url = (
        os.environ.get("ROSBRIDGE_URL")
        or cfg.get("rosbridge_url")
        or "ws://127.0.0.1:9090"
    )
    robot = os.environ.get("SANAD_ROBOT_NAME") or cfg.get("robot") or "sanad"
    return {
        "web_nav3_url": str(web_nav3_url),
        "rosbridge_url": str(rosbridge_url),
        "robot": str(robot),
    }


_CFG = _nav_config()

# ── single module-level client ──────────────────────────────
# One WebNav3Client for the whole dashboard, built from config. If the import
# was guarded out (above), or construction fails, _CLIENT stays None and every
# handler degrades gracefully.
if WebNav3Client is not None:
    try:
        _CLIENT = WebNav3Client(base_url=_CFG["web_nav3_url"], robot=_CFG["robot"])
        log.info("WebNav3Client ready → %s (robot=%s)", _CFG["web_nav3_url"], _CFG["robot"])
    except Exception as exc:  # noqa: BLE001
        _CLIENT = None
        _IMPORT_ERROR = f"construct failed: {type(exc).__name__}: {exc}"
        log.warning("WebNav3Client construction failed — nav routes degraded: %s", exc)
else:
    _CLIENT = None


def _require():
    """Return the live client or raise 503 (for ACTION endpoints)."""
    if _CLIENT is None:
        raise HTTPException(503, f"Navigation client unavailable. {_IMPORT_ERROR or ''}".strip())
    return _CLIENT


def _claim_nav():
    """Arbitration gate: refuse to start a Nav2 goal while manual loco owns legs."""
    if not _arbiter.acquire_nav():
        raise HTTPException(
            409, "Manual movement (Controller) is armed. Disarm it before navigating."
        )


# ── request bodies ──────────────────────────────────────────
class _NameBody(BaseModel):
    name: str


class _IdBody(BaseModel):
    id: object  # mission ids may be int or str; forward as-is


class _StartBody(BaseModel):
    mode: int = 2  # web_nav3 launch mode (e.g. 3 = localize against a saved map)
    db_path: str | None = None  # saved map to load (None = build fresh)


class _PoseBody(BaseModel):
    name: str
    x: float
    y: float
    yaw: float = 0.0


class _RenameBody(BaseModel):
    old: str
    new: str


# ── status (never raises — degraded body when unavailable) ──
@router.get("/status")
async def status():
    if _CLIENT is None:
        return {"available": False, "error": _IMPORT_ERROR}
    nav = await asyncio.to_thread(_CLIENT.status)
    # WebNav3Client.status() returns a NavStatus dataclass.
    body = nav.as_dict() if hasattr(nav, "as_dict") else dict(nav)
    body["available"] = True
    return body


# ── places / navigation ─────────────────────────────────────
@router.get("/places")
async def places(map_name: str | None = Query(None, alias="map")):
    """List saved places. Per-MAP when ?map=<name> is given (each map keeps
    its own places); else the legacy per-robot store."""
    client = _require()
    return await asyncio.to_thread(client.list_places, map_name)


@router.post("/goto")
async def goto(body: _NameBody):
    client = _require()
    _claim_nav()
    res = await asyncio.to_thread(client.goto, body.name)
    # A failed dispatch never drove the legs — release the gate so manual loco
    # isn't locked out by a goto that never started.
    if isinstance(res, dict) and not res.get("ok", True):
        _arbiter.release_nav()
    return res


@router.post("/start")
async def start(body: _StartBody):
    client = _require()
    return await asyncio.to_thread(client.start, body.mode, body.db_path)


class _DbBody(BaseModel):
    db_path: str


@router.post("/load_map")
async def load_map(body: _DbBody):
    """View a saved map: stop any running bringup, then localize against it."""
    client = _require()
    return await asyncio.to_thread(client.load_map, body.db_path)


@router.post("/cancel")
async def cancel():
    client = _require()
    res = await asyncio.to_thread(client.cancel)
    # WebNav3Client.cancel() is a no-op server-side (it only returns a note),
    # so releasing the arbiter without truly stopping Nav2 would let the robot
    # keep driving while manual loco re-acquires the legs (double-drive). Send a
    # REAL goal-cancel over rosbridge first, and disarm the arrival monitor so a
    # stale terminal can't fire, THEN release.
    try:
        from Project.Sanad.navigation.goal_monitor import request_cancel, disarm
        disarm()
        cancelled = await asyncio.to_thread(request_cancel)
        if isinstance(res, dict):
            res = {**res, "cancel_sent": bool(cancelled)}
    except Exception as exc:  # noqa: BLE001
        log.debug("goal cancel skipped: %s", exc)
    _arbiter.release_nav()
    return res


@router.post("/save_here")
async def save_here(body: _NameBody):
    client = _require()
    return await asyncio.to_thread(client.save_here, body.name)


@router.post("/save_at")
async def save_at(body: _PoseBody, map_name: str | None = Query(None, alias="map")):
    """Save a named place at a map coordinate (from clicking the map). Per-MAP
    when ?map=<name> given. Re-saving an existing name MOVES the place."""
    client = _require()
    return await asyncio.to_thread(client.save_at, body.name, body.x, body.y, body.yaw, map_name)


@router.post("/places/delete")
async def delete_place(body: _NameBody, map_name: str | None = Query(None, alias="map")):
    """Delete a saved place (per-map)."""
    client = _require()
    return await asyncio.to_thread(client.delete_place, body.name, map_name)


@router.post("/places/rename")
async def rename_place(body: _RenameBody, map_name: str | None = Query(None, alias="map")):
    """Rename a saved place (per-map)."""
    client = _require()
    return await asyncio.to_thread(client.rename_place, body.old, body.new, map_name)


class _MapEditsBody(BaseModel):
    edits: list  # [[world_x, world_y, value], ...]  value 0=free/erase, 100=wall


@router.get("/map_edits")
async def get_map_edits(map_name: str = Query(..., alias="map")):
    """Saved edit overlay for a map (erased points + painted walls)."""
    client = _require()
    return await asyncio.to_thread(client.get_map_edits, map_name)


@router.post("/map_edits")
async def save_map_edits(body: _MapEditsBody, map_name: str = Query(..., alias="map")):
    """Persist a map's edit overlay (Map Editor)."""
    client = _require()
    return await asyncio.to_thread(client.save_map_edits, map_name, body.edits)


class _VoiceGotoBody(BaseModel):
    place: str


def _resolve_place(client, spoken: str) -> dict:
    """Resolve a spoken place name against the ACTIVE map's places.

    Strategy: exact (case-insensitive) → single substring candidate →
    ambiguous / unknown. Returns a dict the caller (and ultimately Gemini)
    can act on. Never raises.
    """
    try:
        st = client.status()
        body = st.as_dict() if hasattr(st, "as_dict") else dict(st)
    except Exception as exc:  # noqa: BLE001
        return {"ok": False, "reason": "status_error", "detail": str(exc)[:160]}
    if not body.get("bringup_alive"):
        return {"ok": False, "reason": "no_map",
                "detail": "No navigation session is running — load a map first."}
    active_map = body.get("active_map")
    try:
        places = client.list_places(active_map) or []
    except Exception:  # noqa: BLE001
        places = []
    names = [p.get("name") for p in places if isinstance(p, dict) and p.get("name")]
    sl = (spoken or "").strip().lower()
    if not sl:
        return {"ok": False, "reason": "no_place", "map": active_map, "places": names}
    exact = [n for n in names if n.lower() == sl]
    if exact:
        return {"ok": True, "resolved": exact[0], "map": active_map}
    subs = []
    for n in names:
        nl = n.lower()
        if sl in nl or nl in sl:
            subs.append(n)
    subs = list(dict.fromkeys(subs))   # de-dup, preserve order
    if len(subs) == 1:
        return {"ok": True, "resolved": subs[0], "map": active_map}
    if len(subs) > 1:
        return {"ok": False, "reason": "ambiguous", "candidates": subs, "map": active_map}
    return {"ok": False, "reason": "unknown_place", "candidates": names, "map": active_map}


@router.get("/active")
async def active():
    """Navigation context for Gemini: the active map, its mode, and that map's
    place names — one call so the voice tools (list_places / where_am_i) don't
    have to guess the active map."""
    client = _require()
    st = await asyncio.to_thread(client.status)
    body = st.as_dict() if hasattr(st, "as_dict") else dict(st)
    places = []
    if body.get("bringup_alive"):
        try:
            pl = await asyncio.to_thread(client.list_places, body.get("active_map"))
            places = [p.get("name") for p in (pl or [])
                      if isinstance(p, dict) and p.get("name")]
        except Exception:  # noqa: BLE001
            places = []
    return {
        "map": body.get("active_map"),
        "mode": body.get("mode"),
        "mode_label": body.get("mode_label"),
        "localizing": bool(body.get("localizing")),
        "bringup_alive": bool(body.get("bringup_alive")),
        "places": places,
    }


@router.post("/voice_goto")
async def voice_goto(body: _VoiceGotoBody):
    """Resolve a spoken place name and drive there — Gemini's navigate_to_place.

    Arbiter-gated (claims the legs for Nav2) and arms the arrival monitor so
    Gemini later hears [NAV ARRIVED]/[NAV FAILED]. Never raises into the caller;
    returns a structured result the model can speak from.
    """
    client = _require()
    res = await asyncio.to_thread(_resolve_place, client, body.place or "")
    if not res.get("ok"):
        return res
    # Claim the legs for Nav2 — refuse (don't raise) if manual loco is armed.
    if not _arbiter.acquire_nav():
        return {"ok": False, "reason": "manual_armed",
                "detail": "Manual movement (Controller) is armed — disarm it to navigate."}
    drive = await asyncio.to_thread(client.goto, res["resolved"])
    if isinstance(drive, dict) and not drive.get("ok", True):
        _arbiter.release_nav()
        return {"ok": False, "reason": "dispatch_failed",
                "resolved": res["resolved"], "detail": drive}
    # Arm arrival monitoring (best-effort; absence must not fail the drive).
    try:
        from Project.Sanad.navigation.goal_monitor import arm_goal
        arm_goal(res["resolved"])
    except Exception as exc:  # noqa: BLE001
        log.debug("goal monitor arm skipped: %s", exc)
    return {"ok": True, "resolved": res["resolved"], "map": res.get("map")}


@router.post("/goto_pose")
async def goto_pose(body: _PoseBody):
    """Arbiter-gate a coordinate nav goal (click-to-drive).

    The browser publishes the actual /goal_pose over rosbridge; this only
    CLAIMS the legs for Nav2 (409 if manual loco is armed) so the two stacks
    never both drive. The frontend sends the goal only after this returns ok.
    """
    _require()
    _claim_nav()
    # Arm the arrival monitor so this click-to-drive goal releases the arbiter
    # when it ends — without this, nav_active stays True forever after the goal
    # completes (the browser publishes the goal but never arms anything).
    try:
        from Project.Sanad.navigation.goal_monitor import arm_goal
        arm_goal(f"({body.x:.1f}, {body.y:.1f})")
    except Exception as exc:  # noqa: BLE001
        log.debug("goal monitor arm skipped: %s", exc)
    return {"ok": True, "x": body.x, "y": body.y, "yaw": body.yaw}


# ── maps / missions ─────────────────────────────────────────
@router.get("/maps")
async def maps():
    client = _require()
    return await asyncio.to_thread(client.list_maps)


@router.get("/missions")
async def missions():
    client = _require()
    return await asyncio.to_thread(client.list_missions)


@router.post("/missions/run")
async def run_mission(body: _IdBody):
    client = _require()
    _claim_nav()
    res = await asyncio.to_thread(client.run_mission, body.id)
    if isinstance(res, dict) and not res.get("ok", True):
        _arbiter.release_nav()
    return res


# ── config (what the SPA needs to render links / connect) ───
@router.get("/config")
async def config():
    return {
        "web_nav3_url": _CFG["web_nav3_url"],
        "rosbridge_url": _CFG["rosbridge_url"],
        "robot": _CFG["robot"],
    }