"""Unit tests for speed_limiter_node."""

import pytest
import json
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32, String
from saltybot_emergency_msgs.msg import EmergencyEvent

import rclpy
from rclpy.executors import SingleThreadedExecutor

# Import the node under test
from saltybot_speed_limiter.speed_limiter_node import SpeedLimiterNode


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


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


class TestSpeedLimiterNode:
    """Test suite for SpeedLimiterNode."""

    def test_node_initialization(self, node):
        """Test that node initializes with correct defaults."""
        assert node.terrain_scale == 1.0
        assert node.battery_limit == 1.0
        assert node.emergency_scale == 1.0
        assert node.last_cmd_vel is None

    def test_terrain_scale_subscription(self, node):
        """Test terrain scale value update."""
        msg = Float32(data=0.5)
        node._on_terrain_scale(msg)
        assert node.terrain_scale == 0.5

    def test_terrain_scale_clamping(self, node):
        """Test that terrain scale is clamped to [0, 1]."""
        node._on_terrain_scale(Float32(data=1.5))
        assert node.terrain_scale == 1.0

        node._on_terrain_scale(Float32(data=-0.5))
        assert node.terrain_scale == 0.0

    def test_battery_limit_subscription(self, node):
        """Test battery limit value update."""
        msg = Float32(data=0.7)
        node._on_battery_limit(msg)
        assert node.battery_limit == 0.7

    def test_battery_limit_clamping(self, node):
        """Test that battery limit is clamped to [0, 1]."""
        node._on_battery_limit(Float32(data=2.0))
        assert node.battery_limit == 1.0

        node._on_battery_limit(Float32(data=-1.0))
        assert node.battery_limit == 0.0

    def test_emergency_nominal_state(self, node):
        """Test emergency scale in NOMINAL state."""
        msg = EmergencyEvent()
        msg.state = "NOMINAL"
        node._on_emergency(msg)
        assert node.emergency_scale == 1.0

    def test_emergency_recovering_state(self, node):
        """Test emergency scale in RECOVERING state."""
        msg = EmergencyEvent()
        msg.state = "RECOVERING"
        node._on_emergency(msg)
        assert node.emergency_scale == 0.3

    def test_emergency_stopping_state(self, node):
        """Test emergency scale in STOPPING state."""
        msg = EmergencyEvent()
        msg.state = "STOPPING"
        node._on_emergency(msg)
        assert node.emergency_scale == 0.1

    def test_emergency_escalated_state(self, node):
        """Test emergency scale in ESCALATED state (complete stop)."""
        msg = EmergencyEvent()
        msg.state = "ESCALATED"
        node._on_emergency(msg)
        assert node.emergency_scale == 0.0

    def test_cmd_vel_caching(self, node):
        """Test that cmd_vel messages are cached."""
        msg = Twist()
        msg.linear.x = 1.0
        node._on_cmd_vel(msg)
        assert node.last_cmd_vel is not None
        assert node.last_cmd_vel.linear.x == 1.0

    def test_scale_twist_zero(self, node):
        """Test scaling a twist by zero."""
        msg = Twist()
        msg.linear.x = 1.0
        msg.angular.z = 0.5
        result = node._scale_twist(msg, 0.0)
        assert result.linear.x == 0.0
        assert result.angular.z == 0.0

    def test_scale_twist_half(self, node):
        """Test scaling a twist by 0.5."""
        msg = Twist()
        msg.linear.x = 2.0
        msg.linear.y = 4.0
        msg.angular.z = 1.0
        result = node._scale_twist(msg, 0.5)
        assert abs(result.linear.x - 1.0) < 1e-6
        assert abs(result.linear.y - 2.0) < 1e-6
        assert abs(result.angular.z - 0.5) < 1e-6

    def test_scale_twist_full(self, node):
        """Test scaling a twist by 1.0 (no change)."""
        msg = Twist()
        msg.linear.x = 1.5
        msg.angular.z = 0.8
        result = node._scale_twist(msg, 1.0)
        assert abs(result.linear.x - 1.5) < 1e-6
        assert abs(result.angular.z - 0.8) < 1e-6

    def test_scale_twist_all_components(self, node):
        """Test that all components of twist are scaled."""
        msg = Twist()
        msg.linear.x = 1.0
        msg.linear.y = 2.0
        msg.linear.z = 3.0
        msg.angular.x = 0.1
        msg.angular.y = 0.2
        msg.angular.z = 0.3
        result = node._scale_twist(msg, 0.5)
        assert abs(result.linear.x - 0.5) < 1e-6
        assert abs(result.linear.y - 1.0) < 1e-6
        assert abs(result.linear.z - 1.5) < 1e-6
        assert abs(result.angular.x - 0.05) < 1e-6
        assert abs(result.angular.y - 0.1) < 1e-6
        assert abs(result.angular.z - 0.15) < 1e-6

    def test_min_scale_factor_all_one(self, node):
        """Test min-of-three logic with all scales = 1.0."""
        node.terrain_scale = 1.0
        node.battery_limit = 1.0
        node.emergency_scale = 1.0
        # This would be tested via _timer_callback, but we'll test the logic directly
        scale = min(node.terrain_scale, node.battery_limit, node.emergency_scale)
        assert scale == 1.0

    def test_min_scale_factor_terrain_limiting(self, node):
        """Test that terrain scale limits when it's the minimum."""
        node.terrain_scale = 0.3
        node.battery_limit = 0.8
        node.emergency_scale = 0.9
        scale = min(node.terrain_scale, node.battery_limit, node.emergency_scale)
        assert scale == 0.3

    def test_min_scale_factor_battery_limiting(self, node):
        """Test that battery limit constrains when it's the minimum."""
        node.terrain_scale = 0.9
        node.battery_limit = 0.2
        node.emergency_scale = 0.8
        scale = min(node.terrain_scale, node.battery_limit, node.emergency_scale)
        assert scale == 0.2

    def test_min_scale_factor_emergency_limiting(self, node):
        """Test that emergency scale limits when it's the minimum."""
        node.terrain_scale = 0.9
        node.battery_limit = 0.8
        node.emergency_scale = 0.0
        scale = min(node.terrain_scale, node.battery_limit, node.emergency_scale)
        assert scale == 0.0

    def test_timer_callback_no_cmd_vel(self, node):
        """Test timer callback with no cmd_vel received."""
        # Should not crash or publish if last_cmd_vel is None
        node.last_cmd_vel = None
        node._timer_callback()  # Should return early

    def test_info_json_structure(self, node):
        """Test that published info has correct JSON structure."""
        node.last_cmd_vel = Twist()
        node.last_cmd_vel.linear.x = 1.0
        node.terrain_scale = 0.5
        node.battery_limit = 0.8
        node.emergency_scale = 1.0

        # Manually create info dict as the node would
        scale_factor = min(0.5, 0.8, 1.0)
        limited = node._scale_twist(node.last_cmd_vel, scale_factor)

        info = {
            "timestamp": node.get_clock().now().nanoseconds / 1e9,
            "scale_factor": float(scale_factor),
            "terrain_scale": float(0.5),
            "battery_limit": float(0.8),
            "emergency_scale": float(1.0),
            "linear_x": float(limited.linear.x),
            "linear_y": float(limited.linear.y),
            "linear_z": float(limited.linear.z),
            "angular_x": float(limited.angular.x),
            "angular_y": float(limited.angular.y),
            "angular_z": float(limited.angular.z),
        }

        # Verify it can be serialized/deserialized
        json_str = json.dumps(info)
        parsed = json.loads(json_str)

        assert parsed["scale_factor"] == 0.5
        assert parsed["terrain_scale"] == 0.5
        assert parsed["battery_limit"] == 0.8
        assert parsed["emergency_scale"] == 1.0
        assert abs(parsed["linear_x"] - 0.5) < 1e-6


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

    def test_scenario_rocky_terrain_low_battery(self, node):
        """Scenario: rocky terrain with low battery."""
        # Rocky terrain severely limits speed
        node._on_terrain_scale(Float32(data=0.2))
        # Battery is critical
        node._on_battery_limit(Float32(data=0.3))
        # No emergency
        msg = EmergencyEvent()
        msg.state = "NOMINAL"
        node._on_emergency(msg)

        # Scale should be min(0.2, 0.3, 1.0) = 0.2
        scale = min(node.terrain_scale, node.battery_limit, node.emergency_scale)
        assert scale == 0.2

    def test_scenario_emergency_stop(self, node):
        """Scenario: emergency escalation triggers immediate stop."""
        # Nominal terrain and battery levels
        node._on_terrain_scale(Float32(data=1.0))
        node._on_battery_limit(Float32(data=1.0))
        # Emergency escalation
        msg = EmergencyEvent()
        msg.state = "ESCALATED"
        node._on_emergency(msg)

        # Scale should be 0.0 due to emergency override
        scale = min(node.terrain_scale, node.battery_limit, node.emergency_scale)
        assert scale == 0.0

    def test_scenario_smooth_terrain_good_battery(self, node):
        """Scenario: ideal conditions."""
        node._on_terrain_scale(Float32(data=1.0))
        node._on_battery_limit(Float32(data=0.9))
        msg = EmergencyEvent()
        msg.state = "NOMINAL"
        node._on_emergency(msg)

        scale = min(node.terrain_scale, node.battery_limit, node.emergency_scale)
        assert scale == 0.9
