"""
lidar_clustering_node.py — LIDAR object clustering and RViz visualisation (Issue #239).

Subscribes to the 2-D LIDAR scan, segments nearby points into objects using
Euclidean distance threshold clustering, and publishes a MarkerArray so each
cluster is visible as a labelled bounding-box cube in RViz.

Subscribes:
  /scan                    sensor_msgs/LaserScan   (RPLIDAR A1M8)

Publishes:
  /saltybot/lidar_clusters visualization_msgs/MarkerArray

Algorithm
---------
1. Convert LaserScan ranges to Cartesian (x, y) points.
2. Walk scan-order points — start a new cluster when the gap between
   consecutive points exceeds `distance_threshold_m`.
3. Discard clusters below `min_points`.
4. Publish one CUBE marker per cluster (centroid + bbox dims) plus one TEXT
   marker showing cluster index and point count.

Parameters
----------
distance_threshold_m  float  0.20   Max gap between consecutive scan points (m)
min_points            int    3      Min points for a valid cluster
range_min_m           float  0.05   Discard ranges below this (m)
range_max_m           float  12.0   Discard ranges above this (m)
marker_lifetime_s     float  0.15   Marker lifetime — auto-expire if scan stalls
marker_z              float  0.0    Z position of cube markers in the LIDAR frame
marker_height         float  0.3    Fixed height of cube markers (m)
"""

from __future__ import annotations

from typing import List

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from rclpy.duration import Duration

from sensor_msgs.msg import LaserScan
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Point
from std_msgs.msg import ColorRGBA
from builtin_interfaces.msg import Duration as DurationMsg

from ._lidar_clustering import scan_to_cartesian, cluster_points, Cluster


_SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=4,
)

# Distinct hue palette — up to 12 clusters get unique colours; wraps for more
_PALETTE: List[tuple] = [
    (0.90, 0.20, 0.20),  # red
    (0.20, 0.70, 0.20),  # green
    (0.20, 0.50, 0.90),  # blue
    (0.90, 0.70, 0.10),  # yellow
    (0.80, 0.30, 0.90),  # purple
    (0.20, 0.85, 0.85),  # cyan
    (0.95, 0.50, 0.10),  # orange
    (0.50, 0.90, 0.30),  # lime
    (0.90, 0.20, 0.70),  # pink
    (0.40, 0.20, 0.90),  # indigo
    (0.10, 0.70, 0.50),  # teal
    (0.70, 0.50, 0.20),  # brown
]


class LidarClusteringNode(Node):

    def __init__(self) -> None:
        super().__init__('lidar_clustering_node')

        self.declare_parameter('distance_threshold_m', 0.20)
        self.declare_parameter('min_points',           3)
        self.declare_parameter('range_min_m',          0.05)
        self.declare_parameter('range_max_m',          12.0)
        self.declare_parameter('marker_lifetime_s',    0.15)
        self.declare_parameter('marker_z',             0.0)
        self.declare_parameter('marker_height',        0.3)

        self._dist_thresh  = self.get_parameter('distance_threshold_m').value
        self._min_pts      = self.get_parameter('min_points').value
        self._range_min    = self.get_parameter('range_min_m').value
        self._range_max    = self.get_parameter('range_max_m').value
        self._lifetime_s   = self.get_parameter('marker_lifetime_s').value
        self._marker_z     = self.get_parameter('marker_z').value
        self._marker_h     = self.get_parameter('marker_height').value

        self._sub = self.create_subscription(
            LaserScan, '/scan', self._on_scan, _SENSOR_QOS)
        self._pub = self.create_publisher(
            MarkerArray, '/saltybot/lidar_clusters', 10)

        # Track how many markers were published last cycle so we can DELETE extras
        self._prev_count = 0

        self.get_logger().info(
            f'lidar_clustering_node ready — '
            f'dist={self._dist_thresh}m min_pts={self._min_pts}'
        )

    # ── Callback ──────────────────────────────────────────────────────────────

    def _on_scan(self, msg: LaserScan) -> None:
        points = scan_to_cartesian(
            list(msg.ranges),
            msg.angle_min,
            msg.angle_increment,
            range_min=self._range_min,
            range_max=self._range_max,
        )
        clusters = cluster_points(
            points,
            distance_threshold_m=self._dist_thresh,
            min_points=self._min_pts,
        )

        markers: List[Marker] = []
        lifetime = _duration_msg(self._lifetime_s)
        frame_id = msg.header.frame_id or 'laser'
        stamp    = msg.header.stamp

        for idx, cluster in enumerate(clusters):
            color = _PALETTE[idx % len(_PALETTE)]

            # ── CUBE marker (bounding box) ────────────────────────────────────
            cube = Marker()
            cube.header.frame_id = frame_id
            cube.header.stamp    = stamp
            cube.ns              = 'lidar_clusters'
            cube.id              = idx * 2
            cube.type            = Marker.CUBE
            cube.action          = Marker.ADD
            cube.pose.position.x = float(cluster.centroid[0])
            cube.pose.position.y = float(cluster.centroid[1])
            cube.pose.position.z = self._marker_z
            cube.pose.orientation.w = 1.0
            # Minimum visible size of 0.1 m per axis
            cube.scale.x = max(cluster.width_m, 0.10)
            cube.scale.y = max(cluster.depth_m, 0.10)
            cube.scale.z = self._marker_h
            cube.color   = _rgba(*color, a=0.45)
            cube.lifetime = lifetime
            markers.append(cube)

            # ── TEXT marker (id + count) ──────────────────────────────────────
            label = Marker()
            label.header.frame_id = frame_id
            label.header.stamp    = stamp
            label.ns              = 'lidar_clusters'
            label.id              = idx * 2 + 1
            label.type            = Marker.TEXT_VIEW_FACING
            label.action          = Marker.ADD
            label.pose.position.x = float(cluster.centroid[0])
            label.pose.position.y = float(cluster.centroid[1])
            label.pose.position.z = self._marker_z + self._marker_h * 0.5 + 0.05
            label.pose.orientation.w = 1.0
            label.scale.z         = 0.12   # text height (m)
            label.color           = _rgba(1.0, 1.0, 1.0, a=0.9)
            label.text            = f'#{idx}  n={len(cluster.points)}'
            label.lifetime        = lifetime
            markers.append(label)

        # DELETE markers from the previous cycle that no longer have a cluster
        new_count = len(clusters)
        for idx in range(new_count, self._prev_count):
            for offset in (0, 1):
                del_m = Marker()
                del_m.ns     = 'lidar_clusters'
                del_m.id     = idx * 2 + offset
                del_m.action = Marker.DELETE
                markers.append(del_m)

        self._prev_count = new_count

        ma = MarkerArray()
        ma.markers = markers
        self._pub.publish(ma)


# ── Helpers ───────────────────────────────────────────────────────────────────

def _rgba(r: float, g: float, b: float, a: float) -> ColorRGBA:
    c = ColorRGBA()
    c.r = r; c.g = g; c.b = b; c.a = a
    return c


def _duration_msg(seconds: float) -> DurationMsg:
    d = DurationMsg()
    d.sec     = int(seconds)
    d.nanosec = int((seconds - int(seconds)) * 1e9)
    return d


def main(args=None) -> None:
    rclpy.init(args=args)
    node = LidarClusteringNode()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
