from roboticks.fault_injection import (
    drop_messages, delay_messages, kill_node, corrupt_topic,
)

import pytest
from roboticks import confirms, deadline
from roboticks.fault_injection import drop_messages
from roboticks.assertions import assert_topic_published
from std_msgs.msg import Bool

@confirms("REQ-014", "REQ-015")
@deadline(milliseconds=500)
def test_estop_engages_on_heartbeat_loss(ros_context, robot_node):
    with drop_messages("/supervisor/heartbeat", rate=1.0):  # 100% drop
        engaged = assert_topic_published(
            topic="/safety/estop_engaged",
            msg_type=Bool,
            within=0.3,
            predicate=lambda m: m.data is True,
        )
    assert engaged.data

from roboticks import confirms, requires_sim
from roboticks.fault_injection import delay_messages
from roboticks.assertions import assert_topic_published
from geometry_msgs.msg import Twist

@confirms("REQ-061")
@requires_sim("gazebo")
def test_nav_degrades_gracefully_with_lidar_lag(ros_context, nav_stack):
    with delay_messages("/scan", ms=400):
        msg = assert_topic_published("/cmd_vel", Twist, within=10.0)
        # Robot should slow down, not crash, not stop entirely
        assert 0.0 < msg.linear.x < 0.3

from roboticks import confirms
from roboticks.fault_injection import kill_node
from roboticks.assertions import assert_topic_published
from lifecycle_msgs.msg import TransitionEvent

@confirms("REQ-098")
def test_planner_respawned_by_watchdog(ros_context):
    with kill_node("nav2_planner"):
        # Watchdog should observe the death and republish a transition
        evt = assert_topic_published(
            "/nav2_planner/transition_event",
            TransitionEvent,
            within=5.0,
            predicate=lambda m: m.transition.label == "configure",
        )
        assert evt is not None

from roboticks import confirms
from roboticks.fault_injection import corrupt_topic
from sensor_msgs.msg import LaserScan

def clamp_high(msg: LaserScan) -> LaserScan:
    msg.ranges = [float("inf")] * len(msg.ranges)
    return msg

@confirms("REQ-072")
def test_planner_ignores_inf_only_scans(ros_context, nav_stack):
    with corrupt_topic("/scan", mutator=clamp_high):
        # Planner should not emit cmd_vel when LIDAR is all-infinity
        with pytest.raises(TimeoutError):
            assert_topic_published("/cmd_vel", Twist, within=2.0)

with delay_messages("/scan", ms=200), drop_messages("/odom", rate=0.1):
    ...