"""
obstacle_velocity_node.py — Dynamic obstacle velocity estimator (Issue #326).

Subscribes to the 2-D LIDAR scan, clusters points using the existing
gap-segmentation helper, tracks cluster centroids with per-obstacle
constant-velocity Kalman filters, and publishes estimated velocity
vectors on /saltybot/obstacle_velocities.

Subscribes:
  /scan                       sensor_msgs/LaserScan   (BEST_EFFORT)

Publishes:
  /saltybot/obstacle_velocities  saltybot_scene_msgs/ObstacleVelocityArray

Parameters
----------
distance_threshold_m    float  0.20   Max gap between consecutive scan points (m)
min_points              int    3      Minimum LIDAR points per valid cluster
range_min_m             float  0.05   Discard ranges below this (m)
range_max_m             float  12.0   Discard ranges above this (m)
max_association_dist_m  float  0.50   Max centroid distance for track-cluster match
max_coasting_frames     int    5      Missed frames before a track is deleted
n_init_frames           int    3      Updates required to reach confidence=1.0
q_pos                   float  0.05   Process noise — position (m²/s²)
q_vel                   float  0.50   Process noise — velocity (m²/s³)
r_pos                   float  0.10   Measurement noise std-dev (m)
static_speed_threshold  float  0.10   Speed (m/s) below which obstacle is static
"""

from __future__ import annotations

import time
from typing import List

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

from sensor_msgs.msg import LaserScan
from saltybot_scene_msgs.msg import ObstacleVelocity, ObstacleVelocityArray

from ._lidar_clustering import scan_to_cartesian, cluster_points
from ._obstacle_velocity import ObstacleTracker


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


class ObstacleVelocityNode(Node):

    def __init__(self) -> None:
        super().__init__('obstacle_velocity_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('max_association_dist_m', 0.50)
        self.declare_parameter('max_coasting_frames',    5)
        self.declare_parameter('n_init_frames',          3)
        self.declare_parameter('q_pos',                  0.05)
        self.declare_parameter('q_vel',                  0.50)
        self.declare_parameter('r_pos',                  0.10)
        self.declare_parameter('static_speed_threshold', 0.10)

        self._dist_thresh   = float(self.get_parameter('distance_threshold_m').value)
        self._min_pts       = int(self.get_parameter('min_points').value)
        self._range_min     = float(self.get_parameter('range_min_m').value)
        self._range_max     = float(self.get_parameter('range_max_m').value)
        self._static_thresh = float(self.get_parameter('static_speed_threshold').value)

        self._tracker = ObstacleTracker(
            max_association_dist_m = float(self.get_parameter('max_association_dist_m').value),
            max_coasting_frames    = int(self.get_parameter('max_coasting_frames').value),
            n_init_frames          = int(self.get_parameter('n_init_frames').value),
            q_pos                  = float(self.get_parameter('q_pos').value),
            q_vel                  = float(self.get_parameter('q_vel').value),
            r_pos                  = float(self.get_parameter('r_pos').value),
        )

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

        self.get_logger().info(
            f'obstacle_velocity_node ready — '
            f'dist_thresh={self._dist_thresh}m '
            f'static_thresh={self._static_thresh}m/s'
        )

    # ── Scan 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,
        )

        centroids    = [c.centroid for c in clusters]
        widths       = [c.width_m  for c in clusters]
        depths       = [c.depth_m  for c in clusters]
        point_counts = [len(c.points) for c in clusters]

        # Use scan timestamp if available, fall back to wall clock
        stamp_secs = (
            msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            if msg.header.stamp.sec > 0
            else time.time()
        )

        alive_tracks = self._tracker.update(
            centroids,
            timestamp    = stamp_secs,
            widths       = widths,
            depths       = depths,
            point_counts = point_counts,
        )

        # ── Build and publish message ─────────────────────────────────────────
        out              = ObstacleVelocityArray()
        out.header.stamp = msg.header.stamp
        out.header.frame_id = msg.header.frame_id or 'laser'

        for track in alive_tracks:
            pos = track.position
            vel = track.velocity

            obs = ObstacleVelocity()
            obs.header        = out.header
            obs.obstacle_id   = track.track_id
            obs.centroid.x    = float(pos[0])
            obs.centroid.y    = float(pos[1])
            obs.centroid.z    = 0.0
            obs.velocity.x    = float(vel[0])
            obs.velocity.y    = float(vel[1])
            obs.velocity.z    = 0.0
            obs.speed_mps     = float(track.speed)
            obs.width_m       = float(track.last_width)
            obs.depth_m       = float(track.last_depth)
            obs.point_count   = int(track.last_point_count)
            obs.confidence    = float(track.confidence)
            obs.is_static     = track.speed < self._static_thresh
            out.obstacles.append(obs)

        self._pub.publish(out)


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


if __name__ == '__main__':
    main()
