# Sensor HW Abstraction Layer — sensors.yaml loader + QoS normalizer
# 목적: 다양한 HW/드라이버의 토픽 이름·QoS 차이를 한 설정 파일로 흡수.
# 우선순위: sensors.yaml > env vars > auto-detect > defaults.
import os
from pathlib import Path
from typing import Any

from .config import log


# ── Default QoS profiles (ROS2 표준) ──
# Best-effort + volatile = 센서 드라이버 대부분과 호환 (RealSense, Ouster, Velodyne 등)
DEFAULT_SENSOR_QOS = "sensor_data"   # best_effort, volatile, depth=10
DEFAULT_RELIABLE_QOS = "reliable"    # reliable, volatile, depth=10
DEFAULT_LATCHED_QOS = "latched"      # reliable, transient_local (map, static_tf)


# 드라이버별 표준 토픽 이름 (같은 센서 종류에서 변형들)
TOPIC_ALIASES = {
    "camera_image": [
        "/camera/color/image_raw",       # RealSense
        "/camera/camera/color/image_raw",
        "/zed/zed_node/rgb/image_rect_color",  # ZED
        "/rgb/image_raw",                # OAK-D
        "/image_raw",                    # v4l2/usb_cam
        "/csi_cam/image_raw",            # Jetson CSI
    ],
    "camera_info": [
        "/camera/color/camera_info",
        "/camera/camera/color/camera_info",
        "/zed/zed_node/rgb/camera_info",
        "/rgb/camera_info",
        "/camera_info",
    ],
    "scan": [
        "/scan",                         # 표준
        "/scan_filtered",
        "/front_lidar/scan",             # Hokuyo
    ],
    "point_cloud": [
        "/velodyne_points",              # Velodyne
        "/ouster/points",                # Ouster
        "/livox/lidar",                  # Livox
        "/points_raw",
    ],
    "imu": [
        "/imu/data",                     # 표준 (filtered)
        "/imu/data_raw",
        "/camera/imu",                   # RealSense IMU
        "/zed/zed_node/imu/data",
        "/imu",
    ],
    "odom": [
        "/odom",                         # 표준
        "/base_controller/odom",         # ros2_control base_controller
        "/odometry/filtered",            # robot_localization
        "/wheel/odometry",
    ],
    "cmd_vel": [
        "/cmd_vel",                      # 표준
        "/cmd_vel_mux/input/teleop",
    ],
    "map": ["/map"],
    "robot_feedback": ["/former_io_controller/robot_feedback"],
}


def _qos_profile(name: str):
    """Return rclpy QoSProfile for well-known profile name."""
    from rclpy.qos import (
        QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
    )

    name = (name or "").lower()
    if name in ("sensor_data", "best_effort"):
        return QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
        )
    if name in ("latched", "transient_local"):
        return QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )
    # default: reliable / volatile
    return QoSProfile(
        reliability=ReliabilityPolicy.RELIABLE,
        durability=DurabilityPolicy.VOLATILE,
        history=HistoryPolicy.KEEP_LAST,
        depth=10,
    )


class SensorConfig:
    """Sensor topic + QoS resolver.

    Lookup order:
      1. sensors.yaml (explicit user config)
      2. env var (e.g. IMU_TOPIC, CAMERA_IMAGE_TOPIC)
      3. auto-detect from `ros2 topic list` (first matching alias)
      4. alias[0] (safe default)
    """

    ENV_MAP = {
        "imu": "IMU_TOPIC",
        "odom": "ODOM_TOPIC",
        "scan": "SCAN_TOPIC",
        "camera_image": "CAMERA_IMAGE_TOPIC",
        "camera_info": "CAMERA_INFO_TOPIC",
        "cmd_vel": "CMD_VEL_TOPIC",
        "map": "MAP_TOPIC",
        "robot_feedback": "FEEDBACK_TOPIC",
        "point_cloud": "POINT_CLOUD_TOPIC",
    }

    DEFAULT_QOS = {
        "imu": "sensor_data",
        "odom": "reliable",
        "scan": "sensor_data",
        "point_cloud": "sensor_data",
        "camera_image": "sensor_data",
        "camera_info": "sensor_data",
        "cmd_vel": "reliable",
        "map": "latched",
        "robot_feedback": "reliable",
    }

    def __init__(self, config_path: str | None = None):
        self._cfg: dict[str, Any] = {}
        self._detected: dict[str, list[str]] = {}
        path = config_path or os.environ.get(
            "SENSORS_CONFIG",
            str(Path(__file__).resolve().parent.parent / "sensors.yaml"),
        )
        self._load_yaml(path)

    def _load_yaml(self, path: str):
        if not os.path.exists(path):
            log.info(f"sensors.yaml not found at {path} — using env/auto-detect")
            return
        try:
            import yaml  # type: ignore
        except ImportError:
            log.warning("PyYAML not installed — skipping sensors.yaml")
            return
        try:
            with open(path) as f:
                self._cfg = yaml.safe_load(f) or {}
            log.info(f"sensors.yaml loaded: {path} ({len(self._cfg)} sensors)")
        except Exception as e:
            log.warning(f"sensors.yaml parse error: {e}")

    def set_detected_topics(self, topics_by_type: dict[str, list[str]]):
        """Called after ros2 topic discovery to populate auto-detect pool."""
        self._detected = topics_by_type

    def topic(self, sensor: str) -> str:
        """Resolve topic name for a sensor key."""
        entry = self._cfg.get(sensor, {}) if isinstance(self._cfg.get(sensor), dict) else {}

        # 1. yaml
        if entry.get("topic"):
            return entry["topic"]

        # 2. env var
        env_name = self.ENV_MAP.get(sensor)
        if env_name:
            val = os.environ.get(env_name, "").strip()
            if val:
                return val

        # 3. auto-detect from discovered topics
        aliases = TOPIC_ALIASES.get(sensor, [])
        msg_type = self._msg_type_for(sensor)
        if msg_type and msg_type in self._detected:
            for candidate in aliases:
                if candidate in self._detected[msg_type]:
                    return candidate
            # Fallback: first alias pattern match by name substring
            for discovered in self._detected[msg_type]:
                for pat in aliases:
                    key = pat.rstrip("/").split("/")[-1]
                    if key and key in discovered:
                        return discovered

        # 4. first alias as safe default
        return aliases[0] if aliases else ""

    def qos(self, sensor: str):
        """Resolve QoS profile for a sensor key."""
        entry = self._cfg.get(sensor, {}) if isinstance(self._cfg.get(sensor), dict) else {}
        profile = entry.get("qos") or self.DEFAULT_QOS.get(sensor, "reliable")
        return _qos_profile(profile)

    def enabled(self, sensor: str) -> bool:
        """Check if sensor is enabled (default: true)."""
        entry = self._cfg.get(sensor, {}) if isinstance(self._cfg.get(sensor), dict) else {}
        return entry.get("enabled", True)

    def _msg_type_for(self, sensor: str) -> str | None:
        return {
            "imu": "sensor_msgs/msg/Imu",
            "odom": "nav_msgs/msg/Odometry",
            "scan": "sensor_msgs/msg/LaserScan",
            "point_cloud": "sensor_msgs/msg/PointCloud2",
            "camera_image": "sensor_msgs/msg/Image",
            "camera_info": "sensor_msgs/msg/CameraInfo",
            "cmd_vel": "geometry_msgs/msg/Twist",
            "map": "nav_msgs/msg/OccupancyGrid",
        }.get(sensor)


_SINGLETON: SensorConfig | None = None


def get_sensor_config() -> SensorConfig:
    global _SINGLETON
    if _SINGLETON is None:
        _SINGLETON = SensorConfig()
    return _SINGLETON


def probe_publisher_qos(node, topic: str, fallback: str = "sensor_data"):
    """Probe actual publisher QoS for a topic and return a matching QoSProfile.

    Uses node.get_publishers_info_by_topic() to read the first publisher's
    reliability and durability, then builds a compatible QoSProfile.
    Falls back to the named profile if no publisher is found.
    """
    try:
        pubs = node.get_publishers_info_by_topic(topic)
        if not pubs:
            log.info(f"QoS probe: no publishers on {topic} — using {fallback}")
            return _qos_profile(fallback)

        pub_qos = pubs[0].qos_profile
        from rclpy.qos import (
            QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
        )

        # Match subscriber QoS to publisher QoS for compatibility
        # Rule: subscriber must be ≤ publisher reliability
        #   RELIABLE pub → subscriber can be RELIABLE or BEST_EFFORT
        #   BEST_EFFORT pub → subscriber MUST be BEST_EFFORT
        reliability = pub_qos.reliability
        durability = pub_qos.durability

        profile = QoSProfile(
            reliability=reliability,
            durability=durability,
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
        )

        rel_str = "RELIABLE" if reliability == ReliabilityPolicy.RELIABLE else "BEST_EFFORT"
        dur_str = "TRANSIENT_LOCAL" if durability == DurabilityPolicy.TRANSIENT_LOCAL else "VOLATILE"
        log.info(f"QoS probe: {topic} → {rel_str}/{dur_str} (from publisher)")
        return profile

    except Exception as e:
        log.warning(f"QoS probe failed for {topic}: {e} — using {fallback}")
        return _qos_profile(fallback)
