"""Unit tests for cmd_vel_mux_node."""

import pytest
import json
from geometry_msgs.msg import Twist
from std_msgs.msg import String

import rclpy
from rclpy.time import Time

# Import the node and classes under test
from saltybot_cmd_vel_mux.cmd_vel_mux_node import (
    CmdVelMuxNode,
    CmdSource,
)


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


@pytest.fixture
def node(rclpy_fixture):
    """Create a cmd_vel mux node instance."""
    node = CmdVelMuxNode()
    yield node
    node.destroy_node()


class TestCmdSource:
    """Test suite for CmdSource enum."""

    def test_cmd_source_values(self):
        """Test CmdSource enum values."""
        assert CmdSource.TELEOP.value == 1
        assert CmdSource.NAV2.value == 2
        assert CmdSource.DOCKING.value == 3
        assert CmdSource.NONE.value == 0

    def test_cmd_source_names(self):
        """Test CmdSource enum names."""
        assert CmdSource.TELEOP.name == "TELEOP"
        assert CmdSource.NAV2.name == "NAV2"
        assert CmdSource.DOCKING.name == "DOCKING"
        assert CmdSource.NONE.name == "NONE"


class TestCmdVelMuxNode:
    """Test suite for CmdVelMuxNode."""

    def test_node_initialization(self, node):
        """Test that node initializes with correct defaults."""
        assert node.source_timeout == 0.5
        assert node.active_source == CmdSource.NONE

        for source in [CmdSource.TELEOP, CmdSource.NAV2, CmdSource.DOCKING]:
            assert node.last_update[source] is None
            assert node.last_cmd[source] is None

    def test_teleop_subscription(self, node):
        """Test teleop command subscription."""
        cmd = Twist()
        cmd.linear.x = 1.0
        cmd.angular.z = 0.5

        node._on_cmd_vel_teleop(cmd)

        assert node.last_update[CmdSource.TELEOP] is not None
        assert node.last_cmd[CmdSource.TELEOP] is not None
        assert node.last_cmd[CmdSource.TELEOP].linear.x == 1.0

    def test_nav2_subscription(self, node):
        """Test Nav2 command subscription."""
        cmd = Twist()
        cmd.linear.x = 0.5
        cmd.angular.z = 0.2

        node._on_cmd_vel_nav2(cmd)

        assert node.last_update[CmdSource.NAV2] is not None
        assert node.last_cmd[CmdSource.NAV2].linear.x == 0.5

    def test_docking_subscription(self, node):
        """Test docking command subscription."""
        cmd = Twist()
        cmd.linear.x = 0.2

        node._on_cmd_vel_docking(cmd)

        assert node.last_update[CmdSource.DOCKING] is not None
        assert node.last_cmd[CmdSource.DOCKING].linear.x == 0.2

    def test_no_active_source(self, node):
        """Test with no active sources."""
        node._timer_callback()

        assert node.active_source == CmdSource.NONE

    def test_teleop_only(self, node):
        """Test teleop as only active source."""
        teleop_cmd = Twist()
        teleop_cmd.linear.x = 1.5

        node._on_cmd_vel_teleop(teleop_cmd)
        node._timer_callback()

        assert node.active_source == CmdSource.TELEOP

    def test_priority_teleop_over_nav2(self, node):
        """Test teleop has priority over Nav2."""
        teleop_cmd = Twist()
        teleop_cmd.linear.x = 1.0

        nav2_cmd = Twist()
        nav2_cmd.linear.x = 0.5

        node._on_cmd_vel_teleop(teleop_cmd)
        node._on_cmd_vel_nav2(nav2_cmd)
        node._timer_callback()

        assert node.active_source == CmdSource.TELEOP

    def test_priority_nav2_over_docking(self, node):
        """Test Nav2 has priority over docking."""
        nav2_cmd = Twist()
        nav2_cmd.linear.x = 0.5

        docking_cmd = Twist()
        docking_cmd.linear.x = 0.2

        node._on_cmd_vel_nav2(nav2_cmd)
        node._on_cmd_vel_docking(docking_cmd)
        node._timer_callback()

        assert node.active_source == CmdSource.NAV2

    def test_fallback_to_lower_priority(self, node):
        """Test fallback to lower priority when higher priority times out."""
        nav2_cmd = Twist()
        nav2_cmd.linear.x = 0.5

        docking_cmd = Twist()
        docking_cmd.linear.x = 0.2

        # Update both sources
        node._on_cmd_vel_nav2(nav2_cmd)
        node._on_cmd_vel_docking(docking_cmd)

        # Both should be active
        node._timer_callback()
        assert node.active_source == CmdSource.NAV2

        # Simulate Nav2 timeout (>0.5s)
        now = node.get_clock().now()
        old_time = Time(nanoseconds=now.nanoseconds - int(0.6 * 1e9))
        node.last_update[CmdSource.NAV2] = old_time

        # Should fall back to docking
        node._timer_callback()
        assert node.active_source == CmdSource.DOCKING

    def test_source_timeout_detection(self, node):
        """Test that sources timeout after 0.5s."""
        cmd = Twist()
        cmd.linear.x = 1.0

        # Just updated teleop
        node._on_cmd_vel_teleop(cmd)

        # Immediately, it should be active
        node._timer_callback()
        assert node.active_source == CmdSource.TELEOP

        # Simulate time passage: >0.5s
        now = node.get_clock().now()
        old_time = Time(nanoseconds=now.nanoseconds - int(0.6 * 1e9))
        node.last_update[CmdSource.TELEOP] = old_time

        # Should timeout and become inactive
        node._timer_callback()
        assert node.active_source == CmdSource.NONE

    def test_all_three_sources_active(self, node):
        """Test with all three sources active - teleop should win."""
        teleop_cmd = Twist()
        teleop_cmd.linear.x = 1.0

        nav2_cmd = Twist()
        nav2_cmd.linear.x = 0.5

        docking_cmd = Twist()
        docking_cmd.linear.x = 0.2

        node._on_cmd_vel_teleop(teleop_cmd)
        node._on_cmd_vel_nav2(nav2_cmd)
        node._on_cmd_vel_docking(docking_cmd)

        node._timer_callback()

        assert node.active_source == CmdSource.TELEOP

    def test_mux_status_json_format(self, node):
        """Test multiplexer status JSON format."""
        cmd = Twist()
        cmd.linear.x = 1.0

        node._on_cmd_vel_teleop(cmd)
        node._timer_callback()

        now = node.get_clock().now()

        # Manually create status as node would
        sources_status = {}
        for source in [CmdSource.TELEOP, CmdSource.NAV2, CmdSource.DOCKING]:
            last_time = node.last_update[source]

            if last_time is not None:
                elapsed = (now - last_time).nanoseconds / 1e9
                is_active = elapsed <= node.source_timeout
            else:
                elapsed = None
                is_active = False

            sources_status[source.name] = {
                "priority": source.value,
                "active": is_active,
                "elapsed_s": elapsed,
            }

        status = {
            "active_source": node.active_source.name,
            "active_priority": node.active_source.value,
            "sources": sources_status,
        }

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

        assert parsed["active_source"] == "TELEOP"
        assert parsed["active_priority"] == 1
        assert "TELEOP" in parsed["sources"]
        assert parsed["sources"]["TELEOP"]["priority"] == 1


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

    def test_scenario_teleop_control(self, node):
        """Scenario: user teleop control only."""
        cmd = Twist()
        cmd.linear.x = 1.5
        cmd.angular.z = 0.3

        node._on_cmd_vel_teleop(cmd)
        node._timer_callback()

        assert node.active_source == CmdSource.TELEOP

    def test_scenario_nav2_autonomous(self, node):
        """Scenario: Nav2 autonomous navigation."""
        nav2_cmd = Twist()
        nav2_cmd.linear.x = 0.5
        nav2_cmd.angular.z = 0.1

        node._on_cmd_vel_nav2(nav2_cmd)
        node._timer_callback()

        assert node.active_source == CmdSource.NAV2

    def test_scenario_docking_sequence(self, node):
        """Scenario: docking operation with no higher priority."""
        docking_cmd = Twist()
        docking_cmd.linear.x = 0.1

        node._on_cmd_vel_docking(docking_cmd)
        node._timer_callback()

        assert node.active_source == CmdSource.DOCKING

    def test_scenario_teleop_preempts_nav2(self, node):
        """Scenario: user teleop preempts Nav2 navigation."""
        # Nav2 is running
        nav2_cmd = Twist()
        nav2_cmd.linear.x = 0.5

        node._on_cmd_vel_nav2(nav2_cmd)
        node._timer_callback()
        assert node.active_source == CmdSource.NAV2

        # User takes teleop control
        teleop_cmd = Twist()
        teleop_cmd.linear.x = 1.0

        node._on_cmd_vel_teleop(teleop_cmd)
        node._timer_callback()
        assert node.active_source == CmdSource.TELEOP

    def test_scenario_teleop_release_back_to_nav2(self, node):
        """Scenario: user releases teleop, Nav2 resumes."""
        # Both sources active
        nav2_cmd = Twist()
        nav2_cmd.linear.x = 0.5
        teleop_cmd = Twist()
        teleop_cmd.linear.x = 1.0

        node._on_cmd_vel_nav2(nav2_cmd)
        node._on_cmd_vel_teleop(teleop_cmd)
        node._timer_callback()

        # Teleop is active
        assert node.active_source == CmdSource.TELEOP

        # Simulate teleop timeout
        now = node.get_clock().now()
        old_time = Time(nanoseconds=now.nanoseconds - int(0.6 * 1e9))
        node.last_update[CmdSource.TELEOP] = old_time

        node._timer_callback()

        # Nav2 should take over
        assert node.active_source == CmdSource.NAV2

    def test_scenario_timeout_all_sources(self, node):
        """Scenario: all sources timeout, publish zero command."""
        # Update all sources
        cmd = Twist()
        cmd.linear.x = 1.0

        node._on_cmd_vel_teleop(cmd)
        node._on_cmd_vel_nav2(cmd)
        node._on_cmd_vel_docking(cmd)

        # Simulate timeout for all
        now = node.get_clock().now()
        old_time = Time(nanoseconds=now.nanoseconds - int(1.0 * 1e9))

        node.last_update[CmdSource.TELEOP] = old_time
        node.last_update[CmdSource.NAV2] = old_time
        node.last_update[CmdSource.DOCKING] = old_time

        node._timer_callback()

        assert node.active_source == CmdSource.NONE

    def test_scenario_docking_to_nav2_to_teleop(self, node):
        """Scenario: priority escalation through modes."""
        # Start with docking
        docking_cmd = Twist()
        docking_cmd.linear.x = 0.1

        node._on_cmd_vel_docking(docking_cmd)
        node._timer_callback()
        assert node.active_source == CmdSource.DOCKING

        # Nav2 starts
        nav2_cmd = Twist()
        nav2_cmd.linear.x = 0.5

        node._on_cmd_vel_nav2(nav2_cmd)
        node._timer_callback()
        assert node.active_source == CmdSource.NAV2

        # User takes control
        teleop_cmd = Twist()
        teleop_cmd.linear.x = 1.0

        node._on_cmd_vel_teleop(teleop_cmd)
        node._timer_callback()
        assert node.active_source == CmdSource.TELEOP
