from __future__ import annotations

import argparse
import asyncio
import base64
import json
import logging
import math
import os
import time
from http import HTTPStatus
from pathlib import Path
from typing import Any, Dict, List, Optional, Set, Tuple

import websockets
from websockets.server import WebSocketServerProtocol

from .frame_capture import FrameCapture
from .fr_loop import FRLoop
from .messages import (
    Battery,
    ErrorMessage,
    EventLogMessage,
    GoalType,
    MessageType,
    NavStatusMessage,
    Pose,
    RobotState,
    WaypointListMessage,
)
from .nav_backend.base import Pose2D
from .nav_backend.mock_backend import MockNavBackend
from .nav_backend.rosbridge_backend import RosbridgeBackend
from .waypoints import WaypointRegistry, load_waypoints


class EdgeProxyServer:
    def __init__(
        self,
        host: str,
        port: int,
        ws_path: str,
        health_path: str,
        backend: str,
        waypoints_path: str,
        rosbridge_url: Optional[str] = None,
        rosbridge_insecure_tls: bool = True,
    ) -> None:
        self.host = host
        self.port = port
        self.ws_path = ws_path
        self.health_path = health_path
        self.waypoints_path = waypoints_path

        self._clients: Set[WebSocketServerProtocol] = set()
        self._clients_lock = asyncio.Lock()

        self._waypoints: WaypointRegistry = load_waypoints(waypoints_path)
        self._relative_pose_stale_sec = float(os.getenv("EDGE_PROXY_RELATIVE_POSE_STALE_SEC", "3.0"))

        if backend == "mock":
            self._backend = MockNavBackend()
        elif backend == "rosbridge":
            self._backend = RosbridgeBackend(
                rosbridge_url=rosbridge_url or _env("EDGE_PROXY_ROSBRIDGE_URL", "wss://127.0.0.1:9090"),
                insecure_tls=rosbridge_insecure_tls,
            )
        else:
            raise ValueError(f"Unknown backend: {backend}")

        self._nav_task: Optional[asyncio.Task] = None
        self._active_request_id: Optional[str] = None
        self._active_destination: str = ""
        self._nav_state: str = ""
        self._nav_progress: float = 0.0

        self._state_broadcast_task: Optional[asyncio.Task] = None

        # Frame capture — URL overrideable via FRAME_CAPTURE_URL env var
        _snapshot_url = os.getenv(
            "FRAME_CAPTURE_URL", "http://192.168.168.105:8889/cam/jpeg"
        )
        self._frame_capture = FrameCapture(snapshot_url=_snapshot_url)

        # FR loop — endpoint and FPS configurable via env vars
        _fr_endpoint = os.getenv("FR_ENDPOINT", "ws://kluster.klass.dev:42067/")
        _fr_fps = float(os.getenv("FR_FPS", "5"))
        self._fr_loop = FRLoop(
            frame_capture=self._frame_capture,
            fr_endpoint=_fr_endpoint,
            fps=_fr_fps,
        )
        self._fr_loop.on_detections(self._broadcast_fr_detections)
        self._fr_broadcast_count = 0
        self._fr_broadcast_send_timeout = float(
            os.getenv("FR_BROADCAST_SEND_TIMEOUT_SEC", "0.3")
        )
        self._fr_broadcast_log_every = max(
            int(os.getenv("FR_BROADCAST_LOG_EVERY", "10")), 1
        )
        self._fr_clients: Set[WebSocketServerProtocol] = set()
        self._fr_clients_lock = asyncio.Lock()

        # Offline replay journal + scan artifacts.
        self._event_journal_path = Path(
            os.getenv("EDGE_PROXY_EVENT_JOURNAL_PATH", "/tmp/edge_proxy_events.jsonl")
        )
        self._artifact_dir = Path(
            os.getenv("EDGE_PROXY_ARTIFACT_DIR", "/tmp/edge_proxy_artifacts")
        )
        self._event_journal_max_entries = max(
            int(os.getenv("EDGE_PROXY_EVENT_JOURNAL_MAX_ENTRIES", "500")),
            10,
        )
        self._event_seq = 0
        self._event_journal: List[Dict[str, Any]] = []
        self._load_event_journal()

        self.logger = logging.getLogger("edge_proxy")

    async def serve(self) -> None:
        await self._backend.start()

        self._state_broadcast_task = asyncio.create_task(self._periodic_state_broadcast())
        await self._fr_loop.start()

        self.logger.info("Starting Edge Proxy WS server on %s:%s", self.host, self.port)
        try:
            async with websockets.serve(
                self._route_handler,
                self.host,
                self.port,
                ping_interval=20,
                ping_timeout=20,
                process_request=self._process_request,
            ):
                await asyncio.Future()
        finally:
            self._fr_loop.stop()
            self._frame_capture.close()
            self._state_broadcast_task.cancel()
            try:
                await self._state_broadcast_task
            except asyncio.CancelledError:
                pass

    def _load_event_journal(self) -> None:
        """Load pending event logs from disk (best-effort)."""
        self._event_journal = []

        if not self._event_journal_path.exists():
            return

        try:
            with self._event_journal_path.open("r", encoding="utf-8") as handle:
                for raw in handle:
                    raw = raw.strip()
                    if not raw:
                        continue
                    try:
                        entry = json.loads(raw)
                    except Exception:
                        continue
                    if not isinstance(entry, dict):
                        continue
                    if entry.get("type") != MessageType.EVENT_LOG:
                        continue
                    self._event_journal.append(entry)
        except Exception:
            self._event_journal = []
            return

        if len(self._event_journal) > self._event_journal_max_entries:
            self._event_journal = self._event_journal[-self._event_journal_max_entries :]

    def _persist_event_journal(self) -> None:
        """Persist pending event logs to disk (best-effort)."""
        try:
            self._event_journal_path.parent.mkdir(parents=True, exist_ok=True)
            with self._event_journal_path.open("w", encoding="utf-8") as handle:
                for event in self._event_journal:
                    handle.write(json.dumps(event, ensure_ascii=True))
                    handle.write("\n")
        except Exception as exc:
            self.logger.warning("Failed persisting event journal: %s", exc)

    def _next_event_id(self) -> str:
        self._event_seq += 1
        return f"edge-{int(time.time() * 1000)}-{self._event_seq}"

    async def _record_event_log(
        self,
        *,
        event_type: str,
        request_id: str = "",
        status: str = "",
        payload: Optional[Dict[str, Any]] = None,
        timestamp: Optional[float] = None,
    ) -> None:
        event = EventLogMessage(
            event_id=self._next_event_id(),
            event_type=event_type,
            request_id=request_id,
            status=status,
            timestamp=float(timestamp if timestamp is not None else time.time()),
            replay=False,
            payload=payload or {},
        ).to_dict()

        self._event_journal.append(event)
        if len(self._event_journal) > self._event_journal_max_entries:
            self._event_journal = self._event_journal[-self._event_journal_max_entries :]
        self._persist_event_journal()

        # Also stream events live when orchestrator is connected.
        await self._broadcast(event)

    async def _replay_event_journal(self, websocket: WebSocketServerProtocol) -> None:
        """Replay pending event logs to a newly connected client.

        We clear the journal only after all sends succeed.
        """
        if not self._event_journal:
            return

        payloads: List[str] = []
        for event in self._event_journal:
            replay_event = dict(event)
            replay_event["replay"] = True
            payloads.append(json.dumps(replay_event))

        try:
            for payload in payloads:
                await websocket.send(payload)
        except Exception as exc:
            self.logger.warning("Event replay interrupted: %s", exc)
            return

        self.logger.info("Replayed %d pending edge events", len(self._event_journal))
        self._event_journal.clear()
        self._persist_event_journal()

    async def _capture_arrival_artifact(self, *, request_id: str, destination: str) -> None:
        """Capture and save a snapshot when reaching a waypoint."""
        loop = asyncio.get_event_loop()
        try:
            frame = await loop.run_in_executor(None, self._frame_capture.capture)
        except Exception as exc:
            self.logger.warning("arrival artifact capture failed: %s", exc)
            frame = None

        if not frame:
            await self._record_event_log(
                event_type="scan_area_capture",
                request_id=request_id,
                status="failed",
                payload={
                    "destination": destination,
                    "reason": "capture_failed",
                },
            )
            return

        now_ms = int(time.time() * 1000)
        safe_dest = "".join(ch if ch.isalnum() or ch in {"-", "_"} else "_" for ch in destination)
        filename = f"{now_ms}_{request_id or 'nav'}_{safe_dest or 'unknown'}.jpg"
        target = self._artifact_dir / filename
        try:
            self._artifact_dir.mkdir(parents=True, exist_ok=True)
            target.write_bytes(frame)
            await self._record_event_log(
                event_type="scan_area_capture",
                request_id=request_id,
                status="saved",
                payload={
                    "destination": destination,
                    "path": str(target),
                    "size_bytes": len(frame),
                },
            )
        except Exception as exc:
            self.logger.warning("arrival artifact save failed: %s", exc)
            await self._record_event_log(
                event_type="scan_area_capture",
                request_id=request_id,
                status="failed",
                payload={
                    "destination": destination,
                    "reason": f"save_failed:{exc}",
                },
            )

    async def _route_handler(self, websocket: WebSocketServerProtocol, path: str) -> None:
        """Route incoming WS connections based on URL path."""
        if path == "/fr":
            await self._fr_handler(websocket)
        else:
            await self._handler(websocket, path)

    async def _fr_handler(self, websocket: WebSocketServerProtocol) -> None:
        """Read-only endpoint for dashboard FR overlay.

        Clients connect and receive ``fr_detections`` messages.  Any incoming
        messages are silently ignored.
        """
        self.logger.info("FR client connected: %s", websocket.remote_address)
        async with self._fr_clients_lock:
            self._fr_clients.add(websocket)
        try:
            async for _ in websocket:
                pass  # ignore incoming messages
        except Exception as exc:
            self.logger.debug("FR client error: %s", exc)
        finally:
            async with self._fr_clients_lock:
                self._fr_clients.discard(websocket)
            self.logger.info("FR client disconnected: %s", websocket.remote_address)

    async def _broadcast_fr_detections(self, detections: list) -> None:
        """Broadcast FR detections to all /fr clients AND all /edge clients."""
        fr_metrics: Dict[str, Any] = {}
        get_metrics = getattr(self._fr_loop, "get_last_cycle_metrics", None)
        if callable(get_metrics):
            try:
                metrics = get_metrics()
                if isinstance(metrics, dict):
                    fr_metrics = metrics
            except Exception:
                pass

        payload_obj = {
            "type": MessageType.FR_DETECTIONS,
            "detections": detections,
            "timestamp": time.time(),
        }
        if fr_metrics:
            payload_obj["metrics"] = fr_metrics
        payload = json.dumps(payload_obj)

        # Collect all recipients
        async with self._fr_clients_lock:
            fr_clients = list(self._fr_clients)
        async with self._clients_lock:
            edge_clients = list(self._clients)

        all_clients = fr_clients + edge_clients
        if not all_clients:
            return

        t0 = time.monotonic()
        dead_fr = []
        dead_edge = []

        async def _send_one(ws: WebSocketServerProtocol) -> tuple[WebSocketServerProtocol, bool]:
            try:
                await asyncio.wait_for(
                    ws.send(payload),
                    timeout=self._fr_broadcast_send_timeout,
                )
                return ws, True
            except Exception:
                return ws, False

        results = await asyncio.gather(*(_send_one(ws) for ws in all_clients))
        fr_set = set(fr_clients)
        sent_count = 0
        for ws, ok in results:
            if ok:
                sent_count += 1
                continue
            if ws in fr_set:
                dead_fr.append(ws)
            else:
                dead_edge.append(ws)

        if dead_fr:
            async with self._fr_clients_lock:
                for ws in dead_fr:
                    self._fr_clients.discard(ws)
        if dead_edge:
            async with self._clients_lock:
                for ws in dead_edge:
                    self._clients.discard(ws)

        self._fr_broadcast_count += 1
        if self._fr_broadcast_count % self._fr_broadcast_log_every == 0:
            broadcast_ms = (time.monotonic() - t0) * 1000.0

            def _fmt_ms(value: Any) -> str:
                if isinstance(value, (int, float)):
                    return f"{float(value):.1f}ms"
                return "n/a"

            self.logger.info(
                "FR pipeline #%d: clients(fr=%d edge=%d) sent=%d dropped=%d capture=%s frame_age=%s fr_rtt=%s fr_server=%s edge_after_fr=%s broadcast=%s detections=%d",
                self._fr_broadcast_count,
                len(fr_clients),
                len(edge_clients),
                sent_count,
                len(dead_fr) + len(dead_edge),
                _fmt_ms(fr_metrics.get("capture_ms")),
                _fmt_ms(fr_metrics.get("frame_age_ms")),
                _fmt_ms(fr_metrics.get("fr_roundtrip_ms")),
                _fmt_ms(fr_metrics.get("fr_server_total_ms")),
                _fmt_ms(fr_metrics.get("edge_since_fr_receive_ms")),
                _fmt_ms(broadcast_ms),
                len(detections),
            )

    async def _handler(self, websocket: WebSocketServerProtocol, path: str) -> None:
        if path not in ("/", self.ws_path):
            await websocket.close(code=1008, reason="Unsupported path")
            return

        self.logger.info("Client connected: %s", websocket.remote_address)
        async with self._clients_lock:
            self._clients.add(websocket)

        try:
            # Initial sync
            await self._send_waypoint_list(websocket)
            await self._send_robot_state(websocket)
            await self._replay_event_journal(websocket)

            async for message in websocket:
                await self._handle_message(websocket, message)
        except Exception as exc:
            self.logger.warning("Client error: %s", exc)
        finally:
            async with self._clients_lock:
                self._clients.discard(websocket)
            self.logger.info("Client disconnected: %s", websocket.remote_address)

    async def _handle_message(self, websocket: WebSocketServerProtocol, message: Any) -> None:
        if isinstance(message, (bytes, bytearray)):
            try:
                message = message.decode("utf-8", errors="replace")
            except Exception:
                await self._send_error(websocket, request_id="", error="invalid_bytes", msg="invalid bytes")
                return

        if not isinstance(message, str):
            await self._send_error(websocket, request_id="", error="unsupported_message", msg="unsupported")
            return

        try:
            payload = json.loads(message)
        except json.JSONDecodeError:
            await self._send_error(websocket, request_id="", error="invalid_json", msg="invalid json")
            return

        if not isinstance(payload, dict):
            await self._send_error(websocket, request_id="", error="invalid_payload", msg="payload must be object")
            return

        msg_type = payload.get("type")
        if not msg_type:
            await self._send_error(websocket, request_id="", error="missing_type", msg="missing type")
            return

        if msg_type == MessageType.PING:
            await websocket.send(json.dumps({"type": "pong", "state": "ok"}))
            return

        if msg_type == MessageType.GET_STATE:
            await self._send_robot_state(websocket)
            return

        if msg_type == MessageType.NAVIGATE:
            await self._handle_navigate(payload)
            return

        if msg_type == MessageType.CANCEL_NAVIGATION:
            request_id = payload.get("request_id", "")
            reason = payload.get("reason", "")
            await self._handle_cancel(request_id=request_id, reason=reason)
            return

        if msg_type == MessageType.CAPTURE_FRAME:
            await self._handle_capture_frame(websocket, payload)
            return

        await self._send_error(websocket, request_id=payload.get("request_id", ""), error="unknown_message", msg=str(msg_type))

    async def _handle_capture_frame(
        self, websocket: WebSocketServerProtocol, payload: Dict[str, Any]
    ) -> None:
        """Handle a capture_frame request and send frame_response back.

        Runs the synchronous FrameCapture.capture() in an executor so the
        event loop is not blocked during the network/subprocess call.
        """
        request_id = payload.get("request_id", "")
        loop = asyncio.get_event_loop()
        try:
            frame: Optional[bytes] = await loop.run_in_executor(
                None, self._frame_capture.capture
            )
        except Exception as exc:
            self.logger.warning("capture_frame: unexpected error during capture: %s", exc)
            frame = None

        if frame is not None:
            b64 = base64.b64encode(frame).decode("utf-8")
            response: Dict[str, Any] = {
                "type": "frame_response",
                "request_id": request_id,
                "jpeg_b64": b64,
            }
            self.logger.debug(
                "capture_frame: sending %d-byte JPEG (b64 len=%d)", len(frame), len(b64)
            )
        else:
            response = {
                "type": "frame_response",
                "request_id": request_id,
                "error": "capture_failed",
            }
            self.logger.warning("capture_frame: no frame available — sending capture_failed")

        await websocket.send(json.dumps(response))

    async def _handle_navigate(self, payload: Dict[str, Any]) -> None:
        request_id = str(payload.get("request_id", "")).strip() or f"nav_{int(time.time()*1000)}"
        goal = payload.get("goal") or {}
        speed = payload.get("speed", "normal")

        goal_type = goal.get("type")
        destination = ""
        pose: Optional[Pose2D] = None
        frame_id = "map"

        if goal_type == GoalType.WAYPOINT or goal_type == "waypoint":
            destination = str(goal.get("name", "")).strip()
            wp = self._waypoints.get(destination)
            if not wp:
                await self._broadcast(
                    ErrorMessage(
                        request_id=request_id,
                        error="waypoint_not_found",
                        message=f"Unknown waypoint: {destination}",
                    ).to_dict()
                )
                await self._broadcast(
                    NavStatusMessage(
                        request_id=request_id,
                        status="failed",
                        destination=destination,
                        progress=0.0,
                        reason="waypoint_not_found",
                        error_code="NAV_WAYPOINT_NOT_FOUND",
                    ).to_dict()
                )
                await self._record_event_log(
                    event_type="nav_status",
                    request_id=request_id,
                    status="failed",
                    payload={
                        "destination": destination,
                        "reason": "waypoint_not_found",
                        "error_code": "NAV_WAYPOINT_NOT_FOUND",
                        "progress": 0.0,
                    },
                )
                return
            pose = Pose2D(x=wp.x, y=wp.y, theta=wp.theta)

        elif goal_type == GoalType.POSE or goal_type == "pose":
            destination = "pose"
            pose = Pose2D(
                x=float(goal.get("x", 0.0)),
                y=float(goal.get("y", 0.0)),
                theta=float(goal.get("theta", 0.0)),
            )
        elif goal_type == GoalType.RELATIVE or goal_type == "relative":
            direction = str(goal.get("direction", "")).strip().lower()
            try:
                distance = float(goal.get("distance", 0.0))
            except Exception:
                distance = -1.0

            if direction not in {"forward", "backward", "left", "right"} or distance <= 0.0:
                await self._broadcast(
                    ErrorMessage(
                        request_id=request_id,
                        error="invalid_goal",
                        message=f"Invalid relative goal: direction={direction!r}, distance={goal.get('distance')!r}",
                    ).to_dict()
                )
                await self._broadcast(
                    NavStatusMessage(
                        request_id=request_id,
                        status="failed",
                        destination="relative",
                        progress=0.0,
                        reason="invalid_goal",
                        error_code="NAV_INVALID_GOAL",
                    ).to_dict()
                )
                await self._record_event_log(
                    event_type="nav_status",
                    request_id=request_id,
                    status="failed",
                    payload={
                        "destination": "relative",
                        "reason": "invalid_goal",
                        "error_code": "NAV_INVALID_GOAL",
                        "progress": 0.0,
                    },
                )
                return

            anchor_pose = self._relative_anchor_pose()
            if anchor_pose is None:
                await self._broadcast(
                    ErrorMessage(
                        request_id=request_id,
                        error="localization_unavailable",
                        message=(
                            "Cannot resolve relative navigation: missing or stale robot pose "
                            f"(required freshness <= {self._relative_pose_stale_sec:.1f}s)"
                        ),
                    ).to_dict()
                )
                await self._broadcast(
                    NavStatusMessage(
                        request_id=request_id,
                        status="failed",
                        destination="relative",
                        progress=0.0,
                        reason="localization_unavailable",
                        error_code="NAV_LOCALIZATION_LOST",
                    ).to_dict()
                )
                await self._record_event_log(
                    event_type="nav_status",
                    request_id=request_id,
                    status="failed",
                    payload={
                        "destination": "relative",
                        "reason": "localization_unavailable",
                        "error_code": "NAV_LOCALIZATION_LOST",
                        "progress": 0.0,
                    },
                )
                return

            theta = float(anchor_pose.theta)
            destination = f"relative:{direction}:{distance:g}"
            if direction == "forward":
                dx_body, dy_body = distance, 0.0
            elif direction == "backward":
                dx_body, dy_body = -distance, 0.0
            elif direction == "left":
                dx_body, dy_body = 0.0, distance
            else:  # right
                dx_body, dy_body = 0.0, -distance

            # Convert body-frame delta to map-frame delta using current heading.
            dx_map = math.cos(theta) * dx_body - math.sin(theta) * dy_body
            dy_map = math.sin(theta) * dx_body + math.cos(theta) * dy_body
            pose = Pose2D(
                x=float(anchor_pose.x) + dx_map,
                y=float(anchor_pose.y) + dy_map,
                theta=theta,
            )
        else:
            await self._broadcast(
                ErrorMessage(
                    request_id=request_id,
                    error="unsupported_goal",
                    message=f"Unsupported goal type: {goal_type}",
                ).to_dict()
            )
            await self._broadcast(
                NavStatusMessage(
                    request_id=request_id,
                    status="failed",
                    destination=str(goal_type or ""),
                    progress=0.0,
                    reason="unsupported_goal",
                    error_code="NAV_INVALID_GOAL",
                ).to_dict()
            )
            await self._record_event_log(
                event_type="nav_status",
                request_id=request_id,
                status="failed",
                payload={
                    "destination": str(goal_type or ""),
                    "reason": "unsupported_goal",
                    "error_code": "NAV_INVALID_GOAL",
                    "progress": 0.0,
                },
            )
            return

        # Preempt any active navigation
        if self._nav_task and not self._nav_task.done() and self._active_request_id:
            await self._handle_cancel(request_id=self._active_request_id, reason="preempted")

        self._active_request_id = request_id
        self._active_destination = destination
        self._nav_state = "accepted"
        self._nav_progress = 0.0

        async def on_update(update) -> None:
            if self._active_request_id != request_id:
                return

            self._nav_state = update.status
            if update.progress is not None:
                self._nav_progress = float(update.progress)

            msg = NavStatusMessage(
                request_id=request_id,
                status=update.status,
                destination=destination,
                progress=float(update.progress) if update.progress is not None else self._nav_progress,
                reason=update.reason,
                error_code=update.error_code,
            )
            await self._broadcast(msg.to_dict())

            if update.status in {"accepted", "arrived", "failed", "cancelled"}:
                await self._record_event_log(
                    event_type="nav_status",
                    request_id=request_id,
                    status=update.status,
                    payload={
                        "destination": destination,
                        "progress": msg.progress,
                        "reason": update.reason,
                        "error_code": update.error_code,
                    },
                )

            if update.status == "arrived":
                await self._capture_arrival_artifact(
                    request_id=request_id,
                    destination=destination,
                )

        async def run_nav() -> None:
            try:
                await self._backend.navigate_to_pose(
                    request_id=request_id,
                    destination=destination,
                    pose=pose,
                    speed=str(speed),
                    on_update=on_update,
                    frame_id=frame_id,
                )
            except Exception as exc:
                await on_update(
                    type("U", (), {"status": "failed", "progress": None, "reason": str(exc), "error_code": None})()
                )

        self._nav_task = asyncio.create_task(run_nav())

    async def _handle_cancel(self, request_id: str, reason: str = "") -> None:
        request_id = str(request_id or "").strip()
        if not request_id:
            return

        try:
            await self._backend.cancel(request_id=request_id, reason=reason)
        except Exception as exc:
            self.logger.warning("Cancel failed: %s", exc)

        if self._active_request_id == request_id:
            self._nav_state = "cancelled"
            await self._broadcast(
                NavStatusMessage(
                    request_id=request_id,
                    status="cancelled",
                    destination=self._active_destination,
                    progress=self._nav_progress,
                    reason=str(reason or "") or None,
                ).to_dict()
            )
            await self._record_event_log(
                event_type="nav_status",
                request_id=request_id,
                status="cancelled",
                payload={
                    "destination": self._active_destination,
                    "progress": self._nav_progress,
                    "reason": str(reason or "") or None,
                },
            )

    async def _send_waypoint_list(self, websocket: WebSocketServerProtocol) -> None:
        msg = WaypointListMessage(waypoints=self._waypoints.list())
        await websocket.send(json.dumps(msg.to_dict()))

    def _infer_location(self, pose: Pose) -> str:
        """Return the name of the nearest waypoint within 2 m, or empty string."""
        _LOCATION_RADIUS_M = 2.0
        best_name = ""
        best_dist = _LOCATION_RADIUS_M

        for wp in self._waypoints.list():
            dist = math.hypot(wp.x - pose.x, wp.y - pose.y)
            if dist < best_dist:
                best_dist = dist
                best_name = wp.name

        return best_name

    def _relative_anchor_pose(self) -> Optional[Pose]:
        """Return a safe anchor pose for resolving relative goals.

        If backend exposes pose age, require it to be fresh to avoid relative
        movement from stale localization.
        """
        if not hasattr(self._backend, "get_pose"):
            return Pose()

        pose = self._backend.get_pose()

        get_pose_age_sec = getattr(self._backend, "get_pose_age_sec", None)
        if callable(get_pose_age_sec):
            age = get_pose_age_sec()
            if age is None or age > self._relative_pose_stale_sec:
                return None

        return pose

    def _build_robot_state_message(self) -> RobotState:
        """Build a RobotState from live backend telemetry.

        RosbridgeBackend exposes get_pose() / get_battery(); MockNavBackend may
        not, so we fall back to zero-value defaults in that case.
        """
        if hasattr(self._backend, "get_pose"):
            pose = self._backend.get_pose()
        else:
            pose = Pose()

        if hasattr(self._backend, "get_battery"):
            battery = self._backend.get_battery()
        else:
            battery = Battery()

        location = self._infer_location(pose)

        return RobotState(
            timestamp=time.time(),
            pose=pose,
            location=location,
            battery=battery,
            nav_state=self._nav_state,
            nav_progress=self._nav_progress,
            nav_destination=self._active_destination,
        )

    async def _send_robot_state(self, websocket: WebSocketServerProtocol) -> None:
        msg = self._build_robot_state_message()
        await websocket.send(json.dumps(msg.to_dict()))

    async def _periodic_state_broadcast(self) -> None:
        """Broadcast robot_state to all connected clients every 2 seconds."""
        while True:
            await asyncio.sleep(2.0)
            if not self._clients:
                continue
            msg = self._build_robot_state_message()
            await self._broadcast(msg.to_dict())

    async def _send_error(self, websocket: WebSocketServerProtocol, request_id: str, error: str, msg: str) -> None:
        await websocket.send(json.dumps(ErrorMessage(request_id=request_id, error=error, message=msg).to_dict()))

    async def _broadcast(self, payload: Dict[str, Any]) -> None:
        message = json.dumps(payload)
        async with self._clients_lock:
            clients = list(self._clients)

        dead = []
        for ws in clients:
            try:
                await ws.send(message)
            except Exception:
                dead.append(ws)

        if dead:
            async with self._clients_lock:
                for ws in dead:
                    self._clients.discard(ws)

    def _process_request(self, path: str, _request_headers: Any) -> Optional[Tuple[HTTPStatus, list, bytes]]:
        if path == self.health_path:
            body = b"OK"
            headers = [("Content-Type", "text/plain"), ("Content-Length", str(len(body)))]
            return HTTPStatus.OK, headers, body

        if path not in ("/", self.ws_path, "/fr"):
            body = b"Not Found"
            headers = [("Content-Type", "text/plain"), ("Content-Length", str(len(body)))]
            return HTTPStatus.NOT_FOUND, headers, body

        return None


def _env(name: str, default: str) -> str:
    v = os.getenv(name)
    if v is None:
        return default
    return v


def _env_bool(name: str, default: bool) -> bool:
    v = os.getenv(name)
    if v is None:
        return default
    return v.strip().lower() in {"1", "true", "yes", "on"}


def main() -> None:
    parser = argparse.ArgumentParser(description="Edge Proxy WebSocket server")
    parser.add_argument("--host", default=_env("EDGE_PROXY_WS_HOST", "0.0.0.0"))
    parser.add_argument("--port", type=int, default=int(_env("EDGE_PROXY_WS_PORT", "8080")))
    parser.add_argument("--ws-path", default=_env("EDGE_PROXY_WS_PATH", "/edge"))
    parser.add_argument("--health-path", default=_env("EDGE_PROXY_HEALTH_PATH", "/health"))
    parser.add_argument(
        "--backend",
        choices=["mock", "rosbridge"],
        default=_env("EDGE_PROXY_BACKEND", "rosbridge"),
    )
    parser.add_argument("--waypoints", default=_env("EDGE_PROXY_WAYPOINTS_PATH", "/data/waypoints.yaml"))
    parser.add_argument(
        "--rosbridge-url",
        default=_env("EDGE_PROXY_ROSBRIDGE_URL", "wss://127.0.0.1:9090"),
    )
    parser.add_argument(
        "--rosbridge-insecure-tls",
        default=str(_env_bool("EDGE_PROXY_ROSBRIDGE_INSECURE_TLS", True)).lower(),
        choices=["true", "false"],
    )

    args = parser.parse_args()

    logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(name)s: %(message)s")

    server = EdgeProxyServer(
        host=args.host,
        port=args.port,
        ws_path=args.ws_path,
        health_path=args.health_path,
        backend=args.backend,
        waypoints_path=args.waypoints,
        rosbridge_url=args.rosbridge_url,
        rosbridge_insecure_tls=str(args.rosbridge_insecure_tls).lower() == "true",
    )

    try:
        asyncio.run(server.serve())
    except KeyboardInterrupt:
        pass


if __name__ == "__main__":
    main()
