Sanad_Package_3/vendor/Sanad/dashboard/routes/controller.py

"""Controller tab — manual dashboard locomotion control (N2 Phase 1/2).

Routes live under /api/controller. All WRITE actions (move / step / postures /
modes / MotionSwitcher) require the in-memory "Enable movement" arm flag and
return 409 when disarmed. Reads (/status, /joints, /msc, /status/summary),
E-STOP and the arm toggle are ALWAYS available.

`/status/summary` is the aggregate the dashboard polls for the global subsystem
status strip (Camera / Face / Place / Movement). It is kept under /api/controller
(final path /api/controller/status/summary) so no second router is needed; note
/api/status (no /summary) is already used by the SPA, so the suffix matters.
"""

from __future__ import annotations

import asyncio

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

from Project.Sanad.config import BASE_DIR
from Project.Sanad.core.logger import get_logger
from Project.Sanad.vision import recognition_state

from Project.Sanad.dashboard.routes import _arbiter

log = get_logger("controller_routes")

router = APIRouter()

STATE_PATH = BASE_DIR / "data" / ".recognition_state.json"


# ── lazy subsystem accessors ────────────────────────────────

def _get_loco():
    try:
        from Project.Sanad.main import loco_controller  # type: ignore
        return loco_controller
    except Exception:
        return None


def _get_camera():
    try:
        from Project.Sanad.main import camera  # type: ignore
        return camera
    except Exception:
        return None


def _get_live_sub():
    try:
        from Project.Sanad.main import live_sub  # type: ignore
        return live_sub
    except Exception:
        return None


def _get_dispatch():
    try:
        from Project.Sanad.main import movement_dispatch  # type: ignore
        return movement_dispatch
    except Exception:
        return None


def _require_loco():
    lc = _get_loco()
    if lc is None:
        raise HTTPException(503, "Locomotion controller subsystem unavailable.")
    return lc


def _require_armed(lc):
    if not lc.is_armed():
        raise HTTPException(409, "Movement is disarmed. Enable movement first.")


def _claim_loco():
    """Arbitration gate: refuse a leg command while a Nav2 goal owns the legs."""
    if not _arbiter.acquire_loco():
        raise HTTPException(
            409, "Navigation (Nav2) is active. Cancel the nav goal before manual movement."
        )


def _cancel_nav():
    """Cancel any in-flight Nav2 goal and clear the nav arbitration flag.

    Used by E-STOP so the global stop halts the legs no matter which stack is
    driving them. Calls the nav client in-process (no HTTP self-call); never
    raises into the caller.
    """
    try:
        from Project.Sanad.dashboard.routes.navigation import _CLIENT as _nav_client
        if _nav_client is not None:
            _nav_client.cancel()
    except Exception:
        log.exception("estop nav cancel failed")
    finally:
        _arbiter.release_nav()


# ── reads ───────────────────────────────────────────────────

@router.get("/status")
async def get_status():
    lc = _require_loco()
    return await asyncio.to_thread(lc.status)


@router.get("/joints")
async def get_joints():
    lc = _require_loco()
    return await asyncio.to_thread(lc.joints)


@router.get("/msc")
async def get_msc():
    lc = _require_loco()
    return await asyncio.to_thread(lc.msc_check)


# ── arm flag / E-STOP (always available) ────────────────────

@router.post("/arm")
async def set_arm(on: bool = Query(...)):
    lc = _require_loco()
    if on:
        # Refuse to arm manual loco while Nav2 owns the legs.
        _claim_loco()
        try:
            res = await asyncio.to_thread(lc.arm_movement)
        except Exception:
            _arbiter.release_loco()
            raise
        return res
    res = await asyncio.to_thread(lc.disarm_movement)
    _arbiter.release_loco()
    return res


@router.post("/gemini-movement")
async def set_gemini_movement(on: bool = Query(...)):
    """Enable / disable Gemini voice-driven locomotion (N2 Phase 3 gate).

    Writes recognition_state.movement_enabled — SEPARATE from the manual arm
    flag. The Gemini child announces the toggle (spoken), and the parent
    MovementDispatcher starts/stops acting on confirmation phrases. Default OFF.
    """
    st = await asyncio.to_thread(recognition_state.mutate, STATE_PATH,
                                 movement_enabled=bool(on))
    # Enabling Gemini movement also clears any E-STOP latch on the dispatcher.
    if on:
        md = _get_dispatch()
        if md is not None:
            try:
                md.clear_estop()
            except Exception:
                log.exception("clear_estop failed")
    log.info("gemini-movement %s", "ON" if on else "OFF")
    return {"ok": True, "movement_enabled": st.movement_enabled}


@router.post("/estop")
async def estop():
    lc = _require_loco()
    res = await asyncio.to_thread(lc.estop)
    # Full stop: drop the manual arm flag AND latch the voice dispatcher off, so
    # no source (teleop, step, or voice dispatch) can keep driving the robot. The
    # dispatcher latch is used instead of flipping movement_enabled so the Gemini
    # child does not deliver a spoken "movement disabled" line during an E-STOP.
    try:
        await asyncio.to_thread(lc.disarm_movement)
    except Exception:
        log.exception("estop disarm failed")
    md = _get_dispatch()
    if md is not None:
        try:
            md.emergency_stop()
        except Exception:
            log.exception("estop dispatcher latch failed")
    # Cancel any in-flight Nav2 goal too: the legs have exactly one commander,
    # and an E-STOP must halt the legs whether loco or Nav2 is driving them.
    await asyncio.to_thread(_cancel_nav)
    _arbiter.release_loco()
    return {"ok": True, **res}


@router.post("/stop")
async def stop():
    lc = _require_loco()
    # Allowed even when disarmed — StopMove is always safe.
    res = await asyncio.to_thread(lc.stop_move)
    return res


# ── movement (armed) ────────────────────────────────────────

class MoveBody(BaseModel):
    vx: float = 0.0
    vy: float = 0.0
    vyaw: float = 0.0
    run: bool = False


@router.post("/move")
async def move(body: MoveBody):
    lc = _require_loco()
    _require_armed(lc)
    _claim_loco()
    return await asyncio.to_thread(lc.move, body.vx, body.vy, body.vyaw, body.run)


@router.post("/step")
async def step(dir: str = Query(...)):
    lc = _require_loco()
    _require_armed(lc)
    _claim_loco()
    res = await asyncio.to_thread(lc.step, dir)
    if not res.get("ok"):
        raise HTTPException(400, res.get("reason", "step failed"))
    return res


# ── modes / postures (armed) ────────────────────────────────

@router.post("/mode/prep")
async def mode_prep():
    lc = _require_loco()
    _require_armed(lc)
    return await asyncio.to_thread(lc.prep_mode)


@router.post("/mode/ready")
async def mode_ready():
    lc = _require_loco()
    _require_armed(lc)
    return await asyncio.to_thread(lc.ready_start_mode)


@router.post("/posture/{name}")
async def posture(name: str):
    lc = _require_loco()
    _require_armed(lc)
    res = await asyncio.to_thread(lc.posture, name)
    if not res.get("ok") and res.get("reason"):
        raise HTTPException(400, res["reason"])
    return res


@router.post("/balance")
async def balance(mode: int = Query(...)):
    lc = _require_loco()
    _require_armed(lc)
    return await asyncio.to_thread(lc.set_balance_mode, mode)


@router.post("/height")
async def height(h: float = Query(...)):
    lc = _require_loco()
    _require_armed(lc)
    return await asyncio.to_thread(lc.set_stand_height, h)


# ── MotionSwitcher / reconnect (armed) ──────────────────────

@router.post("/msc/select-ai")
async def msc_select_ai():
    lc = _require_loco()
    _require_armed(lc)
    return await asyncio.to_thread(lc.msc_select_ai)


@router.post("/msc/release")
async def msc_release():
    lc = _require_loco()
    _require_armed(lc)
    return await asyncio.to_thread(lc.msc_release)


@router.post("/reconnect")
async def reconnect():
    lc = _require_loco()
    _require_armed(lc)
    return await asyncio.to_thread(lc.reconnect)


# ── aggregate subsystem summary (always available) ──────────

@router.get("/status/summary")
async def status_summary():
    """Live on/off state for the header status strip. Never raises."""
    try:
        st = recognition_state.read(STATE_PATH)
    except Exception:
        st = recognition_state.RecognitionState()

    cam = _get_camera()
    camera_running = False
    try:
        camera_running = bool(cam is not None and cam.is_running())
    except Exception:
        camera_running = False

    lc = _get_loco()
    movement_armed = False
    try:
        movement_armed = bool(lc is not None and lc.is_armed())
    except Exception:
        movement_armed = False

    sub = _get_live_sub()
    gemini_running = False
    try:
        runner = getattr(sub, "is_running", None)
        gemini_running = bool(callable(runner) and runner())
    except Exception:
        gemini_running = False

    # Effective Gemini-movement = the file flag AND not latched off by an E-STOP.
    md = _get_dispatch()
    estopped = False
    try:
        estopped = bool(md is not None and md.is_estopped())
    except Exception:
        estopped = False

    return {
        "vision_enabled": st.vision_enabled,
        "camera_running": camera_running,
        "face_rec_enabled": st.face_rec_enabled,
        "zone_rec_enabled": st.zone_rec_enabled,
        "movement_armed": movement_armed,
        "gemini_movement_enabled": st.movement_enabled and not estopped,
        "gemini_running": gemini_running,
    }