"""Unit tests for node_watchdog_node."""

import pytest
import json
import time
from geometry_msgs.msg import Twist
from std_msgs.msg import UInt32, String

import rclpy
from rclpy.time import Time

# Import the node under test
from saltybot_node_watchdog.node_watchdog_node import NodeWatchdogNode


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


@pytest.fixture
def node(rclpy_fixture):
    """Create a watchdog node instance."""
    node = NodeWatchdogNode()
    yield node
    node.destroy_node()


class TestNodeWatchdogNode:
    """Test suite for NodeWatchdogNode."""

    def test_node_initialization(self, node):
        """Test that node initializes with correct defaults."""
        assert node.heartbeat_timeout == 1.0
        assert node.motor_driver_critical_timeout == 2.0
        assert node.last_cmd_vel is None
        assert node.motor_driver_down is False

        # All heartbeat times should be None initially
        for node_name in node.critical_nodes:
            assert node.last_heartbeat_time[node_name] is None

    def test_balance_heartbeat_received(self, node):
        """Test balance node heartbeat recording."""
        msg = UInt32(data=1)
        node._on_balance_heartbeat(msg)
        assert node.last_heartbeat_time["balance"] is not None

    def test_motor_driver_heartbeat_received(self, node):
        """Test motor driver heartbeat recording."""
        msg = UInt32(data=1)
        node._on_motor_driver_heartbeat(msg)
        assert node.last_heartbeat_time["motor_driver"] is not None
        # Motor driver heartbeat should clear the down flag
        node.motor_driver_down = True
        node._on_motor_driver_heartbeat(msg)
        assert node.motor_driver_down is False

    def test_emergency_heartbeat_received(self, node):
        """Test emergency system heartbeat recording."""
        msg = UInt32(data=1)
        node._on_emergency_heartbeat(msg)
        assert node.last_heartbeat_time["emergency"] is not None

    def test_docking_heartbeat_received(self, node):
        """Test docking node heartbeat recording."""
        msg = UInt32(data=1)
        node._on_docking_heartbeat(msg)
        assert node.last_heartbeat_time["docking"] is not None

    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_health_check_all_unknown(self, node):
        """Test health check when no heartbeats received."""
        health = node._check_node_health()

        assert len(health) == 4
        for node_name in node.critical_nodes:
            assert health[node_name]["status"] == "unknown"
            assert health[node_name]["elapsed_s"] is None
            assert health[node_name]["timeout_s"] == 1.0

    def test_health_check_just_received(self, node):
        """Test health check just after heartbeat received."""
        # Record a heartbeat for balance node
        node.last_heartbeat_time["balance"] = node.get_clock().now()

        health = node._check_node_health()

        # Balance should be up, others unknown
        assert health["balance"]["status"] == "up"
        assert health["balance"]["elapsed_s"] < 0.1
        assert health["emergency"]["status"] == "unknown"

    def test_health_check_timeout_general(self, node):
        """Test that heartbeat timeout is detected (>1s)."""
        # Simulate a heartbeat that arrived >1s ago
        now = node.get_clock().now()
        old_time = Time(
            nanoseconds=now.nanoseconds - int(1.5 * 1e9)
        )  # 1.5 seconds ago
        node.last_heartbeat_time["balance"] = old_time

        health = node._check_node_health()

        assert health["balance"]["status"] == "down"
        assert health["balance"]["elapsed_s"] > 1.4
        assert health["balance"]["elapsed_s"] < 2.0

    def test_health_check_motor_driver_critical(self, node):
        """Test motor driver critical timeout (>2s)."""
        # Simulate motor driver heartbeat >2s ago
        now = node.get_clock().now()
        old_time = Time(nanoseconds=now.nanoseconds - int(2.5 * 1e9))  # 2.5 seconds
        node.last_heartbeat_time["motor_driver"] = old_time

        health = node._check_node_health()

        motor_health = health["motor_driver"]
        assert motor_health["status"] == "down"
        assert motor_health.get("critical", False) is True
        assert motor_health["elapsed_s"] > 2.4

    def test_safety_check_normal_operation(self, node):
        """Test safety check passes through cmd_vel normally."""
        node.motor_driver_down = False
        cmd = Twist()
        cmd.linear.x = 1.5
        cmd.angular.z = 0.3

        safe_cmd = node._apply_safety_checks(cmd)

        assert abs(safe_cmd.linear.x - 1.5) < 1e-6
        assert abs(safe_cmd.angular.z - 0.3) < 1e-6

    def test_safety_check_motor_driver_down(self, node):
        """Test safety check zeros cmd_vel when motor driver is down."""
        node.motor_driver_down = True
        cmd = Twist()
        cmd.linear.x = 1.5
        cmd.linear.y = 0.2
        cmd.angular.z = 0.3

        safe_cmd = node._apply_safety_checks(cmd)

        # All velocities should be zero
        assert safe_cmd.linear.x == 0.0
        assert safe_cmd.linear.y == 0.0
        assert safe_cmd.linear.z == 0.0
        assert safe_cmd.angular.x == 0.0
        assert safe_cmd.angular.y == 0.0
        assert safe_cmd.angular.z == 0.0

    def test_timer_callback_publishes(self, node):
        """Test that timer callback publishes watchdog status."""
        # Record a heartbeat
        node.last_heartbeat_time["balance"] = node.get_clock().now()
        node.last_cmd_vel = Twist()
        node.last_cmd_vel.linear.x = 1.0

        # Call timer callback
        node._timer_callback()
        # Just verify it doesn't crash; actual publishing is tested via integration

    def test_watchdog_status_json_all_healthy(self, node):
        """Test watchdog status JSON when all nodes healthy."""
        # Record all heartbeats
        now = node.get_clock().now()
        for node_name in node.critical_nodes:
            node.last_heartbeat_time[node_name] = now

        health = node._check_node_health()
        watchdog_status = {
            "timestamp": now.nanoseconds / 1e9,
            "all_healthy": all(
                s["status"] == "up" for s in health.values()
            ),
            "health": health,
            "motor_driver_critical": False,
        }

        # Verify it's valid JSON
        json_str = json.dumps(watchdog_status)
        parsed = json.loads(json_str)

        assert parsed["all_healthy"] is True
        assert parsed["motor_driver_critical"] is False

    def test_watchdog_status_json_with_timeout(self, node):
        """Test watchdog status JSON when node has timed out."""
        # Balance heartbeat >1s ago
        now = node.get_clock().now()
        old_time = Time(nanoseconds=now.nanoseconds - int(1.5 * 1e9))
        node.last_heartbeat_time["balance"] = old_time

        # Others are current
        for name in ["motor_driver", "emergency", "docking"]:
            node.last_heartbeat_time[name] = now

        health = node._check_node_health()
        watchdog_status = {
            "timestamp": now.nanoseconds / 1e9,
            "all_healthy": all(s["status"] == "up" for s in health.values()),
            "health": health,
            "motor_driver_critical": False,
        }

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

        assert parsed["all_healthy"] is False
        assert parsed["health"]["balance"]["status"] == "down"


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

    def test_scenario_all_nodes_healthy(self, node):
        """Scenario: all critical nodes sending heartbeats."""
        now = node.get_clock().now()

        # All nodes sending heartbeats
        for name in node.critical_nodes:
            node.last_heartbeat_time[name] = now

        health = node._check_node_health()

        all_up = all(h["status"] == "up" for h in health.values())
        assert all_up is True

    def test_scenario_motor_driver_loss_below_critical(self, node):
        """Scenario: motor driver offline 1.5s (below 2s critical)."""
        now = node.get_clock().now()
        old_time = Time(nanoseconds=now.nanoseconds - int(1.5 * 1e9))

        # Motor driver down 1.5s, others healthy
        node.last_heartbeat_time["motor_driver"] = old_time
        for name in ["balance", "emergency", "docking"]:
            node.last_heartbeat_time[name] = now

        health = node._check_node_health()
        motor = health["motor_driver"]

        assert motor["status"] == "down"
        assert motor.get("critical", False) is False
        # Safety fallback should NOT be triggered yet
        assert node.motor_driver_down is False

    def test_scenario_motor_driver_critical_loss(self, node):
        """Scenario: motor driver offline >2s (triggers critical)."""
        now = node.get_clock().now()
        old_time = Time(nanoseconds=now.nanoseconds - int(2.5 * 1e9))

        node.last_heartbeat_time["motor_driver"] = old_time
        node.last_heartbeat_time["balance"] = now
        node.last_heartbeat_time["emergency"] = now
        node.last_heartbeat_time["docking"] = now

        health = node._check_node_health()
        motor = health["motor_driver"]

        assert motor["status"] == "down"
        assert motor.get("critical", False) is True

    def test_scenario_cascading_node_failures(self, node):
        """Scenario: multiple nodes failing in sequence."""
        now = node.get_clock().now()
        old1s = Time(nanoseconds=now.nanoseconds - int(1.2 * 1e9))
        old2s = Time(nanoseconds=now.nanoseconds - int(2.5 * 1e9))

        # Balance down 1.2s, motor driver down 2.5s, others healthy
        node.last_heartbeat_time["balance"] = old1s
        node.last_heartbeat_time["motor_driver"] = old2s
        node.last_heartbeat_time["emergency"] = now
        node.last_heartbeat_time["docking"] = now

        health = node._check_node_health()

        assert health["balance"]["status"] == "down"
        assert health["balance"].get("critical", False) is False
        assert health["motor_driver"]["status"] == "down"
        assert health["motor_driver"].get("critical", False) is True

    def test_scenario_cmd_vel_safety_fallback(self, node):
        """Scenario: motor driver down triggers safety zeroing of cmd_vel."""
        # Motor driver is critically down
        node.motor_driver_down = True

        cmd = Twist()
        cmd.linear.x = 2.0
        cmd.angular.z = 0.5

        safe_cmd = node._apply_safety_checks(cmd)

        # All should be zeroed
        assert safe_cmd.linear.x == 0.0
        assert safe_cmd.linear.y == 0.0
        assert safe_cmd.linear.z == 0.0
        assert safe_cmd.angular.x == 0.0
        assert safe_cmd.angular.y == 0.0
        assert safe_cmd.angular.z == 0.0

    def test_scenario_motor_driver_recovery(self, node):
        """Scenario: motor driver comes back online after being down."""
        now = node.get_clock().now()

        # Motor driver was down
        node.motor_driver_down = True

        # Motor driver sends heartbeat
        node._on_motor_driver_heartbeat(UInt32(data=1))

        # Should clear the down flag
        assert node.motor_driver_down is False

        # cmd_vel should pass through
        cmd = Twist()
        cmd.linear.x = 1.0
        safe_cmd = node._apply_safety_checks(cmd)
        assert safe_cmd.linear.x == 1.0
