# ROS2 rclpy native node — sensor subscriptions, camera auto-detection, cmd_vel
import os
import time
import threading

from .config import log, _ros2_cache, _ros2_lock
from .sensors_config import get_sensor_config, probe_publisher_qos
import ai_teammate_ros2_bridge.config as _cfg

# Track ROS2 spin thread state for restart
_ros2_thread: threading.Thread | None = None
_ros2_restart_lock = threading.Lock()


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

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

    sc = get_sensor_config()

    # ── Discover topics with boot-wait retry ──
    # After reboot, hardware nodes (base_controller etc.) may take 30-90s to start.
    # Retry discovery until meaningful topics appear (beyond /parameter_events, /rosout).
    _BOOT_WAIT_MAX = 120  # max seconds to wait
    _BOOT_POLL_INTERVAL = 5
    _boot_start = time.time()
    by_type: dict[str, list[str]] = {}
    while time.time() - _boot_start < _BOOT_WAIT_MAX:
        time.sleep(_BOOT_POLL_INTERVAL)
        try:
            discovered = node.get_topic_names_and_types()
            by_type.clear()
            for name, types in discovered:
                for t in types:
                    by_type.setdefault(t, []).append(name)
            real_topics = [n for n, _ in discovered if n not in ("/parameter_events", "/rosout")]
            if real_topics:
                log.info(f"Topic discovery OK: {len(real_topics)} topics found after {int(time.time() - _boot_start)}s")
                break
            elapsed = int(time.time() - _boot_start)
            log.info(f"Waiting for ROS2 topics ({elapsed}s / {_BOOT_WAIT_MAX}s)...")
        except Exception as e:
            elapsed = int(time.time() - _boot_start)
            log.warning(f"Topic discovery retry ({elapsed}s): {e}")
    else:
        log.warning(f"Topic discovery timed out after {_BOOT_WAIT_MAX}s — proceeding with defaults")
    sc.set_detected_topics(by_type)

    # Publisher — detect Twist vs TwistStamped (Jazzy+ uses TwistStamped)
    cmd_vel_topic = sc.topic("cmd_vel") or "/cmd_vel"
    _use_twist_stamped = False
    for tname, ttypes in by_type.items():
        if cmd_vel_topic in ttypes:
            if "geometry_msgs/msg/TwistStamped" in [tname]:
                _use_twist_stamped = True
            break
    # Also check topic info directly
    try:
        for name, types in node.get_topic_names_and_types():
            if name == cmd_vel_topic:
                if "geometry_msgs/msg/TwistStamped" in types:
                    _use_twist_stamped = True
                break
    except Exception:
        pass

    if _use_twist_stamped:
        _cfg._cmd_vel_pub = node.create_publisher(TwistStamped, cmd_vel_topic, 10)
        _cfg._cmd_vel_stamped = True
        log.info(f"cmd_vel publisher created: {cmd_vel_topic} (TwistStamped — Jazzy/Rolling)")
    else:
        _cfg._cmd_vel_pub = node.create_publisher(Twist, cmd_vel_topic, 10)
        _cfg._cmd_vel_stamped = False
        log.info(f"cmd_vel publisher created: {cmd_vel_topic} (Twist)")

    # Subscribers
    _imu_throttle_interval = 0.05  # max 20 Hz (IMU publishes 100+ Hz)
    _last_imu_ts = 0.0

    def on_imu(msg):
        nonlocal _last_imu_ts
        now = time.time()
        if now - _last_imu_ts < _imu_throttle_interval:
            return
        _last_imu_ts = now
        with _ros2_lock:
            import math
            q = msg.orientation
            siny = 2.0 * (q.w * q.z + q.x * q.y)
            cosy = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
            yaw = math.atan2(siny, cosy)
            sinr = 2.0 * (q.w * q.x + q.y * q.z)
            cosr = 1.0 - 2.0 * (q.x * q.x + q.y * q.y)
            roll = math.atan2(sinr, cosr)
            sinp = 2.0 * (q.w * q.y - q.z * q.x)
            pitch = math.asin(max(-1.0, min(1.0, sinp)))
            _ros2_cache["imu_ts"] = time.time()
            _ros2_cache["imu"] = {
                "roll": roll,
                "pitch": pitch,
                "yaw": yaw,
                "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["odom_ts"] = time.time()
            import math
            q = msg.pose.pose.orientation
            siny = 2.0 * (q.w * q.z + q.x * q.y)
            cosy = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
            yaw = math.atan2(siny, cosy)
            _ros2_cache["odometry"] = {
                "x": msg.pose.pose.position.x,
                "y": msg.pose.pose.position.y,
                "theta": yaw,
                "linear_vel": msg.twist.twist.linear.x,
                "angular_vel": msg.twist.twist.angular.z,
            }

    _scan_throttle_interval = 0.2  # max 5 Hz (LiDAR publishes 10-20 Hz)
    _last_scan_ts = 0.0

    def on_scan(msg):
        nonlocal _last_scan_ts
        now = time.time()
        if now - _last_scan_ts < _scan_throttle_interval:
            return
        _last_scan_ts = now
        import math as _math
        with _ros2_lock:
            _ros2_cache["scan_ts"] = now
            ranges = list(msg.ranges)
            valid = [r for r in ranges if msg.range_min < r < msg.range_max]
            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]
                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
                angle_inc = 0

            # Downsample scan to ~60 points for WS relay (every 10th ray)
            scan_points = []
            step = max(1, n // 60)
            for i in range(0, n, step):
                r = ranges[i]
                if msg.range_min < r < msg.range_max:
                    angle = msg.angle_min + i * angle_inc
                    scan_points.append([
                        round(r * _math.cos(angle), 3),
                        round(r * _math.sin(angle), 3),
                    ])

            _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,
            }
            _ros2_cache["scan_points"] = scan_points

    # ── PointCloud2 → 2D scan slice (for 3D LiDAR: Velodyne, Ouster, Livox) ──
    _pc_throttle_interval = 0.2  # max 5 Hz
    _last_pc_ts = 0.0

    def on_pointcloud(msg):
        """Extract 2D scan slice from PointCloud2 by filtering z-axis."""
        nonlocal _last_pc_ts
        now = time.time()
        if now - _last_pc_ts < _pc_throttle_interval:
            return
        _last_pc_ts = now
        import math as _math
        import struct

        try:
            # Parse PointCloud2 fields to find x, y, z offsets
            field_map = {}
            for f in msg.fields:
                field_map[f.name] = (f.offset, f.datatype)

            if "x" not in field_map or "y" not in field_map or "z" not in field_map:
                return

            x_off = field_map["x"][0]
            y_off = field_map["y"][0]
            z_off = field_map["z"][0]
            point_step = msg.point_step
            data = bytes(msg.data)

            # Z-axis filter: keep points in robot's horizontal plane
            z_min, z_max = -0.1, 0.5  # meters relative to sensor frame
            scan_points = []
            valid_ranges = []
            n_points = len(data) // point_step

            # Downsample: process every Nth point to keep ~60 output points
            step = max(1, n_points // 300)
            for i in range(0, n_points, step):
                base = i * point_step
                if base + z_off + 4 > len(data):
                    break
                x = struct.unpack_from('f', data, base + x_off)[0]
                y = struct.unpack_from('f', data, base + y_off)[0]
                z = struct.unpack_from('f', data, base + z_off)[0]

                if not (_math.isfinite(x) and _math.isfinite(y) and _math.isfinite(z)):
                    continue
                if z < z_min or z > z_max:
                    continue

                r = _math.sqrt(x * x + y * y)
                if r < 0.05 or r > 100.0:
                    continue

                valid_ranges.append(r)
                scan_points.append([round(x, 3), round(y, 3)])

            # Further downsample to ~60 points for WS relay
            if len(scan_points) > 60:
                ds_step = len(scan_points) // 60
                scan_points = scan_points[::ds_step]

            with _ros2_lock:
                _ros2_cache["scan_ts"] = now
                front_ranges = [r for r, p in zip(valid_ranges, scan_points)
                                if abs(_math.atan2(p[1], p[0])) < 0.52] if scan_points else []
                _ros2_cache["scan"] = {
                    "range_min": 0.05,
                    "range_max": 100.0,
                    "angle_min": -_math.pi,
                    "angle_max": _math.pi,
                    "num_ranges": len(valid_ranges),
                    "min_distance": round(min(valid_ranges), 3) if valid_ranges else None,
                    "mean_distance": round(sum(valid_ranges) / len(valid_ranges), 3) if valid_ranges else None,
                    "front_min_distance": round(min(front_ranges), 3) if front_ranges else None,
                    "source": "pointcloud2",
                }
                _ros2_cache["scan_points"] = scan_points

        except Exception as e:
            log.debug(f"PointCloud2 parse error: {e}")

    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,
            }

    _image_throttle_interval = 0.5  # seconds — max 2 FPS for cached frame
    _last_image_ts = 0.0

    def on_image(msg):
        """Cache latest camera frame as compressed JPEG bytes (throttled)."""
        nonlocal _last_image_ts
        now = time.time()
        if now - _last_image_ts < _image_throttle_interval:
            return
        _last_image_ts = now
        try:
            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'):
                return
            else:
                img = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, -1)

            _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60])
            with _ros2_lock:
                _ros2_cache["camera_frame"] = jpeg.tobytes()
                _ros2_cache["camera_frame_ts"] = now
        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:
        log.info("Auto-detecting camera topics...")
        try:
            time.sleep(2)
            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:
                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}")

    imu_topic = sc.topic("imu") or "/imu/data"
    odom_topic = sc.topic("odom") or "/odom"
    scan_topic = sc.topic("scan") or "/scan"
    cam_info_topic = sc.topic("camera_info") or CAMERA_INFO_TOPIC
    cam_image_topic = sc.topic("camera_image") or CAMERA_IMAGE_TOPIC

    def _resolve_qos(sensor: str, topic: str, default: str = "sensor_data"):
        """sensors.yaml explicit QoS > probe publisher > default."""
        entry = sc._cfg.get(sensor, {}) if isinstance(sc._cfg.get(sensor), dict) else {}
        if entry.get("qos"):
            return sc.qos(sensor)  # explicit in sensors.yaml
        return probe_publisher_qos(node, topic, default)

    if sc.enabled("imu"):
        node.create_subscription(Imu, imu_topic, on_imu, _resolve_qos("imu", imu_topic, "sensor_data"))
    if sc.enabled("odom"):
        node.create_subscription(Odometry, odom_topic, on_odom, _resolve_qos("odom", odom_topic, "reliable"))
    if sc.enabled("scan"):
        # Check if LaserScan topic exists; if not, try PointCloud2 fallback
        _has_laserscan = scan_topic in (by_type.get("sensor_msgs/msg/LaserScan", []))
        _pc_topic = sc.topic("point_cloud")  # from sensors.yaml or auto-detect
        _has_pointcloud = bool(_pc_topic) or bool(by_type.get("sensor_msgs/msg/PointCloud2"))

        if _has_laserscan or not _has_pointcloud:
            # Standard LaserScan subscription
            node.create_subscription(LaserScan, scan_topic, on_scan, _resolve_qos("scan", scan_topic, "sensor_data"))
            log.info(f"LaserScan subscribed: {scan_topic}")
        else:
            # Fallback: PointCloud2 → 2D slice
            from sensor_msgs.msg import PointCloud2
            if not _pc_topic:
                # Pick first available PointCloud2 topic
                pc_candidates = by_type.get("sensor_msgs/msg/PointCloud2", [])
                _pc_topic = pc_candidates[0] if pc_candidates else "/velodyne_points"
            node.create_subscription(PointCloud2, _pc_topic, on_pointcloud, _resolve_qos("point_cloud", _pc_topic, "sensor_data"))
            log.info(f"PointCloud2 subscribed (LaserScan fallback): {_pc_topic} → 2D slice")
    # Camera subscriptions are created on-demand to save CPU (~10% reduction).
    # Use start_camera() / stop_camera() to control.
    _cfg._camera_sub_info = None
    _cfg._camera_sub_image = None
    _cfg._camera_topic_info = cam_info_topic
    _cfg._camera_topic_image = cam_image_topic
    _cfg._camera_qos_info = _resolve_qos("camera_info", cam_info_topic, "sensor_data") if sc.enabled("camera_info") else None
    _cfg._camera_qos_image = _resolve_qos("camera_image", cam_image_topic, "sensor_data") if sc.enabled("camera_image") else None
    _cfg._camera_on_info = on_camera_info
    _cfg._camera_on_image = on_image
    _cfg._camera_node = node

    # Robot feedback (Former robot: battery voltage, temperature, estop button)
    FEEDBACK_TOPIC = os.environ.get("FEEDBACK_TOPIC", "/former_io_controller/robot_feedback")
    try:
        from former_interfaces.msg import RobotFeedback

        def on_robot_feedback(msg):
            with _ros2_lock:
                _ros2_cache["robot_feedback"] = {
                    "system_voltage": msg.system_voltage,
                    "charging_voltage": msg.charging_voltage,
                    "current_temperature": msg.current_temperature,
                    "estop_button": msg.estop_button,
                    "motor_enabled": msg.motor_enabled,
                    "fault_flag": msg.fault_flag,
                }

        node.create_subscription(RobotFeedback, FEEDBACK_TOPIC, on_robot_feedback, 10)
        log.info(f"Robot feedback subscribed: {FEEDBACK_TOPIC}")
    except ImportError:
        log.info("former_interfaces not available — robot feedback disabled")

    # ── SLAM map subscription (OccupancyGrid) ──
    MAP_TOPIC = os.environ.get("MAP_TOPIC", "/map")
    try:
        from nav_msgs.msg import OccupancyGrid as OccGrid

        _map_throttle_ts = [0.0]  # last update timestamp

        _MAP_MAX_CELLS = int(os.environ.get("MAP_MAX_CELLS", "10000"))  # ~100x100

        def on_map(msg):
            now = time.time()
            if now - _map_throttle_ts[0] < 2.0:
                return  # throttle: max once per 2 seconds
            _map_throttle_ts[0] = now

            w, h = msg.info.width, msg.info.height
            res = msg.info.resolution
            ox = msg.info.origin.position.x
            oy = msg.info.origin.position.y

            # Downsample if exceeds max cells
            total = w * h
            if total > _MAP_MAX_CELLS:
                step = max(2, int((total / _MAP_MAX_CELLS) ** 0.5))
                dw = w // step
                dh = h // step
                raw = msg.data
                ddata = []
                for dy in range(dh):
                    row = dy * step * w
                    for dx in range(dw):
                        ddata.append(raw[row + dx * step])
                map_data = ddata
                map_w, map_h, map_res = dw, dh, res * step
            else:
                map_data = list(msg.data)
                map_w, map_h, map_res = w, h, res

            with _ros2_lock:
                _ros2_cache["slam_map"] = {
                    "width": map_w,
                    "height": map_h,
                    "resolution": map_res,
                    "origin": {"x": ox, "y": oy, "theta": 0.0},
                    "data": map_data,
                }
                _ros2_cache["slam_map_ts"] = now

        from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
        map_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )
        node.create_subscription(OccGrid, MAP_TOPIC, on_map, map_qos)
        log.info(f"SLAM map subscribed: {MAP_TOPIC} (TRANSIENT_LOCAL)")
    except Exception as e:
        log.info(f"SLAM map subscription skipped: {e}")

    # ── TF listener for map-frame pose ──
    try:
        from tf2_ros import Buffer, TransformListener
        import math as _math
        _tf_buffer = Buffer()
        _tf_listener = TransformListener(_tf_buffer, node)

        def _update_map_pose():
            """Lookup map→base_footprint and cache as slam_pose."""
            try:
                t = _tf_buffer.lookup_transform("map", "base_footprint", rclpy.time.Time())
                p = t.transform.translation
                r = t.transform.rotation
                siny = 2.0 * (r.w * r.z + r.x * r.y)
                cosy = 1.0 - 2.0 * (r.y * r.y + r.z * r.z)
                yaw = _math.atan2(siny, cosy)
                with _ros2_lock:
                    _ros2_cache["slam_pose"] = {"x": p.x, "y": p.y, "theta": yaw}
                    _ros2_cache["slam_pose_ts"] = time.time()
                    # Collect trajectory during SLAM mapping (min 5cm apart)
                    if _ros2_cache.get("_trajectory_active"):
                        traj = _ros2_cache.setdefault("_trajectory", [])
                        if not traj or _math.hypot(p.x - traj[-1][0], p.y - traj[-1][1]) > 0.05:
                            traj.append((round(p.x, 3), round(p.y, 3), round(yaw, 3)))
            except Exception as tf_err:
                # Log once per 30s to avoid spam
                if not hasattr(_update_map_pose, '_last_err') or time.time() - _update_map_pose._last_err > 30:
                    log.debug(f"TF lookup map→base_footprint failed: {tf_err}")
                    _update_map_pose._last_err = time.time()

        # Timer: update map pose at 5 Hz
        node.create_timer(0.2, _update_map_pose)
        log.info("TF listener started for map→base_footprint pose")
    except Exception as e:
        log.info(f"TF listener skipped: {e}")

    log.info(f"Subscribed to {imu_topic}, {odom_topic}, {scan_topic} (camera on-demand: {cam_image_topic})")

    try:
        rclpy.spin(node)
    except Exception as e:
        log.error(f"rclpy spin error: {e}")
    finally:
        try:
            node.destroy_node()
        except Exception:
            pass
        try:
            rclpy.shutdown()
        except Exception:
            pass
        log.warning("ROS2 node shut down")
        _cfg._ros2_node = None
        _cfg._cmd_vel_pub = None


_camera_active = False
_camera_last_use_ts: float = 0.0
_CAMERA_AUTO_STOP_SEC = 30.0  # auto-stop after 30s idle


def start_camera() -> bool:
    """Start camera subscriptions on-demand. Returns True if started."""
    global _camera_active, _camera_last_use_ts
    _camera_last_use_ts = time.time()
    if _camera_active:
        return True
    node = getattr(_cfg, '_camera_node', None)
    if node is None:
        return False
    try:
        from sensor_msgs.msg import CameraInfo, Image as RosImage
        topic_info = getattr(_cfg, '_camera_topic_info', '')
        topic_image = getattr(_cfg, '_camera_topic_image', '')
        qos_info = getattr(_cfg, '_camera_qos_info', None)
        qos_image = getattr(_cfg, '_camera_qos_image', None)
        on_info = getattr(_cfg, '_camera_on_info', None)
        on_image = getattr(_cfg, '_camera_on_image', None)
        if topic_info and qos_info and on_info:
            _cfg._camera_sub_info = node.create_subscription(CameraInfo, topic_info, on_info, qos_info)
        if topic_image and qos_image and on_image:
            _cfg._camera_sub_image = node.create_subscription(RosImage, topic_image, on_image, qos_image)
        _camera_active = True
        log.info(f"Camera started (on-demand): {topic_image}")
        return True
    except Exception as e:
        log.warning(f"Camera start failed: {e}")
        return False


def stop_camera() -> None:
    """Stop camera subscriptions to save CPU."""
    global _camera_active
    if not _camera_active:
        return
    node = getattr(_cfg, '_camera_node', None)
    try:
        if _cfg._camera_sub_info and node:
            node.destroy_subscription(_cfg._camera_sub_info)
            _cfg._camera_sub_info = None
        if _cfg._camera_sub_image and node:
            node.destroy_subscription(_cfg._camera_sub_image)
            _cfg._camera_sub_image = None
    except Exception as e:
        log.debug(f"Camera stop cleanup: {e}")
    _camera_active = False
    with _ros2_lock:
        _ros2_cache.pop("camera_frame", None)
        _ros2_cache.pop("camera_frame_ts", None)
    log.info("Camera stopped (on-demand)")


def check_camera_idle():
    """Called periodically — auto-stop camera if idle beyond timeout."""
    global _camera_last_use_ts
    if _camera_active and time.time() - _camera_last_use_ts > _CAMERA_AUTO_STOP_SEC:
        log.info(f"Camera auto-stop: idle {int(time.time() - _camera_last_use_ts)}s")
        stop_camera()


def is_camera_active() -> bool:
    return _camera_active


def is_ros2_healthy() -> bool:
    """Check if rclpy context + node are alive.

    토픽 데이터 수신 여부는 건강 지표가 아님 — 센서 침묵(IMU 미장착 등)이 있어도
    노드는 정상. rclpy.ok() 와 노드 핸들 존재만으로 판단.
    """
    if _cfg._ros2_node is None or _cfg._cmd_vel_pub is None:
        return False
    try:
        import rclpy
        return rclpy.ok()
    except Exception:
        return False


_last_restart_ts: float = 0.0
_RESTART_COOLDOWN_SEC = 60.0  # 같은 기동 내 재시작 최소 간격


def restart_ros2_node():
    """Restart the ROS2 spin thread. Called when context becomes invalid."""
    global _ros2_thread, _last_restart_ts
    with _ros2_restart_lock:
        now = time.time()
        if now - _last_restart_ts < _RESTART_COOLDOWN_SEC:
            log.info(
                f"ROS2 restart skipped — cooldown ({int(now - _last_restart_ts)}s < {int(_RESTART_COOLDOWN_SEC)}s)"
            )
            return
        _last_restart_ts = now
        log.warning("Restarting ROS2 node...")

        # Shutdown existing
        if _cfg._ros2_node:
            try:
                _cfg._ros2_node.destroy_node()
            except Exception:
                pass
        try:
            import rclpy
            rclpy.shutdown()
        except Exception:
            pass

        _cfg._ros2_node = None
        _cfg._cmd_vel_pub = None
        with _ros2_lock:
            _ros2_cache.clear()

        # Start new thread
        _ros2_thread = threading.Thread(target=_ros2_spin_thread, daemon=True)
        _ros2_thread.start()
        log.info("ROS2 node restart initiated")
