"""Unit tests for motor_protection_node."""

import pytest
import json
import math
from std_msgs.msg import Float32, String

import rclpy

# Import the node and classes under test
from saltybot_motor_protection.motor_protection_node import (
    MotorProtectionNode,
    ProtectionState,
    ThermalModel,
)


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


@pytest.fixture
def node(rclpy_fixture):
    """Create a motor protection node instance."""
    node = MotorProtectionNode()
    yield node
    node.destroy_node()


class TestThermalModel:
    """Test suite for ThermalModel."""

    def test_thermal_model_initialization(self):
        """Test ThermalModel initializes with correct defaults."""
        model = ThermalModel()

        assert model.motor_resistance == 1.5
        assert model.thermal_mass == 100.0
        assert model.ambient_temp == 25.0
        assert model.max_temp == 80.0
        assert model.cooling_constant == 0.05
        assert model.motor_temp == 25.0

    def test_thermal_model_custom_params(self):
        """Test ThermalModel with custom parameters."""
        model = ThermalModel(
            motor_resistance=2.0,
            thermal_mass=50.0,
            max_temp=100.0,
        )

        assert model.motor_resistance == 2.0
        assert model.thermal_mass == 50.0
        assert model.max_temp == 100.0


class TestMotorProtectionNode:
    """Test suite for MotorProtectionNode."""

    def test_node_initialization(self, node):
        """Test that node initializes with correct defaults."""
        assert node.soft_limit == 5.0
        assert node.hard_limit == 8.0
        assert node.current_left == 0.0
        assert node.current_right == 0.0
        assert node.current_max == 0.0
        assert node.state == ProtectionState.NORMAL
        assert node.soft_fault_duration == 0.0

    def test_current_left_subscription(self, node):
        """Test left motor current subscription."""
        msg = Float32(data=2.5)
        node._on_current_left(msg)
        assert node.current_left == 2.5

    def test_current_right_subscription(self, node):
        """Test right motor current subscription."""
        msg = Float32(data=3.0)
        node._on_current_right(msg)
        assert node.current_right == 3.0

    def test_current_negative_clamping(self, node):
        """Test that negative currents are clamped to zero."""
        node._on_current_left(Float32(data=-1.0))
        assert node.current_left == 0.0

        node._on_current_right(Float32(data=-2.5))
        assert node.current_right == 0.0

    def test_max_current_calculation(self, node):
        """Test maximum current selection."""
        node.current_left = 2.0
        node.current_right = 3.5

        # Simulate timer callback to update max
        node._timer_callback()

        assert node.current_max == 3.5

    def test_normal_state_low_current(self, node):
        """Test normal state with low current."""
        node.current_max = 2.0

        node._check_overcurrent()

        assert node.state == ProtectionState.NORMAL
        assert node.soft_fault_duration == 0.0

    def test_soft_fault_detection(self, node):
        """Test soft overcurrent detection at 5A."""
        node.current_max = 5.5

        node._check_overcurrent()

        assert node.state == ProtectionState.SOFT_FAULT

    def test_hard_fault_detection(self, node):
        """Test hard overcurrent detection at 8A."""
        node.current_max = 8.5

        node._check_overcurrent()

        assert node.state == ProtectionState.HARD_FAULT

    def test_soft_fault_duration_tracking(self, node):
        """Test soft fault duration accumulation."""
        node.current_max = 5.5
        node.state = ProtectionState.NORMAL

        # First cycle
        node._check_overcurrent()
        assert node.soft_fault_duration == 0.02

        # Second cycle
        node._check_overcurrent()
        assert node.soft_fault_duration == 0.04

    def test_fault_recovery(self, node):
        """Test recovery from soft fault."""
        # Start in soft fault
        node.state = ProtectionState.SOFT_FAULT
        node.current_max = 5.5

        # Current drops below threshold
        node.current_max = 4.0
        node._check_overcurrent()

        assert node.state == ProtectionState.NORMAL
        assert node.soft_fault_duration == 0.0

    def test_joule_heating_calculation(self, node):
        """Test I^2R heating calculation."""
        # Current = 5A, Resistance = 1.5 Ohm
        # P = I^2 * R = 25 * 1.5 = 37.5W per motor
        node.current_left = 5.0
        node.current_right = 5.0

        # Simulate thermal update
        node._update_thermal_state(1.0)

        # Temperature should increase
        assert node.thermal.motor_temp > 25.0

    def test_temperature_increase(self, node):
        """Test motor temperature increases with current."""
        initial_temp = node.thermal.motor_temp

        node.current_left = 4.0
        node.current_right = 4.0

        # Run multiple iterations
        for _ in range(10):
            node._update_thermal_state(0.1)

        # Temperature should be higher
        assert node.thermal.motor_temp > initial_temp

    def test_temperature_saturation(self, node):
        """Test temperature doesn't exceed ambient with zero current."""
        node.thermal.motor_temp = 50.0
        node.current_left = 0.0
        node.current_right = 0.0

        # Cool down for many iterations
        for _ in range(100):
            node._update_thermal_state(0.1)

        # Temperature should cool toward ambient
        assert node.thermal.motor_temp < 50.0
        assert node.thermal.motor_temp >= 25.0  # Don't go below ambient

    def test_derate_hard_fault(self, node):
        """Test derate returns 0 on hard fault."""
        node.state = ProtectionState.HARD_FAULT

        derate = node._calculate_derate()

        assert derate == 0.0

    def test_derate_normal(self, node):
        """Test derate returns 1.0 in normal state."""
        node.state = ProtectionState.NORMAL
        node.thermal.motor_temp = 25.0  # Ambient

        derate = node._calculate_derate()

        assert derate == 1.0

    def test_derate_soft_fault(self, node):
        """Test derate reduces in soft fault state."""
        node.state = ProtectionState.SOFT_FAULT
        node.current_max = 6.0  # Midway between soft and hard
        node.thermal.motor_temp = 25.0

        derate = node._calculate_derate()

        # Should be reduced but not zero
        assert 0.1 <= derate < 1.0

    def test_derate_high_temperature(self, node):
        """Test thermal derating at high temperature."""
        node.state = ProtectionState.NORMAL
        node.thermal.motor_temp = 75.0  # Near limit (80°C)

        derate = node._calculate_derate()

        # Should be significantly reduced
        assert derate < 0.5

    def test_derate_maximum_temperature(self, node):
        """Test derate approaches zero at max temperature."""
        node.state = ProtectionState.NORMAL
        node.thermal.motor_temp = 80.0  # At limit

        derate = node._calculate_derate()

        assert derate == 0.0

    def test_derate_clipping(self, node):
        """Test derate is clipped to [0, 1]."""
        # Even with extreme calculations, should be in range
        node.state = ProtectionState.NORMAL
        node.thermal.motor_temp = -50.0  # Invalid, but test robustness
        node.current_max = 20.0  # Extreme

        derate = node._calculate_derate()

        assert 0.0 <= derate <= 1.0

    def test_protection_status_json(self, node):
        """Test protection status JSON format."""
        node.current_left = 2.5
        node.current_right = 3.0
        node.state = ProtectionState.NORMAL
        node.thermal.motor_temp = 45.0

        # Publish would be called, but test JSON creation
        status = {
            "state": node.state.name,
            "current_left": float(node.current_left),
            "current_right": float(node.current_right),
            "current_max": float(node.current_max),
            "motor_temp": float(node.thermal.motor_temp),
            "soft_limit": node.soft_limit,
            "hard_limit": node.hard_limit,
        }

        json_str = json.dumps(status)
        parsed = json.loads(json_str)

        assert parsed["state"] == "NORMAL"
        assert parsed["current_left"] == 2.5
        assert parsed["motor_temp"] == 45.0


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

    def test_scenario_normal_operation(self, node):
        """Scenario: normal motor operation."""
        node.current_left = 2.0
        node.current_right = 2.5

        node._timer_callback()

        assert node.state == ProtectionState.NORMAL
        derate = node._calculate_derate()
        assert derate > 0.9

    def test_scenario_slow_thermal_rise(self, node):
        """Scenario: gradual temperature increase from sustained current."""
        node.current_left = 3.0
        node.current_right = 3.0

        # Simulate 10 seconds at 50Hz
        for _ in range(500):
            node._update_thermal_state(0.02)

        # Temperature should have risen significantly
        assert node.thermal.motor_temp > 40.0
        assert node.thermal.motor_temp < 80.0

    def test_scenario_soft_overcurrent_trip(self, node):
        """Scenario: soft overcurrent detection and derating."""
        node.current_max = 5.5

        node._check_overcurrent()
        assert node.state == ProtectionState.SOFT_FAULT

        derate = node._calculate_derate()
        assert 0.0 < derate < 1.0

    def test_scenario_hard_overcurrent_emergency(self, node):
        """Scenario: hard overcurrent triggers immediate shutdown."""
        node.current_max = 9.0

        node._check_overcurrent()
        assert node.state == ProtectionState.HARD_FAULT

        derate = node._calculate_derate()
        assert derate == 0.0

    def test_scenario_combined_thermal_current_derating(self, node):
        """Scenario: both current and thermal derating apply."""
        # Soft overcurrent
        node.current_max = 5.5
        node.state = ProtectionState.SOFT_FAULT

        # Also high temperature
        node.thermal.motor_temp = 70.0

        derate = node._calculate_derate()

        # Should be reduced by both factors
        assert derate < 0.5

    def test_scenario_current_spike_recovery(self, node):
        """Scenario: brief current spike then recovery."""
        # Spike
        node.current_max = 8.5
        node._check_overcurrent()
        assert node.state == ProtectionState.HARD_FAULT

        derate = node._calculate_derate()
        assert derate == 0.0

        # Recovery
        node.current_max = 2.0
        node._check_overcurrent()
        assert node.state == ProtectionState.NORMAL

        derate = node._calculate_derate()
        assert derate > 0.9

    def test_scenario_thermal_limit_shutdown(self, node):
        """Scenario: motor heats up and speed is derated to zero."""
        node.state = ProtectionState.NORMAL

        # Simulate sustained high current heating
        node.current_left = 5.0
        node.current_right = 5.0

        # Heat up to max temp
        while node.thermal.motor_temp < 80.0:
            node._update_thermal_state(0.1)

        # At max temp, derate should be zero
        derate = node._calculate_derate()
        assert derate == 0.0

    def test_scenario_asymmetric_motor_loading(self, node):
        """Scenario: one motor draws more current than the other."""
        node.current_left = 2.0
        node.current_right = 6.5

        node._timer_callback()

        # Should use worst case (right motor)
        assert node.current_max == 6.5
        assert node.state == ProtectionState.SOFT_FAULT
