"""Unit tests for compass_heading_node."""

import pytest
import math
from geometry_msgs.msg import Quaternion, QuaternionStamped
from std_msgs.msg import Float64
from sensor_msgs.msg import Imu, MagneticField
from geometry_msgs.msg import Vector3

import rclpy
from rclpy.time import Time

# Import the node under test
from saltybot_compass.compass_heading_node import CompassHeadingNode


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


@pytest.fixture
def node(rclpy_fixture):
    """Create a compass heading node instance."""
    node = CompassHeadingNode()
    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.declination_rad == pytest.approx(math.radians(1.5))
        assert node.mag_field is None
        assert node.imu_data is None

    def test_declination_parameter(self, node):
        """Test magnetic declination parameter is set correctly."""
        declination = node.get_parameter("declination_deg").value
        assert declination == 1.5

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


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_z_rotation(self, node):
        """Test 90 degree rotation around Z axis."""
        # Quaternion for 90 degree Z rotation: sin(45°)=0.707, cos(45°)=0.707
        roll, pitch, yaw = node._quaternion_to_euler(0, 0, 0.707, 0.707)
        assert roll == pytest.approx(0, abs=0.01)
        assert pitch == pytest.approx(0, abs=0.01)
        assert yaw == pytest.approx(math.pi / 2, abs=0.01)

    def test_90deg_x_rotation(self, node):
        """Test 90 degree rotation around X axis (roll)."""
        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)

    def test_45deg_y_rotation(self, node):
        """Test 45 degree rotation around Y axis (pitch)."""
        # sin(22.5°) ≈ 0.383, cos(22.5°) ≈ 0.924
        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)


class TestHeadingQuaternion:
    """Test suite for heading quaternion creation."""

    def test_zero_heading_quaternion(self, node):
        """Test quaternion for 0 degree heading (north)."""
        quat = node._create_heading_quaternion(0)
        assert quat.x == 0
        assert quat.y == 0
        assert quat.z == pytest.approx(0)
        assert quat.w == pytest.approx(1)

    def test_90deg_heading_quaternion(self, node):
        """Test quaternion for 90 degree heading (east)."""
        quat = node._create_heading_quaternion(math.pi / 2)
        assert quat.x == 0
        assert quat.y == 0
        assert quat.z == pytest.approx(math.sin(math.pi / 4))
        assert quat.w == pytest.approx(math.cos(math.pi / 4))

    def test_180deg_heading_quaternion(self, node):
        """Test quaternion for 180 degree heading (south)."""
        quat = node._create_heading_quaternion(math.pi)
        assert quat.x == 0
        assert quat.y == 0
        assert quat.z == pytest.approx(1, abs=0.01)
        assert quat.w == pytest.approx(0, abs=0.01)

    def test_heading_quaternion_normalized(self, node):
        """Test that heading quaternion is normalized."""
        for angle in [0, math.pi / 4, math.pi / 2, math.pi]:
            quat = node._create_heading_quaternion(angle)
            norm_sq = quat.x**2 + quat.y**2 + quat.z**2 + quat.w**2
            assert norm_sq == pytest.approx(1)


class TestTiltCompensation:
    """Test suite for tilt-compensated heading calculation."""

    def test_level_heading_north(self, node):
        """Test heading calculation when level and pointing north."""
        # Level IMU (no tilt): roll=0, pitch=0
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0, z=0, w=1)

        # Magnetometer pointing north
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=1.0, y=0.0, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Heading should be 0 (north) + 1.5° declination = 1.5°
        assert True  # Timer callback executed without error

    def test_level_heading_east(self, node):
        """Test heading calculation when level and pointing east."""
        # Level IMU (no tilt): roll=0, pitch=0
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0, z=0, w=1)

        # Magnetometer pointing east
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=0.0, y=1.0, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should complete without error
        assert True

    def test_level_heading_south(self, node):
        """Test heading calculation when level and pointing south."""
        # Level IMU (no tilt): roll=0, pitch=0
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0, z=0, w=1)

        # Magnetometer pointing south
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=-1.0, y=0.0, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should complete without error
        assert True

    def test_heading_with_roll_tilt(self, node):
        """Test heading calculation with roll tilt (side tilt)."""
        # IMU tilted 45 degrees around X axis (roll)
        # sin(22.5°) ≈ 0.383, cos(22.5°) ≈ 0.924
        imu = Imu()
        imu.orientation = Quaternion(x=0.383, y=0, z=0, w=0.924)

        # Magnetometer pointing north
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=1.0, y=0.0, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should complete with tilt compensation
        assert True

    def test_heading_with_pitch_tilt(self, node):
        """Test heading calculation with pitch tilt (forward tilt)."""
        # IMU tilted 30 degrees around Y axis (pitch)
        # sin(15°) ≈ 0.259, cos(15°) ≈ 0.966
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0.259, z=0, w=0.966)

        # Magnetometer pointing north
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=1.0, y=0.0, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should complete with tilt compensation
        assert True

    def test_heading_with_combined_tilt(self, node):
        """Test heading calculation with combined roll and pitch tilt."""
        # IMU tilted 30 degrees around X (roll) and Y (pitch)
        imu = Imu()
        imu.orientation = Quaternion(x=0.223, y=0.223, z=0.064, w=0.942)

        # Magnetometer pointing north
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=1.0, y=0.0, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should complete with full tilt compensation
        assert True


class TestDeclinationCorrection:
    """Test suite for magnetic declination correction."""

    def test_declination_application(self, node):
        """Test that declination is applied correctly."""
        # Without declination, heading would be 0 (north)
        # With 1.5° declination, heading should be 1.5°
        assert node.declination_rad == pytest.approx(math.radians(1.5))

    def test_custom_declination(self, rclpy_fixture):
        """Test node with custom declination value."""
        # Create node with custom declination
        rclpy.init()
        node = CompassHeadingNode()
        # Override declination
        node.set_parameters_from_file = lambda: None
        node.declare_parameter("declination_deg", 5.0)
        node.declination_rad = math.radians(5.0)

        assert node.declination_rad == pytest.approx(math.radians(5.0))
        node.destroy_node()


class TestSubscriptions:
    """Test suite for sensor data subscriptions."""

    def test_imu_subscription_handler(self, node):
        """Test IMU 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 imu

    def test_mag_subscription_handler(self, node):
        """Test magnetometer subscription updates node state."""
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=1.0, y=0.0, z=0.0)

        node._on_mag_field(mag)

        assert node.mag_field is mag

    def test_timer_skips_without_imu(self, node):
        """Test timer returns early if IMU data not available."""
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=1.0, y=0.0, z=0.0)
        node.mag_field = mag
        node.imu_data = None

        # Should return early without error
        node._timer_callback()
        assert True

    def test_timer_skips_without_mag(self, node):
        """Test timer returns early if mag data not available."""
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0, z=0, w=1)
        node.imu_data = imu
        node.mag_field = None

        # Should return early without error
        node._timer_callback()
        assert True


class TestHeadingNormalization:
    """Test suite for heading angle normalization."""

    def test_heading_wraps_at_360(self, node):
        """Test that heading wraps correctly around 360 degrees."""
        # This is tested implicitly in timer_callback through heading calculation
        # Testing the normalization logic separately
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0, z=0, w=1)

        # Magnetometer pointing west (negative y)
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=0.0, y=-1.0, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should compute heading in 0-360 range
        assert True

    def test_negative_heading_correction(self, node):
        """Test that negative headings are corrected to 0-360 range."""
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0, z=0, w=1)

        # Magnetometer pointing northwest
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=1.0, y=-1.0, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should wrap to 0-360 range
        assert True


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

    def test_scenario_level_pointing_north(self, node):
        """Scenario: Robot level and pointing magnetic north."""
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0, z=0, w=1)

        mag = MagneticField()
        mag.magnetic_field = Vector3(x=1.0, y=0.0, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Expected: ~1.5° (declination)
        assert True

    def test_scenario_tilted_uphill_pointing_north(self, node):
        """Scenario: Robot tilted uphill (pitched forward) pointing north."""
        # 20 degree forward pitch
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0.174, z=0, w=0.985)

        mag = MagneticField()
        mag.magnetic_field = Vector3(x=1.0, y=0.0, z=0.1)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should compensate for pitch tilt
        assert True

    def test_scenario_tilted_sideways_pointing_north(self, node):
        """Scenario: Robot tilted sideways (rolled 15°) pointing north."""
        imu = Imu()
        imu.orientation = Quaternion(x=0.131, y=0, z=0, w=0.991)

        mag = MagneticField()
        mag.magnetic_field = Vector3(x=0.966, y=0.0, z=0.259)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should compensate for roll tilt
        assert True

    def test_scenario_full_3d_tilt(self, node):
        """Scenario: Robot tilted in 3D with yaw and pointing northeast."""
        # Random 3D orientation
        imu = Imu()
        imu.orientation = Quaternion(x=0.2, y=0.3, z=0.1, w=0.9)

        # Magnetometer pointing northeast
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=0.707, y=0.707, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should compute heading with full tilt compensation
        assert True

    def test_scenario_weak_magnetometer_signal(self, node):
        """Scenario: Weak magnetometer signal due to interference."""
        imu = Imu()
        imu.orientation = Quaternion(x=0, y=0, z=0, w=1)

        # Weak signal: 0.2 Tesla instead of nominal 1.0
        mag = MagneticField()
        mag.magnetic_field = Vector3(x=0.2, y=0.0, z=0.0)

        node.imu_data = imu
        node.mag_field = mag
        node._timer_callback()

        # Should still compute heading (SNR issue is filtering problem, not this node)
        assert True

    def test_scenario_continuous_rotation(self, node):
        """Scenario: Robot rotating 360 degrees."""
        imu = Imu()

        for angle_deg in range(0, 361, 45):
            angle_rad = math.radians(angle_deg / 2)  # Quaternion half-angle
            imu.orientation = Quaternion(
                x=0, y=0, z=math.sin(angle_rad), w=math.cos(angle_rad)
            )

            mag = MagneticField()
            mag.magnetic_field = Vector3(x=1.0, y=0.0, z=0.0)

            node.imu_data = imu
            node.mag_field = mag
            node._timer_callback()

        # Should complete all rotations without error
        assert True
