# Command handler — central dispatcher for all device commands
import asyncio
import base64
import json
import math
import os
import subprocess
import time

from .config import (
    log, _ros2_cache, _ros2_lock, _estop_event,
    _subprocesses, _gw_context, _keyframe_buffer,
    OBSTACLE_DISTANCE, OBSTACLE_WAIT_TIMEOUT, DETECT_COOLDOWN, FINGERPRINTS_FILE, MODE,
)
import ai_teammate_ros2_bridge.config as _cfg
from .motion import (
    publish_cmd_vel, _check_front_obstacle, _wait_for_clear,
    _handle_safe_move, _handle_safe_navigate_path, _handle_navigate_path,
)
from .slam import (
    _launch_ros2, _kill_ros2, _handle_slam, _handle_nav2,
    _upload_map_files, _load_locations, _save_locations,
)
from .keyframe import _flush_keyframe_buffer, _kf_wifi_scan_loop
from .sensors import get_ros2_data
from .utils import _safe_name, _safe_dir


async def handle_command(message: dict):
    """Handle incoming command from Device Gateway."""
    msg_type = message.get("type", "")
    data = message.get("data", {})

    if msg_type == "command":
        command = data.get("command_type", "") or data.get("command", "")
        params = data.get("parameters", {}) or data.get("params", {})
        log.info(f"Received command: {command} params={params}")

        if command in ("EMERGENCY_STOP", "ESTOP"):
            _estop_event.set()
            from geometry_msgs.msg import Twist
            pub = _cfg._cmd_vel_pub
            if pub:
                for _ in range(5):
                    pub.publish(Twist())
            log.warning("EMERGENCY STOP triggered")
            return {"status": "ok", "message": "emergency stop executed, all motion interrupted"}

        elif command == "MOVE":
            distance = float(params.get("distance", 0))
            speed = float(params.get("speed", 0.3))
            if distance == 0:
                return {"status": "error", "message": "distance is 0"}
            direction = 1.0 if distance > 0 else -1.0
            duration = abs(distance) / speed
            loop = asyncio.get_event_loop()
            result = await loop.run_in_executor(None, publish_cmd_vel, direction * speed, 0.0, duration)
            return {"status": result, "distance": distance, "speed": speed, "duration": round(duration, 2)}

        elif command == "TURN":
            angle_deg = float(params.get("angle_deg", 0))
            speed = float(params.get("speed", 0.5))
            if angle_deg == 0:
                return {"status": "error", "message": "angle is 0"}
            direction = 1.0 if angle_deg > 0 else -1.0
            duration = abs(math.radians(angle_deg)) / speed
            loop = asyncio.get_event_loop()
            result = await loop.run_in_executor(None, publish_cmd_vel, 0.0, direction * speed, duration)
            return {"status": result, "angle_deg": angle_deg, "duration": round(duration, 2)}

        elif command == "STOP":
            _estop_event.set()
            from geometry_msgs.msg import Twist
            pub = _cfg._cmd_vel_pub
            if pub:
                for _ in range(5):
                    pub.publish(Twist())
            log.warning("STOP triggered — estop event set, zero velocity published")
            return {"status": "ok", "message": "stop executed, all motion interrupted"}

        elif command == "GET_POSE":
            with _ros2_lock:
                odom = _ros2_cache.get("odometry", {})
            return {"status": "ok", "x": odom.get("x", 0), "y": odom.get("y", 0), "theta": odom.get("theta", 0),
                    "linear_vel": odom.get("linear_vel", 0), "angular_vel": odom.get("angular_vel", 0)}

        elif command == "SAFE_MOVE":
            distance = float(params.get("distance", 0))
            speed = float(params.get("speed", 0.3))
            step = float(params.get("step", 0.3))
            threshold = float(params.get("threshold", OBSTACLE_DISTANCE))
            if distance == 0:
                return {"status": "error", "message": "distance is 0"}
            return await _handle_safe_move(distance, speed, step, threshold)

        elif command == "SAFE_NAVIGATE_PATH":
            waypoints = params.get("waypoints", [])
            speed = float(params.get("speed", 0.3))
            step = float(params.get("step", 0.3))
            threshold = float(params.get("threshold", OBSTACLE_DISTANCE))
            if len(waypoints) < 2:
                return {"status": "error", "message": "at least 2 waypoints required"}
            return await _handle_safe_navigate_path(waypoints, speed, step, threshold)

        elif command == "CHECK_OBSTACLE":
            threshold = float(params.get("threshold", OBSTACLE_DISTANCE))
            with _ros2_lock:
                scan = _ros2_cache.get("scan", {})
            front = scan.get("front_min_distance")
            overall = scan.get("min_distance")
            blocked = front is not None and front < threshold
            return {
                "status": "ok",
                "blocked": blocked,
                "front_distance": front,
                "min_distance": overall,
                "threshold": threshold,
                "message": f"Obstacle at {front:.2f}m — BLOCKED" if blocked else f"Clear (front: {front:.2f}m)" if front else "No scan data",
            }

        elif command in ("GET_SCAN", "SCAN_AREA"):
            with _ros2_lock:
                scan = _ros2_cache.get("scan")
            if scan:
                return {"status": "ok", **scan}
            return {"status": "ok", "message": "no scan data available", "ranges": []}

        elif command in ("CAPTURE_IMAGE", "CAMERA_CAPTURE"):
            with _ros2_lock:
                cam_info = _ros2_cache.get("camera", {})
                frame = _ros2_cache.get("camera_frame")
                frame_ts = _ros2_cache.get("camera_frame_ts", 0)
            if frame:
                age = time.time() - frame_ts
                b64 = base64.b64encode(frame).decode('ascii')
                return {
                    "status": "ok",
                    **cam_info,
                    "image_base64": b64,
                    "image_size_bytes": len(frame),
                    "image_age_sec": round(age, 1),
                    "format": "jpeg",
                }
            if cam_info.get("available"):
                return {"status": "ok", **cam_info, "message": "camera active but no frame yet"}
            return {"status": "ok", "message": "no camera data available"}

        elif command == "START_CAMERA_STREAM":
            fps = int(params.get("fps", 2))
            with _ros2_lock:
                _ros2_cache["camera_stream"] = True
                _ros2_cache["camera_stream_fps"] = fps
            log.info(f"Camera stream started at {fps} fps")
            return {"status": "ok", "message": f"Camera streaming at {fps} fps", "fps": fps}

        elif command == "STOP_CAMERA_STREAM":
            with _ros2_lock:
                _ros2_cache["camera_stream"] = False
            log.info("Camera stream stopped")
            return {"status": "ok", "message": "Camera streaming stopped"}

        elif command == "START_FOLLOW":
            target = params.get("target", "person")
            max_speed = float(params.get("max_speed", 0.3))
            follow_distance = float(params.get("follow_distance", 2.0))
            with _ros2_lock:
                _ros2_cache["follow_state"] = {"active": True, "target": target, "max_speed": max_speed, "follow_distance": follow_distance}
            log.info(f"Follow started: target={target} speed={max_speed} distance={follow_distance}")
            return {"status": "ok", "target": target, "max_speed": max_speed, "follow_distance": follow_distance}

        elif command == "STOP_FOLLOW":
            with _ros2_lock:
                _ros2_cache["follow_state"] = {"active": False}
            loop = asyncio.get_event_loop()
            await loop.run_in_executor(None, publish_cmd_vel, 0.0, 0.0, 0)
            log.info("Follow stopped")
            return {"status": "ok", "message": "follow stopped"}

        elif command == "GET_FOLLOW_STATUS":
            with _ros2_lock:
                state = _ros2_cache.get("follow_state", {"active": False})
            return {"status": "ok", **state}

        elif command in ("CIRCLE", "SAFE_CIRCLE"):
            radius = float(params.get("radius", 1.0))
            speed = float(params.get("speed", 0.3))
            direction = 1.0 if params.get("direction", "ccw") == "ccw" else -1.0
            laps = float(params.get("laps", 1.0))
            threshold = float(params.get("threshold", OBSTACLE_DISTANCE))
            safe = command == "SAFE_CIRCLE" or params.get("safe", False)
            if radius <= 0:
                return {"status": "error", "message": "radius must be positive"}
            angular = speed / radius * direction
            total_duration = 2 * math.pi * radius * laps / speed

            if safe:
                step_duration = 0.5
                elapsed = 0.0
                loop = asyncio.get_event_loop()
                while elapsed < total_duration:
                    if _estop_event.is_set():
                        return {"status": "estop", "elapsed": round(elapsed, 1), "total": round(total_duration, 1)}
                    blocked, front_dist = _check_front_obstacle(threshold)
                    if blocked:
                        await loop.run_in_executor(None, publish_cmd_vel, 0.0, 0.0, 0)
                        log.warning(f"SAFE_CIRCLE: obstacle at {front_dist:.2f}m — waiting for clear")
                        wait_result = await loop.run_in_executor(None, _wait_for_clear, threshold)
                        if wait_result == "estop":
                            return {"status": "estop", "elapsed": round(elapsed, 1), "total": round(total_duration, 1)}
                        if wait_result == "timeout":
                            return {"status": "obstacle_timeout", "elapsed": round(elapsed, 1), "total": round(total_duration, 1),
                                    "front_distance": front_dist, "message": f"Obstacle not cleared after {OBSTACLE_WAIT_TIMEOUT}s"}
                        log.info("SAFE_CIRCLE: obstacle cleared, resuming")
                    chunk = min(step_duration, total_duration - elapsed)
                    result = await loop.run_in_executor(None, publish_cmd_vel, speed, angular, chunk)
                    if result == "estop":
                        return {"status": "estop", "elapsed": round(elapsed + chunk, 1), "total": round(total_duration, 1)}
                    elapsed += chunk
                return {"status": "ok", "radius": radius, "laps": laps, "duration": round(total_duration, 1)}
            else:
                loop = asyncio.get_event_loop()
                result = await loop.run_in_executor(None, publish_cmd_vel, speed, angular, total_duration)
                return {"status": result, "radius": radius, "speed": speed, "laps": laps,
                        "duration": round(total_duration, 1), "direction": "ccw" if direction > 0 else "cw"}

        elif command == "NAVIGATE_PATH":
            return await _handle_navigate_path(params)

        elif command == "PATROL":
            locations = params.get("locations", [])
            waypoints = params.get("waypoints", [])
            if not waypoints and not locations:
                return {"status": "error", "message": "waypoints or locations required"}
            if waypoints:
                return await _handle_navigate_path({"waypoints": waypoints, "speed": params.get("speed", 0.3)})
            return {"status": "error", "message": "location-based patrol not yet implemented, use waypoints"}

        elif command in ("SLAM_START", "SLAM_STOP", "SLAM_SAVE"):
            return await _handle_slam(command, params)

        # ── Keyframe capture ──
        elif command == "KEYFRAME_START":
            from .ws_client import _send_service_request
            if _cfg._kf_capture_active:
                return {"status": "ok", "message": "already active", "buffer": len(_keyframe_buffer)}
            _cfg._kf_capture_active = True
            with _ros2_lock:
                cam = _ros2_cache.get("camera", {})
            intrinsics = params.get("camera_intrinsics") or {
                "fx": cam.get("width", 640) * 0.8,
                "fy": cam.get("height", 480) * 0.8,
                "cx": cam.get("width", 640) / 2.0,
                "cy": cam.get("height", 480) / 2.0,
            }
            init_result = await _send_service_request({
                "service": "vslam_session_init",
                "camera_intrinsics": intrinsics,
            }, timeout=10.0)
            asyncio.create_task(_kf_wifi_scan_loop())
            log.info("Keyframe capture started")
            return {"status": "ok", "message": "keyframe capture started",
                    "vslam_init": init_result.get("status"),
                    "session_id": _gw_context.get("session_id")}

        elif command == "KEYFRAME_STOP":
            from .ws_client import _send_service_request
            _cfg._kf_capture_active = False
            await _flush_keyframe_buffer()
            stop_result = await _send_service_request({"service": "vslam_session_stop"}, timeout=10.0)
            log.info("Keyframe capture stopped")
            return {"status": "ok", "message": "keyframe capture stopped",
                    "total_captured": _cfg._kf_total_captured,
                    "vslam_stop": stop_result.get("status")}

        elif command == "KEYFRAME_STATUS":
            return {
                "status": "ok",
                "active": _cfg._kf_capture_active,
                "buffer_size": len(_keyframe_buffer),
                "total_captured": _cfg._kf_total_captured,
                "last_upload": _cfg._kf_last_upload_ts,
                "session_id": _gw_context.get("session_id"),
            }

        # ── Map management ──
        elif command == "MAP_UPLOAD":
            map_name = _safe_name(params.get("map_name", "map"))
            save_dir = _safe_dir(params.get("save_dir", "~/maps"))
            result = await _upload_map_files(save_dir, map_name, params.get("metadata"))
            return result

        elif command == "MAP_LIST":
            from .ws_client import _send_service_request
            result = await _send_service_request({
                "service": "map_list",
                "offset": params.get("offset", 0),
                "limit": params.get("limit", 20),
            })
            return result

        elif command == "MAP_DOWNLOAD":
            from .ws_client import _send_service_request
            import struct
            map_id = _safe_name(params.get("map_id", ""), default="")
            if not map_id:
                return {"status": "error", "message": "map_id required"}
            save_dir = _safe_dir(params.get("save_dir", "~/maps"))
            os.makedirs(save_dir, exist_ok=True)
            result = await _send_service_request({
                "service": "map_download",
                "map_id": map_id,
            }, timeout=60.0)
            if result.get("status") != "ok":
                return result
            payload = base64.b64decode(result["data_base64"])
            try:
                hdr_len = struct.unpack(">I", payload[:4])[0]
                hdr = json.loads(payload[4:4 + hdr_len])
                offset = 4 + hdr_len
                saved_files = []
                for f_info in hdr.get("files", []):
                    fname = _safe_name(f_info["name"], default="data")
                    fsize = f_info["size"]
                    fdata = payload[offset:offset + fsize]
                    offset += fsize
                    fpath = os.path.join(save_dir, f"downloaded_{map_id}.{fname}")
                    with open(fpath, "wb") as fp:
                        fp.write(fdata)
                    saved_files.append(fpath)
                return {"status": "ok", "files": saved_files, "size_bytes": result["size_bytes"]}
            except Exception:
                fpath = os.path.join(save_dir, f"downloaded_{map_id}.bin")
                with open(fpath, "wb") as fp:
                    fp.write(payload)
                return {"status": "ok", "files": [fpath], "size_bytes": len(payload)}

        elif command in ("NAV2_GOAL", "NAV2_CANCEL", "NAV2_STATUS"):
            return await _handle_nav2(command, params)

        elif command == "GET_STATUS":
            data = get_ros2_data()
            slam_active = _subprocesses.get("slam") is not None and _subprocesses["slam"].poll() is None
            nav2_active = _subprocesses.get("nav2") is not None and _subprocesses["nav2"].poll() is None
            speed_limit = _ros2_cache.get("speed_limit", None)
            return {"status": "ok", "mode": MODE, "ros2_connected": _cfg._ros2_node is not None,
                    "slam_active": slam_active, "nav2_active": nav2_active,
                    "speed_limit": speed_limit, "sensors": data or {}}

        # ── Named Locations ──
        elif command == "SAVE_LOCATION":
            name = params.get("name", "").strip()
            if not name:
                return {"status": "error", "message": "name required"}
            with _ros2_lock:
                odom = _ros2_cache.get("odometry", {})
            loc = {"x": odom.get("x", 0), "y": odom.get("y", 0), "theta": odom.get("theta", 0)}
            locs = _load_locations()
            locs[name] = loc
            _save_locations(locs)
            return {"status": "ok", "name": name, **loc}

        elif command == "GO_TO_LOCATION":
            name = params.get("name", "").strip()
            locs = _load_locations()
            if name not in locs:
                return {"status": "error", "message": f"Location '{name}' not found", "available": list(locs.keys())}
            loc = locs[name]
            return await _handle_nav2("NAV2_GOAL", {"x": loc["x"], "y": loc["y"], "theta": loc.get("theta", 0)})

        elif command == "LIST_LOCATIONS":
            locs = _load_locations()
            return {"status": "ok", "locations": locs, "count": len(locs)}

        elif command == "DELETE_LOCATION":
            name = params.get("name", "").strip()
            locs = _load_locations()
            if name in locs:
                del locs[name]
                _save_locations(locs)
                return {"status": "ok", "message": f"Location '{name}' deleted"}
            return {"status": "error", "message": f"Location '{name}' not found"}

        elif command == "GET_MAP":
            map_dir = os.path.expanduser("~/maps")
            map_name = params.get("map_name", "map")
            pgm_path = f"{map_dir}/{map_name}.pgm"
            yaml_path = f"{map_dir}/{map_name}.yaml"
            if not os.path.exists(pgm_path):
                return {"status": "error", "message": f"Map '{map_name}' not found at {pgm_path}"}
            with open(pgm_path, "rb") as f:
                pgm_b64 = base64.b64encode(f.read()).decode("ascii")
            yaml_content = ""
            if os.path.exists(yaml_path):
                with open(yaml_path) as f:
                    yaml_content = f.read()
            return {"status": "ok", "map_name": map_name, "pgm_base64": pgm_b64, "yaml": yaml_content}

        elif command == "LOCALIZE":
            x = float(params.get("x", 0))
            y = float(params.get("y", 0))
            theta = float(params.get("theta", 0))
            try:
                cmd = f'source /etc/env.sh && ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{{header: {{frame_id: map}}, pose: {{pose: {{position: {{x: {x}, y: {y}}}, orientation: {{z: {math.sin(theta/2)}, w: {math.cos(theta/2)}}}}}}}}}"'
                subprocess.run(["bash", "-c", cmd], capture_output=True, timeout=10)
                return {"status": "ok", "x": x, "y": y, "theta": theta, "message": "Initial pose set"}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        # ── Operations ──
        elif command == "DOCK":
            try:
                cmd = "source /etc/env.sh && ros2 action send_goal /dock_robot former_interfaces/action/Dock '{}'"
                result = subprocess.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=30)
                return {"status": "ok", "message": "Dock command sent", "output": result.stdout[:200]}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        elif command == "UNDOCK":
            try:
                cmd = "source /etc/env.sh && ros2 action send_goal /undock_robot former_interfaces/action/Undock '{}'"
                result = subprocess.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=30)
                return {"status": "ok", "message": "Undock command sent", "output": result.stdout[:200]}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        elif command == "GET_BATTERY":
            data = get_ros2_data()
            battery = data.get("battery", {}) if data else {}
            with _ros2_lock:
                feedback = _ros2_cache.get("robot_feedback", {})
            return {"status": "ok", **battery, "system_voltage": feedback.get("system_voltage"),
                    "charging_voltage": feedback.get("charging_voltage"), "charging": battery.get("charging", False)}

        elif command == "SET_SPEED_LIMIT":
            limit = float(params.get("limit", 0.5))
            with _ros2_lock:
                _ros2_cache["speed_limit"] = limit
            log.info(f"Speed limit set to {limit} m/s")
            return {"status": "ok", "speed_limit": limit}

        elif command == "SPEAK":
            text = params.get("text", "")
            lang = params.get("lang", "ko")
            if not text:
                return {"status": "error", "message": "text required"}
            try:
                cmd = f'espeak-ng -v {lang} "{text}" 2>/dev/null || echo "{text}" | festival --tts 2>/dev/null || echo "TTS not available"'
                subprocess.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=10)
                return {"status": "ok", "text": text, "lang": lang}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        elif command == "PLAY_SOUND":
            sound = params.get("sound", "beep")
            try:
                sounds = {"beep": "440 0.3", "alert": "880 0.5", "warning": "660 0.2 0.1 660 0.2", "success": "523 0.2 0.05 659 0.2 0.05 784 0.3"}
                freq_dur = sounds.get(sound, "440 0.3")
                parts = freq_dur.split()
                beep_cmds = []
                i = 0
                while i < len(parts):
                    freq, dur = parts[i], parts[i+1]
                    beep_cmds.append(f"speaker-test -t sine -f {freq} -l 1 & sleep {dur} && kill $!")
                    i += 2
                    if i < len(parts) and not parts[i].replace('.','').isdigit():
                        pass
                    elif i < len(parts):
                        beep_cmds.append(f"sleep {parts[i]}")
                        i += 1
                cmd = " ; ".join(beep_cmds)
                subprocess.Popen(["bash", "-c", cmd], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
                return {"status": "ok", "sound": sound}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        elif command == "SET_LED":
            color = params.get("color", "off")
            pattern = params.get("pattern", "solid")
            with _ros2_lock:
                _ros2_cache["led_state"] = {"color": color, "pattern": pattern}
            log.info(f"LED set: {color} {pattern}")
            return {"status": "ok", "color": color, "pattern": pattern}

        elif command == "GET_DIAGNOSTICS":
            try:
                cmd = "source /etc/env.sh && ros2 topic echo /diagnostics --once 2>/dev/null"
                result = subprocess.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=5)
                import psutil
                cpu = psutil.cpu_percent(interval=0.5)
                mem = psutil.virtual_memory()
                disk = psutil.disk_usage('/')
                return {"status": "ok", "cpu_percent": cpu,
                        "memory": {"total_gb": round(mem.total/1e9, 1), "used_percent": mem.percent},
                        "disk": {"total_gb": round(disk.total/1e9, 1), "used_percent": disk.percent},
                        "ros2_diagnostics": result.stdout[:500] if result.stdout else "no diagnostics"}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        elif command == "RECORD_BAG":
            topics = params.get("topics", ["/scan", "/odom", "/imu/data", "/camera/camera/color/image_raw"])
            duration = int(params.get("duration", 60))
            bag_name = params.get("name", f"bag_{int(time.time())}")
            bag_dir = os.path.expanduser(f"~/bags/{bag_name}")
            cmd = f"source /etc/env.sh && ros2 bag record -o {bag_dir} {' '.join(topics)}"
            if duration > 0:
                cmd = f"timeout {duration} bash -c '{cmd}'"
            ok, msg = _launch_ros2("rosbag", cmd)
            return {"status": "ok" if ok else "error", "message": msg, "bag_dir": bag_dir, "topics": topics, "duration": duration}

        elif command == "STOP_BAG":
            ok, msg = _kill_ros2("rosbag")
            return {"status": "ok" if ok else "error", "message": msg}

        elif command == "REBOOT":
            delay = int(params.get("delay", 5))
            subprocess.Popen(["bash", "-c", f"sleep {delay} && sudo reboot"], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
            return {"status": "ok", "message": f"Rebooting in {delay} seconds"}

        elif command == "UPDATE_BRIDGE":
            try:
                cmd = 'pip3 install --upgrade ai-teammate-ros2-bridge 2>&1 && echo "updated"'
                result = subprocess.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=60)
                if "updated" in result.stdout:
                    return {"status": "ok", "message": "Bridge updated. Restart with: sudo systemctl restart ai-teammate-bridge",
                            "output": result.stdout[-200:]}
                return {"status": "error", "message": result.stderr[:200] or result.stdout[-200:]}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        # ── Radio Fingerprint ──
        elif command == "SCAN_WIFI":
            try:
                result = subprocess.run(["nmcli", "-t", "-f", "SSID,SIGNAL,BSSID,FREQ,CHAN", "dev", "wifi", "list", "--rescan", "yes"],
                                        capture_output=True, text=True, timeout=15)
                aps = []
                for line in result.stdout.strip().split("\n"):
                    parts = line.split(":")
                    if len(parts) >= 3:
                        ssid = parts[0].replace("\\:", ":")
                        signal_val = int(parts[1]) if parts[1].lstrip('-').isdigit() else 0
                        bssid = ":".join(parts[2:8]).replace("\\", "")
                        aps.append({"ssid": ssid, "signal": signal_val, "bssid": bssid})
                return {"status": "ok", "access_points": aps, "count": len(aps)}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        elif command == "SCAN_BLE":
            duration = float(params.get("duration", 5.0))
            try:
                from bleak import BleakScanner
                found = []
                def _on_detect(device, adv_data):
                    found.append({"name": device.name or "unknown", "address": device.address, "rssi": adv_data.rssi})
                scanner = BleakScanner(detection_callback=_on_detect)
                await scanner.start()
                await asyncio.sleep(duration)
                await scanner.stop()
                seen = {}
                for d in found:
                    if d["address"] not in seen or d["rssi"] > seen[d["address"]]["rssi"]:
                        seen[d["address"]] = d
                beacons = sorted(seen.values(), key=lambda x: x["rssi"], reverse=True)
                return {"status": "ok", "devices": beacons, "count": len(beacons)}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        elif command == "SAVE_FINGERPRINT":
            name = params.get("name", "").strip()
            if not name:
                return {"status": "error", "message": "name required"}
            with _ros2_lock:
                odom = _ros2_cache.get("odometry", {})
            pose = {"x": odom.get("x", 0), "y": odom.get("y", 0), "theta": odom.get("theta", 0)}
            wifi_aps = []
            try:
                result = subprocess.run(["nmcli", "-t", "-f", "BSSID,SIGNAL", "dev", "wifi", "list"],
                                        capture_output=True, text=True, timeout=10)
                for line in result.stdout.strip().split("\n"):
                    parts = line.split(":")
                    if len(parts) >= 7:
                        bssid = ":".join(parts[:6]).replace("\\", "")
                        signal_val = int(parts[6]) if parts[6].lstrip('-').isdigit() else 0
                        wifi_aps.append({"bssid": bssid, "signal": signal_val})
            except Exception:
                pass
            try:
                with open(FINGERPRINTS_FILE) as f:
                    fps = json.load(f)
            except Exception:
                fps = {}
            fps[name] = {"pose": pose, "wifi": wifi_aps, "timestamp": time.time()}
            with open(FINGERPRINTS_FILE, "w") as f:
                json.dump(fps, f, indent=2)
            return {"status": "ok", "name": name, "pose": pose, "wifi_count": len(wifi_aps)}

        elif command == "LIST_FINGERPRINTS":
            try:
                with open(FINGERPRINTS_FILE) as f:
                    fps = json.load(f)
                summary = {k: {"pose": v["pose"], "wifi_count": len(v.get("wifi", []))} for k, v in fps.items()}
                return {"status": "ok", "fingerprints": summary, "count": len(fps)}
            except Exception:
                return {"status": "ok", "fingerprints": {}, "count": 0}

        elif command == "LOCALIZE_FINGERPRINT":
            try:
                result = subprocess.run(["nmcli", "-t", "-f", "BSSID,SIGNAL", "dev", "wifi", "list"],
                                        capture_output=True, text=True, timeout=10)
                current = {}
                for line in result.stdout.strip().split("\n"):
                    parts = line.split(":")
                    if len(parts) >= 7:
                        bssid = ":".join(parts[:6]).replace("\\", "")
                        signal_val = int(parts[6]) if parts[6].lstrip('-').isdigit() else 0
                        current[bssid] = signal_val
            except Exception:
                return {"status": "error", "message": "WiFi scan failed"}

            try:
                with open(FINGERPRINTS_FILE) as f:
                    fps = json.load(f)
            except Exception:
                return {"status": "error", "message": "No fingerprints saved"}

            best_name, best_score = None, float("inf")
            for name, fp in fps.items():
                score = 0
                matched = 0
                for ap in fp.get("wifi", []):
                    if ap["bssid"] in current:
                        diff = ap["signal"] - current[ap["bssid"]]
                        score += diff * diff
                        matched += 1
                if matched > 0:
                    avg_score = score / matched
                    if avg_score < best_score:
                        best_score = avg_score
                        best_name = name

            if best_name:
                loc = fps[best_name]
                return {"status": "ok", "location": best_name, "pose": loc["pose"],
                        "confidence": round(1.0 / (1.0 + best_score / 100), 2),
                        "matched_aps": len(current)}
            return {"status": "ok", "location": None, "message": "No matching fingerprint found"}

        elif command in ("DETECT_OBJECTS", "DETECT_AND_INGEST"):
            from .ws_client import _send_service_request
            now = time.time()
            elapsed = now - _cfg._detect_last_ts
            if elapsed < DETECT_COOLDOWN:
                return {"status": "error", "message": f"cooldown: retry in {DETECT_COOLDOWN - elapsed:.1f}s"}
            _cfg._detect_last_ts = now

            with _ros2_lock:
                frame = _ros2_cache.get("camera_frame")
            if not frame:
                return {"status": "error", "message": "no camera frame available"}

            service = "vision_detect" if command == "DETECT_OBJECTS" else "detect_and_ingest"
            svc_data = {
                "service": service,
                "frame_base64": base64.b64encode(frame).decode("ascii"),
                "model_size": params.get("model_size", "tiny"),
            }

            if command == "DETECT_AND_INGEST":
                footprint = {}
                try:
                    wifi_result = subprocess.run(
                        ["nmcli", "-t", "-f", "SSID,SIGNAL,BSSID", "dev", "wifi", "list", "--rescan", "no"],
                        capture_output=True, text=True, timeout=5,
                    )
                    wifi_aps = []
                    for line in wifi_result.stdout.strip().split("\n"):
                        parts = line.split(":")
                        if len(parts) >= 3:
                            wifi_aps.append({"ssid": parts[0],
                                             "signal": int(parts[1]) if parts[1].lstrip('-').isdigit() else 0,
                                             "bssid": ":".join(parts[2:8]).replace("\\", "")})
                    if wifi_aps:
                        footprint["wifi"] = wifi_aps
                except Exception:
                    pass
                svc_data["footprint"] = footprint or None
                svc_data["name"] = params.get("name")
                svc_data["category"] = params.get("category", "object_detection")
                odom = _ros2_cache.get("odometry", {})
                svc_data["position"] = odom.get("position")

            result = await _send_service_request(svc_data)
            return result

        else:
            log.warning(f"Unknown command: {command}")
            return {"status": "unknown_command", "command": command}

    return None
