#!/usr/bin/env python3
"""Expose visualization-compatible ROS topics for the web plugin."""

from collections import defaultdict

import rclpy
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
from sensor_msgs.msg import Image

from bcr_bot.srv import GetVisualTopics


IMAGE_TYPE = "sensor_msgs/msg/Image"
CAMERA_INFO_TYPE = "sensor_msgs/msg/CameraInfo"
POINT_CLOUD_TYPE = "sensor_msgs/msg/PointCloud2"

DEPTH_ENCODINGS = {"16uc1", "32fc1", "mono16"}
RGB_ENCODINGS = {
    "rgb8",
    "bgr8",
    "rgba8",
    "bgra8",
    "8uc3",
    "8uc4",
    "r8g8b8",
    "b8g8r8",
}


class VisualTopicClassifier(Node):
    def __init__(self):
        super().__init__("visual_topic_classifier")

        self._image_encodings = {}
        self._image_subscriptions = {}
        self._topics_by_type = defaultdict(set)

        self.create_service(
            GetVisualTopics, "get_visual_topics", self._get_visual_topics_callback
        )
        self.create_timer(1.0, self._scan_topics)
        self._scan_topics()

        self.get_logger().info(
            "Visual topic classifier is ready on service '/get_visual_topics'."
        )

    def _scan_topics(self):
        topics_by_type = defaultdict(set)

        for topic_name, topic_types in self.get_topic_names_and_types():
            for topic_type in topic_types:
                topics_by_type[topic_type].add(topic_name)

            if IMAGE_TYPE in topic_types:
                self._subscribe_to_image_encoding(topic_name)

        self._topics_by_type = topics_by_type

    def _subscribe_to_image_encoding(self, topic_name):
        if topic_name in self._image_subscriptions:
            return

        qos = QoSProfile(
            depth=1,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
        )
        self._image_subscriptions[topic_name] = self.create_subscription(
            Image,
            topic_name,
            lambda message, name=topic_name: self._cache_image_encoding(name, message),
            qos,
        )

    def _cache_image_encoding(self, topic_name, message):
        encoding = message.encoding.lower()
        if self._image_encodings.get(topic_name) != encoding:
            self._image_encodings[topic_name] = encoding
            self.get_logger().info(
                "Detected image encoding '%s' on '%s'." % (encoding, topic_name)
            )

    def _get_visual_topics_callback(self, request, response):
        if request.refresh:
            self._scan_topics()

        response.depth_image_topics = self._image_topics_for(DEPTH_ENCODINGS)
        response.rgb_image_topics = self._image_topics_for(RGB_ENCODINGS)
        response.camera_info_topics = sorted(self._topics_by_type[CAMERA_INFO_TYPE])
        response.point_cloud_topics = sorted(self._topics_by_type[POINT_CLOUD_TYPE])
        return response

    def _image_topics_for(self, expected_encodings):
        return sorted(
            topic_name
            for topic_name in self._topics_by_type[IMAGE_TYPE]
            if self._image_encodings.get(topic_name) in expected_encodings
        )


def main(args=None):
    rclpy.init(args=args)
    node = VisualTopicClassifier()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == "__main__":
    main()
