#!/usr/bin/env python3
"""Cliff and drop-off detection node for SaltyBot.

Monitors downward-facing IR range sensors for drop-offs/stair edges.
Triggers emergency stop when any sensor reading exceeds threshold.

Subscribed topics:
  /saltybot/cliff_sensors (sensor_msgs/LaserScan) - IR range array

Published topics:
  /saltybot/cliff_detected (std_msgs/Bool) - Cliff/drop-off detected
"""

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Bool


class CliffDetectorNode(Node):
    """ROS2 node for cliff and drop-off detection."""

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

        # Parameters
        self.declare_parameter("cliff_threshold", 0.5)
        self.declare_parameter("debounce_count", 3)
        self.declare_parameter("min_sensors", 2)

        self.cliff_threshold = self.get_parameter("cliff_threshold").value
        self.debounce_count = self.get_parameter("debounce_count").value
        self.min_sensors = self.get_parameter("min_sensors").value

        # State tracking
        self.cliff_detected = False
        self.consecutive_detections = 0

        # Subscription to cliff sensors
        self.sub_cliff = self.create_subscription(
            LaserScan, "/saltybot/cliff_sensors", self._on_cliff_sensors, 10
        )

        # Publisher for cliff detection
        self.pub_cliff = self.create_publisher(Bool, "/saltybot/cliff_detected", 10)

        self.get_logger().info(
            f"Cliff detector initialized. "
            f"Threshold: {self.cliff_threshold}m, "
            f"Debounce: {self.debounce_count} frames, "
            f"Min sensors: {self.min_sensors}"
        )

    def _on_cliff_sensors(self, msg: LaserScan) -> None:
        """Process cliff sensor readings."""
        # Filter out NaN/inf values and check against threshold
        valid_ranges = [r for r in msg.ranges if 0 < r < float('inf')]

        if len(valid_ranges) == 0:
            # No valid readings - assume safe
            self._update_cliff_state(False)
            return

        # Count how many sensors exceed threshold
        exceeding_sensors = sum(1 for r in valid_ranges if r > self.cliff_threshold)

        # Cliff detected if min_sensors or more exceed threshold
        cliff_detected_now = exceeding_sensors >= self.min_sensors

        if cliff_detected_now:
            self.consecutive_detections += 1
            if self.consecutive_detections >= self.debounce_count:
                self._update_cliff_state(True)
        else:
            self.consecutive_detections = 0
            self._update_cliff_state(False)

    def _update_cliff_state(self, detected: bool) -> None:
        """Update cliff state and publish."""
        if detected != self.cliff_detected:
            self.cliff_detected = detected
            if detected:
                self.get_logger().warn("CLIFF DETECTED! Emergency stop triggered!")
            else:
                self.get_logger().info("Cliff cleared, continuing operation")

        # Always publish current state
        msg = Bool(data=self.cliff_detected)
        self.pub_cliff.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    node = CliffDetectorNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
