"""Unit tests for wheel_slip_detector_node."""

import pytest
import math
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import Bool

import rclpy

from saltybot_wheel_slip_detector.wheel_slip_detector_node import WheelSlipDetectorNode


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


@pytest.fixture
def node(rclpy_fixture):
    """Create a wheel slip detector node instance."""
    node = WheelSlipDetectorNode()
    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.cmd_vel is None
        assert node.actual_vel is None
        assert node.slip_threshold == 0.1
        assert node.slip_timeout == 0.5
        assert node.slip_duration == 0.0
        assert node.slip_detected is False

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

    def test_slip_threshold_parameter(self, node):
        """Test slip threshold parameter is set correctly."""
        threshold = node.get_parameter("slip_threshold").value
        assert threshold == 0.1

    def test_slip_timeout_parameter(self, node):
        """Test slip timeout parameter is set correctly."""
        timeout = node.get_parameter("slip_timeout").value
        assert timeout == 0.5

    def test_period_calculation(self, node):
        """Test that time period is correctly calculated from frequency."""
        assert node.period == pytest.approx(0.1)


class TestSubscriptions:
    """Test suite for subscription handling."""

    def test_cmd_vel_subscription(self, node):
        """Test that cmd_vel subscription updates node state."""
        cmd = Twist()
        cmd.linear.x = 1.0
        cmd.linear.y = 0.5
        node._on_cmd_vel(cmd)
        assert node.cmd_vel is cmd
        assert node.cmd_vel.linear.x == 1.0

    def test_odom_subscription(self, node):
        """Test that odometry subscription updates actual velocity."""
        odom = Odometry()
        odom.twist.twist.linear.x = 0.95
        odom.twist.twist.linear.y = 0.48
        node._on_odom(odom)
        assert node.actual_vel is odom.twist.twist
        assert node.actual_vel.linear.x == 0.95


class TestSlipDetection:
    """Test suite for slip detection logic."""

    def test_no_slip_perfect_match(self, node):
        """Test no slip when commanded equals actual."""
        cmd = Twist()
        cmd.linear.x = 1.0
        odom = Odometry()
        odom.twist.twist.linear.x = 1.0
        node._on_cmd_vel(cmd)
        node._on_odom(odom)
        assert node._check_slip() is False

    def test_no_slip_small_difference(self, node):
        """Test no slip when difference is below threshold."""
        cmd = Twist()
        cmd.linear.x = 1.0
        odom = Odometry()
        odom.twist.twist.linear.x = 0.95
        node._on_cmd_vel(cmd)
        node._on_odom(odom)
        assert node._check_slip() is False

    def test_slip_exceeds_threshold(self, node):
        """Test slip detection when difference exceeds threshold."""
        cmd = Twist()
        cmd.linear.x = 1.0
        odom = Odometry()
        odom.twist.twist.linear.x = 0.85
        node._on_cmd_vel(cmd)
        node._on_odom(odom)
        assert node._check_slip() is True

    def test_slip_large_difference(self, node):
        """Test slip detection with large velocity difference."""
        cmd = Twist()
        cmd.linear.x = 1.0
        odom = Odometry()
        odom.twist.twist.linear.x = 0.5
        node._on_cmd_vel(cmd)
        node._on_odom(odom)
        assert node._check_slip() is True

    def test_no_slip_both_zero(self, node):
        """Test no slip when both commanded and actual are zero."""
        cmd = Twist()
        cmd.linear.x = 0.0
        odom = Odometry()
        odom.twist.twist.linear.x = 0.0
        node._on_cmd_vel(cmd)
        node._on_odom(odom)
        assert node._check_slip() is False

    def test_no_slip_both_near_zero(self, node):
        """Test no slip when both are near zero (tolerance)."""
        cmd = Twist()
        cmd.linear.x = 0.01
        odom = Odometry()
        odom.twist.twist.linear.x = 0.02
        node._on_cmd_vel(cmd)
        node._on_odom(odom)
        assert node._check_slip() is False

    def test_slip_2d_velocity(self, node):
        """Test slip detection with 2D velocity (x and y)."""
        cmd = Twist()
        cmd.linear.x = 0.7
        cmd.linear.y = 0.7
        odom = Odometry()
        odom.twist.twist.linear.x = 0.5
        odom.twist.twist.linear.y = 0.5
        node._on_cmd_vel(cmd)
        node._on_odom(odom)
        assert node._check_slip() is True


class TestSlipPersistence:
    """Test suite for slip persistence timing."""

    def test_slip_not_triggered_immediately(self, node):
        """Test that slip is not triggered immediately but requires timeout."""
        cmd = Twist()
        cmd.linear.x = 1.0
        odom = Odometry()
        odom.twist.twist.linear.x = 0.5
        node._on_cmd_vel(cmd)
        node._on_odom(odom)
        node._timer_callback()
        assert node.slip_duration > 0.0
        assert node.slip_detected is False

    def test_slip_declared_after_timeout(self, node):
        """Test that slip is declared after timeout period."""
        cmd = Twist()
        cmd.linear.x = 1.0
        odom = Odometry()
        odom.twist.twist.linear.x = 0.5
        node._on_cmd_vel(cmd)
        node._on_odom(odom)
        for _ in range(6):
            node._timer_callback()
        assert node.slip_detected is True

    def test_slip_recovery_resets_duration(self, node):
        """Test that slip duration resets when condition clears."""
        cmd = Twist()
        cmd.linear.x = 1.0
        odom1 = Odometry()
        odom1.twist.twist.linear.x = 0.5
        node._on_cmd_vel(cmd)
        node._on_odom(odom1)
        for _ in range(3):
            node._timer_callback()
        odom2 = Odometry()
        odom2.twist.twist.linear.x = 1.0
        node._on_odom(odom2)
        node._timer_callback()
        assert node.slip_duration == pytest.approx(0.0)
        assert node.slip_detected is False

    def test_slip_cumulative_time(self, node):
        """Test that slip duration accumulates across callbacks."""
        cmd = Twist()
        cmd.linear.x = 1.0
        odom = Odometry()
        odom.twist.twist.linear.x = 0.5
        node._on_cmd_vel(cmd)
        node._on_odom(odom)
        for _ in range(3):
            node._timer_callback()
        assert node.slip_duration == pytest.approx(0.3)
        assert node.slip_detected is False
        for _ in range(3):
            node._timer_callback()
        assert node.slip_duration == pytest.approx(0.6)
        assert node.slip_detected is True


class TestNoDataConditions:
    """Test suite for behavior when sensor data is unavailable."""

    def test_no_slip_without_cmd_vel(self, node):
        """Test no slip declared when cmd_vel not received."""
        node.cmd_vel = None
        odom = Odometry()
        odom.twist.twist.linear.x = 0.5
        node._on_odom(odom)
        node._timer_callback()
        assert node.slip_detected is False

    def test_no_slip_without_odometry(self, node):
        """Test no slip declared when odometry not received."""
        cmd = Twist()
        cmd.linear.x = 1.0
        node._on_cmd_vel(cmd)
        node.actual_vel = None
        node._timer_callback()
        assert node.slip_detected is False


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

    def test_scenario_normal_motion_no_slip(self, node):
        """Scenario: Normal motion with good wheel traction."""
        cmd = Twist()
        cmd.linear.x = 0.5
        for i in range(10):
            odom = Odometry()
            odom.twist.twist.linear.x = 0.5 + (i * 0.001)
            node._on_cmd_vel(cmd)
            node._on_odom(odom)
            node._timer_callback()
        assert node.slip_detected is False

    def test_scenario_ice_slip_persistent(self, node):
        """Scenario: Ice causes persistent wheel slip."""
        cmd = Twist()
        cmd.linear.x = 1.0
        for _ in range(10):
            odom = Odometry()
            odom.twist.twist.linear.x = 0.7
            node._on_cmd_vel(cmd)
            node._on_odom(odom)
            node._timer_callback()
        assert node.slip_detected is True

    def test_scenario_sandy_surface_intermittent_slip(self, node):
        """Scenario: Sandy surface causes intermittent slip."""
        cmd = Twist()
        cmd.linear.x = 0.8
        speeds = [0.7, 0.8, 0.6, 0.8, 0.7, 0.8]
        for speed in speeds:
            odom = Odometry()
            odom.twist.twist.linear.x = speed
            node._on_cmd_vel(cmd)
            node._on_odom(odom)
            node._timer_callback()
        assert node.slip_detected is False

    def test_scenario_sudden_obstacle_slip(self, node):
        """Scenario: Robot hits obstacle and wheels slip."""
        cmd = Twist()
        cmd.linear.x = 1.0
        for _ in range(3):
            odom = Odometry()
            odom.twist.twist.linear.x = 1.0
            node._on_cmd_vel(cmd)
            node._on_odom(odom)
            node._timer_callback()
        for _ in range(8):
            odom = Odometry()
            odom.twist.twist.linear.x = 0.2
            node._on_odom(odom)
            node._timer_callback()
        assert node.slip_detected is True

    def test_scenario_wet_surface_recovery(self, node):
        """Scenario: Wet surface slip, then wheel regains traction."""
        cmd = Twist()
        cmd.linear.x = 1.0
        for _ in range(6):
            odom = Odometry()
            odom.twist.twist.linear.x = 0.8
            node._on_cmd_vel(cmd)
            node._on_odom(odom)
            node._timer_callback()
        assert node.slip_detected is True
        for _ in range(3):
            odom = Odometry()
            odom.twist.twist.linear.x = 1.0
            node._on_odom(odom)
            node._timer_callback()
        assert node.slip_detected is False

    def test_scenario_backward_motion(self, node):
        """Scenario: Backward motion with slip."""
        cmd = Twist()
        cmd.linear.x = -0.8
        for _ in range(6):
            odom = Odometry()
            odom.twist.twist.linear.x = -0.4
            node._on_cmd_vel(cmd)
            node._on_odom(odom)
            node._timer_callback()
        assert node.slip_detected is True

    def test_scenario_diagonal_motion_slip(self, node):
        """Scenario: Diagonal motion with slip."""
        cmd = Twist()
        cmd.linear.x = 0.7
        cmd.linear.y = 0.7
        for _ in range(6):
            odom = Odometry()
            odom.twist.twist.linear.x = 0.5
            odom.twist.twist.linear.y = 0.5
            node._on_cmd_vel(cmd)
            node._on_odom(odom)
            node._timer_callback()
        assert node.slip_detected is True
