# WebSocket client — main event loop, service request/response, DeviceBridge
import asyncio
import base64
import json
import os
import threading
import time
import uuid

from .config import (
    log, _ros2_cache, _ros2_lock, _gw_context,
    _service_futures,
    DEVICE_ID, GATEWAY_URL, API_KEY, MODE, INTERVAL,
)
from .utils import get_lan_ip, rosbridge_port, rosbridge_alive
import ai_teammate_ros2_bridge.config as _cfg
try:
    from ai_teammate_ros2_skills.commands import handle_command as _skills_handle_command
    from ai_teammate_ros2_skills.keyframe import (
        _should_capture_keyframe, _capture_keyframe, _maybe_upload_keyframe_batch,
        _ensure_fresh_wifi_scan,
    )
except ImportError:
    _skills_handle_command = None
    _should_capture_keyframe = None
    _capture_keyframe = None
    _maybe_upload_keyframe_batch = None
    _ensure_fresh_wifi_scan = None
from .device_bridge import handle_command as _bridge_handle_command


_BRIDGE_FIRST_COMMANDS = {"CAPTURE_IMAGE", "CAMERA_CAPTURE", "START_CAMERA_STREAM", "STOP_CAMERA_STREAM"}

async def handle_command(message: dict):
    """Dispatch to skills first, fallback to bridge device_bridge.
    Camera commands go to bridge first (needs shared config cache)."""
    cmd_type = message.get("data", {}).get("command_type", "")
    if cmd_type in _BRIDGE_FIRST_COMMANDS:
        return await _bridge_handle_command(message)
    if _skills_handle_command:
        result = await _skills_handle_command(message)
        if result and result.get("status") != "unknown_command":
            return result
    return await _bridge_handle_command(message)
from .ros2_node import _ros2_spin_thread, is_ros2_healthy, restart_ros2_node
from .sensors import get_ros2_data, get_sim_data


def _on_mqtt_message(client, userdata, msg):
    """Handle incoming MQTT messages — fleet/session broadcast + device commands."""
    try:
        topic = msg.topic
        payload_str = msg.payload.decode()

        # Fleet or session broadcast stop
        if "/broadcast/stop" in topic:
            log.warning(f"BROADCAST STOP received on {topic}: {payload_str}")
            _cfg._estop_event.set()
            pub = _cfg._cmd_vel_pub
            if pub:
                try:
                    for _ in range(5):
                        pub.publish(_cfg.make_cmd_vel_msg())
                except Exception:
                    pass
            log.warning("Stop executed: estop set, zero velocity published")
            return

        # Direct device command (robots/{device_id}/command)
        if topic.endswith("/command"):
            payload = json.loads(payload_str)
            cmd = payload.get("command", "")
            params = payload.get("params", {})
            log.info(f"MQTT device command: cmd={cmd}, params={params}")
            import asyncio
            loop = asyncio.get_event_loop()
            if loop.is_running():
                asyncio.ensure_future(_exec_mqtt_command(cmd, params))
            return

        # Session broadcast — general command
        if "/broadcast/" in topic:
            payload = json.loads(payload_str)
            cmd = payload.get("command", "")
            params = payload.get("params", {})
            log.info(f"Session broadcast: cmd={cmd}, params={params}")
            import asyncio
            loop = asyncio.get_event_loop()
            if loop.is_running():
                asyncio.ensure_future(_exec_mqtt_command(cmd, params))
            return
    except Exception as e:
        log.error(f"MQTT message error: {e}")


async def _exec_mqtt_command(cmd: str, params: dict):
    """Execute a command received via MQTT."""
    try:
        msg = {"type": "command", "data": {"command_type": cmd.upper(), "parameters": params, "command_id": f"mqtt_{int(time.time())}"}}
        result = await handle_command(msg)
        log.info(f"MQTT command result: {cmd} -> {result}")
    except Exception as e:
        log.error(f"MQTT command exec error: {cmd} -> {e}")


def _subscribe_fleet_topic(client):
    """Subscribe to fleet broadcast topic if FLEET_ID is known."""
    fleet_id = _cfg.FLEET_ID or _cfg._gw_context.get("project_id") or _cfg._gw_context.get("fleet_id")
    if fleet_id and not _cfg._mqtt_fleet_subscribed:
        topic = f"fleet/{fleet_id}/broadcast/stop"
        client.subscribe(topic, qos=1)
        _cfg._mqtt_fleet_subscribed = True
        log.info(f"📡 MQTT subscribed to {topic}")


def _subscribe_session_topic(client):
    """Subscribe to session broadcast topics if session_id is known."""
    session_id = _cfg._gw_context.get("session_id")
    if session_id and not getattr(_cfg, '_mqtt_session_subscribed', False):
        topic = f"session/{session_id}/broadcast/+"
        client.subscribe(topic, qos=1)
        _cfg._mqtt_session_subscribed = True
        log.info(f"📡 MQTT subscribed to {topic}")


def _init_mqtt():
    """Initialize MQTT client if MQTT_BROKER is configured."""
    from .config import MQTT_BROKER, MQTT_PORT, MQTT_USER, MQTT_PASS
    if not MQTT_BROKER:
        return None
    try:
        import paho.mqtt.client as mqtt
        client = mqtt.Client(client_id=f"bridge-{DEVICE_ID}", clean_session=True)
        if MQTT_USER:
            client.username_pw_set(MQTT_USER, MQTT_PASS)
        client.on_message = _on_mqtt_message
        client.connect(MQTT_BROKER, MQTT_PORT, keepalive=60)
        client.loop_start()
        _cfg._mqtt_client = client
        log.info(f"MQTT connected to {MQTT_BROKER}:{MQTT_PORT}")
        # Subscribe to direct device commands
        device_topic = f"robots/{DEVICE_ID}/command"
        client.subscribe(device_topic, qos=1)
        log.info(f"MQTT subscribed to {device_topic}")
        # Subscribe to fleet broadcast if FLEET_ID already known
        _subscribe_fleet_topic(client)
        return client
    except Exception as e:
        log.warning(f"📡 MQTT init failed: {e}")
        return None


async def _auto_update_check():
    """Periodically check for bridge + skills updates and self-upgrade if available."""
    import subprocess
    import platform
    import sys
    import tarfile
    import tempfile
    from pathlib import Path
    from . import __version__ as current_version

    # Wait 60s after startup before first check
    await asyncio.sleep(60)

    CHECK_INTERVAL = 3600  # 1 hour
    gw_http = GATEWAY_URL.replace("wss://", "https://").replace("ws://", "http://").replace("/gw", "")

    # Detect skills version
    try:
        from ai_teammate_ros2_skills import __version__ as skills_version
    except ImportError:
        skills_version = "unknown"

    while True:
        needs_restart = False
        try:
            import aiohttp
            url = (
                f"{gw_http}/api/b2b/version-check"
                f"?key={API_KEY}&bridge_version={current_version}"
                f"&skills_version={skills_version}&device_id={DEVICE_ID}"
            )
            async with aiohttp.ClientSession() as session:
                async with session.get(url, timeout=aiohttp.ClientTimeout(total=10)) as resp:
                    if resp.status == 200:
                        data = await resp.json()

                        # --- Bridge update ---
                        if data.get("bridge", {}).get("update_available"):
                            latest = data["bridge"]["latest"]
                            log.info(f"Bridge update available: {current_version} -> {latest}")
                            result = subprocess.run(
                                ["pip3", "install", "--user", "--no-cache-dir",
                                 f"ai-teammate-ros2-bridge=={latest}"],
                                capture_output=True, text=True, timeout=120,
                            )
                            if result.returncode == 0:
                                log.info(f"Bridge updated to {latest}")
                                needs_restart = True
                            else:
                                log.warning(f"Bridge update failed: {result.stderr[:200]}")
                        else:
                            log.debug(f"Bridge up to date: {current_version}")

                        # --- Skills update (.so tarball from server) ---
                        if data.get("skills", {}).get("update_available"):
                            latest_skills = data["skills"]["latest"]
                            log.info(f"Skills update available: {skills_version} -> {latest_skills}")
                            arch = platform.machine()  # x86_64 or aarch64
                            pyver = f"cp{sys.version_info.major}.{sys.version_info.minor}"
                            dl_url = f"{gw_http}/api/b2b/skills/download?key={API_KEY}&arch={arch}&python={pyver}"

                            async with session.get(dl_url, timeout=aiohttp.ClientTimeout(total=300)) as dl_resp:
                                if dl_resp.status == 200:
                                    with tempfile.NamedTemporaryFile(suffix=".tar.gz", delete=False) as tmp:
                                        tmp.write(await dl_resp.read())
                                        tmp_path = tmp.name

                                    # Extract to site-packages
                                    site_packages = Path(sys.modules["ai_teammate_ros2_skills"].__file__).parent.parent
                                    with tarfile.open(tmp_path, "r:gz") as tar:
                                        tar.extractall(path=str(site_packages))
                                    Path(tmp_path).unlink(missing_ok=True)
                                    log.info(f"Skills updated to {latest_skills}")
                                    needs_restart = True
                                else:
                                    log.warning(f"Skills download failed: HTTP {dl_resp.status}")

                        # --- Restart if anything was updated ---
                        if needs_restart:
                            log.info("Restarting bridge service after update...")
                            subprocess.Popen(
                                ["systemctl", "--user", "restart", "ai-teammate-bridge"],
                                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
                            )
                            return
        except ImportError:
            log.debug("aiohttp not installed, skipping auto-update check")
            return
        except Exception as e:
            log.debug(f"Auto-update check failed: {e}")

        await asyncio.sleep(CHECK_INTERVAL)


def _mqtt_publish(topic_suffix: str, payload: dict, retain: bool = False):
    """Publish to MQTT if connected. Topic: robots/{device_id}/{suffix}."""
    client = _cfg._mqtt_client
    if not client:
        return
    try:
        topic = f"robots/{DEVICE_ID}/{topic_suffix}"
        client.publish(topic, json.dumps(payload, default=str), qos=0, retain=retain)
    except Exception:
        pass


def _check_sensor_health() -> dict:
    """Check all sensor channels and return health status + alerts."""
    now = time.time()
    STALE_THRESHOLD = 10.0  # seconds

    with _ros2_lock:
        imu_ts = _ros2_cache.get("imu_ts", 0)
        odom_ts = _ros2_cache.get("odom_ts", 0)
        scan_ts = _ros2_cache.get("scan_ts", 0)
        cam_ts = _ros2_cache.get("camera_frame_ts", 0)

    # Camera is on-demand — only report if active, never affects all_ok
    from .ros2_node import is_camera_active
    core_sensors = {
        "imu": {"ok": (now - imu_ts) < STALE_THRESHOLD, "age": round(now - imu_ts, 1) if imu_ts else None},
        "odom": {"ok": (now - odom_ts) < STALE_THRESHOLD, "age": round(now - odom_ts, 1) if odom_ts else None},
        "lidar": {"ok": (now - scan_ts) < STALE_THRESHOLD, "age": round(now - scan_ts, 1) if scan_ts else None},
    }
    if is_camera_active():
        cam_ok = cam_ts > 0 and (now - cam_ts) < STALE_THRESHOLD
        core_sensors["camera"] = {"ok": cam_ok, "age": round(now - cam_ts, 1) if cam_ts else None, "mode": "on-demand"}
    else:
        core_sensors["camera"] = {"ok": True, "age": None, "mode": "on-demand"}

    ros2_ok = is_ros2_healthy()
    alerts = []
    for name, s in core_sensors.items():
        if s.get("mode") == "on-demand":
            continue  # on-demand sensors don't trigger alerts
        if not s["ok"]:
            age_str = f"{s['age']}s" if s["age"] is not None else "never received"
            alerts.append(f"{name}: no data ({age_str})")
    if not ros2_ok:
        alerts.append("ros2: node unhealthy")

    # all_ok excludes on-demand sensors (camera)
    essential = {k: v for k, v in core_sensors.items() if v.get("mode") != "on-demand"}
    return {
        "ros2_ok": ros2_ok,
        "sensors": core_sensors,
        "alerts": alerts,
        "all_ok": ros2_ok and all(s["ok"] for s in essential.values()),
    }


def _draw_bbox_overlay(jpeg_bytes: bytes, follow_state: dict) -> bytes:
    """Draw tracking bboxes on JPEG frame. Returns new JPEG bytes."""
    try:
        import cv2
        import numpy as np
        frame = cv2.imdecode(np.frombuffer(jpeg_bytes, np.uint8), cv2.IMREAD_COLOR)
        if frame is None:
            return jpeg_bytes

        target_id = follow_state.get("target_id")
        tracks = follow_state.get("tracks", [])

        for t in tracks:
            x1, y1, x2, y2 = [int(v) for v in t["bbox"]]
            tid = t["id"]
            is_target = (tid == target_id)
            color = (0, 255, 0) if is_target else (128, 128, 128)
            thickness = 2 if is_target else 1

            cv2.rectangle(frame, (x1, y1), (x2, y2), color, thickness)
            label = f"ID:{tid}"
            if is_target:
                label += " [TARGET]"
            cv2.putText(frame, label, (x1, y1 - 8), cv2.FONT_HERSHEY_SIMPLEX,
                        0.5, color, 1, cv2.LINE_AA)

        _, jpeg = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 60])
        return jpeg.tobytes()
    except Exception:
        return jpeg_bytes


# ── Service request/response ──

async def _send_service_request(data: dict, timeout: float = 15.0) -> dict:
    """Send service_request WS message and wait for service_response."""
    ws = _cfg._ws_ref
    if ws is None:
        return {"status": "error", "message": "WebSocket not connected"}

    request_id = str(uuid.uuid4())[:8]
    data["request_id"] = request_id

    future = asyncio.get_event_loop().create_future()
    _service_futures[request_id] = future

    try:
        await ws.send(json.dumps({"type": "service_request", "data": data}))
        result = await asyncio.wait_for(future, timeout=timeout)
        return result
    except asyncio.TimeoutError:
        return {"status": "error", "message": "service request timeout"}
    except Exception as e:
        return {"status": "error", "message": f"service request failed: {e}"}
    finally:
        _service_futures.pop(request_id, None)


def _handle_service_response(data: dict):
    """Route service_response to pending future."""
    request_id = data.get("request_id", "")
    future = _service_futures.get(request_id)
    if future and not future.done():
        future.set_result(data)


# ── DeviceBridge class ──

class DeviceBridge:
    """Main device bridge — connects ROS2 robot to AI Teammate Gateway."""

    def __init__(self, device_id: str, gateway_url: str, api_key: str, mode: str = "rclpy"):
        self.device_id = device_id
        self.gateway_url = gateway_url
        self.api_key = api_key
        self.mode = mode

    @classmethod
    def from_env(cls):
        return cls(
            device_id=DEVICE_ID,
            gateway_url=GATEWAY_URL,
            api_key=API_KEY,
            mode=MODE,
        )

    def run(self):
        log.info(f"Bridge v2 starting: device={self.device_id} mode={self.mode}")
        asyncio.run(self._run())

    async def _run(self):
        import websockets

        url = f"{self.gateway_url}/ws/{self.device_id}"
        tick = 0

        # ── Proxy support (enterprise networks) ──
        # websockets lib doesn't auto-read HTTPS_PROXY; route via python-socks if set.
        proxy_url = os.environ.get("HTTPS_PROXY") or os.environ.get("HTTP_PROXY")
        ws_connect_kwargs = {"ping_interval": 20, "ping_timeout": 10, "max_size": 2**20}
        if proxy_url:
            try:
                from python_socks.async_.asyncio import Proxy  # type: ignore
                log.info(f"WS routing via proxy: {proxy_url}")
                # websockets >= 12 supports `sock=` from Proxy.connect()
                ws_connect_kwargs["sock"] = None  # placeholder; built per-iteration below
                self._proxy_url = proxy_url
            except ImportError:
                log.warning("python-socks not installed — proxy ignored for WS")
                self._proxy_url = None
        else:
            self._proxy_url = None

        # Initialize MQTT publisher (optional)
        _init_mqtt()

        # Start auto-update checker (background)
        asyncio.create_task(_auto_update_check())

        # Start LAN WS server — phone 등 같은 AP 내 클라이언트가 bridge에 직접 접속.
        # asyncio.create_task 는 weak-ref 만 잡아 GC 가 수거하면 "Task was destroyed
        # but it is pending" 로 죽음 — self 에 강참조를 걸어둬야 계속 산다.
        from .lan_server import run_lan_server
        self._lan_server_task = asyncio.create_task(run_lan_server())

        # Start rclpy spin thread
        if self.mode in ("rclpy", "rosbridge"):
            t = threading.Thread(target=_ros2_spin_thread, daemon=True)
            t.start()
            log.info("rclpy spin thread started, waiting for node...")
            for _ in range(50):
                if _cfg._cmd_vel_pub is not None:
                    break
                await asyncio.sleep(0.1)
            if _cfg._cmd_vel_pub:
                log.info("rclpy node ready")
            else:
                log.warning("rclpy node not ready after 5s, continuing anyway")

        while True:
            try:
                connect_kwargs = {"ping_interval": 20, "ping_timeout": 10, "max_size": 2**20}
                if self._proxy_url:
                    try:
                        from python_socks.async_.asyncio import Proxy  # type: ignore
                        from urllib.parse import urlparse
                        parsed = urlparse(url if url.startswith(("ws://", "wss://")) else "wss://" + url)
                        host = parsed.hostname
                        port = parsed.port or (443 if parsed.scheme == "wss" else 80)
                        proxy = Proxy.from_url(self._proxy_url)
                        sock = await proxy.connect(dest_host=host, dest_port=port)
                        connect_kwargs["sock"] = sock
                        connect_kwargs["server_hostname"] = host if parsed.scheme == "wss" else None
                    except Exception as pe:
                        log.warning(f"Proxy connect failed, trying direct: {pe}")
                async with websockets.connect(url, **connect_kwargs) as ws:
                    log.info(f"Connected to {self.gateway_url} as {self.device_id} (mode={self.mode})")

                    # Auth (include bridge version for version tracking)
                    from . import __version__ as _bridge_version
                    await ws.send(json.dumps({
                        "type": "auth",
                        "data": {"api_key": self.api_key, "bridge_version": _bridge_version},
                    }))
                    log.info(f"Auth message sent (v{_bridge_version})")

                    try:
                        welcome = await asyncio.wait_for(ws.recv(), timeout=5)
                        log.info(f"Welcome received: {str(welcome)[:200]}")
                        try:
                            wdata = json.loads(welcome) if isinstance(welcome, str) else welcome
                            ctx = wdata.get("data", {}).get("context", {})
                            if ctx:
                                _gw_context.update(ctx)
                                log.info(f"Gateway context: {_gw_context}")
                        except (json.JSONDecodeError, AttributeError):
                            pass
                    except asyncio.TimeoutError:
                        log.warning("Welcome message timeout")

                    # Apply calibration from context if available
                    cal = _gw_context.get("calibration")
                    if cal and isinstance(cal, dict):
                        if cal.get("rotation_scale") and 0.5 <= cal["rotation_scale"] <= 2.0:
                            _cfg.ROTATION_SCALE = cal["rotation_scale"]
                        if cal.get("distance_scale") and 0.5 <= cal["distance_scale"] <= 2.0:
                            _cfg.DISTANCE_SCALE = cal["distance_scale"]
                        log.info(f"Calibration loaded: rot={_cfg.ROTATION_SCALE}, dist={_cfg.DISTANCE_SCALE}")

                    # Late-subscribe to fleet/session broadcast from welcome context
                    if _cfg._mqtt_client:
                        if not _cfg._mqtt_fleet_subscribed:
                            _subscribe_fleet_topic(_cfg._mqtt_client)
                        if not _cfg._mqtt_session_subscribed:
                            _subscribe_session_topic(_cfg._mqtt_client)

                    # Set WS ref
                    _cfg._ws_ref = ws

                    _LONG_COMMANDS = {
                        "MOVE", "TURN", "SAFE_MOVE", "SAFE_NAVIGATE_PATH", "SAFE_CIRCLE", "CIRCLE",
                        "NAVIGATE_PATH", "PATROL", "AUTO_EXPLORE",
                        "SLAM_SAVE", "NAV2_START", "NAV2_STOP", "NAV2_GOAL", "GO_TO_LOCATION", "DOCK", "UNDOCK",
                        "KEYFRAME_START", "KEYFRAME_STOP", "MAP_UPLOAD", "MAP_DOWNLOAD",
                        "CALIBRATE_ROTATION", "CALIBRATE_DISTANCE", "CALIBRATE_FULL",
                        "SAVE_FINGERPRINT", "SCAN_BLE",
                        "DETECT_OBJECTS", "DETECT_AND_INGEST",
                    }

                    async def _run_and_respond(message):
                        try:
                            result = await handle_command(message)
                            if result:
                                resp = json.dumps({
                                    "type": "command_response",
                                    "data": {
                                        "command_id": message.get("data", {}).get("command_id", ""),
                                        **result,
                                    },
                                })
                                await ws.send(resp)
                                log.info(f"Command response sent: {result.get('status')}")
                        except Exception as e:
                            log.error(f"Command execution error: {e}")

                    async def on_recv(raw):
                        log.info(f"WS RECV: {str(raw)[:120]}")
                        try:
                            message = json.loads(raw)
                            msg_type = message.get("type", "")

                            if msg_type in ("pong",):
                                return

                            if msg_type == "service_response":
                                _handle_service_response(message.get("data", {}))
                                return

                            cmd_type = message.get("data", {}).get("command_type", "")

                            if cmd_type in _LONG_COMMANDS:
                                asyncio.create_task(_run_and_respond(message))
                            else:
                                await _run_and_respond(message)
                        except json.JSONDecodeError:
                            pass

                    async def recv_loop():
                        try:
                            async for raw in ws:
                                await on_recv(raw)
                        except Exception as e:
                            log.warning(f"recv_loop error: {e}")

                    recv_task = asyncio.create_task(recv_loop())

                    # Camera stream runs on its own timer (independent of status interval)
                    last_frame_ts = 0.0
                    status_tick = 0
                    _prev_ros2_ok: bool | None = None
                    _prev_alerts: tuple = ()
                    # Reset slam sent flag so map is re-sent on reconnect
                    with _ros2_lock:
                        _ros2_cache.pop("_slam_map_sent", None)
                    _rosbridge_alive_cache = False
                    _lan_ip_cache = "127.0.0.1"
                    from .lan_server import DEFAULT_PORT as _LAN_PORT

                    while True:
                        now = time.time()

                        # Status update (every INTERVAL)
                        if self.mode in ("rclpy", "rosbridge"):
                            data = get_ros2_data()
                            if not data:
                                data = get_sim_data(status_tick)
                        else:
                            data = get_sim_data(status_tick)

                        status_data = {"device_id": self.device_id, "timestamp": int(now * 1000), **data}

                        # Capability advertisement (re-probed every 30s):
                        # - bridge_lan_port: LAN WS skill 경로 (폰 앱 등 LAN direct용)
                        # - rosbridge: rosbridge_server 포트 (reachable일 때만)
                        if status_tick == 0 or status_tick % 30 == 0:
                            _rosbridge_alive_cache = rosbridge_alive()
                            _lan_ip_cache = get_lan_ip()
                        caps = {
                            "lan_ip": _lan_ip_cache,
                            "bridge_lan_port": _LAN_PORT,
                        }
                        if _rosbridge_alive_cache:
                            _rb_port = rosbridge_port()
                            caps["rosbridge"] = {
                                "url": f"ws://{_lan_ip_cache}:{_rb_port}",
                                "port": _rb_port,
                            }
                        status_data["capabilities"] = caps

                        # MQTT: primary telemetry path (1Hz)
                        _mqtt_publish("telemetry", {
                            k: data[k] for k in ("imu", "odometry", "battery", "temperature") if k in data
                        })
                        if "odometry" in data:
                            odom = data["odometry"]
                            _mqtt_publish("position", {
                                "x": odom.get("x", 0), "y": odom.get("y", 0), "theta": odom.get("theta", 0),
                            }, retain=True)

                        # WS: fallback telemetry (every 10s) — MQTT이 주 경로
                        if _cfg._mqtt_client and status_tick % 10 == 0:
                            await ws.send(json.dumps({"type": "status_update", "data": status_data}))
                        elif not _cfg._mqtt_client:
                            await ws.send(json.dumps({"type": "status_update", "data": status_data}))

                        # Camera stream — fps-based timing, bbox overlay
                        with _ros2_lock:
                            streaming = _ros2_cache.get("camera_stream", False)
                            stream_fps = _ros2_cache.get("camera_stream_fps", 2)
                            frame = _ros2_cache.get("camera_frame") if streaming else None
                            follow_state = _ros2_cache.get("follow_state", {}) if streaming else {}
                        frame_interval = 1.0 / max(1, stream_fps)
                        if frame and (now - last_frame_ts) >= frame_interval:
                            send_frame = frame
                            # Draw bbox overlay if following a target
                            if follow_state.get("tracking") and follow_state.get("bbox"):
                                send_frame = _draw_bbox_overlay(frame, follow_state)
                            await ws.send(json.dumps({
                                "type": "frame_data",
                                "data": {
                                    "device_id": self.device_id,
                                    "image_base64": base64.b64encode(send_frame).decode('ascii'),
                                    "format": "jpeg",
                                    "timestamp": int(now * 1000),
                                },
                            }))
                            last_frame_ts = now

                        # SLAM live: scan points + pose (every 2 ticks = 2s)
                        if status_tick % 2 == 0:
                            with _ros2_lock:
                                scan_pts = _ros2_cache.get("scan_points")
                                odom = _ros2_cache.get("odometry")
                            if scan_pts and odom:
                                await ws.send(json.dumps({
                                    "type": "slam_live",
                                    "data": {
                                        "device_id": self.device_id,
                                        "scan_points": scan_pts,
                                        "pose": {
                                            "x": odom["x"], "y": odom["y"],
                                            "theta": odom.get("theta", 0),
                                        },
                                    },
                                }))

                        # SLAM map relay (throttled: every 5 status ticks = 5s)
                        if status_tick % 5 == 0:
                            with _ros2_lock:
                                slam_map = _ros2_cache.get("slam_map")
                                slam_ts = _ros2_cache.get("slam_map_ts", 0)
                                slam_sent = _ros2_cache.get("_slam_map_sent", False)
                            # Send if: fresh (<10s) OR first time (never sent yet)
                            if slam_map and ((now - slam_ts) < 10.0 or not slam_sent):
                                try:
                                    # Include map-frame pose if available
                                    with _ros2_lock:
                                        slam_pose = _ros2_cache.get("slam_pose")
                                    slam_data = {
                                        "device_id": self.device_id,
                                        **slam_map,
                                    }
                                    if slam_pose:
                                        slam_data["robot_pose"] = slam_pose
                                    slam_payload = json.dumps({
                                        "type": "slam_map",
                                        "data": slam_data,
                                    })
                                    await ws.send(slam_payload)
                                    with _ros2_lock:
                                        _ros2_cache["_slam_map_sent"] = True
                                    log.info(f"SLAM map sent: {slam_map.get('width')}x{slam_map.get('height')} ({len(slam_payload)} bytes)")
                                except Exception as e:
                                    log.error(f"SLAM map send error: {e}")
                            elif status_tick % 50 == 0:
                                log.info(f"SLAM map skip: has_map={slam_map is not None}, age={now - slam_ts:.1f}s")

                        # Keyframe capture
                        if _should_capture_keyframe and _cfg._kf_capture_active and _should_capture_keyframe():
                            if _ensure_fresh_wifi_scan:
                                await _ensure_fresh_wifi_scan()
                            _capture_keyframe()
                            await _maybe_upload_keyframe_batch()

                        # Health check every 30 ticks (30s), skip first 60s grace period
                        if status_tick % 30 == 0 and status_tick >= 60 and self.mode in ("rclpy", "rosbridge"):
                            health = _check_sensor_health()
                            # Always publish health to MQTT (retained) so fleet summary stays current
                            _mqtt_publish("health", health, retain=True)
                            if health["alerts"]:
                                log.warning(f"Health alerts: {health['alerts']}")
                            # Send health_alert only when state changes (edge-triggered)
                            _cur_ros2_ok = health.get("ros2_ok", True)
                            _cur_alerts = tuple(health.get("alerts", []))
                            if (_cur_ros2_ok, _cur_alerts) != (_prev_ros2_ok, _prev_alerts):
                                health_data = {
                                    "device_id": self.device_id,
                                    "health": health,
                                    "timestamp": int(time.time() * 1000),
                                }
                                await ws.send(json.dumps({"type": "health_alert", "data": health_data}))
                                _prev_ros2_ok = _cur_ros2_ok
                                _prev_alerts = _cur_alerts
                            # Auto-recover ROS2 if dead
                            if not health["ros2_ok"]:
                                log.warning("ROS2 unhealthy — auto-restarting...")
                                restart_ros2_node()
                                for _ in range(50):
                                    if _cfg._cmd_vel_pub is not None:
                                        break
                                    await asyncio.sleep(0.1)

                        if status_tick % 10 == 0:
                            log.info(f"Sent {status_tick} status_updates")
                        # Auto-stop camera if idle
                        if status_tick % 30 == 0:
                            from .ros2_node import check_camera_idle
                            check_camera_idle()
                        status_tick += 1
                        await asyncio.sleep(INTERVAL)

                        if recv_task.done():
                            break

            except Exception as e:
                # Clear WS ref so pending service_requests fail fast
                _cfg._ws_ref = None
                # Cancel all pending service request futures
                for rid, fut in list(_service_futures.items()):
                    if not fut.done():
                        fut.set_result({"status": "error", "message": "WebSocket disconnected"})
                _service_futures.clear()

                log.warning(f"Disconnected: {e}, reconnecting in 5s...")
                # Check ROS2 health on reconnect
                if self.mode in ("rclpy", "rosbridge") and not is_ros2_healthy():
                    log.warning("ROS2 node unhealthy after disconnect — restarting...")
                    restart_ros2_node()
                    for _ in range(50):
                        if _cfg._cmd_vel_pub is not None:
                            break
                        await asyncio.sleep(0.1)
                    if _cfg._cmd_vel_pub:
                        log.info("ROS2 node recovered")
                    else:
                        log.error("ROS2 node failed to recover")
                await asyncio.sleep(5)
