"""Unit tests for balance controller node."""

import pytest
import math
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion
from std_msgs.msg import String

import rclpy
from saltybot_balance_controller.balance_controller_node import BalanceControllerNode


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


@pytest.fixture
def node(rclpy_fixture):
    """Create a balance controller node instance."""
    node = BalanceControllerNode()
    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.port == "/dev/ttyUSB0"
        assert node.baudrate == 115200
        assert node.pid_p == 0.5
        assert node.pid_i == 0.1
        assert node.pid_d == 0.05

    def test_tilt_threshold_parameter(self, node):
        """Test tilt threshold parameter is set correctly."""
        assert node.tilt_threshold == 45.0

    def test_tilt_kill_duration_parameter(self, node):
        """Test tilt kill duration parameter is set correctly."""
        assert node.tilt_kill_duration == 0.5


class TestQuaternionToEuler:
    """Test suite for quaternion to Euler conversion."""

    def test_identity_quaternion(self, node):
        """Test identity quaternion (no rotation)."""
        roll, pitch, yaw = node._quaternion_to_euler(0, 0, 0, 1)
        assert roll == pytest.approx(0)
        assert pitch == pytest.approx(0)
        assert yaw == pytest.approx(0)

    def test_90deg_pitch_rotation(self, node):
        """Test 90 degree pitch rotation."""
        # Quaternion for 90 degree Y rotation
        roll, pitch, yaw = node._quaternion_to_euler(0, 0.707, 0, 0.707)
        assert roll == pytest.approx(0, abs=0.01)
        assert pitch == pytest.approx(math.pi / 2, abs=0.01)
        assert yaw == pytest.approx(0, abs=0.01)

    def test_45deg_pitch_rotation(self, node):
        """Test 45 degree pitch rotation."""
        roll, pitch, yaw = node._quaternion_to_euler(0, 0.383, 0, 0.924)
        assert roll == pytest.approx(0, abs=0.01)
        assert pitch == pytest.approx(math.pi / 4, abs=0.01)
        assert yaw == pytest.approx(0, abs=0.01)

    def test_roll_rotation(self, node):
        """Test roll rotation around X axis."""
        roll, pitch, yaw = node._quaternion_to_euler(0.707, 0, 0, 0.707)
        assert roll == pytest.approx(math.pi / 2, abs=0.01)
        assert pitch == pytest.approx(0, abs=0.01)
        assert yaw == pytest.approx(0, abs=0.01)


class TestIMUDataParsing:
    """Test suite for IMU data parsing."""

    def test_imu_data_subscription(self, node):
        """Test IMU data subscription updates node state."""
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0, z=0, w=1)

        node._on_imu_data(imu)

        assert node.imu_data is not None
        assert node.imu_data.pitch_deg == pytest.approx(0, abs=0.1)
        assert node.imu_data.roll_deg == pytest.approx(0, abs=0.1)

    def test_imu_pitch_tilted_forward(self, node):
        """Test IMU data with forward pitch tilt."""
        # 45 degree forward pitch
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0.383, z=0, w=0.924)

        node._on_imu_data(imu)

        assert node.imu_data is not None
        # Should be approximately 45 degrees (in radians converted to degrees)
        assert node.imu_data.pitch_deg == pytest.approx(45, abs=1)


class TestTiltSafety:
    """Test suite for tilt safety checks."""

    def test_tilt_warning_state_entry(self, node):
        """Test entry into tilt warning state."""
        imu = Imu()
        # 50 degree pitch (exceeds 45 degree threshold)
        imu.orientation = Quaternion(x=0, y=0.438, z=0, w=0.899)
        node._on_imu_data(imu)

        # Call check with small duration
        node._check_tilt_safety()

        assert node.state.value in ["tilt_warning", "startup"]

    def test_level_no_tilt_warning(self, node):
        """Test no tilt warning when level."""
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0, z=0, w=1)
        node._on_imu_data(imu)

        node._check_tilt_safety()

        # Tilt start time should be None when level
        assert node.tilt_start_time is None


class TestStartupRamp:
    """Test suite for startup ramp functionality."""

    def test_startup_ramp_begins_at_zero(self, node):
        """Test startup ramp begins at 0."""
        ramp = node._check_startup_ramp()

        # At startup time, ramp should be close to 0
        assert ramp <= 0.05  # Very small value at start

    def test_startup_ramp_reaches_one(self, node):
        """Test startup ramp reaches 1.0 after duration."""
        # Simulate startup ramp completion
        node.startup_time = 0
        node.startup_ramp_time = 0.001  # Very short ramp

        import time
        time.sleep(0.01)  # Sleep longer than ramp time

        ramp = node._check_startup_ramp()

        # Should be complete after time has passed
        assert ramp >= 0.99

    def test_startup_ramp_linear(self, node):
        """Test startup ramp is linear."""
        node.startup_ramp_time = 1.0

        # At 25% of startup time
        node.startup_time = 0
        import time
        time.sleep(0.25)

        ramp = node._check_startup_ramp()
        assert 0.2 < ramp < 0.3
