"""Unit tests for LIDAR avoidance node."""

import math
import pytest
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from std_msgs.msg import Bool


class MockNode:
    """Mock ROS2 node for testing."""

    def __init__(self):
        from saltybot_lidar_avoidance.lidar_avoidance_node import LidarAvoidanceNode
        import rclpy

        rclpy.init(allow_reuse=True)
        self.node = LidarAvoidanceNode()

    def cleanup(self):
        self.node.destroy_node()


def create_scan(
    ranges, angle_min=-math.pi, angle_increment=2 * math.pi / 360
):
    """Create a LaserScan message."""
    scan = LaserScan()
    scan.ranges = ranges
    scan.angle_min = angle_min
    scan.angle_increment = angle_increment
    scan.angle_max = angle_min + angle_increment * (len(ranges) - 1)
    return scan


def test_safety_distance_zero_speed():
    """Test safety distance at zero speed."""
    import rclpy

    rclpy.init(allow_reuse=True)
    from saltybot_lidar_avoidance.lidar_avoidance_node import LidarAvoidanceNode

    node = LidarAvoidanceNode()
    safety = node._get_safety_distance(0.0)
    assert safety == node.min_safety_zone
    node.destroy_node()


def test_safety_distance_max_speed():
    """Test safety distance at max speed."""
    import rclpy

    rclpy.init(allow_reuse=True)
    from saltybot_lidar_avoidance.lidar_avoidance_node import LidarAvoidanceNode

    node = LidarAvoidanceNode()
    safety = node._get_safety_distance(node.max_speed_reference)
    assert abs(safety - node.safety_zone_at_max_speed) < 0.01
    node.destroy_node()


def test_safety_distance_interpolation():
    """Test safety distance linear interpolation."""
    import rclpy

    rclpy.init(allow_reuse=True)
    from saltybot_lidar_avoidance.lidar_avoidance_node import LidarAvoidanceNode

    node = LidarAvoidanceNode()
    half_speed = node.max_speed_reference / 2.0
    safety = node._get_safety_distance(half_speed)

    expected = node.min_safety_zone + 0.5 * (
        node.safety_zone_at_max_speed - node.min_safety_zone
    )
    assert abs(safety - expected) < 0.01
    node.destroy_node()


def test_min_distance_forward():
    """Test forward distance detection."""
    import rclpy

    rclpy.init(allow_reuse=True)
    from saltybot_lidar_avoidance.lidar_avoidance_node import LidarAvoidanceNode

    node = LidarAvoidanceNode()

    # Create scan with obstacle at 0° (forward)
    ranges = [float('inf')] * 360
    ranges[180] = 1.5  # Obstacle at index 180 (0° in normalized coordinates)

    scan = create_scan(ranges)
    min_dist, angle = node._get_min_distance_forward(scan)

    assert min_dist == 1.5
    assert abs(angle) < 10  # Close to forward direction
    node.destroy_node()


if __name__ == "__main__":
    pytest.main([__file__, "-v"])
