"""Unit tests for battery speed limiter node."""

import pytest
from sensor_msgs.msg import BatteryState
from std_msgs.msg import Float32

import rclpy

from saltybot_battery_speed_limiter.battery_speed_limiter_node import (
    BatterySpeedLimiterNode,
)


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


@pytest.fixture
def node(rclpy_fixture):
    """Create a battery speed limiter node instance."""
    node = BatterySpeedLimiterNode()
    yield node
    node.destroy_node()


class TestBatterySpeedLimiterNode:
    """Test suite for BatterySpeedLimiterNode."""

    def test_node_initialization(self, node):
        """Test that node initializes with correct defaults."""
        assert node.current_battery_percent == 100.0
        assert node.enable_limiting is True
        assert node.speed_factor_full == 1.0
        assert node.speed_factor_reduced == 0.7
        assert node.speed_factor_critical == 0.4
        assert node.speed_factor_stop == 0.0

    def test_battery_state_subscription(self, node):
        """Test battery state subscription updates percentage."""
        battery_msg = BatteryState()
        battery_msg.percentage = 0.75  # 75% (0-1 range)

        node._on_battery_state(battery_msg)

        assert node.current_battery_percent == 75.0

    def test_battery_percentage_conversion(self, node):
        """Test percentage conversion from 0-1 to 0-100 range."""
        battery_msg = BatteryState()
        battery_msg.percentage = 0.5  # 50%

        node._on_battery_state(battery_msg)

        assert node.current_battery_percent == 50.0

    def test_battery_zero_percent(self, node):
        """Test zero battery percentage."""
        battery_msg = BatteryState()
        battery_msg.percentage = 0.0

        node._on_battery_state(battery_msg)

        assert node.current_battery_percent == 0.0

    def test_battery_full_percent(self, node):
        """Test full battery percentage."""
        battery_msg = BatteryState()
        battery_msg.percentage = 1.0

        node._on_battery_state(battery_msg)

        assert node.current_battery_percent == 100.0

    def test_speed_limit_threshold_full_100_percent(self, node):
        """Test speed limit at 100% battery (full speed)."""
        speed_limit = node._calculate_speed_limit(100.0)
        assert speed_limit == 1.0

    def test_speed_limit_threshold_full_75_percent(self, node):
        """Test speed limit at 75% battery (full speed range)."""
        speed_limit = node._calculate_speed_limit(75.0)
        assert speed_limit == 1.0

    def test_speed_limit_threshold_full_50_percent(self, node):
        """Test speed limit at 50% battery (boundary)."""
        speed_limit = node._calculate_speed_limit(50.0)
        assert speed_limit == 1.0

    def test_speed_limit_threshold_reduced_49_percent(self, node):
        """Test speed limit at 49% battery (just below full threshold)."""
        speed_limit = node._calculate_speed_limit(49.0)
        assert speed_limit == 0.7

    def test_speed_limit_threshold_reduced_37_percent(self, node):
        """Test speed limit at 37% battery (reduced range)."""
        speed_limit = node._calculate_speed_limit(37.0)
        assert speed_limit == 0.7

    def test_speed_limit_threshold_reduced_25_percent(self, node):
        """Test speed limit at 25% battery (boundary)."""
        speed_limit = node._calculate_speed_limit(25.0)
        assert speed_limit == 0.7

    def test_speed_limit_threshold_critical_24_percent(self, node):
        """Test speed limit at 24% battery (just below reduced threshold)."""
        speed_limit = node._calculate_speed_limit(24.0)
        assert speed_limit == 0.4

    def test_speed_limit_threshold_critical_20_percent(self, node):
        """Test speed limit at 20% battery (critical range)."""
        speed_limit = node._calculate_speed_limit(20.0)
        assert speed_limit == 0.4

    def test_speed_limit_threshold_critical_15_percent(self, node):
        """Test speed limit at 15% battery (boundary)."""
        speed_limit = node._calculate_speed_limit(15.0)
        assert speed_limit == 0.4

    def test_speed_limit_threshold_stop_14_percent(self, node):
        """Test speed limit at 14% battery (just below critical threshold)."""
        speed_limit = node._calculate_speed_limit(14.0)
        assert speed_limit == 0.0

    def test_speed_limit_threshold_stop_5_percent(self, node):
        """Test speed limit at 5% battery (stop range)."""
        speed_limit = node._calculate_speed_limit(5.0)
        assert speed_limit == 0.0

    def test_speed_limit_threshold_stop_0_percent(self, node):
        """Test speed limit at 0% battery (empty)."""
        speed_limit = node._calculate_speed_limit(0.0)
        assert speed_limit == 0.0

    def test_speed_limit_limiting_disabled(self, node):
        """Test that limiting can be disabled."""
        node.enable_limiting = False

        # Even at critical battery, should return full speed
        speed_limit = node._calculate_speed_limit(10.0)
        assert speed_limit == 1.0

    def test_speed_limit_limiting_enabled(self, node):
        """Test that limiting is enabled by default."""
        node.enable_limiting = True

        speed_limit = node._calculate_speed_limit(10.0)
        assert speed_limit == 0.0

    def test_threshold_transitions_high_to_low(self, node):
        """Test transitions from high to low battery levels."""
        # Start at 100%
        assert node._calculate_speed_limit(100.0) == 1.0

        # Transition to reduced at 49%
        assert node._calculate_speed_limit(49.0) == 0.7

        # Transition to critical at 24%
        assert node._calculate_speed_limit(24.0) == 0.4

        # Transition to stop at 14%
        assert node._calculate_speed_limit(14.0) == 0.0

    def test_threshold_transitions_low_to_high(self, node):
        """Test transitions from low to high battery levels (charging)."""
        # Start at 0%
        assert node._calculate_speed_limit(0.0) == 0.0

        # Transition to critical at 15%
        assert node._calculate_speed_limit(15.0) == 0.4

        # Transition to reduced at 25%
        assert node._calculate_speed_limit(25.0) == 0.7

        # Transition to full at 50%
        assert node._calculate_speed_limit(50.0) == 1.0

    def test_battery_state_with_voltage_fallback(self, node):
        """Test battery state extraction when percentage not available."""
        battery_msg = BatteryState()
        battery_msg.percentage = -1.0  # Invalid percentage
        battery_msg.voltage = 8.0  # 2S LiPo approximate mid-range

        node._on_battery_state(battery_msg)

        # Should calculate from voltage
        assert 0 <= node.current_battery_percent <= 100

    def test_timer_callback_publishes(self, node):
        """Test that timer callback executes without error."""
        node._timer_callback()

        # Should execute without crashing

    def test_speed_limit_continuity(self, node):
        """Test that speed limits are continuous across range."""
        for percent in range(0, 101, 5):
            speed_limit = node._calculate_speed_limit(float(percent))
            assert 0.0 <= speed_limit <= 1.0

    def test_custom_threshold_values(self, node):
        """Test that custom threshold parameters are respected."""
        # Modify thresholds
        node.threshold_full_2 = 60
        node.threshold_reduced_2 = 30
        node.threshold_critical_2 = 10

        # Test new boundaries
        assert node._calculate_speed_limit(60.0) == 1.0
        assert node._calculate_speed_limit(59.0) == 0.7
        assert node._calculate_speed_limit(30.0) == 0.7
        assert node._calculate_speed_limit(29.0) == 0.4
        assert node._calculate_speed_limit(10.0) == 0.4
        assert node._calculate_speed_limit(9.0) == 0.0

    def test_custom_speed_factors(self, node):
        """Test that custom speed factor parameters are respected."""
        # Modify speed factors
        node.speed_factor_full = 1.0
        node.speed_factor_reduced = 0.5
        node.speed_factor_critical = 0.25
        node.speed_factor_stop = 0.0

        assert node._calculate_speed_limit(100.0) == 1.0
        assert node._calculate_speed_limit(50.0) == 0.5
        assert node._calculate_speed_limit(25.0) == 0.25
        assert node._calculate_speed_limit(10.0) == 0.0


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

    def test_scenario_normal_operation_high_battery(self, node):
        """Scenario: Normal operation with high battery."""
        battery_msg = BatteryState()
        battery_msg.percentage = 0.85  # 85%

        node._on_battery_state(battery_msg)
        speed_limit = node._calculate_speed_limit(node.current_battery_percent)

        assert speed_limit == 1.0

    def test_scenario_moderate_battery_level(self, node):
        """Scenario: Moderate battery level requires reduced speed."""
        battery_msg = BatteryState()
        battery_msg.percentage = 0.4  # 40%

        node._on_battery_state(battery_msg)
        speed_limit = node._calculate_speed_limit(node.current_battery_percent)

        assert speed_limit == 0.7

    def test_scenario_critical_battery_level(self, node):
        """Scenario: Critical battery level requires restricted speed."""
        battery_msg = BatteryState()
        battery_msg.percentage = 0.2  # 20%

        node._on_battery_state(battery_msg)
        speed_limit = node._calculate_speed_limit(node.current_battery_percent)

        assert speed_limit == 0.4

    def test_scenario_near_empty_battery(self, node):
        """Scenario: Battery near empty, speed stopped to preserve charge."""
        battery_msg = BatteryState()
        battery_msg.percentage = 0.08  # 8%

        node._on_battery_state(battery_msg)
        speed_limit = node._calculate_speed_limit(node.current_battery_percent)

        assert speed_limit == 0.0

    def test_scenario_battery_charge_cycle(self, node):
        """Scenario: Battery through complete charge/discharge cycle."""
        battery_percentages = [5, 10, 15, 20, 25, 30, 40, 50, 60, 75, 90, 100]
        expected_factors = [0.0, 0.0, 0.4, 0.4, 0.7, 0.7, 0.7, 1.0, 1.0, 1.0, 1.0, 1.0]

        for percent, expected_factor in zip(battery_percentages, expected_factors):
            speed_limit = node._calculate_speed_limit(float(percent))
            assert speed_limit == expected_factor

    def test_scenario_continuous_battery_update(self, node):
        """Scenario: Continuous battery state updates."""
        for percent in range(100, 0, -5):
            battery_msg = BatteryState()
            battery_msg.percentage = percent / 100.0

            node._on_battery_state(battery_msg)
            speed_limit = node._calculate_speed_limit(node.current_battery_percent)

            # Should always return valid factor
            assert 0.0 <= speed_limit <= 1.0

    def test_scenario_rapid_threshold_crossing(self, node):
        """Scenario: Rapid crossing of speed limit thresholds."""
        thresholds = [100, 50, 49, 25, 24, 15, 14, 0]

        for percent in thresholds:
            speed_limit = node._calculate_speed_limit(float(percent))
            assert 0.0 <= speed_limit <= 1.0

    def test_scenario_hysteresis_behavior(self, node):
        """Test behavior near threshold boundaries."""
        # Test just below and above each threshold
        test_cases = [
            (50.1, 1.0),  # Just above 50% boundary (full speed)
            (49.9, 0.7),  # Just below 50% boundary (reduced)
            (25.1, 0.7),  # Just above 25% boundary (reduced)
            (24.9, 0.4),  # Just below 25% boundary (critical)
            (15.1, 0.4),  # Just above 15% boundary (critical)
            (14.9, 0.0),  # Just below 15% boundary (stop)
        ]

        for percent, expected_factor in test_cases:
            speed_limit = node._calculate_speed_limit(percent)
            assert speed_limit == expected_factor
