"""
scan_height_filter_node.py — Ground-plane LIDAR filter with IMU pitch compensation.

Subscribes:
  /scan          sensor_msgs/LaserScan  raw RPLIDAR A1M8 scan (~5.5 Hz)
  /camera/imu    sensor_msgs/Imu        raw D435i IMU (~200 Hz)

Publishes:
  /scan_filtered sensor_msgs/LaserScan  ground-filtered scan for Nav2

Algorithm
---------
For each incoming LaserScan, the current robot pitch and roll are estimated
from the D435i accelerometer (low-pass filtered to suppress vibration).

Each ray at bearing θ and range r is assigned an estimated world-frame height:

    z = lidar_height + r·cos(θ)·sin(pitch) − r·sin(θ)·sin(roll)

Rays with z < z_min (ground plane) or z > z_max (ceiling/spurious) are set to
inf and are therefore treated by Nav2 as free space.

Parameters
----------
lidar_height_m      float  0.10   LIDAR mount height above floor (m)
z_min_m             float  0.05   Minimum valid obstacle height (m)
z_max_m             float  2.00   Maximum valid obstacle height (m)
imu_alpha           float  0.10   Accelerometer low-pass factor (0–1)
imu_topic           str    /camera/imu
scan_topic          str    /scan
filtered_topic      str    /scan_filtered
"""

from __future__ import annotations

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

from sensor_msgs.msg import Imu, LaserScan

from ._scan_height_filter import filter_scan_by_height, AttitudeEstimator


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


class ScanHeightFilterNode(Node):

    def __init__(self):
        super().__init__('scan_height_filter')

        self.declare_parameter('lidar_height_m',  0.10)
        self.declare_parameter('z_min_m',         0.05)
        self.declare_parameter('z_max_m',         2.00)
        self.declare_parameter('imu_alpha',        0.10)
        self.declare_parameter('imu_topic',        '/camera/imu')
        self.declare_parameter('scan_topic',       '/scan')
        self.declare_parameter('filtered_topic',   '/scan_filtered')

        self._lidar_h = self.get_parameter('lidar_height_m').value
        self._z_min   = self.get_parameter('z_min_m').value
        self._z_max   = self.get_parameter('z_max_m').value
        alpha         = self.get_parameter('imu_alpha').value
        imu_topic     = self.get_parameter('imu_topic').value
        scan_topic    = self.get_parameter('scan_topic').value
        filtered_topic = self.get_parameter('filtered_topic').value

        self._attitude = AttitudeEstimator(alpha=alpha)

        self.create_subscription(Imu,       imu_topic,  self._on_imu,  _SENSOR_QOS)
        self.create_subscription(LaserScan, scan_topic, self._on_scan, _SENSOR_QOS)

        self._pub = self.create_publisher(LaserScan, filtered_topic, 10)

        self.get_logger().info(
            f'scan_height_filter ready — '
            f'lidar_h={self._lidar_h}m z=[{self._z_min},{self._z_max}]m '
            f'imu_alpha={alpha}'
        )

    # ── Callbacks ─────────────────────────────────────────────────────────────

    def _on_imu(self, msg: Imu) -> None:
        a = msg.linear_acceleration
        self._attitude.update(a.x, a.y, a.z)

    def _on_scan(self, msg: LaserScan) -> None:
        filtered_ranges = filter_scan_by_height(
            ranges          = list(msg.ranges),
            angle_min       = msg.angle_min,
            angle_increment = msg.angle_increment,
            lidar_height_m  = self._lidar_h,
            pitch_rad       = self._attitude.pitch,
            roll_rad        = self._attitude.roll,
            z_min_m         = self._z_min,
            z_max_m         = self._z_max,
            range_min       = msg.range_min,
            range_max       = msg.range_max,
        )

        out = LaserScan()
        out.header          = msg.header
        out.angle_min       = msg.angle_min
        out.angle_max       = msg.angle_max
        out.angle_increment = msg.angle_increment
        out.time_increment  = msg.time_increment
        out.scan_time       = msg.scan_time
        out.range_min       = msg.range_min
        out.range_max       = msg.range_max
        out.ranges          = filtered_ranges
        out.intensities     = list(msg.intensities) if msg.intensities else []
        self._pub.publish(out)


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


if __name__ == '__main__':
    main()
