"""Tests for cliff detector node."""

import pytest
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Bool
import rclpy

from saltybot_cliff_detector.cliff_detector_node import CliffDetectorNode


@pytest.fixture
def rclpy_fixture():
    rclpy.init()
    yield
    rclpy.shutdown()


@pytest.fixture
def node(rclpy_fixture):
    node = CliffDetectorNode()
    yield node
    node.destroy_node()


class TestInit:
    def test_node_initialization(self, node):
        assert node.cliff_threshold == 0.5
        assert node.debounce_count == 3
        assert node.min_sensors == 2
        assert node.cliff_detected is False
        assert node.consecutive_detections == 0


class TestCliffDetection:
    def test_cliff_threshold_exceeded(self, node):
        """Test detection when sensor exceeds threshold."""
        msg = LaserScan()
        msg.ranges = [0.3, 0.6, 0.4]  # 1 sensor exceeds threshold

        node._on_cliff_sensors(msg)
        assert node.consecutive_detections == 1

    def test_multiple_sensors_exceed(self, node):
        """Test detection with multiple sensors exceeding threshold."""
        msg = LaserScan()
        msg.ranges = [0.6, 0.7, 0.4]  # 2 sensors exceed threshold

        node._on_cliff_sensors(msg)
        assert node.consecutive_detections == 1

    def test_debounce_threshold(self, node):
        """Test debouncing requires multiple consecutive detections."""
        node.cliff_threshold = 0.5
        node.debounce_count = 3
        node.min_sensors = 1

        msg = LaserScan()
        msg.ranges = [0.6, 0.3, 0.4]  # 1 exceeds

        # First detection
        node._on_cliff_sensors(msg)
        assert node.cliff_detected is False
        assert node.consecutive_detections == 1

        # Second detection
        node._on_cliff_sensors(msg)
        assert node.cliff_detected is False
        assert node.consecutive_detections == 2

        # Third detection - should trigger
        node._on_cliff_sensors(msg)
        assert node.cliff_detected is True
        assert node.consecutive_detections == 3

    def test_debounce_reset_on_clear(self, node):
        """Test debounce counter resets when cliff clears."""
        node.debounce_count = 2
        node.min_sensors = 1

        msg_cliff = LaserScan()
        msg_cliff.ranges = [0.6, 0.3]

        msg_clear = LaserScan()
        msg_clear.ranges = [0.3, 0.2]

        # Detection
        node._on_cliff_sensors(msg_cliff)
        assert node.consecutive_detections == 1

        # Clear
        node._on_cliff_sensors(msg_clear)
        assert node.consecutive_detections == 0
        assert node.cliff_detected is False

    def test_min_sensors_voting(self, node):
        """Test minimum sensor threshold (majority voting)."""
        node.min_sensors = 2
        node.debounce_count = 1

        msg = LaserScan()
        msg.ranges = [0.6, 0.3, 0.4]  # Only 1 exceeds

        node._on_cliff_sensors(msg)
        assert node.cliff_detected is False

        msg.ranges = [0.6, 0.7, 0.4]  # 2 exceed
        node._on_cliff_sensors(msg)
        assert node.cliff_detected is True

    def test_nan_values_filtered(self, node):
        """Test that NaN/inf values are filtered out."""
        node.debounce_count = 1
        node.min_sensors = 1

        msg = LaserScan()
        msg.ranges = [float('nan'), 0.6, float('inf')]

        node._on_cliff_sensors(msg)
        # Only 0.6 is valid and exceeds threshold
        assert node.cliff_detected is True

    def test_empty_ranges(self, node):
        """Test behavior with empty or all-invalid ranges."""
        msg = LaserScan()
        msg.ranges = []

        node._on_cliff_sensors(msg)
        assert node.cliff_detected is False

        msg.ranges = [float('nan'), float('inf')]
        node._on_cliff_sensors(msg)
        assert node.cliff_detected is False


class TestStateTransitions:
    def test_cliff_to_clear_transition(self, node):
        """Test state transition from cliff to clear."""
        node.debounce_count = 1
        node.min_sensors = 1

        # Trigger cliff
        msg_cliff = LaserScan()
        msg_cliff.ranges = [0.6, 0.3]
        node._on_cliff_sensors(msg_cliff)

        assert node.cliff_detected is True

        # Clear cliff
        msg_clear = LaserScan()
        msg_clear.ranges = [0.3, 0.2]
        node._on_cliff_sensors(msg_clear)

        assert node.cliff_detected is False

    def test_clear_to_cliff_transition(self, node):
        """Test state transition from clear to cliff."""
        node.debounce_count = 1
        node.min_sensors = 1

        # Start clear
        assert node.cliff_detected is False

        # Trigger cliff
        msg_cliff = LaserScan()
        msg_cliff.ranges = [0.6, 0.3]
        node._on_cliff_sensors(msg_cliff)

        assert node.cliff_detected is True


class TestEdgeCases:
    def test_single_sensor_exceeds(self, node):
        """Test with single sensor exceeding threshold."""
        node.min_sensors = 1
        node.debounce_count = 1

        msg = LaserScan()
        msg.ranges = [0.6]

        node._on_cliff_sensors(msg)
        assert node.cliff_detected is True

    def test_zero_readings(self, node):
        """Test that zero readings are filtered (invalid)."""
        msg = LaserScan()
        msg.ranges = [0.0, 0.6, 0.0]  # Only 0.6 is valid

        node._on_cliff_sensors(msg)
        # 0.6 exceeds 0.5, but need 2 sensors with min_sensors=2
        assert node.cliff_detected is False

    def test_threshold_boundary(self, node):
        """Test boundary condition at exact threshold."""
        node.debounce_count = 1
        node.min_sensors = 1

        msg = LaserScan()
        msg.ranges = [0.5, 0.3]  # Exactly at threshold (not exceeding)

        node._on_cliff_sensors(msg)
        assert node.cliff_detected is False

        msg.ranges = [0.50001, 0.3]  # Just above threshold
        node._on_cliff_sensors(msg)
        assert node.cliff_detected is True
