"""Unit tests for pure pursuit path follower node."""

import pytest
import math
from nav_msgs.msg import Path, Odometry
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, Twist
from std_msgs.msg import Header

import rclpy

from saltybot_pure_pursuit.pure_pursuit_node import PurePursuitNode


@pytest.fixture
def rclpy_fixture():
    """Initialize and cleanup rclpy."""
    rclpy.init()
    yield
    rclpy.shutdown()


@pytest.fixture
def node(rclpy_fixture):
    """Create a pure pursuit node instance."""
    node = PurePursuitNode()
    yield node
    node.destroy_node()


class TestPurePursuitGeometry:
    """Test suite for pure pursuit geometric calculations."""

    def test_node_initialization(self, node):
        """Test that node initializes with correct defaults."""
        assert node.lookahead_distance == 0.5
        assert node.goal_tolerance == 0.1
        assert node.max_linear_velocity == 1.0
        assert node.enable_path_following is True

    def test_distance_calculation(self, node):
        """Test Euclidean distance calculation."""
        p1 = Point(x=0.0, y=0.0, z=0.0)
        p2 = Point(x=3.0, y=4.0, z=0.0)
        dist = node._distance(p1, p2)
        assert abs(dist - 5.0) < 0.01

    def test_distance_same_point(self, node):
        """Test distance between same point is zero."""
        p = Point(x=1.0, y=2.0, z=0.0)
        dist = node._distance(p, p)
        assert abs(dist) < 0.001

    def test_quaternion_to_yaw_north(self, node):
        """Test quaternion to yaw conversion for north heading."""
        quat = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        yaw = node._quaternion_to_yaw(quat)
        assert abs(yaw) < 0.01

    def test_quaternion_to_yaw_east(self, node):
        """Test quaternion to yaw conversion for east heading."""
        # 90 degree rotation around z-axis
        quat = Quaternion(x=0.0, y=0.0, z=0.7071, w=0.7071)
        yaw = node._quaternion_to_yaw(quat)
        assert abs(yaw - math.pi / 2) < 0.01

    def test_yaw_to_quaternion_identity(self, node):
        """Test yaw to quaternion conversion."""
        quat = node._yaw_to_quaternion(0.0)
        assert abs(quat.w - 1.0) < 0.01
        assert abs(quat.z) < 0.01


class TestPathFollowing:
    """Test suite for path following logic."""

    def test_empty_path(self, node):
        """Test handling of empty path."""
        node.current_pose = Pose(position=Point(x=0.0, y=0.0, z=0.0))
        node.current_path = Path()

        lookahead = node._find_lookahead_point()
        assert lookahead is None

    def test_single_point_path(self, node):
        """Test handling of single-point path."""
        node.current_pose = Pose(position=Point(x=0.0, y=0.0, z=0.0))

        path = Path()
        path.poses = [PoseStamped(pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)))]
        node.current_path = path

        lookahead = node._find_lookahead_point()
        assert lookahead is None or lookahead.x == 1.0

    def test_straight_line_path(self, node):
        """Test path following on straight line."""
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        # Create straight path along x-axis
        path = Path()
        for i in range(5):
            pose = PoseStamped(pose=Pose(
                position=Point(x=float(i), y=0.0, z=0.0)
            ))
            path.poses.append(pose)
        node.current_path = path

        # Robot heading east towards path
        lin_vel, ang_vel = node._calculate_steering_command(path.poses[1].pose.position)
        assert lin_vel >= 0
        assert abs(ang_vel) < 0.5  # Should have small steering error

    def test_curved_path(self, node):
        """Test path following on curved path."""
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        # Create circular arc path
        path = Path()
        for i in range(9):
            angle = (i / 8.0) * (math.pi / 2)
            x = math.sin(angle)
            y = 1.0 - math.cos(angle)
            pose = PoseStamped(pose=Pose(position=Point(x=x, y=y, z=0.0)))
            path.poses.append(pose)
        node.current_path = path

        lookahead = node._find_lookahead_point()
        assert lookahead is not None

    def test_goal_reached(self, node):
        """Test goal reached detection."""
        goal_pos = Point(x=1.0, y=0.0, z=0.0)
        node.current_pose = Pose(
            position=Point(x=1.05, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        lin_vel, ang_vel = node._calculate_steering_command(goal_pos)
        # With goal_tolerance=0.1, we should be at goal
        assert abs(lin_vel) < 0.01


class TestSteeringCalculation:
    """Test suite for steering command calculations."""

    def test_zero_heading_error(self, node):
        """Test steering when robot already aligned with target."""
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)  # Facing east
        )

        # Target directly ahead
        target = Point(x=1.0, y=0.0, z=0.0)
        lin_vel, ang_vel = node._calculate_steering_command(target)

        assert lin_vel > 0
        assert abs(ang_vel) < 0.1  # Minimal steering

    def test_90_degree_heading_error(self, node):
        """Test steering with 90 degree heading error."""
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)  # Facing east
        )

        # Target north
        target = Point(x=0.0, y=1.0, z=0.0)
        lin_vel, ang_vel = node._calculate_steering_command(target)

        assert lin_vel >= 0
        assert ang_vel != 0  # Should have significant steering

    def test_velocity_limits(self, node):
        """Test that velocities are within limits."""
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        target = Point(x=100.0, y=100.0, z=0.0)
        lin_vel, ang_vel = node._calculate_steering_command(target)

        assert lin_vel <= node.max_linear_velocity
        assert abs(ang_vel) <= node.max_angular_velocity

    def test_heading_correction_enabled(self, node):
        """Test heading correction when enabled."""
        node.use_heading_correction = True
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        # Large heading error
        target = Point(x=0.0, y=10.0, z=0.0)  # Due north
        lin_vel, ang_vel = node._calculate_steering_command(target)

        # Linear velocity should be reduced due to heading error
        assert lin_vel < node.max_linear_velocity

    def test_heading_correction_disabled(self, node):
        """Test that heading correction can be disabled."""
        node.use_heading_correction = False
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        target = Point(x=0.0, y=10.0, z=0.0)
        lin_vel, ang_vel = node._calculate_steering_command(target)

        # Linear velocity should be full
        assert abs(lin_vel - node.max_linear_velocity) < 0.01

    def test_negative_lookahead_distance(self, node):
        """Test behavior with invalid lookahead distance."""
        node.lookahead_distance = 0.0
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        target = Point(x=1.0, y=1.0, z=0.0)
        lin_vel, ang_vel = node._calculate_steering_command(target)

        # Should not crash, handle gracefully
        assert isinstance(lin_vel, float)
        assert isinstance(ang_vel, float)


class TestPurePursuitScenarios:
    """Integration-style tests for realistic scenarios."""

    def test_scenario_follow_straight_path(self, node):
        """Scenario: Follow a straight horizontal path."""
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        # Horizontal path
        path = Path()
        for i in range(10):
            pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0)))
            path.poses.append(pose)
        node.current_path = path

        lin_vel, ang_vel = node._calculate_steering_command(path.poses[1].pose.position)
        assert lin_vel > 0
        assert abs(ang_vel) < 0.5

    def test_scenario_s_shaped_path(self, node):
        """Scenario: Follow an S-shaped path."""
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        # S-shaped path
        path = Path()
        for i in range(20):
            x = float(i) * 0.5
            y = 2.0 * math.sin(x)
            pose = PoseStamped(pose=Pose(position=Point(x=x, y=y, z=0.0)))
            path.poses.append(pose)
        node.current_path = path

        lookahead = node._find_lookahead_point()
        assert lookahead is not None

    def test_scenario_spiral_path(self, node):
        """Scenario: Follow a spiral path."""
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        # Spiral path
        path = Path()
        for i in range(30):
            angle = (i / 30.0) * (4 * math.pi)
            radius = 0.5 + (i / 30.0) * 2.0
            x = radius * math.cos(angle)
            y = radius * math.sin(angle)
            pose = PoseStamped(pose=Pose(position=Point(x=x, y=y, z=0.0)))
            path.poses.append(pose)
        node.current_path = path

        lin_vel, ang_vel = node._calculate_steering_command(path.poses[5].pose.position)
        assert isinstance(lin_vel, float)
        assert isinstance(ang_vel, float)

    def test_scenario_control_loop(self, node):
        """Scenario: Control loop with valid path and odometry."""
        node.enable_path_following = True
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        # Create a simple path
        path = Path()
        for i in range(5):
            pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0)))
            path.poses.append(pose)
        node.current_path = path

        # Run control loop
        node._control_loop()

        # Should complete without error

    def test_scenario_disabled_path_following(self, node):
        """Scenario: Path following disabled."""
        node.enable_path_following = False
        node.current_pose = Pose(position=Point(x=0.0, y=0.0, z=0.0))

        # Create path
        path = Path()
        path.poses = [PoseStamped(pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)))]
        node.current_path = path

        # Run control loop
        node._control_loop()

        # Should publish zero velocity

    def test_scenario_no_odometry(self, node):
        """Scenario: Control loop when odometry not received."""
        node.current_pose = None
        path = Path()
        path.poses = [PoseStamped(pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)))]
        node.current_path = path

        # Run control loop
        node._control_loop()

        # Should handle gracefully

    def test_scenario_velocity_scaling(self, node):
        """Scenario: Velocity scaling parameters."""
        node.linear_velocity_scale = 0.5
        node.angular_velocity_scale = 0.5

        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        target = Point(x=1.0, y=0.0, z=0.0)
        lin_vel, ang_vel = node._calculate_steering_command(target)

        # Scaled velocities should be smaller
        assert lin_vel <= node.max_linear_velocity * 0.5 + 0.01

    def test_scenario_large_lookahead_distance(self, node):
        """Scenario: Large lookahead distance."""
        node.lookahead_distance = 5.0
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        # Path
        path = Path()
        for i in range(10):
            pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0)))
            path.poses.append(pose)
        node.current_path = path

        lookahead = node._find_lookahead_point()
        assert lookahead is not None

    def test_scenario_small_lookahead_distance(self, node):
        """Scenario: Small lookahead distance."""
        node.lookahead_distance = 0.05
        node.current_pose = Pose(
            position=Point(x=0.0, y=0.0, z=0.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
        )

        # Path
        path = Path()
        for i in range(10):
            pose = PoseStamped(pose=Pose(position=Point(x=float(i), y=0.0, z=0.0)))
            path.poses.append(pose)
        node.current_path = path

        lookahead = node._find_lookahead_point()
        assert lookahead is not None
