api_server.py

#!/usr/bin/env python3
"""
Unitree G1 Robot API Server — v2

A production-grade FastAPI server for controlling the Unitree G1 humanoid
robot from a Raspberry Pi. Features:

- Real serial I/O with auto-detection and reconnection
- Velocity commands with safety ramping and watchdog
- Command sequencing / macro system
- WebSocket real-time telemetry streaming
- Documented state-transition actions via controller button combos
- Structured event logging
- YAML / env-var configuration
"""

from __future__ import annotations

import asyncio
import json as _json
import logging
import math
import time
import uuid
from contextlib import asynccontextmanager
from enum import Enum
from pathlib import Path
from typing import Any, Optional

import serial
import serial.tools.list_ports
from fastapi import FastAPI, HTTPException, WebSocket, WebSocketDisconnect, Query
from fastapi.middleware.cors import CORSMiddleware
from fastapi.responses import HTMLResponse
from pydantic import BaseModel, Field

from config import AppConfig, load_config
from controller_protocol import ControllerButtons, ControllerState
from sequence_library import SequenceLibrary
from telemetry import EventLog, EventType

# ---------------------------------------------------------------------------
# Logging
# ---------------------------------------------------------------------------
logging.basicConfig(
    level=logging.INFO,
    format="%(asctime)s [%(levelname)s] %(name)s: %(message)s",
)
logger = logging.getLogger("g1api")

# ---------------------------------------------------------------------------
# Pydantic models
# ---------------------------------------------------------------------------


class RobotMode(str, Enum):
    IDLE = "idle"
    DAMPING = "damping"
    STAND = "stand"
    WALKING = "walking"
    RUNNING = "running"


class MoveCommand(BaseModel):
    """Velocity command for robot movement."""

    vx: float = Field(0.0, description="Forward velocity m/s (neg = backward)")
    vy: float = Field(0.0, description="Lateral velocity m/s (pos = left)")
    vyaw: float = Field(0.0, description="Yaw rate rad/s (pos = CCW)")
    duration: Optional[float] = Field(
        None, ge=0.0, description="Auto-stop after N seconds"
    )


class SequenceStep(BaseModel):
    """One step in a command sequence."""

    action: str = Field(..., description="Action type: move, action, wait")
    params: dict = Field(default_factory=dict)
    duration: float = Field(
        1.0, ge=0.0, description="How long this step runs (seconds)"
    )


class SequenceCommand(BaseModel):
    """A sequence of steps to execute in order."""

    name: str = Field("unnamed", description="Sequence name for logging")
    steps: list[SequenceStep] = Field(..., min_length=1)
    loop: bool = Field(False, description="Loop the sequence continuously")


class NavigateCommand(BaseModel):
    """Navigate to a position (requires vision/SLAM)."""

    x: float = Field(..., description="Target X position (meters)")
    y: float = Field(..., description="Target Y position (meters)")
    heading: Optional[float] = Field(None, description="Target heading (radians)")


class SpeedModeCommand(BaseModel):
    """Switch between walk and run modes."""

    mode: str = Field("walk", description="Speed mode: walk or run")


class RobotStatus(BaseModel):
    mode: RobotMode = RobotMode.IDLE
    speed_mode: str = "walk"
    battery_percent: int = 0
    controller_connected: bool = False
    serial_connected: bool = False
    api_control_active: bool = False
    current_velocity: dict = {"vx": 0.0, "vy": 0.0, "vyaw": 0.0}
    target_velocity: dict = {"vx": 0.0, "vy": 0.0, "vyaw": 0.0}
    uptime_seconds: float = 0.0
    active_sequence: Optional[str] = None
    safety: dict = {"watchdog_ok": True, "ramping": False}


class ActionResponse(BaseModel):
    success: bool
    message: str
    action: str
    id: Optional[str] = None


# ---------------------------------------------------------------------------
# Available actions — documented button combos from official G1 Quick Start
# Source: https://support.unitree.com/home/en/G1_developer/quick_start
#
# These are verified state-transition combos for the G1 Basic (sitting boot,
# 1-DOF waist).  Gesture actions (wave, bow, shake hand, etc.) require the
# DDS LocoClient / ArmActionClient network API and are NOT available via
# controller packet injection.
# ---------------------------------------------------------------------------
ACTION_MAP: dict[str, int] = {
    "damping": ControllerButtons.L1 | ControllerButtons.A,
    "stand_up": ControllerButtons.L1 | ControllerButtons.UP,
    "sit_down": ControllerButtons.L1 | ControllerButtons.LEFT,
    "start_control": ControllerButtons.R1 | ControllerButtons.X,
    "toggle_walk": ControllerButtons.START,
    "emergency_stop": ControllerButtons.L2 | ControllerButtons.B,
    "debug_mode": ControllerButtons.L2 | ControllerButtons.R2,
}


# ---------------------------------------------------------------------------
# Safety Layer
# ---------------------------------------------------------------------------


class SafetyManager:
    """
    Enforces velocity limits, ramping, and watchdog timeout.

    - Clamps velocities to configured maximums
    - Ramps velocity changes to limit acceleration
    - Stops robot if no command received within watchdog timeout
    """

    def __init__(self, config: AppConfig, event_log: EventLog):
        self.cfg = config.safety
        self._log = event_log
        self._last_cmd_time: float = time.time()
        self._current_vx: float = 0.0
        self._current_vy: float = 0.0
        self._current_vyaw: float = 0.0
        self._last_tick: float = time.time()
        self.watchdog_tripped = False

    def clamp(self, vx: float, vy: float, vyaw: float) -> tuple[float, float, float]:
        """Clamp velocities to safety limits."""
        orig = (vx, vy, vyaw)
        vx = max(-self.cfg.max_vx, min(self.cfg.max_vx, vx))
        vy = max(-self.cfg.max_vy, min(self.cfg.max_vy, vy))
        vyaw = max(-self.cfg.max_vyaw, min(self.cfg.max_vyaw, vyaw))
        if (vx, vy, vyaw) != orig:
            self._log.record(
                EventType.VELOCITY_CLAMPED,
                {
                    "original": {"vx": orig[0], "vy": orig[1], "vyaw": orig[2]},
                    "clamped": {"vx": vx, "vy": vy, "vyaw": vyaw},
                },
                source="safety",
            )
        return vx, vy, vyaw

    def ramp(
        self, target_vx: float, target_vy: float, target_vyaw: float
    ) -> tuple[float, float, float]:
        """Apply acceleration ramping to smooth velocity changes."""
        if not self.cfg.enable_ramp:
            self._current_vx = target_vx
            self._current_vy = target_vy
            self._current_vyaw = target_vyaw
            return target_vx, target_vy, target_vyaw

        now = time.time()
        dt = min(now - self._last_tick, 0.1)  # cap to avoid jumps
        self._last_tick = now
        max_delta = self.cfg.velocity_ramp_rate * dt

        def _approach(current: float, target: float) -> float:
            diff = target - current
            if abs(diff) <= max_delta:
                return target
            return current + math.copysign(max_delta, diff)

        self._current_vx = _approach(self._current_vx, target_vx)
        self._current_vy = _approach(self._current_vy, target_vy)
        self._current_vyaw = _approach(self._current_vyaw, target_vyaw)
        return self._current_vx, self._current_vy, self._current_vyaw

    def feed_command(self):
        """Call on every new command to reset the watchdog."""
        self._last_cmd_time = time.time()
        self.watchdog_tripped = False

    def check_watchdog(self) -> bool:
        """Returns True if watchdog is OK, False if timed out."""
        if not self.cfg.enable_watchdog:
            return True
        elapsed = time.time() - self._last_cmd_time
        if elapsed > self.cfg.watchdog_timeout:
            if not self.watchdog_tripped:
                self.watchdog_tripped = True
                self._log.record(
                    EventType.WATCHDOG_TIMEOUT,
                    {
                        "elapsed": round(elapsed, 2),
                        "timeout": self.cfg.watchdog_timeout,
                    },
                    source="safety",
                )
                logger.warning("Watchdog timeout after %.1fs — stopping", elapsed)
            return False
        return True

    def reset(self):
        """Reset ramp state to zero."""
        self._current_vx = 0.0
        self._current_vy = 0.0
        self._current_vyaw = 0.0
        self._last_tick = time.time()

    @property
    def is_ramping(self) -> bool:
        return self.cfg.enable_ramp


# ---------------------------------------------------------------------------
# Serial Manager — handles real serial I/O with auto-reconnect
# ---------------------------------------------------------------------------


class SerialManager:
    """
    Manages serial connections to the controller receiver and robot MCU.

    Features:
    - Auto-detection of CP210x USB receiver
    - Non-blocking reads via asyncio
    - Automatic reconnection on disconnect
    - Simulation mode for development without hardware
    """

    def __init__(self, config: AppConfig, event_log: EventLog):
        self.cfg = config.serial
        self.simulation = config.simulation
        self._log = event_log
        self._receiver: Optional[serial.Serial] = None
        self._robot: Optional[serial.Serial] = None
        self._read_buffer = bytearray()

    @property
    def receiver_connected(self) -> bool:
        return self._receiver is not None and self._receiver.is_open

    @property
    def robot_connected(self) -> bool:
        return self._robot is not None and self._robot.is_open

    def auto_detect_port(self) -> Optional[str]:
        """Find CP210x controller receiver."""
        for port in serial.tools.list_ports.comports():
            desc = port.description or ""
            if "CP210" in desc or (port.vid == 0x10C4 and port.pid == 0xEA60):
                logger.info("Auto-detected controller receiver at %s", port.device)
                return port.device
        return None

    def open_receiver(self) -> bool:
        """Open the controller receiver serial port."""
        if self.simulation:
            logger.info("Simulation mode — skipping receiver serial open")
            return True

        port = self.cfg.receiver_port
        if self.cfg.auto_detect:
            detected = self.auto_detect_port()
            if detected:
                port = detected

        try:
            self._receiver = serial.Serial(
                port,
                self.cfg.baud_rate,
                timeout=self.cfg.timeout,
                bytesize=serial.EIGHTBITS,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
            )
            self._receiver.reset_input_buffer()
            self._log.record(EventType.SERIAL_OPEN, {"port": port, "role": "receiver"})
            logger.info("Receiver serial opened: %s @ %d", port, self.cfg.baud_rate)
            return True
        except serial.SerialException as e:
            logger.error("Cannot open receiver serial %s: %s", port, e)
            return False

    def open_robot(self) -> bool:
        """Open the robot MCU serial port."""
        if self.simulation:
            logger.info("Simulation mode — skipping robot serial open")
            return True

        try:
            self._robot = serial.Serial(
                self.cfg.robot_port,
                self.cfg.baud_rate,
                timeout=self.cfg.timeout,
                bytesize=serial.EIGHTBITS,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
            )
            self._robot.reset_input_buffer()
            self._log.record(
                EventType.SERIAL_OPEN, {"port": self.cfg.robot_port, "role": "robot"}
            )
            logger.info(
                "Robot serial opened: %s @ %d", self.cfg.robot_port, self.cfg.baud_rate
            )
            return True
        except serial.SerialException as e:
            logger.warning("Cannot open robot serial %s: %s", self.cfg.robot_port, e)
            return False

    def read_controller_packet(self) -> Optional[ControllerState]:
        """
        Non-blocking read of a 40-byte controller packet.
        Returns None if no complete packet available.
        """
        if self.simulation or not self.receiver_connected:
            return None

        try:
            waiting = self._receiver.in_waiting
            if waiting > 0:
                self._read_buffer.extend(self._receiver.read(min(waiting, 1024)))
        except (serial.SerialException, OSError):
            logger.warning("Receiver serial read error — will reconnect")
            self._close_port(self._receiver)
            self._receiver = None
            return None

        # Search for valid 40-byte packet with known header
        HEADERS = [b"\xfe\xef", b"\xfe\xfe", b"\xaa\x55", b"\x55\xaa"]
        for hdr in HEADERS:
            idx = self._read_buffer.find(hdr)
            if idx >= 0 and idx + 40 <= len(self._read_buffer):
                packet = bytes(self._read_buffer[idx : idx + 40])
                # Discard everything up to and including this packet
                del self._read_buffer[: idx + 40]
                try:
                    return ControllerState.from_bytes(packet)
                except Exception:
                    pass

        # Prevent buffer from growing unbounded
        if len(self._read_buffer) > 4096:
            self._read_buffer = self._read_buffer[-256:]

        return None

    def send_to_robot(self, state: ControllerState) -> bool:
        """Send a controller state packet to the robot MCU."""
        if self.simulation:
            return True

        if not self.robot_connected:
            return False

        try:
            self._robot.write(state.to_bytes())
            return True
        except (serial.SerialException, OSError):
            logger.warning("Robot serial write error — will reconnect")
            self._close_port(self._robot)
            self._robot = None
            return False

    def try_reconnect(self):
        """Attempt to reconnect any disconnected ports."""
        if not self.simulation:
            if not self.receiver_connected:
                self.open_receiver()
            if not self.robot_connected:
                self.open_robot()

    def close(self):
        """Close all serial ports."""
        self._close_port(self._receiver)
        self._close_port(self._robot)
        self._receiver = None
        self._robot = None
        self._log.record(EventType.SERIAL_CLOSE, {})

    @staticmethod
    def _close_port(port: Optional[serial.Serial]):
        if port and port.is_open:
            try:
                port.close()
            except Exception:
                pass


# ---------------------------------------------------------------------------
# Sequence Runner
# ---------------------------------------------------------------------------


class SequenceRunner:
    """
    Executes multi-step command sequences asynchronously.

    Each sequence runs as an asyncio task. Only one sequence can be
    active at a time — starting a new one cancels the current one.
    """

    def __init__(self, controller: RobotController):
        self._ctrl = controller
        self._task: Optional[asyncio.Task] = None
        self._active_name: Optional[str] = None
        self._cancelled = False

    @property
    def active(self) -> Optional[str]:
        return self._active_name

    @property
    def running(self) -> bool:
        return self._task is not None and not self._task.done()

    async def start(self, seq: SequenceCommand) -> str:
        """Start executing a sequence. Returns sequence ID."""
        self.cancel()
        seq_id = str(uuid.uuid4())[:8]
        self._active_name = seq.name
        self._cancelled = False
        self._ctrl.event_log.record(
            EventType.SEQUENCE_START,
            {"id": seq_id, "name": seq.name, "steps": len(seq.steps), "loop": seq.loop},
            source="sequence",
        )
        self._task = asyncio.create_task(self._run(seq, seq_id))
        return seq_id

    def cancel(self):
        """Cancel any running sequence."""
        if self._task and not self._task.done():
            self._cancelled = True
            self._task.cancel()
        self._active_name = None

    async def _run(self, seq: SequenceCommand, seq_id: str):
        try:
            while True:
                for i, step in enumerate(seq.steps):
                    if self._cancelled:
                        return
                    self._ctrl.event_log.record(
                        EventType.SEQUENCE_STEP,
                        {
                            "id": seq_id,
                            "step": i,
                            "action": step.action,
                            "params": step.params,
                        },
                        source="sequence",
                    )
                    await self._execute_step(step)
                    await asyncio.sleep(step.duration)

                if not seq.loop:
                    break

        except asyncio.CancelledError:
            pass
        finally:
            # Return to idle
            self._ctrl.clear_api_command()
            self._active_name = None
            self._ctrl.event_log.record(
                EventType.SEQUENCE_END,
                {"id": seq_id, "name": seq.name},
                source="sequence",
            )

    async def _execute_step(self, step: SequenceStep):
        """Execute a single sequence step."""
        if step.action == "move":
            vx = step.params.get("vx", 0.0)
            vy = step.params.get("vy", 0.0)
            vyaw = step.params.get("vyaw", 0.0)
            cmd = MoveCommand(vx=vx, vy=vy, vyaw=vyaw, duration=step.duration + 0.5)
            self._ctrl.set_move_command(cmd)

        elif step.action == "action":
            name = step.params.get("name", "")
            self._ctrl.execute_action(name)

        elif step.action == "wait":
            # Just wait — duration handled by caller
            self._ctrl.clear_api_command()

        else:
            logger.warning("Unknown sequence action: %s", step.action)


# ---------------------------------------------------------------------------
# RobotController — the core
# ---------------------------------------------------------------------------


class RobotController:
    """
    Central robot controller managing serial I/O, safety, and command mixing.

    Lifecycle: start() → control loop runs → stop()
    """

    def __init__(self, config: AppConfig):
        self.config = config
        self.event_log = EventLog(log_to_file=True)
        self.serial = SerialManager(config, self.event_log)
        self.safety = SafetyManager(config, self.event_log)
        self.sequence_runner = SequenceRunner(self)

        self.running = False
        self.start_time = time.time()

        # State
        self.controller_state = ControllerState()
        self.controller_connected = False
        self.current_mode = RobotMode.IDLE
        self.speed_mode: str = "walk"  # walk, run (internal speed scaling)

        # API command state
        self._api_vx: float = 0.0
        self._api_vy: float = 0.0
        self._api_vyaw: float = 0.0
        self._api_cmd_expires: float = 0.0
        self._api_active: bool = False

        # Button hold queue: list of (buttons, expire_time)
        self._button_holds: list[tuple[int, float]] = []
        self._action_hold_seconds: float = 0.35  # how long to hold buttons

        # Control loop
        self._control_task: Optional[asyncio.Task] = None
        self._reconnect_task: Optional[asyncio.Task] = None

        # WebSocket subscribers
        self._ws_clients: set[WebSocket] = set()

    async def start(self):
        self.running = True
        self.start_time = time.time()

        # Open serial ports
        self.serial.open_receiver()
        self.serial.open_robot()

        # Start loops
        self._control_task = asyncio.create_task(self._control_loop())
        self._reconnect_task = asyncio.create_task(self._reconnect_loop())

        self.event_log.record(
            EventType.SERVER_START, {"simulation": self.config.simulation}
        )
        logger.info("Robot controller started (simulation=%s)", self.config.simulation)

    async def stop(self):
        self.running = False
        self.sequence_runner.cancel()

        for task in (self._control_task, self._reconnect_task):
            if task:
                task.cancel()
                try:
                    await task
                except asyncio.CancelledError:
                    pass

        self.serial.close()
        self.event_log.record(EventType.SERVER_STOP, {})
        self.event_log.close()
        logger.info("Robot controller stopped")

    # -- Control loop ---------------------------------------------------

    async def _control_loop(self):
        interval = 1.0 / self.config.control.command_rate_hz
        while self.running:
            try:
                self._tick()
                await asyncio.sleep(interval)
            except asyncio.CancelledError:
                break
            except Exception as e:
                logger.error("Control loop error: %s", e, exc_info=True)
                await asyncio.sleep(0.1)

    def _tick(self):
        """Single control loop iteration."""
        # 1. Read controller input
        pkt = self.serial.read_controller_packet()
        if pkt:
            self.controller_state = pkt
            if not self.controller_connected:
                self.controller_connected = True
                self.event_log.record(EventType.CONTROLLER_CONNECT, {})
                logger.info("Controller connected")

        # 2. Determine target velocity
        target_vx, target_vy, target_vyaw = self._compute_target()

        # 3. Safety: clamp, ramp, watchdog
        vx, vy, vyaw = self.safety.clamp(target_vx, target_vy, target_vyaw)

        if self._api_active:
            vx, vy, vyaw = self.safety.ramp(vx, vy, vyaw)
        else:
            self.safety.reset()

        if not self.safety.check_watchdog() and self._api_active:
            vx = vy = vyaw = 0.0
            self._api_active = False
            self.safety.reset()

        # 4. Build final controller state to send
        final = ControllerState()
        final.ly = vx / self.config.safety.max_vx if self.config.safety.max_vx else 0
        final.lx = -vy / self.config.safety.max_vy if self.config.safety.max_vy else 0
        final.rx = (
            -vyaw / self.config.safety.max_vyaw if self.config.safety.max_vyaw else 0
        )

        # 4b. Apply any active button holds
        now = time.time()
        active_buttons = 0
        self._button_holds = [(b, t) for b, t in self._button_holds if t > now]
        for buttons, _ in self._button_holds:
            active_buttons |= buttons
        final.buttons = active_buttons

        # 5. Update mode tracking
        self._update_mode()

        # 6. Send to robot
        self.serial.send_to_robot(final)

    def _compute_target(self) -> tuple[float, float, float]:
        """Compute target velocity from API or controller."""
        now = time.time()

        # API command active?
        if now < self._api_cmd_expires:
            self._api_active = True
            return self._api_vx, self._api_vy, self._api_vyaw

        self._api_active = False

        # Passthrough from physical controller
        if self.config.control.passthrough_enabled and self.controller_connected:
            return (
                self.controller_state.ly * self.config.safety.max_vx,
                -self.controller_state.lx * self.config.safety.max_vy,
                -self.controller_state.rx * self.config.safety.max_vyaw,
            )

        return 0.0, 0.0, 0.0

    # -- Command entrypoints -------------------------------------------

    def _update_mode(self):
        """Derive current_mode from robot state."""
        # If button holds include emergency stop, we're in damping
        now = time.time()
        active_buttons = 0
        for buttons, t in self._button_holds:
            if t > now:
                active_buttons |= buttons
        estop = ControllerButtons.START | ControllerButtons.SELECT
        if active_buttons & estop == estop:
            self.current_mode = RobotMode.DAMPING
            return

        # Check if moving
        v = (
            abs(self.safety._current_vx)
            + abs(self.safety._current_vy)
            + abs(self.safety._current_vyaw)
        )
        if v > 0.02:
            self.current_mode = (
                RobotMode.RUNNING if self.speed_mode == "run" else RobotMode.WALKING
            )
        elif self._api_active or self.controller_connected:
            self.current_mode = RobotMode.STAND
        else:
            self.current_mode = RobotMode.IDLE

    def set_move_command(self, cmd: MoveCommand):
        self._api_vx = cmd.vx
        self._api_vy = cmd.vy
        self._api_vyaw = cmd.vyaw
        duration = (
            cmd.duration if cmd.duration else self.config.control.api_command_timeout
        )
        self._api_cmd_expires = time.time() + duration
        self.safety.feed_command()

        self.event_log.record(
            EventType.MOVE_CMD,
            {"vx": cmd.vx, "vy": cmd.vy, "vyaw": cmd.vyaw, "duration": duration},
            source="api",
        )

    def clear_api_command(self):
        self._api_vx = 0.0
        self._api_vy = 0.0
        self._api_vyaw = 0.0
        self._api_cmd_expires = 0.0
        self._api_active = False
        self.safety.reset()

    def execute_action(self, action_name: str) -> bool:
        if action_name not in ACTION_MAP:
            return False

        buttons = ACTION_MAP[action_name]

        # Queue button hold — the control loop will include these buttons
        # in every packet until the hold expires (~350ms)
        expire = time.time() + self._action_hold_seconds
        self._button_holds.append((buttons, expire))

        self.safety.feed_command()
        self.event_log.record(
            EventType.ACTION_CMD,
            {
                "action": action_name,
                "buttons": f"0x{buttons:04X}",
                "hold_ms": int(self._action_hold_seconds * 1000),
            },
            source="api",
        )
        logger.info(
            "Action: %s (buttons=0x%04X, hold=%dms)",
            action_name,
            buttons,
            int(self._action_hold_seconds * 1000),
        )
        return True

    def set_speed_mode(self, mode: str) -> bool:
        """Switch between walk and run speed modes."""
        if mode not in ("walk", "run"):
            return False
        if mode == self.speed_mode:
            return True
        self.speed_mode = mode
        # Run mode is typically activated with a D-pad combo
        # Using UP for run, DOWN for walk as common convention
        if mode == "run":
            expire = time.time() + self._action_hold_seconds
            self._button_holds.append(
                (ControllerButtons.L2 | ControllerButtons.UP, expire)
            )
        else:
            expire = time.time() + self._action_hold_seconds
            self._button_holds.append(
                (ControllerButtons.L2 | ControllerButtons.DOWN, expire)
            )
        self.event_log.record(
            EventType.MODE_CHANGE,
            {"speed_mode": mode},
            source="api",
        )
        logger.info("Speed mode: %s", mode)
        return True

    def emergency_stop(self):
        self.clear_api_command()
        self.sequence_runner.cancel()
        self.execute_action("emergency_stop")
        self.event_log.record(EventType.EMERGENCY_STOP, {}, source="api")
        logger.warning("EMERGENCY STOP triggered")

    def get_status(self) -> RobotStatus:
        return RobotStatus(
            mode=self.current_mode,
            speed_mode=self.speed_mode,
            battery_percent=100,  # TODO: read from robot telemetry
            controller_connected=self.controller_connected,
            serial_connected=self.serial.receiver_connected or self.config.simulation,
            api_control_active=self._api_active,
            current_velocity={
                "vx": round(self.safety._current_vx, 3),
                "vy": round(self.safety._current_vy, 3),
                "vyaw": round(self.safety._current_vyaw, 3),
            },
            target_velocity={
                "vx": round(self._api_vx, 3),
                "vy": round(self._api_vy, 3),
                "vyaw": round(self._api_vyaw, 3),
            },
            uptime_seconds=round(time.time() - self.start_time, 1),
            active_sequence=self.sequence_runner.active,
            safety={
                "watchdog_ok": not self.safety.watchdog_tripped,
                "ramping": self.safety.is_ramping,
            },
        )

    # -- WebSocket broadcasting ----------------------------------------

    def add_ws_client(self, ws: WebSocket):
        self._ws_clients.add(ws)

    def remove_ws_client(self, ws: WebSocket):
        self._ws_clients.discard(ws)

    async def broadcast_state(self):
        """Periodically broadcast state to all WebSocket clients."""
        while self.running:
            if self._ws_clients:
                status = self.get_status()
                data = status.model_dump()
                dead: list[WebSocket] = []
                for ws in self._ws_clients:
                    try:
                        await ws.send_json(data)
                    except Exception:
                        dead.append(ws)
                for ws in dead:
                    self._ws_clients.discard(ws)
            await asyncio.sleep(0.1)  # 10 Hz broadcast

    # -- Reconnect loop ------------------------------------------------

    async def _reconnect_loop(self):
        """Periodically try to reconnect lost serial ports."""
        while self.running:
            try:
                self.serial.try_reconnect()
                await asyncio.sleep(5.0)
            except asyncio.CancelledError:
                break
            except Exception:
                await asyncio.sleep(5.0)


# ---------------------------------------------------------------------------
# Global instance
# ---------------------------------------------------------------------------

robot_controller: Optional[RobotController] = None
_ws_broadcast_task: Optional[asyncio.Task] = None


@asynccontextmanager
async def lifespan(app: FastAPI):
    global robot_controller, _ws_broadcast_task

    config = load_config()
    robot_controller = RobotController(config)
    await robot_controller.start()
    _ws_broadcast_task = asyncio.create_task(robot_controller.broadcast_state())

    yield

    if _ws_broadcast_task:
        _ws_broadcast_task.cancel()
    await robot_controller.stop()


# ---------------------------------------------------------------------------
# FastAPI app
# ---------------------------------------------------------------------------

app = FastAPI(
    title="Unitree G1 Robot API",
    description=(
        "REST + WebSocket API for controlling the Unitree G1 humanoid robot.\n\n"
        "## Features\n"
        "- Velocity movement commands with safety ramping\n"
        "- Predefined actions (documented state transitions)\n"
        "- Command sequencing / macros\n"
        "- Real-time WebSocket telemetry\n"
        "- Structured event logging\n"
    ),
    version="0.3.0",
    lifespan=lifespan,
)

app.add_middleware(
    CORSMiddleware,
    allow_origins=["*"],
    allow_credentials=True,
    allow_methods=["*"],
    allow_headers=["*"],
)


# ---------------------------------------------------------------------------
# Helpers
# ---------------------------------------------------------------------------


def _ctrl() -> RobotController:
    if not robot_controller:
        raise HTTPException(503, "Controller not initialized")
    return robot_controller


# ---------------------------------------------------------------------------
# REST endpoints
# ---------------------------------------------------------------------------


@app.get("/", tags=["System"])
async def root():
    return {
        "name": "Unitree G1 Robot API",
        "version": "0.3.0",
        "docs": "/docs",
        "dashboard": "/dashboard",
        "status": "running",
    }


@app.get("/dashboard", response_class=HTMLResponse, tags=["System"])
async def dashboard():
    """Serve the web control dashboard."""
    html_path = Path(__file__).parent / "dashboard.html"
    return HTMLResponse(html_path.read_text())


# -- Status -----------------------------------------------------------------


@app.get("/api/v1/status", response_model=RobotStatus, tags=["Status"])
async def get_status():
    """Full robot status including serial, safety, gait, and active sequence."""
    return _ctrl().get_status()


@app.get("/api/v1/actions", tags=["Status"])
async def list_actions():
    """List all available predefined actions."""
    return {
        "actions": list(ACTION_MAP.keys()),
        "description": "Use POST /api/v1/action/{name} to execute",
    }


# -- Movement ---------------------------------------------------------------


@app.post("/api/v1/move", response_model=ActionResponse, tags=["Movement"])
async def move(cmd: MoveCommand):
    """
    Send a velocity command.

    The robot will move at the specified velocity until:
    - **duration** expires (if set)
    - A new move/stop command is received
    - The watchdog times out (default 2s)

    Velocities are automatically clamped and ramped for safety.
    """
    _ctrl().set_move_command(cmd)
    return ActionResponse(
        success=True,
        message=f"Moving: vx={cmd.vx}, vy={cmd.vy}, vyaw={cmd.vyaw}",
        action="move",
    )


@app.post("/api/v1/stop", response_model=ActionResponse, tags=["Movement"])
async def stop():
    """Emergency stop — immediately halts all movement and cancels sequences."""
    _ctrl().emergency_stop()
    return ActionResponse(success=True, message="Robot stopped", action="stop")


@app.post("/api/v1/speed_mode", response_model=ActionResponse, tags=["Movement"])
async def set_speed_mode(cmd: SpeedModeCommand):
    """Switch between walk and run speed modes."""
    if not _ctrl().set_speed_mode(cmd.mode):
        raise HTTPException(
            400, f"Invalid speed mode: {cmd.mode}. Use 'walk' or 'run'."
        )
    return ActionResponse(
        success=True, message=f"Speed mode: {cmd.mode}", action="speed_mode"
    )


@app.post(
    "/api/v1/action/{action_name}", response_model=ActionResponse, tags=["Actions"]
)
async def execute_action(action_name: str):
    """
    Execute a predefined action by name.

    Use `GET /api/v1/actions` to see the full list.
    """
    if not _ctrl().execute_action(action_name):
        raise HTTPException(
            400, f"Unknown action: {action_name}. GET /api/v1/actions for options."
        )
    return ActionResponse(
        success=True, message=f"Executing: {action_name}", action=action_name
    )


# -- Sequences ---------------------------------------------------------------


@app.post("/api/v1/sequence", response_model=ActionResponse, tags=["Sequences"])
async def run_sequence(seq: SequenceCommand):
    """
    Execute a multi-step command sequence.

    Example — walk a square:
    ```json
    {
      "name": "walk_square",
      "steps": [
        {"action": "action", "params": {"name": "stand_up"}, "duration": 2},
        {"action": "move", "params": {"vx": 0.5}, "duration": 3},
        {"action": "move", "params": {"vyaw": 0.3}, "duration": 2.6},
        {"action": "move", "params": {"vx": 0.5}, "duration": 3},
        {"action": "move", "params": {"vyaw": 0.3}, "duration": 2.6},
        {"action": "move", "params": {"vx": 0.5}, "duration": 3},
        {"action": "move", "params": {"vyaw": 0.3}, "duration": 2.6},
        {"action": "move", "params": {"vx": 0.5}, "duration": 3},
        {"action": "action", "params": {"name": "sit_down"}, "duration": 2}
      ]
    }
    ```
    """
    seq_id = await _ctrl().sequence_runner.start(seq)
    return ActionResponse(
        success=True,
        message=f"Sequence '{seq.name}' started ({len(seq.steps)} steps)",
        action="sequence",
        id=seq_id,
    )


@app.delete("/api/v1/sequence", response_model=ActionResponse, tags=["Sequences"])
async def cancel_sequence():
    """Cancel the currently running sequence."""
    _ctrl().sequence_runner.cancel()
    _ctrl().clear_api_command()
    return ActionResponse(
        success=True, message="Sequence cancelled", action="sequence_cancel"
    )


# -- Sequence Library --------------------------------------------------------

_seq_lib = SequenceLibrary()


@app.get("/api/v1/sequences", tags=["Sequences"])
async def list_sequences():
    """List all available named sequences (built-in + user-saved)."""
    lib = _seq_lib.list()
    return {
        "sequences": {
            name: {
                "steps": len(s.get("steps", [])),
                "loop": s.get("loop", False),
            }
            for name, s in lib.items()
        }
    }


@app.get("/api/v1/sequences/{name}", tags=["Sequences"])
async def get_sequence(name: str):
    """Get full definition of a named sequence."""
    seq = _seq_lib.get(name)
    if not seq:
        raise HTTPException(404, f"Sequence not found: {name}")
    return seq


@app.post(
    "/api/v1/sequences/{name}/run", response_model=ActionResponse, tags=["Sequences"]
)
async def run_named_sequence(name: str):
    """Load a named sequence from the library and run it."""
    data = _seq_lib.get(name)
    if not data:
        raise HTTPException(404, f"Sequence not found: {name}")
    seq = SequenceCommand(
        name=data.get("name", name),
        steps=[SequenceStep(**s) for s in data["steps"]],
        loop=data.get("loop", False),
    )
    seq_id = await _ctrl().sequence_runner.start(seq)
    return ActionResponse(
        success=True,
        message=f"Running library sequence '{name}' ({len(seq.steps)} steps)",
        action="sequence_library",
        id=seq_id,
    )


@app.post(
    "/api/v1/sequences/{name}/save", response_model=ActionResponse, tags=["Sequences"]
)
async def save_sequence(name: str, seq: SequenceCommand):
    """Save a sequence to the library for later use."""
    data = {
        "name": name,
        "steps": [s.model_dump() for s in seq.steps],
        "loop": seq.loop,
    }
    ok = _seq_lib.save(name, data)
    if not ok:
        raise HTTPException(500, "Failed to save sequence")
    return ActionResponse(
        success=True, message=f"Sequence '{name}' saved", action="sequence_save"
    )


@app.delete(
    "/api/v1/sequences/{name}", response_model=ActionResponse, tags=["Sequences"]
)
async def delete_sequence(name: str):
    """Delete a user-saved sequence (cannot delete built-ins)."""
    ok = _seq_lib.delete(name)
    if not ok:
        raise HTTPException(400, f"Cannot delete '{name}' (built-in or not found)")
    return ActionResponse(
        success=True, message=f"Sequence '{name}' deleted", action="sequence_delete"
    )


# -- Navigation (placeholder) -----------------------------------------------


@app.post("/api/v1/navigate", response_model=ActionResponse, tags=["Navigation"])
async def navigate(cmd: NavigateCommand):
    """Navigate to a position. Requires vision system (not yet implemented)."""
    raise HTTPException(501, "Navigation requires vision system — not yet implemented")


# -- Controller state (debug) -----------------------------------------------


@app.get("/api/v1/controller", tags=["Debug"])
async def get_controller_state():
    """Raw controller state — useful for debugging."""
    ctrl = _ctrl()
    state = ctrl.controller_state
    return {
        "connected": ctrl.controller_connected,
        "lx": round(state.lx, 4),
        "ly": round(state.ly, 4),
        "rx": round(state.rx, 4),
        "ry": round(state.ry, 4),
        "l2": round(state.l2, 4),
        "buttons": f"0x{state.buttons:04X}",
        "buttons_pressed": state.get_pressed_buttons(),
    }


# -- Telemetry / Events -----------------------------------------------------


@app.get("/api/v1/events", tags=["Telemetry"])
async def get_events(
    n: int = Query(50, ge=1, le=500, description="Number of recent events"),
    event_type: Optional[str] = Query(None, description="Filter by event type"),
):
    """Retrieve recent events from the telemetry log."""
    ctrl = _ctrl()
    et = None
    if event_type:
        try:
            et = EventType(event_type)
        except ValueError:
            raise HTTPException(
                400,
                f"Unknown event type: {event_type}. Options: {[e.value for e in EventType]}",
            )
    return {
        "events": ctrl.event_log.recent(n=n, event_type=et),
        "stats": ctrl.event_log.stats(),
    }


@app.get("/api/v1/events/types", tags=["Telemetry"])
async def list_event_types():
    """List all possible event types."""
    return {"types": [e.value for e in EventType]}


# -- Safety config -----------------------------------------------------------


@app.get("/api/v1/safety", tags=["Safety"])
async def get_safety():
    """Get current safety configuration."""
    ctrl = _ctrl()
    cfg = ctrl.config.safety
    return {
        "max_vx": cfg.max_vx,
        "max_vy": cfg.max_vy,
        "max_vyaw": cfg.max_vyaw,
        "velocity_ramp_rate": cfg.velocity_ramp_rate,
        "watchdog_timeout": cfg.watchdog_timeout,
        "enable_watchdog": cfg.enable_watchdog,
        "enable_ramp": cfg.enable_ramp,
        "watchdog_ok": not ctrl.safety.watchdog_tripped,
    }


# ---------------------------------------------------------------------------
# Agent API — endpoints optimized for LLM / AGiXT agent consumption
# ---------------------------------------------------------------------------


class AgentCommand(BaseModel):
    """
    A unified command structure for LLM agents.

    The agent sends a single command with a type and parameters.
    This avoids the agent needing to know multiple endpoint URLs.
    """

    command: str = Field(
        ...,
        description=(
            "Command to execute. One of: "
            "move, stop, action, sequence, speed_mode, status"
        ),
    )
    parameters: dict = Field(
        default_factory=dict,
        description="Parameters for the command (varies by command type)",
    )


@app.get("/api/v1/agent/context", tags=["Agent"])
async def agent_context():
    """
    Get a complete summary of robot capabilities and current state.

    Designed for LLM agents — returns everything needed to make
    informed decisions about robot control in a single call. Include
    this in your agent's system prompt or call it before issuing commands.
    """
    ctrl = _ctrl()
    status = ctrl.get_status()

    v = status.current_velocity
    is_moving = (
        abs(v.get("vx", 0)) > 0.01
        or abs(v.get("vy", 0)) > 0.01
        or abs(v.get("vyaw", 0)) > 0.01
    )

    return {
        "robot": "Unitree G1 Basic Humanoid",
        "api_version": "0.3.0",
        "state": {
            "mode": status.mode,
            "speed_mode": status.speed_mode,
            "battery_percent": status.battery_percent,
            "is_moving": is_moving,
            "current_velocity": status.current_velocity,
            "serial_connected": status.serial_connected,
            "api_control_active": status.api_control_active,
            "uptime_seconds": status.uptime_seconds,
            "active_sequence": status.active_sequence,
            "watchdog_ok": status.safety.get("watchdog_ok", True),
        },
        "actions": list(ACTION_MAP.keys()),
        "action_descriptions": {
            "damping": "Enter damping/soft mode (motors off, robot will fall)",
            "stand_up": "Stand up from damping into ready position",
            "sit_down": "Sit down (use before shutdown)",
            "start_control": "Enter operation control / walking mode",
            "toggle_walk": "Toggle between standing still and walking state",
            "emergency_stop": "Emergency stop — enters damping immediately",
            "debug_mode": "Enter debug mode (stops motion control program)",
        },
        "sequences": _seq_lib.list(),
        "safety_limits": {
            "max_forward_speed": ctrl.config.safety.max_vx,
            "max_lateral_speed": ctrl.config.safety.max_vy,
            "max_turn_rate": ctrl.config.safety.max_vyaw,
            "watchdog_timeout": ctrl.config.safety.watchdog_timeout,
            "max_duration_per_command": 30.0,
        },
        "commands": {
            "move": {
                "description": "Move the robot with velocity",
                "parameters": {
                    "vx": "Forward speed m/s (negative=backward)",
                    "vy": "Lateral speed m/s (positive=left)",
                    "vyaw": "Turn rate rad/s (positive=counter-clockwise)",
                    "duration": "Seconds to move (auto-stops after)",
                },
            },
            "stop": {
                "description": "Emergency stop all movement",
                "parameters": {},
            },
            "action": {
                "description": "Execute a predefined action",
                "parameters": {"name": "Action name from the actions list"},
            },
            "sequence": {
                "description": "Run a named sequence",
                "parameters": {"name": "Sequence name from the sequences list"},
            },
            "speed_mode": {
                "description": "Switch walk/run mode",
                "parameters": {"mode": "'walk' or 'run'"},
            },
            "status": {
                "description": "Get current robot status",
                "parameters": {},
            },
        },
        "tips": [
            "Boot sequence: damping → stand_up → start_control → toggle_walk → move",
            "Always call 'stand_up' then 'start_control' before sending move commands",
            "Use duration on move commands to auto-stop (safer than relying on a follow-up stop)",
            "Check status before issuing commands to know the robot's current state",
            "The watchdog will auto-stop the robot if no command is received for 2 seconds",
            "Speed limits are enforced automatically — you cannot exceed them",
            "Gesture actions (wave, bow, etc.) require the DDS network API, not available via serial",
        ],
    }


@app.post("/api/v1/agent/command", response_model=ActionResponse, tags=["Agent"])
async def agent_command(cmd: AgentCommand):
    """
    Unified command endpoint for LLM agents.

    Instead of calling multiple different endpoints, the agent sends all
    commands here with a `command` type and `parameters` dict.

    Supported commands:
    - **move**: `{"vx": 0.5, "vy": 0, "vyaw": 0, "duration": 2}`
    - **stop**: `{}` (no parameters needed)
    - **action**: `{"name": "stand_up"}`
    - **sequence**: `{"name": "greeting"}`
    - **speed_mode**: `{"mode": "walk"}` or `{"mode": "run"}`
    - **status**: `{}` (returns status in message field)
    """
    ctrl = _ctrl()
    p = cmd.parameters

    try:
        if cmd.command == "move":
            move_cmd = MoveCommand(
                vx=float(p.get("vx", p.get("forward_speed", 0))),
                vy=float(p.get("vy", p.get("lateral_speed", 0))),
                vyaw=float(p.get("vyaw", p.get("turn_rate", 0))),
                duration=min(float(p.get("duration", 2.0)), 30.0),
            )
            ctrl.set_move_command(move_cmd)
            return ActionResponse(
                success=True,
                message=(
                    f"Moving: forward={move_cmd.vx}m/s, "
                    f"lateral={move_cmd.vy}m/s, turn={move_cmd.vyaw}rad/s "
                    f"for {move_cmd.duration}s"
                ),
                action="move",
            )

        elif cmd.command == "stop":
            ctrl.emergency_stop()
            return ActionResponse(
                success=True,
                message="Robot stopped. All movement halted.",
                action="stop",
            )

        elif cmd.command == "action":
            name = p.get("name", "")
            if not name:
                return ActionResponse(
                    success=False,
                    message=f"Missing 'name' parameter. Available actions: {list(ACTION_MAP.keys())}",
                    action="action",
                )
            if not ctrl.execute_action(name):
                return ActionResponse(
                    success=False,
                    message=f"Unknown action '{name}'. Available: {list(ACTION_MAP.keys())}",
                    action="action",
                )
            return ActionResponse(
                success=True,
                message=f"Executing action: {name}",
                action="action",
            )

        elif cmd.command == "sequence":
            name = p.get("name", "")
            if not name:
                seqs = list(_seq_lib.list().keys())
                return ActionResponse(
                    success=False,
                    message=f"Missing 'name' parameter. Available sequences: {seqs}",
                    action="sequence",
                )
            data = _seq_lib.get(name)
            if not data:
                seqs = list(_seq_lib.list().keys())
                return ActionResponse(
                    success=False,
                    message=f"Sequence '{name}' not found. Available: {seqs}",
                    action="sequence",
                )
            seq = SequenceCommand(
                name=data.get("name", name),
                steps=[SequenceStep(**s) for s in data["steps"]],
                loop=data.get("loop", False),
            )
            seq_id = await ctrl.sequence_runner.start(seq)
            return ActionResponse(
                success=True,
                message=f"Running sequence '{name}' ({len(seq.steps)} steps)",
                action="sequence",
                id=seq_id,
            )

        elif cmd.command == "speed_mode":
            mode = p.get("mode", "walk")
            if not ctrl.set_speed_mode(mode):
                return ActionResponse(
                    success=False,
                    message=f"Invalid mode '{mode}'. Use 'walk' or 'run'.",
                    action="speed_mode",
                )
            return ActionResponse(
                success=True,
                message=f"Speed mode set to: {mode}",
                action="speed_mode",
            )

        elif cmd.command == "status":
            status = ctrl.get_status()
            status_dict = status.model_dump()
            return ActionResponse(
                success=True,
                message=_json.dumps(status_dict),
                action="status",
            )

        else:
            return ActionResponse(
                success=False,
                message=(
                    f"Unknown command '{cmd.command}'. "
                    f"Available: move, stop, action, sequence, speed_mode, status"
                ),
                action="unknown",
            )

    except (ValueError, TypeError) as e:
        return ActionResponse(
            success=False,
            message=f"Invalid parameters for '{cmd.command}': {e}",
            action=cmd.command,
        )


# ---------------------------------------------------------------------------
# WebSocket endpoint — real-time telemetry
# ---------------------------------------------------------------------------


@app.websocket("/ws/telemetry")
async def ws_telemetry(ws: WebSocket):
    """
    Real-time robot state streamed at ~10 Hz.
    Also accepts JSON commands:
        {"cmd": "move", "vx": 0.5, "vy": 0, "vyaw": 0, "duration": 2}
        {"cmd": "action", "name": "stand_up"}
        {"cmd": "stop"}
        {"cmd": "speed_mode", "mode": "run"}
    """
    await ws.accept()
    ctrl = _ctrl()
    ctrl.add_ws_client(ws)
    logger.info("WebSocket client connected")
    try:
        while True:
            raw = await ws.receive_text()
            try:
                msg = _json.loads(raw)
                cmd = msg.get("cmd", "")
                if cmd == "move":
                    ctrl.set_move_command(
                        MoveCommand(
                            vx=msg.get("vx", 0.0),
                            vy=msg.get("vy", 0.0),
                            vyaw=msg.get("vyaw", 0.0),
                            duration=msg.get("duration"),
                        )
                    )
                    await ws.send_json({"ack": "move"})
                elif cmd == "action":
                    name = msg.get("name", "")
                    ok = ctrl.execute_action(name)
                    await ws.send_json({"ack": "action", "name": name, "ok": ok})
                elif cmd == "stop":
                    ctrl.emergency_stop()
                    await ws.send_json({"ack": "stop"})
                elif cmd == "speed_mode":
                    mode = msg.get("mode", "walk")
                    ok = ctrl.set_speed_mode(mode)
                    await ws.send_json({"ack": "speed_mode", "mode": mode, "ok": ok})
                else:
                    await ws.send_json({"error": f"unknown cmd: {cmd}"})
            except Exception as e:
                await ws.send_json({"error": str(e)})
    except WebSocketDisconnect:
        pass
    finally:
        ctrl.remove_ws_client(ws)
        logger.info("WebSocket client disconnected")


# ---------------------------------------------------------------------------
# Health check
# ---------------------------------------------------------------------------


@app.get("/health", tags=["System"])
async def health():
    """Health check for monitoring / load balancers."""
    ctrl = _ctrl()
    return {
        "status": "healthy",
        "uptime": round(time.time() - ctrl.start_time, 1),
        "serial": ctrl.serial.receiver_connected or ctrl.config.simulation,
        "simulation": ctrl.config.simulation,
    }


# ---------------------------------------------------------------------------
# Main
# ---------------------------------------------------------------------------

if __name__ == "__main__":
    import uvicorn

    config = load_config()
    print("=" * 60)
    print("  Unitree G1 Robot API Server v0.3.0")
    print("=" * 60)
    print(f"  Host:       {config.server.host}:{config.server.port}")
    print(f"  Simulation: {config.simulation}")
    print(f"  Receiver:   {config.serial.receiver_port}")
    print(f"  Robot:      {config.serial.robot_port}")
    print(f"  Docs:       http://{config.server.host}:{config.server.port}/docs")
    print("=" * 60)

    uvicorn.run(
        app,
        host=config.server.host,
        port=config.server.port,
        log_level=config.server.log_level,
    )