# AI Teammate Device Bridge v2 — rclpy native (no rosbridge dependency)
import asyncio, json, logging, math, os, random, signal, subprocess, time, threading
from pathlib import Path

logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s")
log = logging.getLogger("bridge")

# Load .env
env_file = Path(__file__).parent / ".env"
if env_file.exists():
    for line in env_file.read_text().splitlines():
        if "=" in line and not line.startswith("#"):
            k, v = line.split("=", 1)
            os.environ.setdefault(k.strip(), v.strip())

DEVICE_ID = os.environ["DEVICE_ID"]
GATEWAY_URL = os.environ["GATEWAY_URL"]
API_KEY = os.environ["API_KEY"]
MODE = os.environ.get("CONNECTION_MODE", "rclpy")
INTERVAL = float(os.environ.get("STATUS_REPORT_INTERVAL", "1.0"))
CMD_VEL_TOPIC = os.environ.get("CMD_VEL_TOPIC", "/cmd_vel")
IMU_TOPIC = os.environ.get("IMU_TOPIC", "/imu/data")
ODOM_TOPIC = os.environ.get("ODOM_TOPIC", "/odom")
SCAN_TOPIC = os.environ.get("SCAN_TOPIC", "/scan")
# Vision/Ingest URL과 JWT_SECRET_KEY는 Gateway가 관리 (service_proxy)
# Bridge는 WS service_request로 Gateway에 위임

# ── Input sanitization ──

import re as _re

_SAFE_NAME_RE = _re.compile(r'^[a-zA-Z0-9_\-]+$')
_ALLOWED_SAVE_DIRS = [os.path.expanduser("~/maps"), "/tmp/maps"]


def _safe_name(name: str, default: str = "map") -> str:
    """파일명/맵 이름에서 위험 문자를 제거합니다."""
    if not name or not _SAFE_NAME_RE.match(name):
        return default
    return name


def _safe_dir(path: str) -> str:
    """허용된 디렉토리인지 검증합니다."""
    resolved = os.path.realpath(os.path.expanduser(path))
    for allowed in _ALLOWED_SAVE_DIRS:
        allowed_resolved = os.path.realpath(os.path.expanduser(allowed))
        if resolved.startswith(allowed_resolved):
            return resolved
    return os.path.realpath(os.path.expanduser(_ALLOWED_SAVE_DIRS[0]))


# ── ROS2 rclpy native ──

_ros2_cache = {}
_ros2_lock = threading.Lock()
_ros2_node = None  # rclpy Node

# Gateway context (populated from welcome message)
_gw_context: dict = {}  # owner_id, project_id, session_id

# Rate limit for detection commands
_detect_last_ts: float = 0.0
DETECT_COOLDOWN = float(os.environ.get("DETECT_COOLDOWN_SEC", "2.0"))

# WS reference for service requests (set in run())
_ws_ref = None  # websockets connection
_service_futures: dict = {}  # request_id → asyncio.Future

# ── Keyframe capture ──
_keyframe_buffer: list = []
_last_kf_pose: dict | None = None
_last_kf_ts: float = 0.0
_kf_capture_active: bool = False
_kf_total_captured: int = 0
_kf_last_upload_ts: float = 0.0
_kf_wifi_cache: dict | None = None  # cached WiFi scan result
_kf_wifi_cache_ts: float = 0.0
_slam_radio_active: bool = False  # WiFi collection during SLAM mapping
WIFI_BLOCKLIST_FILE = os.path.join(os.path.dirname(__file__) or ".", "wifi_blocklist.json")
KF_MAX_BUFFER = int(os.environ.get("KF_MAX_BUFFER", "20"))
KF_BATCH_SIZE = int(os.environ.get("KF_BATCH_SIZE", "10"))
KF_MIN_DISPLACEMENT = float(os.environ.get("KF_MIN_DISPLACEMENT", "0.3"))  # meters
KF_MIN_ROTATION = float(os.environ.get("KF_MIN_ROTATION", "0.26"))  # ~15 degrees in radians
KF_MIN_INTERVAL = float(os.environ.get("KF_MIN_INTERVAL", "2.0"))  # seconds
KF_WIFI_SCAN_INTERVAL = float(os.environ.get("KF_WIFI_SCAN_INTERVAL", "10.0"))  # seconds
KF_THUMB_WIDTH = 160
KF_THUMB_HEIGHT = 120
KF_THUMB_QUALITY = 40
_cmd_vel_pub = None  # rclpy Publisher
_estop_event = threading.Event()  # Set to interrupt running MOVE/TURN
OBSTACLE_DISTANCE = float(os.environ.get("OBSTACLE_DISTANCE", "0.4"))  # meters — stop if closer


def _ros2_spin_thread():
    """Background thread: spin rclpy node for subscriptions."""
    import rclpy
    from rclpy.node import Node
    from geometry_msgs.msg import Twist
    from sensor_msgs.msg import Imu, LaserScan, CameraInfo, Image as RosImage
    from nav_msgs.msg import Odometry
    import cv2
    import numpy as np

    global _ros2_node, _cmd_vel_pub

    rclpy.init()
    node = Node("ai_teammate_bridge")
    _ros2_node = node

    # Publisher
    _cmd_vel_pub = node.create_publisher(Twist, CMD_VEL_TOPIC, 10)
    log.info(f"cmd_vel publisher created: {CMD_VEL_TOPIC}")

    # Subscribers
    def on_imu(msg):
        with _ros2_lock:
            _ros2_cache["imu"] = {
                "roll": msg.orientation.x,
                "pitch": msg.orientation.y,
                "yaw": msg.orientation.z,
                "accel_x": msg.linear_acceleration.x,
                "accel_y": msg.linear_acceleration.y,
                "accel_z": msg.linear_acceleration.z,
            }

    def on_odom(msg):
        with _ros2_lock:
            _ros2_cache["odometry"] = {
                "x": msg.pose.pose.position.x,
                "y": msg.pose.pose.position.y,
                "theta": msg.pose.pose.orientation.z,
                "linear_vel": msg.twist.twist.linear.x,
                "angular_vel": msg.twist.twist.angular.z,
            }

    def on_scan(msg):
        with _ros2_lock:
            ranges = list(msg.ranges)
            valid = [r for r in ranges if msg.range_min < r < msg.range_max]
            # Front cone: ±30 degrees from center
            n = len(ranges)
            if n > 0:
                angle_inc = (msg.angle_max - msg.angle_min) / n
                front_indices = [i for i in range(n) if abs(msg.angle_min + i * angle_inc) < 0.52]  # ~30 deg
                front_ranges = [ranges[i] for i in front_indices if msg.range_min < ranges[i] < msg.range_max]
                front_min = round(min(front_ranges), 3) if front_ranges else None
            else:
                front_min = None
            _ros2_cache["scan"] = {
                "range_min": msg.range_min,
                "range_max": msg.range_max,
                "angle_min": msg.angle_min,
                "angle_max": msg.angle_max,
                "num_ranges": n,
                "min_distance": round(min(valid), 3) if valid else None,
                "mean_distance": round(sum(valid) / len(valid), 3) if valid else None,
                "front_min_distance": front_min,
            }

    def on_camera_info(msg):
        with _ros2_lock:
            _ros2_cache["camera"] = {
                "width": msg.width,
                "height": msg.height,
                "distortion_model": msg.distortion_model,
                "frame_id": msg.header.frame_id,
                "available": True,
            }

    def on_image(msg):
        """Cache latest camera frame as compressed JPEG bytes."""
        try:
            # Convert ROS Image to numpy
            if msg.encoding == 'rgb8':
                img = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3)
                img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
            elif msg.encoding == 'bgr8':
                img = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3)
            elif msg.encoding in ('16UC1', '32FC1'):
                # Depth image — skip for color capture
                return
            else:
                img = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, -1)

            # Compress to JPEG (quality 60 for speed)
            _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60])
            with _ros2_lock:
                _ros2_cache["camera_frame"] = jpeg.tobytes()
                _ros2_cache["camera_frame_ts"] = time.time()
        except Exception as e:
            log.debug(f"Image conversion error: {e}")

    # ── Camera auto-detection ──
    CAMERA_INFO_TOPIC = os.environ.get("CAMERA_INFO_TOPIC", "")
    CAMERA_IMAGE_TOPIC = os.environ.get("CAMERA_IMAGE_TOPIC", "")

    if not CAMERA_IMAGE_TOPIC:
        # Auto-detect camera topics from ROS2
        log.info("🔍 Auto-detecting camera topics...")
        try:
            import time as _t
            _t.sleep(2)  # Wait for topic discovery
            topic_list = node.get_topic_names_and_types()
            image_topics = [
                name for name, types in topic_list
                if any("sensor_msgs/msg/Image" in t for t in types)
            ]
            info_topics = [
                name for name, types in topic_list
                if any("sensor_msgs/msg/CameraInfo" in t for t in types)
            ]
            if image_topics:
                # Prefer color over depth
                CAMERA_IMAGE_TOPIC = next(
                    (t for t in image_topics if "color" in t and "depth" not in t),
                    image_topics[0]
                )
                CAMERA_INFO_TOPIC = next(
                    (t for t in info_topics if "color" in t),
                    info_topics[0] if info_topics else ""
                )
                log.info(f"📷 Camera auto-detected: image={CAMERA_IMAGE_TOPIC}, info={CAMERA_INFO_TOPIC}")
                log.info(f"📷 All image topics: {image_topics}")
            else:
                log.info("📷 No camera topics found — camera disabled")
        except Exception as e:
            log.warning(f"Camera auto-detection failed: {e}")

    if not CAMERA_IMAGE_TOPIC:
        CAMERA_IMAGE_TOPIC = "/camera/camera/color/image_raw"
        CAMERA_INFO_TOPIC = "/camera/camera/color/camera_info"
        log.info(f"📷 Using default camera topics: {CAMERA_IMAGE_TOPIC}")

    node.create_subscription(Imu, IMU_TOPIC, on_imu, 10)
    node.create_subscription(Odometry, ODOM_TOPIC, on_odom, 10)
    node.create_subscription(LaserScan, SCAN_TOPIC, on_scan, 10)
    node.create_subscription(CameraInfo, CAMERA_INFO_TOPIC, on_camera_info, 10)
    node.create_subscription(RosImage, CAMERA_IMAGE_TOPIC, on_image, 1)  # queue 1 = latest only
    log.info(f"Subscribed to {IMU_TOPIC}, {ODOM_TOPIC}, {SCAN_TOPIC}, {CAMERA_INFO_TOPIC}, {CAMERA_IMAGE_TOPIC}")

    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


def _make_cmd_vel_msg(linear_x: float = 0.0, angular_z: float = 0.0):
    """Create Twist or TwistStamped — delegates to config.make_cmd_vel_msg."""
    from .config import make_cmd_vel_msg
    return make_cmd_vel_msg(linear_x, angular_z)


def publish_cmd_vel(linear_x: float, angular_z: float, duration: float = 1.0):
    """Publish cmd_vel via rclpy native publisher at 10Hz for duration (blocking).
    Returns 'ok', 'estop' (interrupted), or 'error'."""
    pub = _cmd_vel_pub
    if not pub:
        log.warning("ROS2 node not ready, cannot publish cmd_vel")
        return "error"

    _estop_event.clear()
    rate_hz = 10
    try:
        msg = _make_cmd_vel_msg(linear_x, angular_z)

        interrupted = False
        if duration > 0:
            ticks = int(duration * rate_hz)
            for i in range(ticks):
                if _estop_event.is_set():
                    log.warning(f"E-STOP: interrupted at tick {i}/{ticks}")
                    interrupted = True
                    break
                pub.publish(msg)
                time.sleep(1.0 / rate_hz)
            if not interrupted:
                log.info(f"Published cmd_vel: linear={linear_x}, angular={angular_z} for {duration:.1f}s ({ticks} ticks)")
        else:
            pub.publish(msg)
            log.info(f"Published cmd_vel: linear={linear_x}, angular={angular_z}")

        # Always send STOP after movement
        pub.publish(_make_cmd_vel_msg())
        log.info("Published cmd_vel STOP")

        return "estop" if interrupted else "ok"
    except Exception as e:
        log.error(f"cmd_vel publish error: {e}")
        return "error"


# ── Subprocess management (SLAM, Nav2) ──

_subprocesses: dict[str, subprocess.Popen | None] = {"slam": None, "nav2": None, "cmd_vel_relay": None}


async def _set_initial_pose(params: dict):
    """Set AMCL initial pose from params or current odometry."""
    try:
        with _ros2_lock:
            odom = _ros2_cache.get("odometry", {})
        x = float(params.get("init_x", odom.get("x", 0)))
        y = float(params.get("init_y", odom.get("y", 0)))
        theta = float(params.get("init_theta", odom.get("theta", 0)))
        import math
        qz = math.sin(theta / 2)
        qw = math.cos(theta / 2)
        cmd = (
            f'source /etc/env.sh && ros2 topic pub --once /initialpose '
            f'geometry_msgs/msg/PoseWithCovarianceStamped '
            f'"{{header: {{frame_id: map}}, pose: {{pose: {{position: {{x: {x}, y: {y}}}, '
            f'orientation: {{z: {qz}, w: {qw}}}}}}}}}"'
        )
        sp = subprocess
        result = sp.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=10)
        log.info(f"Initial pose set: x={x:.2f} y={y:.2f} theta={theta:.2f}")
    except Exception as e:
        log.warning(f"Failed to set initial pose: {e}")


def _launch_ros2(name: str, cmd: str) -> tuple[bool, str]:
    """Launch a ROS2 subprocess. Returns (success, message)."""
    sp = subprocess
    proc = _subprocesses.get(name)
    if proc and proc.poll() is None:
        return False, f"{name} is already running (pid={proc.pid})"
    try:
        full_cmd = f"source /etc/env.sh && {cmd}"
        p = sp.Popen(["bash", "-c", full_cmd], stdout=sp.DEVNULL, stderr=sp.DEVNULL, preexec_fn=os.setsid)
        _subprocesses[name] = p
        log.info(f"{name} launched: pid={p.pid} cmd={cmd}")
        return True, f"{name} started (pid={p.pid})"
    except Exception as e:
        log.error(f"{name} launch failed: {e}")
        return False, str(e)


def _kill_ros2(name: str) -> tuple[bool, str]:
    """Kill a ROS2 subprocess. Returns (success, message)."""
    proc = _subprocesses.get(name)
    if not proc or proc.poll() is not None:
        _subprocesses[name] = None
        return True, f"{name} is not running"
    try:
        os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
        proc.wait(timeout=5)
    except Exception:
        try:
            os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
        except Exception:
            pass
    _subprocesses[name] = None
    log.info(f"{name} stopped")
    return True, f"{name} stopped"


async def _handle_slam(command: str, params: dict) -> dict:
    """Handle SLAM_START, SLAM_STOP, SLAM_SAVE."""
    global _slam_radio_active
    loop = asyncio.get_event_loop()

    if command == "SLAM_START":
        launch_file = _safe_name(params.get("launch_file", "online_async_launch"), default="online_async_launch")
        if not launch_file.endswith(".py"):
            launch_file += ".py"
        import shlex
        # Start trajectory + radio fingerprint collection
        from .config import _ros2_cache as shared_cache, _ros2_lock as shared_lock
        with shared_lock:
            shared_cache["_trajectory"] = []
            shared_cache["_trajectory_active"] = True
            shared_cache["_slam_fingerprints"] = []  # radio fingerprints collected during SLAM
        # Start background WiFi scan loop for SLAM
        _slam_radio_active = True
        asyncio.create_task(_slam_wifi_scan_loop(shared_cache, shared_lock))
        ok, msg = await loop.run_in_executor(None, _launch_ros2, "slam",
            f"ros2 launch slam_toolbox {shlex.quote(launch_file)}")
        return {"status": "ok" if ok else "error", "message": msg}

    elif command == "SLAM_STOP":
        _slam_radio_active = False
        from .config import _ros2_cache as shared_cache, _ros2_lock as shared_lock
        with shared_lock:
            shared_cache["_trajectory_active"] = False
            traj_len = len(shared_cache.get("_trajectory", []))
            fp_len = len(shared_cache.get("_slam_fingerprints", []))
        ok, msg = await loop.run_in_executor(None, _kill_ros2, "slam")
        return {"status": "ok" if ok else "error", "message": msg,
                "trajectory_points": traj_len, "fingerprints_collected": fp_len}

    elif command == "SLAM_SAVE":
        import shlex
        from .config import _ros2_cache as shared_cache, _ros2_lock as shared_lock
        map_name = _safe_name(params.get("map_name", "map"))
        save_dir = _safe_dir(params.get("save_dir", "~/maps"))
        auto_upload = params.get("auto_upload", False)
        os.makedirs(save_dir, exist_ok=True)
        sp = subprocess
        try:
            safe_path = shlex.quote(f"{save_dir}/{map_name}")
            cmd = f"source /etc/env.sh && ros2 run nav2_map_server map_saver_cli -f {safe_path} --ros-args -p save_map_timeout:=10.0"
            result = sp.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=15)
            if result.returncode != 0:
                return {"status": "error", "message": result.stderr[:200] or "map save failed"}
            # Collect trajectory + fingerprints and stop recording
            _slam_radio_active = False
            with shared_lock:
                trajectory = list(shared_cache.get("_trajectory", []))
                fingerprints = list(shared_cache.get("_slam_fingerprints", []))
                shared_cache["_trajectory_active"] = False
            # Auto-link fingerprints to nearby saved locations (predicted)
            linked = 0
            if fingerprints:
                locs = _load_locations()
                fp_file = os.path.join(os.path.dirname(__file__) or ".", "fingerprints.json")
                try:
                    with open(fp_file) as f:
                        saved_fps = json.load(f)
                except Exception:
                    saved_fps = {}
                for fp in fingerprints:
                    fp_pose = fp.get("pose", {})
                    fx, fy = fp_pose.get("x", 0), fp_pose.get("y", 0)
                    # Find closest saved location within 1.5m
                    best_name, best_dist = None, 1.5
                    for loc_name, loc_data in locs.items():
                        dx = fx - loc_data.get("x", 0)
                        dy = fy - loc_data.get("y", 0)
                        dist = (dx * dx + dy * dy) ** 0.5
                        if dist < best_dist:
                            best_dist = dist
                            best_name = loc_name
                    if best_name and best_name not in saved_fps:
                        saved_fps[best_name] = {
                            "pose": fp_pose,
                            "wifi": fp.get("wifi", []),
                            "timestamp": fp.get("ts", time.time()),
                            "source": "predicted",  # auto-linked, not directly scanned
                            "linked_distance": round(best_dist, 2),
                        }
                        linked += 1
                if linked > 0:
                    with open(fp_file, "w") as f:
                        json.dump(saved_fps, f, indent=2)
                    log.info(f"SLAM: auto-linked {linked} fingerprints to saved locations (predicted)")

            # Detect mobile APs: same BSSID seen at distant locations → blocklist
            blocked = _update_wifi_blocklist(fingerprints)

            save_result = {"status": "ok", "message": f"Map saved to {save_dir}/{map_name}",
                           "path": f"{save_dir}/{map_name}",
                           "trajectory_points": len(trajectory),
                           "fingerprints_collected": len(fingerprints),
                           "fingerprints_linked": linked,
                           "mobile_aps_blocked": blocked}
            # 자동 업로드 — include trajectory + wifi fingerprints in metadata
            if auto_upload:
                upload_meta = {
                    "trajectory": [{"x": p[0], "y": p[1], "theta": p[2]} for p in trajectory],
                    "wifi_fingerprints": fingerprints,
                }
                upload_result = await _upload_map_files(save_dir, map_name, metadata=upload_meta)
                save_result["upload"] = upload_result
            return save_result
        except Exception as e:
            return {"status": "error", "message": str(e)}

    return {"status": "error", "message": f"Unknown SLAM command: {command}"}


async def _upload_map_files(save_dir: str, map_name: str, metadata: dict = None) -> dict:
    """PGM + YAML 맵 파일을 읽어서 Gateway → Map API로 chunked upload."""
    CHUNK_SIZE = 256 * 1024  # 256KB

    pgm_path = os.path.join(save_dir, f"{map_name}.pgm")
    yaml_path = os.path.join(save_dir, f"{map_name}.yaml")

    if not os.path.exists(pgm_path):
        return {"success": False, "error": f"PGM file not found: {pgm_path}"}

    # PGM + YAML 파일을 하나의 바이너리로 패킹 (YAML 먼저, 구분자, PGM)
    parts = []
    if os.path.exists(yaml_path):
        parts.append(("yaml", open(yaml_path, "rb").read()))
    parts.append(("pgm", open(pgm_path, "rb").read()))

    # 단순 패킹: JSON header + raw bytes
    import struct
    header = json.dumps({"files": [{"name": p[0], "size": len(p[1])} for p in parts]}).encode()
    payload = struct.pack(">I", len(header)) + header
    for _, data in parts:
        payload += data

    raw_size = len(payload)
    compression_type = "none"
    try:
        import lz4.frame
        compressed = lz4.frame.compress(payload)
        if len(compressed) < raw_size:
            payload = compressed
            compression_type = "lz4"
            log.info(f"LZ4 compressed: {raw_size} → {len(payload)} bytes ({100*len(payload)/raw_size:.1f}%)")
    except ImportError:
        log.warning("lz4 not available — uploading uncompressed")

    # Chunk 분할 (hex encoding)
    chunks_hex = []
    for i in range(0, len(payload), CHUNK_SIZE):
        chunks_hex.append(payload[i:i + CHUNK_SIZE].hex())

    # YAML에서 해상도 추출
    map_metadata = metadata or {}
    if os.path.exists(yaml_path):
        try:
            yaml_content = open(yaml_path).read()
            for line in yaml_content.split("\n"):
                if line.startswith("resolution:"):
                    map_metadata["resolution"] = float(line.split(":")[1].strip())
                elif line.startswith("origin:"):
                    map_metadata["origin"] = line.split(":")[1].strip()
        except Exception:
            pass
    map_metadata["format"] = "pgm+yaml"

    svc_data = {
        "service": "map_upload",
        "name": map_name,
        "map_type": "local",
        "compression_type": compression_type,
        "chunks": chunks_hex,
        "metadata": map_metadata,
    }

    log.info(f"Uploading map '{map_name}': {len(payload)} bytes, {len(chunks_hex)} chunks, compression={compression_type}")
    result = await _send_service_request(svc_data, timeout=60.0)
    return result


OBSTACLE_WAIT_TIMEOUT = float(os.environ.get("OBSTACLE_WAIT_TIMEOUT", "30"))
LOCATIONS_FILE = os.path.join(os.path.dirname(__file__) or ".", "locations.json")


def _load_locations() -> dict:
    try:
        with open(LOCATIONS_FILE) as f:
            return json.load(f)
    except Exception:
        return {}


def _save_locations(locs: dict):
    with open(LOCATIONS_FILE, "w") as f:
        json.dump(locs, f, indent=2)  # max seconds to wait


def _check_front_obstacle(threshold: float) -> tuple[bool, float | None]:
    """Check front obstacle from LiDAR cache. Returns (blocked, distance)."""
    with _ros2_lock:
        scan = _ros2_cache.get("scan", {})
    front = scan.get("front_min_distance")
    if front is None:
        return False, None  # no data = assume clear
    return front < threshold, front


def _wait_for_clear(threshold: float) -> str:
    """Block until obstacle clears or E-STOP or timeout. Returns 'clear', 'estop', 'timeout'."""
    start = time.time()
    log.info(f"Waiting for obstacle to clear (threshold={threshold}m, timeout={OBSTACLE_WAIT_TIMEOUT}s)...")
    while True:
        if _estop_event.is_set():
            return "estop"
        if time.time() - start > OBSTACLE_WAIT_TIMEOUT:
            return "timeout"
        blocked, dist = _check_front_obstacle(threshold)
        if not blocked:
            log.info(f"Obstacle cleared (front={dist}m)")
            return "clear"
        time.sleep(0.2)  # poll at 5Hz


async def _handle_safe_move(distance: float, speed: float, step: float, threshold: float) -> dict:
    """Move with periodic obstacle checks every `step` meters."""
    loop = asyncio.get_event_loop()
    direction = 1.0 if distance > 0 else -1.0
    remaining = abs(distance)
    moved = 0.0

    while remaining > 0.01:
        if _estop_event.is_set():
            return {"status": "estop", "moved": round(moved, 3), "remaining": round(remaining, 3)}

        # Check obstacle before each step (only when moving forward)
        if direction > 0:
            blocked, front_dist = _check_front_obstacle(threshold)
            if blocked:
                log.warning(f"SAFE_MOVE: obstacle at {front_dist:.2f}m after {moved:.2f}m — waiting for clear")
                wait_result = await loop.run_in_executor(None, _wait_for_clear, threshold)
                if wait_result == "estop":
                    return {"status": "estop", "moved": round(moved, 3), "remaining": round(remaining, 3)}
                if wait_result == "timeout":
                    return {"status": "obstacle_timeout", "moved": round(moved, 3), "remaining": round(remaining, 3),
                            "front_distance": front_dist, "threshold": threshold,
                            "message": f"Obstacle not cleared after {OBSTACLE_WAIT_TIMEOUT}s"}

        chunk = min(step, remaining)
        duration = chunk / speed
        result = await loop.run_in_executor(None, publish_cmd_vel, direction * speed, 0.0, duration)

        if result == "estop":
            return {"status": "estop", "moved": round(moved + chunk, 3), "remaining": round(remaining - chunk, 3)}

        moved += chunk
        remaining -= chunk

    return {"status": "ok", "moved": round(moved, 3), "distance": distance, "speed": speed}


async def _handle_safe_navigate_path(waypoints: list, speed: float, step: float, threshold: float) -> dict:
    """Navigate waypoints with obstacle checks between segments."""
    loop = asyncio.get_event_loop()
    results = []
    prev = waypoints[0]

    for i, wp in enumerate(waypoints[1:], 1):
        if _estop_event.is_set():
            results.append({"wp": i, "status": "estop"})
            break

        dx = wp[0] - prev[0]
        dy = wp[1] - prev[1]
        distance = math.sqrt(dx * dx + dy * dy)

        if distance > 0.01:
            # Turn toward waypoint
            target_angle = math.atan2(dy, dx)
            angle_deg = math.degrees(target_angle)
            if abs(angle_deg) > 1:
                turn_duration = abs(math.radians(angle_deg)) / 0.5
                result = await loop.run_in_executor(None, publish_cmd_vel, 0.0, (0.5 if angle_deg > 0 else -0.5), turn_duration)
                if result == "estop":
                    results.append({"wp": i, "status": "estop"})
                    break

            # Safe move to waypoint
            move_result = await _handle_safe_move(distance, speed, step, threshold)
            results.append({"wp": i, "x": wp[0], "y": wp[1], **move_result})

            if move_result["status"] in ("obstacle", "estop"):
                break
        else:
            results.append({"wp": i, "status": "skipped"})

        prev = wp

    completed = all(r.get("status") == "ok" for r in results)
    return {"status": "ok" if completed else "partial", "waypoints_executed": len(results),
            "total": len(waypoints) - 1, "results": results}


async def _handle_navigate_path(params: dict) -> dict:
    """Navigate through a sequence of (x, y) waypoints using cmd_vel dead reckoning.

    Waypoints: [[x1,y1], [x2,y2], ...] — relative moves between consecutive points.
    """
    waypoints = params.get("waypoints", [])
    speed = float(params.get("speed", 0.3))
    if not waypoints or len(waypoints) < 2:
        return {"status": "error", "message": "at least 2 waypoints required"}

    loop = asyncio.get_event_loop()
    results = []
    prev = waypoints[0]

    for i, wp in enumerate(waypoints[1:], 1):
        if _estop_event.is_set():
            results.append({"wp": i, "status": "estop"})
            break

        dx = wp[0] - prev[0]
        dy = wp[1] - prev[1]
        distance = math.sqrt(dx * dx + dy * dy)

        if distance > 0.01:
            # Turn toward next waypoint
            target_angle = math.atan2(dy, dx)
            angle_deg = math.degrees(target_angle)
            if abs(angle_deg) > 1:
                turn_duration = abs(math.radians(angle_deg)) / 0.5
                result = await loop.run_in_executor(None, publish_cmd_vel, 0.0, (0.5 if angle_deg > 0 else -0.5), turn_duration)
                if result == "estop":
                    results.append({"wp": i, "status": "estop"})
                    break

            # Move forward
            duration = distance / speed
            result = await loop.run_in_executor(None, publish_cmd_vel, speed, 0.0, duration)
            results.append({"wp": i, "x": wp[0], "y": wp[1], "distance": round(distance, 3), "status": result})
        else:
            results.append({"wp": i, "x": wp[0], "y": wp[1], "status": "skipped"})

        prev = wp

    completed = all(r.get("status") == "ok" for r in results)
    return {"status": "ok" if completed else "partial", "waypoints_executed": len(results), "total": len(waypoints) - 1, "results": results}


def _start_cmd_vel_relay():
    """Start cmd_vel relay subprocess: /cmd_vel -> robot's cmd_vel topic."""
    proc = _subprocesses.get("cmd_vel_relay")
    if proc and proc.poll() is None:
        return  # already running
    # Detect robot cmd_vel topic
    robot_cmd_vel = os.environ.get("CMD_VEL_TOPIC", "/base_controller/cmd_vel_unstamped")
    if robot_cmd_vel == "/cmd_vel":
        return  # no relay needed
    import ai_teammate_ros2_bridge.config as _cfg_mod
    use_stamped = getattr(_cfg_mod, "_cmd_vel_stamped", False)
    if use_stamped:
        relay_script = (
            f'import rclpy, time; from rclpy.node import Node\n'
            f'from geometry_msgs.msg import Twist, TwistStamped\n'
            f'from builtin_interfaces.msg import Time\n'
            f'rclpy.init()\n'
            f'n = Node("cmd_vel_relay")\n'
            f'p = n.create_publisher(TwistStamped, "{robot_cmd_vel}", 10)\n'
            f'def cb(m):\n'
            f'    s = TwistStamped(); now = time.time()\n'
            f'    s.header.stamp = Time(sec=int(now), nanosec=int((now%1)*1e9))\n'
            f'    s.header.frame_id = "base_link"; s.twist = m; p.publish(s)\n'
            f'n.create_subscription(Twist, "/cmd_vel", cb, 10)\n'
            f'n.get_logger().info("Relaying /cmd_vel (Twist) -> {robot_cmd_vel} (TwistStamped)")\n'
            f'rclpy.spin(n)\n'
        )
    else:
        relay_script = (
            f'import rclpy; from rclpy.node import Node; from geometry_msgs.msg import Twist\n'
            f'rclpy.init()\n'
            f'n = Node("cmd_vel_relay")\n'
            f'p = n.create_publisher(Twist, "{robot_cmd_vel}", 10)\n'
            f'n.create_subscription(Twist, "/cmd_vel", lambda m: p.publish(m), 10)\n'
            f'n.get_logger().info("Relaying /cmd_vel -> {robot_cmd_vel}")\n'
            f'rclpy.spin(n)\n'
        )
    sp = subprocess
    p = sp.Popen(
        ["bash", "-c", f"source /etc/env.sh && python3 -c '{relay_script}'"],
        stdout=sp.DEVNULL, stderr=sp.DEVNULL, preexec_fn=os.setsid,
    )
    _subprocesses["cmd_vel_relay"] = p
    log.info(f"cmd_vel relay started: /cmd_vel -> {robot_cmd_vel} (pid={p.pid})")


def _stop_cmd_vel_relay():
    """Stop cmd_vel relay subprocess."""
    proc = _subprocesses.get("cmd_vel_relay")
    if proc and proc.poll() is None:
        try:
            os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
            proc.wait(timeout=3)
        except Exception:
            try:
                os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
            except Exception:
                pass
    _subprocesses["cmd_vel_relay"] = None


async def _handle_nav2(command: str, params: dict) -> dict:
    """Handle NAV2_START, NAV2_STOP, NAV2_GOAL, NAV2_CANCEL, NAV2_STATUS."""
    loop = asyncio.get_event_loop()

    if command == "NAV2_START":
        map_path = params.get("map", "")
        if not map_path:
            # Find latest map in ~/maps
            maps_dir = os.path.expanduser("~/maps")
            if os.path.isdir(maps_dir):
                yamls = sorted(
                    [f for f in os.listdir(maps_dir) if f.endswith(".yaml")],
                    key=lambda f: os.path.getmtime(os.path.join(maps_dir, f)),
                    reverse=True,
                )
                if yamls:
                    map_path = os.path.join(maps_dir, yamls[0])
            if not map_path:
                return {"status": "error", "message": "No map specified and no maps found in ~/maps"}

        if not map_path.endswith(".yaml"):
            map_path += ".yaml"
        if not os.path.isabs(map_path) and not os.path.exists(map_path):
            map_path = os.path.expanduser(f"~/maps/{map_path}")
        if not os.path.exists(map_path):
            return {"status": "error", "message": f"Map not found: {map_path}"}

        # Stop SLAM if running (can't run both)
        if _subprocesses.get("slam") and _subprocesses["slam"].poll() is None:
            await loop.run_in_executor(None, _kill_ros2, "slam")
            log.info("SLAM stopped before Nav2 start")

        import shlex
        ok, msg = await loop.run_in_executor(
            None, _launch_ros2, "nav2",
            f"ros2 launch nav2_bringup bringup_launch.py map:={shlex.quote(map_path)} use_sim_time:=false autostart:=true",
        )
        if ok:
            # Start cmd_vel relay
            await loop.run_in_executor(None, _start_cmd_vel_relay)
            # Set initial pose after Nav2 activates
            asyncio.get_event_loop().call_later(8.0, lambda: asyncio.ensure_future(_set_initial_pose(params)))
        return {"status": "ok" if ok else "error", "message": msg}

    elif command == "NAV2_STOP":
        _stop_cmd_vel_relay()
        ok, msg = await loop.run_in_executor(None, _kill_ros2, "nav2")
        return {"status": "ok" if ok else "error", "message": msg}

    elif command == "NAV2_GOAL":
        x = float(params.get("x", 0))
        y = float(params.get("y", 0))
        theta = float(params.get("theta", 0))
        sp = subprocess
        try:
            qz = math.sin(theta / 2)
            qw = math.cos(theta / 2)
            cmd = (
                f'source /etc/env.sh && ros2 topic pub --once /goal_pose '
                f'geometry_msgs/msg/PoseStamped '
                f'"{{header: {{frame_id: map}}, pose: {{position: {{x: {x}, y: {y}}}, '
                f'orientation: {{z: {qz}, w: {qw}}}}}}}"'
            )
            result = await loop.run_in_executor(
                None, lambda: sp.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=10)
            )
            if result.returncode == 0:
                return {"status": "ok", "message": f"Nav2 goal sent: x={x}, y={y}, theta={theta}"}
            return {"status": "error", "message": result.stderr[:200] or "nav2 goal failed"}
        except Exception as e:
            return {"status": "error", "message": str(e)}

    elif command == "NAV2_CANCEL":
        sp = subprocess
        try:
            cmd = 'source /etc/env.sh && ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{}" --cancel'
            await loop.run_in_executor(None, lambda: sp.run(["bash", "-c", cmd], capture_output=True, timeout=5))
            return {"status": "ok", "message": "Navigation cancelled"}
        except Exception as e:
            return {"status": "error", "message": str(e)}

    elif command == "NAV2_STATUS":
        # Check if nav2 lifecycle nodes are active
        sp = subprocess
        try:
            cmd = "source /etc/env.sh && ros2 node list 2>/dev/null | grep -c nav2"
            result = await loop.run_in_executor(None, lambda: sp.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=5))
            count = int(result.stdout.strip()) if result.stdout.strip().isdigit() else 0
            slam_active = _subprocesses.get("slam") is not None and _subprocesses["slam"].poll() is None
            return {"status": "ok", "nav2_nodes": count, "slam_active": slam_active}
        except Exception as e:
            return {"status": "error", "message": str(e)}

    return {"status": "error", "message": f"Unknown nav2 command: {command}"}


# ── Command handling ──

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"):
            # Interrupt any running MOVE/TURN immediately
            _estop_event.set()
            pub = _cmd_vel_pub
            if pub:
                pub.publish(_make_cmd_vel_msg())  # immediate zero velocity
            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()  # also interrupt running commands
            loop = asyncio.get_event_loop()
            result = await loop.run_in_executor(None, publish_cmd_vel, 0.0, 0.0, 0)
            return {"status": result}

        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))  # check every N meters
            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"):
            # Return latest LiDAR scan from cache
            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"):
            import base64
            from .ros2_node import start_camera, is_camera_active
            from .config import _ros2_cache as shared_cache, _ros2_lock as shared_lock

            # Ensure camera subscription is active (idempotent — just refreshes idle timer if already on)
            ok = start_camera()
            if not ok:
                log.warning("CAPTURE_IMAGE: start_camera() failed — _camera_node not initialized")
                return {"status": "error", "message": "Camera not available on this device"}

            # Check for existing fresh frame (< 5s old)
            with shared_lock:
                frame = shared_cache.get("camera_frame")
                frame_ts = shared_cache.get("camera_frame_ts", 0)
            if frame and (time.time() - frame_ts) < 5.0:
                log.info("CAPTURE_IMAGE: using cached frame (age=%.1fs)", time.time() - frame_ts)
            else:
                # Wait for fresh frame — camera may need time to warm up after subscribe
                log.info("CAPTURE_IMAGE: waiting for fresh frame (active=%s, had_frame=%s)",
                         is_camera_active(), bool(frame))
                deadline = time.time() + 10.0  # 10s max wait
                while time.time() < deadline:
                    with shared_lock:
                        frame = shared_cache.get("camera_frame")
                        frame_ts = shared_cache.get("camera_frame_ts", 0)
                    if frame and (time.time() - frame_ts) < 3.0:
                        break
                    time.sleep(0.2)

            with shared_lock:
                cam_info = shared_cache.get("camera", {})
                frame = shared_cache.get("camera_frame")
                frame_ts = shared_cache.get("camera_frame_ts", 0)
            if frame:
                age = time.time() - frame_ts
                b64 = base64.b64encode(frame).decode('ascii')
                log.info("CAPTURE_IMAGE: OK (size=%dKB, age=%.1fs)", len(frame) // 1024, age)
                return {
                    "status": "ok",
                    **cam_info,
                    "image_base64": b64,
                    "image_size_bytes": len(frame),
                    "image_age_sec": round(age, 1),
                    "format": "jpeg",
                }
            log.warning("CAPTURE_IMAGE: no frame after 10s wait")
            return {"status": "ok", "message": "Camera timeout — no frame received. Check camera hardware/driver."}

        elif command == "START_CAMERA_STREAM":
            from .ros2_node import start_camera
            from .config import _ros2_cache as shared_cache, _ros2_lock as shared_lock
            start_camera()
            fps = int(params.get("fps", 2))
            with shared_lock:
                shared_cache["camera_stream"] = True
                shared_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":
            from .ros2_node import stop_camera
            from .config import _ros2_cache as shared_cache, _ros2_lock as shared_lock
            stop_camera()
            with shared_lock:
                shared_cache["camera_stream"] = False
            log.info("Camera stream stopped")
            return {"status": "ok", "message": "Camera streaming stopped"}

        elif command == "START_FOLLOW":
            from .config import _ros2_cache as shared_cache, _ros2_lock as shared_lock
            target = params.get("target", "person")
            max_speed = float(params.get("max_speed", 0.3))
            follow_distance = float(params.get("follow_distance", 2.0))
            with shared_lock:
                shared_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":
            from .config import _ros2_cache as shared_cache, _ros2_lock as shared_lock
            with shared_lock:
                shared_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:
                # Safe circle: move in arc segments, check obstacle between each
                step_duration = 0.5  # check every 0.5 seconds
                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)

        # AUTO_EXPLORE: handled by ai_teammate_ros2_skills (private package)

        elif command == "PATROL":
            locations = params.get("locations", [])
            repeat = params.get("repeat", False)
            # Patrol is navigate_path with location names → for now, same as navigate_path
            # TODO: location name → coordinate lookup
            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 for server VSLAM ──
        elif command == "KEYFRAME_START":
            global _kf_capture_active
            if _kf_capture_active:
                return {"status": "ok", "message": "already active", "buffer": len(_keyframe_buffer)}
            from .ros2_node import start_camera
            start_camera()
            _kf_capture_active = True
            # VSLAM 세션 초기화
            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)
            # 백그라운드 WiFi 스캔 시작
            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 .ros2_node import stop_camera
            _kf_capture_active = False
            stop_camera()
            # 잔여 버퍼 flush
            await _flush_keyframe_buffer()
            # VSLAM 세션 종료
            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": _kf_total_captured,
                    "vslam_stop": stop_result.get("status")}

        elif command == "KEYFRAME_STATUS":
            return {
                "status": "ok",
                "active": _kf_capture_active,
                "buffer_size": len(_keyframe_buffer),
                "total_captured": _kf_total_captured,
                "last_upload": _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":
            result = await _send_service_request({
                "service": "map_list",
                "offset": params.get("offset", 0),
                "limit": params.get("limit", 20),
            })
            return result

        elif command == "MAP_DOWNLOAD":
            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
            # base64 → 파일 저장
            import base64 as b64, struct
            payload = b64.b64decode(result["data_base64"])
            # 패킹 형식: 4-byte header length + JSON header + file data
            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 as e:
                # 패킹 형식이 아니면 raw 저장
                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_START", "NAV2_STOP", "NAV2_GOAL", "NAV2_CANCEL", "NAV2_STATUS"):
            return await _handle_nav2(command, params)

        elif command == "SET_INITIAL_POSE":
            await _set_initial_pose(params)
            import math as _math
            x = float(params.get("init_x", 0))
            y = float(params.get("init_y", 0))
            theta_rad = float(params.get("init_theta", 0))
            return {"status": "ok", "message": f"Initial pose set: x={x:.2f} y={y:.2f} θ={_math.degrees(theta_rad):.1f}°"}

        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": _ros2_node is not None,
                    "slam_active": slam_active, "nav2_active": nav2_active,
                    "speed_limit": speed_limit, "sensors": data or {}}

        # ── Phase 1: Named Locations ──
        elif command == "SAVE_LOCATION":
            name = params.get("name", "").strip()
            if not name:
                return {"status": "error", "message": "name required"}
            # Use explicit coords if provided, else current odometry
            if "x" in params and "y" in params:
                loc = {"x": float(params["x"]), "y": float(params["y"]),
                       "theta": float(params.get("theta", 0)),
                       "source": params.get("source", "map_pin")}  # map_pin = placed on map
            else:
                with _ros2_lock:
                    odom = _ros2_cache.get("odometry", {})
                loc = {"x": odom.get("x", 0), "y": odom.get("y", 0), "theta": odom.get("theta", 0),
                       "source": params.get("source", "robot_pose")}  # robot_pose = at robot
            locs = _load_locations()
            locs[name] = loc
            _save_locations(locs)

            # P0-2: Gateway로 place 업서트 전파 (best-effort, 실패해도 local은 성공)
            cloud_sync = {"synced": False}
            try:
                sync_result = await _send_service_request(
                    {
                        "service": "place_upsert",
                        "name": name,
                        "pose": loc,
                        "source": "named_location",
                        # map_id는 bridge가 알면 포함 — 현재 SLAM active 여부로 힌트
                    },
                    timeout=5.0,
                )
                if isinstance(sync_result, dict) and sync_result.get("status") == "ok":
                    cloud_sync = {
                        "synced": True,
                        "place_id": (sync_result.get("place") or {}).get("place_id"),
                    }
                else:
                    cloud_sync = {
                        "synced": False,
                        "error": (sync_result or {}).get("message", "no response"),
                    }
            except Exception as e:
                cloud_sync = {"synced": False, "error": str(e)[:120]}
            return {"status": "ok", "name": name, **loc, "cloud": cloud_sync}

        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 == "LIST_LOCAL_MAPS":
            map_dir = os.path.expanduser("~/maps")
            if not os.path.isdir(map_dir):
                return {"status": "ok", "maps": []}
            maps = []
            for f in sorted(os.listdir(map_dir)):
                if not f.endswith(".yaml"):
                    continue
                name = f[:-5]
                yaml_path = os.path.join(map_dir, f)
                pgm_path = os.path.join(map_dir, f"{name}.pgm")
                stat = os.stat(yaml_path)
                maps.append({
                    "name": name,
                    "has_pgm": os.path.exists(pgm_path),
                    "size_bytes": os.path.getsize(pgm_path) if os.path.exists(pgm_path) else 0,
                    "modified": stat.st_mtime,
                })
            return {"status": "ok", "maps": maps, "total": len(maps)}

        elif command == "DELETE_LOCAL_MAP":
            map_dir = os.path.expanduser("~/maps")
            map_name = params.get("map_name", "")
            if not map_name or ".." in map_name or "/" in map_name:
                return {"status": "error", "message": "Invalid map_name"}
            deleted = []
            for ext in (".yaml", ".pgm", ".pbstream"):
                p = os.path.join(map_dir, f"{map_name}{ext}")
                if os.path.exists(p):
                    os.remove(p)
                    deleted.append(f"{map_name}{ext}")
            return {"status": "ok", "deleted": deleted} if deleted else {"status": "error", "message": "Map not found"}

        elif command == "GET_MAP":
            import base64
            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))
            sp = subprocess
            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)}}}}}}}}}"'
                sp.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)}

        # ── Phase 2: Operations ──
        elif command == "DOCK":
            # Former robot has auto_docking node
            sp = subprocess
            try:
                cmd = "source /etc/env.sh && ros2 action send_goal /dock_robot former_interfaces/action/Dock '{}'"
                result = sp.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":
            sp = subprocess
            try:
                cmd = "source /etc/env.sh && ros2 action send_goal /undock_robot former_interfaces/action/Undock '{}'"
                result = sp.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", {})
            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"}
            sp = subprocess
            try:
                cmd = f'espeak-ng -v {lang} "{text}" 2>/dev/null || echo "{text}" | festival --tts 2>/dev/null || echo "TTS not available"'
                result = sp.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")
            sp = subprocess
            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)
                sp.Popen(["bash", "-c", cmd], stdout=sp.DEVNULL, stderr=sp.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")
            # Platform-specific: write to GPIO or publish to LED topic
            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":
            sp = subprocess
            try:
                cmd = "source /etc/env.sh && ros2 topic echo /diagnostics --once 2>/dev/null"
                result = sp.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=5)
                # Also get system info
                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}

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

        elif command == "SHUTDOWN":
            delay = int(params.get("delay", 5))
            sp = subprocess
            sp.Popen(["bash", "-c", f"sleep {delay} && sudo shutdown -h now"], stdout=sp.DEVNULL, stderr=sp.DEVNULL)
            return {"status": "ok", "message": f"Shutting down in {delay} seconds"}

        elif command == "UPDATE_BRIDGE":
            sp = subprocess
            try:
                cmd = 'cd ~/ai-teammate-bridge && curl -sL "https://raw.githubusercontent.com/mobiolabs2025/ai-teammate-robot-bridge/main/device_bridge.py" -o device_bridge.py.new && mv device_bridge.py.new device_bridge.py && echo "updated"'
                result = sp.run(["bash", "-c", cmd], capture_output=True, text=True, timeout=15)
                if "updated" in result.stdout:
                    return {"status": "ok", "message": "Bridge updated. Restart with: sudo systemctl restart ai-teammate-bridge"}
                return {"status": "error", "message": result.stderr[:200]}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        # ── Radio Fingerprint ──
        elif command == "SCAN_WIFI":
            sp = subprocess
            try:
                result = sp.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 = int(parts[1]) if parts[1].lstrip('-').isdigit() else 0
                        bssid = ":".join(parts[2:8]).replace("\\", "")
                        aps.append({"ssid": ssid, "signal": signal, "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()
                # Deduplicate by address, keep strongest signal
                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"}
            # Collect current pose + WiFi + BLE
            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
            sp = subprocess
            wifi_aps = []
            try:
                result = sp.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 = int(parts[6]) if parts[6].lstrip('-').isdigit() else 0
                        wifi_aps.append({"bssid": bssid, "signal": signal})
            except Exception:
                pass
            # Save
            fp_file = os.path.join(os.path.dirname(__file__) or ".", "fingerprints.json")
            try:
                with open(fp_file) as f:
                    fps = json.load(f)
            except Exception:
                fps = {}
            fps[name] = {
                "pose": pose,
                "wifi": wifi_aps,
                "timestamp": time.time(),
                "source": params.get("source", "verified"),  # verified=직접 스캔, predicted=SLAM 근접매칭
            }
            with open(fp_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":
            fp_file = os.path.join(os.path.dirname(__file__) or ".", "fingerprints.json")
            try:
                with open(fp_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":
            # Match current WiFi scan against saved fingerprints
            blocklist = _load_wifi_blocklist()
            sp = subprocess
            try:
                result = sp.run(["nmcli", "-t", "-f", "BSSID,SIGNAL", "dev", "wifi", "list"],
                                capture_output=True, text=True, timeout=10)
                current = {}
                blocked_count = 0
                for line in result.stdout.strip().split("\n"):
                    parts = line.split(":")
                    if len(parts) >= 7:
                        bssid = ":".join(parts[:6]).replace("\\", "")
                        if bssid in blocklist:
                            blocked_count += 1
                            continue  # skip mobile APs
                        signal = int(parts[6]) if parts[6].lstrip('-').isdigit() else 0
                        current[bssid] = signal
            except Exception:
                return {"status": "error", "message": "WiFi scan failed"}

            fp_file = os.path.join(os.path.dirname(__file__) or ".", "fingerprints.json")
            try:
                with open(fp_file) as f:
                    fps = json.load(f)
            except Exception:
                return {"status": "error", "message": "No fingerprints saved"}

            # Simple nearest-neighbor matching (sum of squared RSSI differences)
            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 blocklist:
                        continue  # skip mobile APs in saved data too
                    if ap["bssid"] in current:
                        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),
                        "blocked_aps": blocked_count}
            return {"status": "ok", "location": None, "message": "No matching fingerprint found"}

        elif command in ("DETECT_OBJECTS", "DETECT_AND_INGEST"):
            global _detect_last_ts
            from .ros2_node import start_camera, is_camera_active
            if not is_camera_active():
                start_camera()
                for _ in range(20):
                    with _ros2_lock:
                        if _ros2_cache.get("camera_frame"):
                            break
                    time.sleep(0.1)
            # Rate limit
            now = time.time()
            elapsed = now - _detect_last_ts
            if elapsed < DETECT_COOLDOWN:
                return {"status": "error", "message": f"cooldown: retry in {DETECT_COOLDOWN - elapsed:.1f}s"}
            _detect_last_ts = now

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

            import base64
            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"),
            }

            # DETECT_AND_INGEST: WiFi fingerprint + position 첨부
            if command == "DETECT_AND_INGEST":
                footprint = {}
                try:
                    sp = __import__("subprocess")
                    wifi_result = sp.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")

            # Gateway에 service_request 전송 → 응답 대기
            result = await _send_service_request(svc_data)
            return result

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

    return None


async def _send_service_request(data: dict, timeout: float = 15.0) -> dict:
    """Gateway에 service_request WS 메시지를 보내고 service_response를 대기합니다."""
    if _ws_ref is None:
        return {"status": "error", "message": "WebSocket not connected"}

    import uuid as _uuid
    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_ref.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):
    """Gateway로부터 받은 service_response를 pending future에 전달합니다."""
    request_id = data.get("request_id", "")
    future = _service_futures.get(request_id)
    if future and not future.done():
        future.set_result(data)


# ── Keyframe fallback (skills package is canonical; these remain only for
#     legacy handle_command paths used when ai-teammate-ros2-skills is absent) ──

def _should_capture_keyframe() -> bool:
    """오도메트리 변화 기반으로 키프레임 캡처 여부를 판단합니다."""
    global _last_kf_pose, _last_kf_ts
    now = time.time()
    if now - _last_kf_ts < KF_MIN_INTERVAL:
        return False

    with _ros2_lock:
        odom = _ros2_cache.get("odometry")
        frame = _ros2_cache.get("camera_frame")
    if not odom or not frame:
        return False

    if _last_kf_pose is None:
        return True

    dx = odom.get("x", 0) - _last_kf_pose.get("x", 0)
    dy = odom.get("y", 0) - _last_kf_pose.get("y", 0)
    displacement = (dx * dx + dy * dy) ** 0.5
    dtheta = abs(odom.get("theta", 0) - _last_kf_pose.get("theta", 0))

    return displacement >= KF_MIN_DISPLACEMENT or dtheta >= KF_MIN_ROTATION


def _capture_keyframe():
    """현재 프레임 + 오도메트리를 키프레임 버퍼에 추가합니다."""
    global _last_kf_pose, _last_kf_ts, _kf_total_captured
    import uuid as _uuid, cv2

    with _ros2_lock:
        frame = _ros2_cache.get("camera_frame")
        odom = dict(_ros2_cache.get("odometry", {}))
        cam = dict(_ros2_cache.get("camera", {}))

    if not frame:
        return

    # 썸네일 생성 (160x120, quality 40)
    import numpy as np
    img = cv2.imdecode(np.frombuffer(frame, np.uint8), cv2.IMREAD_COLOR)
    if img is not None:
        thumb = cv2.resize(img, (KF_THUMB_WIDTH, KF_THUMB_HEIGHT))
        _, jpeg = cv2.imencode('.jpg', thumb, [cv2.IMWRITE_JPEG_QUALITY, KF_THUMB_QUALITY])
        thumb_b64 = __import__("base64").b64encode(jpeg.tobytes()).decode("ascii")
    else:
        thumb_b64 = None

    # wheel odometry 기반 scale: 이전 키프레임과의 거리
    scale_factor = None
    if _last_kf_pose:
        dx = odom.get("x", 0) - _last_kf_pose.get("x", 0)
        dy = odom.get("y", 0) - _last_kf_pose.get("y", 0)
        scale_factor = (dx * dx + dy * dy) ** 0.5

    kf = {
        "keyframe_id": str(_uuid.uuid4())[:8],
        "timestamp": int(time.time() * 1000),
        "pose": odom,
        "features": [],
        "image_thumbnail": thumb_b64,
        "quality": {
            "feature_count": 0,
            "blur_score": 0.0,
        },
        "scale_factor": scale_factor,
        "scale_confidence": 0.9 if scale_factor else None,
        "scale_source": "odometry" if scale_factor else None,
        "fingerprint": _kf_wifi_cache,
    }

    _keyframe_buffer.append(kf)
    if len(_keyframe_buffer) > KF_MAX_BUFFER:
        _keyframe_buffer.pop(0)

    _last_kf_pose = odom
    _last_kf_ts = time.time()
    _kf_total_captured += 1
    log.info(f"Keyframe captured #{_kf_total_captured} (buffer: {len(_keyframe_buffer)})")


async def _maybe_upload_keyframe_batch():
    """버퍼가 배치 크기에 도달하면 Gateway로 업로드합니다."""
    global _kf_last_upload_ts
    if len(_keyframe_buffer) < KF_BATCH_SIZE:
        return
    await _flush_keyframe_buffer()


async def _flush_keyframe_buffer():
    """키프레임 버퍼를 Gateway에 업로드하고 비웁니다."""
    global _kf_last_upload_ts
    if not _keyframe_buffer:
        return

    batch = list(_keyframe_buffer)
    _keyframe_buffer.clear()

    # fingerprint를 metadata에 포함 (radio scale correction용)
    for kf in batch:
        fp = kf.pop("fingerprint", None)
        if fp:
            kf.setdefault("metadata", {})["wifi_fingerprint"] = fp

    svc_data = {
        "service": "vslam_keyframe_batch",
        "radio_scale_enabled": True,
        "keyframes": batch,
        "merge_map": True,
        "device_type": "robot",
    }

    log.info(f"Uploading keyframe batch: {len(batch)} keyframes")
    result = await _send_service_request(svc_data, timeout=30.0)
    if result.get("status") == "ok":
        _kf_last_upload_ts = time.time()
        log.info(f"Keyframe batch uploaded successfully")
    else:
        log.warning(f"Keyframe batch upload failed: {result.get('message', '')}")
        # 실패 시 버퍼에 다시 넣기 (앞쪽에)
        _keyframe_buffer[:0] = batch


def _load_wifi_blocklist() -> set:
    """Load BSSID blocklist (mobile APs to exclude from localization)."""
    try:
        with open(WIFI_BLOCKLIST_FILE) as f:
            data = json.load(f)
        return set(data.get("bssids", []))
    except Exception:
        return set()


def _save_wifi_blocklist(bssids: set, reason_map: dict | None = None):
    """Save BSSID blocklist."""
    data = {"bssids": sorted(bssids)}
    if reason_map:
        data["reasons"] = {k: v for k, v in reason_map.items() if k in bssids}
    with open(WIFI_BLOCKLIST_FILE, "w") as f:
        json.dump(data, f, indent=2)


def _update_wifi_blocklist(fingerprints: list) -> int:
    """Analyze fingerprints to detect mobile APs.

    Mobile AP detection: if the same BSSID appears at locations
    spread across a large area (max distance > 10m), it's likely
    a phone hotspot or moving device.

    Returns count of newly blocked BSSIDs.
    """
    if len(fingerprints) < 3:
        return 0

    # Collect all positions where each BSSID was seen
    bssid_positions: dict[str, list[tuple[float, float]]] = {}
    for fp in fingerprints:
        pose = fp.get("pose", {})
        px, py = pose.get("x", 0), pose.get("y", 0)
        for ap in fp.get("wifi", []):
            bssid = ap.get("bssid", "")
            if bssid:
                bssid_positions.setdefault(bssid, []).append((px, py))

    blocklist = _load_wifi_blocklist()
    reason_map: dict[str, str] = {}
    new_blocked = 0
    SPREAD_THRESHOLD = 10.0  # meters — if max distance between sightings > this, it's mobile

    for bssid, positions in bssid_positions.items():
        if bssid in blocklist:
            continue
        if len(positions) < 2:
            continue
        # Calculate max pairwise distance (just check first vs rest for speed)
        max_dist = 0.0
        x0, y0 = positions[0]
        for x1, y1 in positions[1:]:
            d = ((x1 - x0) ** 2 + (y1 - y0) ** 2) ** 0.5
            if d > max_dist:
                max_dist = d
        if max_dist > SPREAD_THRESHOLD:
            blocklist.add(bssid)
            reason_map[bssid] = f"spread={max_dist:.1f}m across {len(positions)} points"
            new_blocked += 1

    if new_blocked > 0:
        _save_wifi_blocklist(blocklist, reason_map)
        log.info(f"WiFi blocklist: {new_blocked} mobile APs detected (total {len(blocklist)} blocked)")

    return new_blocked


async def _slam_wifi_scan_loop(shared_cache: dict, shared_lock):
    """SLAM 맵핑 중 WiFi fingerprint를 주기적으로 수집하여 pose와 함께 저장."""
    SCAN_INTERVAL = 5.0  # 5초마다 스캔
    MIN_DISPLACEMENT = 0.5  # 최소 50cm 이동 후 수집

    last_fp_pose: tuple | None = None

    while _slam_radio_active:
        try:
            # 현재 pose 확인
            with shared_lock:
                pose = shared_cache.get("slam_pose")
            if not pose:
                await asyncio.sleep(SCAN_INTERVAL)
                continue

            px, py = pose["x"], pose["y"]

            # 최소 이동 거리 체크
            if last_fp_pose:
                dist = ((px - last_fp_pose[0]) ** 2 + (py - last_fp_pose[1]) ** 2) ** 0.5
                if dist < MIN_DISPLACEMENT:
                    await asyncio.sleep(SCAN_INTERVAL)
                    continue

            # WiFi 스캔
            sp = __import__("subprocess")
            result = sp.run(
                ["nmcli", "-t", "-f", "BSSID,SIGNAL", "dev", "wifi", "list", "--rescan", "yes"],
                capture_output=True, text=True, timeout=10,
            )
            aps = []
            for line in result.stdout.strip().split("\n"):
                parts = line.split(":")
                if len(parts) >= 7:
                    bssid = ":".join(parts[:6]).replace("\\", "")
                    signal = int(parts[6]) if parts[6].lstrip('-').isdigit() else 0
                    if signal != 0:
                        aps.append({"bssid": bssid, "signal": signal, "stable": abs(signal) > 20})

            if aps:
                fp = {
                    "pose": {"x": round(px, 3), "y": round(py, 3), "theta": round(pose["theta"], 3)},
                    "wifi": aps,
                    "ts": int(time.time() * 1000),
                }
                with shared_lock:
                    shared_cache.setdefault("_slam_fingerprints", []).append(fp)
                last_fp_pose = (px, py)
                log.info(f"SLAM radio fingerprint #{len(shared_cache.get('_slam_fingerprints', []))}: "
                         f"({px:.1f}, {py:.1f}) {len(aps)} APs")

        except Exception as e:
            log.debug(f"SLAM WiFi scan error: {e}")
        await asyncio.sleep(SCAN_INTERVAL)


async def _kf_wifi_scan_loop():
    """백그라운드 WiFi 스캔 — 캐시를 주기적으로 갱신합니다."""
    global _kf_wifi_cache, _kf_wifi_cache_ts
    while _kf_capture_active:
        try:
            sp = __import__("subprocess")
            result = sp.run(
                ["nmcli", "-t", "-f", "SSID,SIGNAL,BSSID", "dev", "wifi", "list", "--rescan", "yes"],
                capture_output=True, text=True, timeout=10,
            )
            aps = []
            for line in result.stdout.strip().split("\n"):
                parts = line.split(":")
                if len(parts) >= 3:
                    aps.append({
                        "ssid": parts[0],
                        "signal": int(parts[1]) if parts[1].lstrip('-').isdigit() else 0,
                        "bssid": ":".join(parts[2:8]).replace("\\", ""),
                    })
            if aps:
                _kf_wifi_cache = {"wifi": aps}
                _kf_wifi_cache_ts = time.time()
        except Exception as e:
            log.debug(f"WiFi scan error: {e}")
        await asyncio.sleep(KF_WIFI_SCAN_INTERVAL)


# ── Sensor data ──

def get_ros2_data():
    """Get latest cached ROS2 data from rclpy subscriptions."""
    # Exclude binary data (camera_frame) from status updates
    _EXCLUDE_KEYS = {"camera_frame", "camera_frame_ts", "camera_stream", "camera_stream_fps"}
    with _ros2_lock:
        data = {k: v for k, v in _ros2_cache.items() if k not in _EXCLUDE_KEYS}

    try:
        import psutil
        battery = psutil.sensors_battery()
        data["battery"] = {
            "voltage": 24.0,
            "percentage": battery.percent if battery else 100,
            "charging": battery.power_plugged if battery else False,
        }
    except Exception:
        data.setdefault("battery", {"voltage": 24.0, "percentage": 100, "charging": False})

    try:
        import psutil
        temps = psutil.sensors_temperatures()
        if temps:
            first = list(temps.values())[0]
            data["temperature"] = first[0].current if first else 40.0
        else:
            data["temperature"] = 40.0
    except Exception:
        data.setdefault("temperature", 40.0)

    return data if data.get("imu") or data.get("odometry") else None


def get_sim_data(tick):
    """Generate simulated sensor data."""
    s = tick * 0.1
    return {
        "imu": {
            "roll": math.sin(s) * 0.05,
            "pitch": math.cos(s * 0.7) * 0.03,
            "yaw": (s * 0.2) % (math.pi * 2) - math.pi,
            "accel_x": math.sin(s * 2) * 0.5,
            "accel_y": math.cos(s * 1.5) * 0.3,
            "accel_z": 9.81 + (random.random() - 0.5) * 0.05,
        },
        "odometry": {
            "x": math.sin(s * 0.2) * 2,
            "y": math.cos(s * 0.2) * 2,
            "theta": s * 0.1,
            "linear_vel": abs(math.sin(s * 0.3)) * 0.5,
            "angular_vel": math.sin(s * 0.5) * 0.2,
        },
        "battery": {
            "voltage": 24.0 - tick * 0.001,
            "percentage": max(0, 95 - tick * 0.01),
            "charging": False,
        },
        "temperature": 35 + math.sin(s * 0.1) * 5,
    }

