# 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,
    CMD_VEL_TOPIC, IMU_TOPIC, ODOM_TOPIC, SCAN_TOPIC,
)
import ai_teammate_ros2_bridge.config as _cfg


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

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

    # Publisher
    _cfg._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]
            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
            _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:
            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"] = 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:
        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}")

    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)
    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()
