#!/usr/bin/env python3
"""
Multi-Sensor Fusion Node for Obstacle Avoidance
Issue #490: Fuse RPLIDAR A1M8 (360° 2D) + RealSense D435i (front 87° 3D)

Fuses:
- /scan: RPLIDAR A1M8 (360° 2D LaserScan, ~5.5Hz)
- /depth_scan: RealSense D435i depth_to_laserscan (front 87°, ~30Hz)

Outputs:
- /scan_fused: Unified LaserScan (360°, high-fidelity front + LIDAR sides/rear)
- /scan_fused_cloud: PointCloud2 for 3D costmap voxel layer

Strategy:
- Front sector (±45°): Prefer depth_scan (closer range, 3D data) if available
- Sides/rear: Use LIDAR only
"""

import numpy as np
import math
from typing import Optional
import logging

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

from sensor_msgs.msg import LaserScan, PointCloud2, PointField
from std_msgs.msg import Header

_LOGGER = logging.getLogger(__name__)

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


def _create_pointcloud2_from_laserscan(scan: LaserScan, frame_id: str) -> PointCloud2:
    """Convert LaserScan to PointCloud2."""
    points = []
    for i, range_val in enumerate(scan.ranges):
        if scan.range_min <= range_val <= scan.range_max:
            angle = scan.angle_min + i * scan.angle_increment
            x = range_val * math.cos(angle)
            y = range_val * math.sin(angle)
            z = 0.0
            intensity = scan.intensities[i] if i < len(scan.intensities) else 0.0
            points.append([x, y, z, intensity])

    if not points:
        points = [[0, 0, 0, 0]]

    points_array = np.array(points, dtype=np.float32)

    # Create PointCloud2 message
    cloud = PointCloud2()
    cloud.header = Header(frame_id=frame_id, stamp=scan.header.stamp)
    cloud.height = 1
    cloud.width = len(points)
    cloud.is_bigendian = False
    cloud.point_step = 16
    cloud.row_step = cloud.point_step * cloud.width

    cloud.fields = [
        PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
        PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
        PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
        PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
    ]

    cloud.data = points_array.astype(np.float32).tobytes()
    return cloud


class SensorFusionNode(Node):
    """Multi-sensor fusion for obstacle avoidance."""

    def __init__(self):
        super().__init__("sensor_fusion")

        self.declare_parameter("lidar_topic", "/scan")
        self.declare_parameter("depth_topic", "/depth_scan")
        self.declare_parameter("fused_topic", "/scan_fused")
        self.declare_parameter("cloud_topic", "/scan_fused_cloud")
        self.declare_parameter("frame_id", "base_link")
        self.declare_parameter("front_sector_angle", 45.0)
        self.declare_parameter("depth_range_multiplier", 0.9)
        self.declare_parameter("max_range_limit", 5.0)

        self.lidar_topic = self.get_parameter("lidar_topic").value
        self.depth_topic = self.get_parameter("depth_topic").value
        self.fused_topic = self.get_parameter("fused_topic").value
        self.cloud_topic = self.get_parameter("cloud_topic").value
        self.frame_id = self.get_parameter("frame_id").value
        self.front_sector_angle = self.get_parameter("front_sector_angle").value
        self.depth_range_multiplier = self.get_parameter("depth_range_multiplier").value
        self.max_range_limit = self.get_parameter("max_range_limit").value

        # Subscriptions with message_filters sync
        lidar_sub = message_filters.Subscriber(
            self, LaserScan, self.lidar_topic, qos_profile=_SENSOR_QOS
        )
        depth_sub = message_filters.Subscriber(
            self, LaserScan, self.depth_topic, qos_profile=_SENSOR_QOS
        )

        self.sync = message_filters.ApproximateTimeSynchronizer(
            [lidar_sub, depth_sub], queue_size=10, slop=0.2
        )
        self.sync.registerCallback(self._on_scans)

        # Publishers
        self.fused_scan_pub = self.create_publisher(LaserScan, self.fused_topic, _SENSOR_QOS)
        self.cloud_pub = self.create_publisher(PointCloud2, self.cloud_topic, _SENSOR_QOS)

        self.get_logger().info(
            f"SensorFusionNode initialized: lidar={self.lidar_topic}, depth={self.depth_topic}"
        )

    def _on_scans(self, lidar_scan: LaserScan, depth_scan: LaserScan):
        """Fuse synchronized scans."""
        try:
            fused = self._fuse_scans(lidar_scan, depth_scan)
            self.fused_scan_pub.publish(fused)
            cloud = _create_pointcloud2_from_laserscan(fused, self.frame_id)
            self.cloud_pub.publish(cloud)
        except Exception as e:
            self.get_logger().error(f"Fusion error: {e}")

    def _fuse_scans(self, lidar_scan: LaserScan, depth_scan: LaserScan) -> LaserScan:
        """Fuse LIDAR and depth scans."""
        fused = LaserScan()
        fused.header = lidar_scan.header
        fused.angle_min = lidar_scan.angle_min
        fused.angle_max = lidar_scan.angle_max
        fused.angle_increment = lidar_scan.angle_increment
        fused.time_increment = lidar_scan.time_increment
        fused.scan_time = lidar_scan.scan_time
        fused.range_min = min(lidar_scan.range_min, depth_scan.range_min)
        fused.range_max = min(lidar_scan.range_max, depth_scan.range_max)

        num_rays = len(lidar_scan.ranges)
        fused.ranges = list(lidar_scan.ranges)
        fused.intensities = list(lidar_scan.intensities) if lidar_scan.intensities else [0.0] * num_rays

        front_angle_rad = math.radians(self.front_sector_angle)

        # Fuse depth scan into front sector
        for i in range(len(depth_scan.ranges)):
            depth_range = depth_scan.ranges[i]

            if not (depth_scan.range_min <= depth_range <= depth_scan.range_max):
                continue

            depth_angle = depth_scan.angle_min + i * depth_scan.angle_increment

            # Check if in front sector
            if abs(depth_angle) > front_angle_rad:
                continue

            # Normalize angle
            if depth_angle < lidar_scan.angle_min:
                depth_angle += 2 * math.pi
            if depth_angle > lidar_scan.angle_max:
                depth_angle -= 2 * math.pi

            # Find closest LIDAR ray
            lidar_index = int(round((depth_angle - lidar_scan.angle_min) / lidar_scan.angle_increment))
            lidar_index = max(0, min(num_rays - 1, lidar_index))

            # Apply safety multiplier and take minimum
            adjusted_depth_range = depth_range * self.depth_range_multiplier
            adjusted_depth_range = min(adjusted_depth_range, self.max_range_limit)
            current_range = fused.ranges[lidar_index]

            if adjusted_depth_range < current_range or current_range < fused.range_min:
                fused.ranges[lidar_index] = adjusted_depth_range

                if i < len(depth_scan.intensities):
                    fused.intensities[lidar_index] = depth_scan.intensities[i]

        return fused


def main(args=None):
    rclpy.init(args=args)
    node = SensorFusionNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()
