"""Unit tests for accel_limiter_node."""

import pytest
import math
from geometry_msgs.msg import Twist, Vector3

import rclpy

# Import the node under test
from saltybot_accel_limiter.accel_limiter_node import AccelLimiterNode


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


@pytest.fixture
def node(rclpy_fixture):
    """Create an acceleration limiter node instance."""
    node = AccelLimiterNode()
    yield node
    node.destroy_node()


class TestNodeInitialization:
    """Test suite for node initialization."""

    def test_node_initialization(self, node):
        """Test that node initializes with correct defaults."""
        assert node.linear_accel_limit == 0.5
        assert node.angular_accel_limit == 1.0
        assert node.target_vel is None
        assert node.current_vel.linear.x == 0.0
        assert node.current_vel.linear.y == 0.0
        assert node.current_vel.linear.z == 0.0
        assert node.current_vel.angular.x == 0.0
        assert node.current_vel.angular.y == 0.0
        assert node.current_vel.angular.z == 0.0

    def test_frequency_parameter(self, node):
        """Test frequency parameter is set correctly."""
        frequency = node.get_parameter("frequency").value
        assert frequency == 50

    def test_linear_accel_parameter(self, node):
        """Test linear acceleration limit parameter."""
        limit = node.get_parameter("linear_accel_limit").value
        assert limit == 0.5

    def test_angular_accel_parameter(self, node):
        """Test angular acceleration limit parameter."""
        limit = node.get_parameter("angular_accel_limit").value
        assert limit == 1.0

    def test_period_calculation(self, node):
        """Test that time period is correctly calculated from frequency."""
        # 50 Hz = 20ms = 0.02s
        assert node.period == pytest.approx(0.02)


class TestLimitComponent:
    """Test suite for _limit_component method."""

    def test_no_change_needed(self, node):
        """Test when target equals current (no acceleration needed)."""
        result = node._limit_component(1.0, 1.0, 0.5)
        assert result == pytest.approx(1.0)

    def test_zero_acceleration(self, node):
        """Test when current is zero and target is zero."""
        result = node._limit_component(0.0, 0.0, 0.5)
        assert result == pytest.approx(0.0)

    def test_small_change_within_limit(self, node):
        """Test small change that is within acceleration limit."""
        # Max change in 20ms at 0.5 m/s²: 0.5 * 0.02 = 0.01 m/s
        # Desired change: 0.005 m/s (less than limit)
        result = node._limit_component(0.0, 0.005, 0.5)
        assert result == pytest.approx(0.005)

    def test_acceleration_exceeds_limit(self, node):
        """Test acceleration that exceeds the limit."""
        # Max change in 20ms at 0.5 m/s²: 0.5 * 0.02 = 0.01 m/s
        # Desired change: 0.1 m/s (much greater than limit)
        result = node._limit_component(0.0, 0.1, 0.5)
        assert result == pytest.approx(0.01)

    def test_deceleration_exceeds_limit(self, node):
        """Test deceleration that exceeds the limit."""
        # Max change in 20ms at 0.5 m/s²: -0.5 * 0.02 = -0.01 m/s
        # Desired change: -0.1 m/s (much more negative than limit)
        result = node._limit_component(0.1, 0.0, 0.5)
        assert result == pytest.approx(0.09)

    def test_negative_to_positive_acceleration(self, node):
        """Test acceleration from negative to positive velocity."""
        result = node._limit_component(-0.1, 0.1, 0.5)
        # Max change: 0.01, so -0.1 + 0.01 = -0.09
        assert result == pytest.approx(-0.09)

    def test_positive_to_negative_deceleration(self, node):
        """Test deceleration from positive to negative velocity."""
        result = node._limit_component(0.1, -0.1, 0.5)
        # Max change: -0.01, so 0.1 - 0.01 = 0.09
        assert result == pytest.approx(0.09)

    def test_angular_acceleration_limit(self, node):
        """Test with angular acceleration limit."""
        # Max change in 20ms at 1.0 rad/s²: 1.0 * 0.02 = 0.02 rad/s
        # Desired change: 0.1 rad/s (exceeds limit)
        result = node._limit_component(0.0, 0.1, 1.0)
        assert result == pytest.approx(0.02)

    def test_large_acceleration_value(self, node):
        """Test with large acceleration limit."""
        # Max change in 20ms at 10.0 m/s²: 10.0 * 0.02 = 0.2 m/s
        # Desired change: 0.1 m/s (within limit)
        result = node._limit_component(0.0, 0.1, 10.0)
        assert result == pytest.approx(0.1)


class TestSubscriptionHandling:
    """Test suite for cmd_vel subscription."""

    def test_cmd_vel_subscription(self, node):
        """Test that cmd_vel subscription updates target velocity."""
        cmd = Twist()
        cmd.linear.x = 1.0
        cmd.linear.y = 0.5
        cmd.angular.z = 0.2

        node._on_cmd_vel(cmd)

        assert node.target_vel is cmd
        assert node.target_vel.linear.x == 1.0
        assert node.target_vel.linear.y == 0.5
        assert node.target_vel.angular.z == 0.2

    def test_multiple_cmd_vel_updates(self, node):
        """Test that subscription updates replace previous command."""
        cmd1 = Twist()
        cmd1.linear.x = 1.0

        cmd2 = Twist()
        cmd2.linear.x = 2.0

        node._on_cmd_vel(cmd1)
        assert node.target_vel.linear.x == 1.0

        node._on_cmd_vel(cmd2)
        assert node.target_vel.linear.x == 2.0


class TestTimerCallback:
    """Test suite for timer callback and rate limiting."""

    def test_timer_callback_no_target(self, node):
        """Test timer callback when no target velocity received yet."""
        # Should publish current velocity (zero)
        node._timer_callback()
        assert node.current_vel.linear.x == 0.0
        assert node.current_vel.angular.z == 0.0

    def test_timer_callback_linear_acceleration(self, node):
        """Test linear acceleration limiting."""
        # Target: 1.0 m/s
        target = Twist()
        target.linear.x = 1.0
        node._on_cmd_vel(target)

        # First callback: current should move towards target, limited
        node._timer_callback()
        # Max acceleration: 0.5 m/s², period: 0.02s = 0.01 m/s change
        assert node.current_vel.linear.x == pytest.approx(0.01)

        # Second callback: should continue accelerating
        node._timer_callback()
        assert node.current_vel.linear.x == pytest.approx(0.02)

    def test_timer_callback_linear_deceleration(self, node):
        """Test linear deceleration limiting."""
        # Start with velocity
        node.current_vel.linear.x = 1.0

        # Target: 0 m/s (deceleration)
        target = Twist()
        target.linear.x = 0.0
        node._on_cmd_vel(target)

        # Callback should decelerate limited by rate limit
        node._timer_callback()
        # Max deceleration: -0.01 m/s, so 1.0 - 0.01 = 0.99
        assert node.current_vel.linear.x == pytest.approx(0.99)

    def test_timer_callback_angular_acceleration(self, node):
        """Test angular acceleration limiting."""
        target = Twist()
        target.angular.z = 1.0
        node._on_cmd_vel(target)

        node._timer_callback()
        # Max angular acceleration: 1.0 rad/s², period: 0.02s = 0.02 rad/s
        assert node.current_vel.angular.z == pytest.approx(0.02)

    def test_timer_callback_all_components(self, node):
        """Test rate limiting across all velocity components."""
        target = Twist()
        target.linear.x = 1.0
        target.linear.y = 0.5
        target.linear.z = 0.2
        target.angular.x = 0.1
        target.angular.y = 0.2
        target.angular.z = 1.0

        node._on_cmd_vel(target)
        node._timer_callback()

        # All linear components should be limited by linear_accel_limit
        assert node.current_vel.linear.x == pytest.approx(0.01)
        assert node.current_vel.linear.y == pytest.approx(0.005)
        assert node.current_vel.linear.z == pytest.approx(0.002)

        # All angular components should be limited by angular_accel_limit
        assert node.current_vel.angular.x == pytest.approx(0.002)
        assert node.current_vel.angular.y == pytest.approx(0.004)
        assert node.current_vel.angular.z == pytest.approx(0.02)

    def test_timer_reaches_target(self, node):
        """Test that velocity eventually reaches target after multiple callbacks."""
        target = Twist()
        target.linear.x = 0.1
        node._on_cmd_vel(target)

        # Max change per callback: 0.01
        # Target: 0.1, so needs 10 callbacks to reach it
        for _ in range(5):
            node._timer_callback()

        # After 5 callbacks: 0.05
        assert node.current_vel.linear.x == pytest.approx(0.05)

        # Continue for 5 more callbacks
        for _ in range(5):
            node._timer_callback()

        # Should be at target now or very close
        assert node.current_vel.linear.x == pytest.approx(0.1)

    def test_timer_stops_at_target(self, node):
        """Test that velocity stops changing once target is reached."""
        target = Twist()
        target.linear.x = 0.01
        node._on_cmd_vel(target)

        node._timer_callback()
        assert node.current_vel.linear.x == pytest.approx(0.01)

        # Next callback should stay at target
        node._timer_callback()
        assert node.current_vel.linear.x == pytest.approx(0.01)


class TestRateLimit:
    """Test suite for rate limiting behavior."""

    def test_linear_vs_angular_different_limits(self, node):
        """Test that linear and angular limits are independent."""
        target = Twist()
        target.linear.x = 1.0
        target.angular.z = 1.0

        node._on_cmd_vel(target)
        node._timer_callback()

        # Linear limited by 0.5 m/s²: 0.5 * 0.02 = 0.01
        assert node.current_vel.linear.x == pytest.approx(0.01)

        # Angular limited by 1.0 rad/s²: 1.0 * 0.02 = 0.02
        assert node.current_vel.angular.z == pytest.approx(0.02)

    def test_emergency_stop(self, node):
        """Test emergency deceleration from high speed."""
        # Start at high speed
        node.current_vel.linear.x = 2.0

        # Sudden stop command
        target = Twist()
        target.linear.x = 0.0
        node._on_cmd_vel(target)

        # Should decelerate gradually, not instantly
        node._timer_callback()
        assert node.current_vel.linear.x == pytest.approx(1.99)
        assert node.current_vel.linear.x > 0.0  # Not zero yet

    def test_alternating_direction(self, node):
        """Test rapid direction changes are smoothed."""
        # Accelerate forward
        target1 = Twist()
        target1.linear.x = 0.5
        node._on_cmd_vel(target1)

        for _ in range(10):
            node._timer_callback()

        vel_forward = node.current_vel.linear.x

        # Suddenly reverse
        target2 = Twist()
        target2.linear.x = -0.5
        node._on_cmd_vel(target2)

        node._timer_callback()
        vel_after_reverse = node.current_vel.linear.x

        # Should still be positive but decreasing
        assert vel_after_reverse < vel_forward
        assert vel_after_reverse > -0.5


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

    def test_scenario_gradual_acceleration(self, node):
        """Scenario: Smooth acceleration from stop to cruising speed."""
        target = Twist()
        target.linear.x = 0.5  # Cruise at 0.5 m/s

        node._on_cmd_vel(target)

        velocities = []
        for _ in range(100):
            node._timer_callback()
            velocities.append(node.current_vel.linear.x)

        # Velocities should be monotonically increasing
        for i in range(1, len(velocities)):
            assert velocities[i] >= velocities[i - 1]

        # Should reach target
        assert velocities[-1] == pytest.approx(0.5)

    def test_scenario_smooth_stop(self, node):
        """Scenario: Smooth deceleration to stop."""
        # Start at cruising speed
        node.current_vel.linear.x = 0.5

        # Command to stop
        target = Twist()
        target.linear.x = 0.0
        node._on_cmd_vel(target)

        velocities = []
        for _ in range(100):
            node._timer_callback()
            velocities.append(node.current_vel.linear.x)

        # Velocities should be monotonically decreasing
        for i in range(1, len(velocities)):
            assert velocities[i] <= velocities[i - 1]

        # Should reach zero
        assert velocities[-1] == pytest.approx(0.0)

    def test_scenario_turn_while_moving(self, node):
        """Scenario: Robot turns while moving forward."""
        node.current_vel.linear.x = 0.5
        node.current_vel.linear.y = 0.0

        target = Twist()
        target.linear.x = 0.5
        target.linear.y = 0.2
        target.angular.z = 0.5

        node._on_cmd_vel(target)

        # Multiple iterations to see smooth change
        for _ in range(50):
            node._timer_callback()

        # Forward velocity unchanged (same target)
        assert node.current_vel.linear.x == pytest.approx(0.5)

        # Lateral velocity increased toward target
        assert node.current_vel.linear.y > 0.0
        assert node.current_vel.linear.y <= 0.2

        # Angular velocity increased
        assert node.current_vel.angular.z > 0.0
        assert node.current_vel.angular.z <= 0.5

    def test_scenario_obstacle_avoidance(self, node):
        """Scenario: Quick lateral movement due to obstacle."""
        target = Twist()
        target.linear.x = 0.5
        target.linear.y = 0.3  # Quick dodge sideways
        node._on_cmd_vel(target)

        # Y velocity should increase gradually, not jump to 0.3
        accelerations = []
        for _ in range(10):
            node._timer_callback()
            accelerations.append(node.current_vel.linear.y)

        # Each step should be limited
        for i in range(1, len(accelerations)):
            delta = accelerations[i] - accelerations[i - 1]
            max_delta = 0.5 * 0.02  # linear_accel_limit * period
            assert abs(delta) <= max_delta + 1e-6  # Small tolerance

    def test_scenario_continuous_motion(self, node):
        """Scenario: Continuous velocity updates (tracking)."""
        for i in range(100):
            # Gradually increasing target
            target = Twist()
            target.linear.x = i * 0.001

            node._on_cmd_vel(target)
            node._timer_callback()

        # Current velocity should lag behind target
        assert node.current_vel.linear.x < 0.1

    def test_scenario_oscillating_command(self, node):
        """Scenario: Target velocity oscillates."""
        oscillations = []

        for i in range(200):
            # Sinusoidal target
            target = Twist()
            target.linear.x = 0.5 * (1 + math.sin(i * 0.05))

            node._on_cmd_vel(target)
            node._timer_callback()
            oscillations.append(node.current_vel.linear.x)

        # Velocity should lag behind target but not diverge
        assert all(0.0 <= v <= 1.0 for v in oscillations)
