"""Unit tests for VESC CAN health monitor — recovery FSM.

No ROS2, python-can, or hardware required.  Tests cover:
  - VescMonitor state machine transitions
  - Timeout → DEGRADED → ESTOP escalation
  - Frame-received recovery from DEGRADED and ESTOP
  - Bus-off override and recovery
  - Alive retry interval in DEGRADED
  - HealthFsm dual-VESC wrapper
  - Edge cases (never seen, exact boundary, bus_ok when not bus_off)
"""

import pytest

from saltybot_vesc_health.recovery_fsm import (
    Action,
    HealthFsm,
    VescHealthState,
    VescMonitor,
)

# ---------------------------------------------------------------------------
# Helpers
# ---------------------------------------------------------------------------

T0 = 1_000.0  # arbitrary monotonic baseline (seconds)


def fresh_mon(**kwargs) -> VescMonitor:
    """Return a VescMonitor that has never seen a frame."""
    return VescMonitor(can_id=61, **kwargs)


def alive_mon(last_ts: float = T0, **kwargs) -> VescMonitor:
    """Return a VescMonitor whose last frame was at last_ts."""
    mon = VescMonitor(can_id=61, **kwargs)
    mon.last_frame_ts = last_ts
    return mon


# ---------------------------------------------------------------------------
# VescMonitor — initial state
# ---------------------------------------------------------------------------

class TestVescMonitorInit:
    def test_starts_healthy(self):
        assert fresh_mon().state == VescHealthState.HEALTHY

    def test_never_received_is_unhealthy_after_timeout(self):
        mon = fresh_mon(timeout_s=0.5)
        # First tick records startup time; second tick 600ms later sees timeout
        mon.tick(T0)                        # startup: elapsed=0, still healthy
        actions = mon.tick(T0 + 0.6)       # 600 ms > timeout → DEGRADED
        assert Action.SEND_ALIVE in actions
        assert Action.LOG_WARN in actions
        assert mon.state == VescHealthState.DEGRADED

    def test_elapsed_never_received(self):
        mon = fresh_mon()
        assert mon.elapsed(T0) == float("inf")


# ---------------------------------------------------------------------------
# VescMonitor — healthy zone
# ---------------------------------------------------------------------------

class TestHealthyZone:
    def test_no_actions_when_fresh_frames_arriving(self):
        mon = alive_mon(last_ts=T0)
        actions = mon.tick(T0 + 0.1)  # 100 ms — well within 500 ms
        assert actions == []
        assert mon.state == VescHealthState.HEALTHY

    def test_no_transition_at_just_under_timeout(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5)
        actions = mon.tick(T0 + 0.499)
        assert actions == []
        assert mon.state == VescHealthState.HEALTHY


# ---------------------------------------------------------------------------
# VescMonitor — timeout → DEGRADED
# ---------------------------------------------------------------------------

class TestTimeoutToDegraded:
    def test_transitions_at_timeout(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5)
        actions = mon.tick(T0 + 0.5)
        assert mon.state == VescHealthState.DEGRADED
        assert Action.SEND_ALIVE in actions
        assert Action.LOG_WARN in actions

    def test_send_alive_not_repeated_immediately(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5, alive_retry_s=0.2)
        mon.tick(T0 + 0.5)   # → DEGRADED, sends alive
        actions = mon.tick(T0 + 0.55)  # only 50 ms later → no retry yet
        assert Action.SEND_ALIVE not in actions

    def test_alive_retry_at_interval(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5, alive_retry_s=0.2)
        mon.tick(T0 + 0.5)             # → DEGRADED, first alive
        actions = mon.tick(T0 + 0.71)  # 210 ms later → retry
        assert Action.SEND_ALIVE in actions


# ---------------------------------------------------------------------------
# VescMonitor — DEGRADED → ESTOP
# ---------------------------------------------------------------------------

class TestDegradedToEstop:
    def test_escalates_at_estop_threshold(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
        mon.tick(T0 + 0.5)   # → DEGRADED
        actions = mon.tick(T0 + 2.0)
        assert mon.state == VescHealthState.ESTOP
        assert Action.TRIGGER_ESTOP in actions
        assert Action.LOG_ERROR in actions

    def test_estop_keeps_asserting(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
        mon.tick(T0 + 0.5)
        mon.tick(T0 + 2.0)   # → ESTOP
        actions1 = mon.tick(T0 + 2.1)
        actions2 = mon.tick(T0 + 3.0)
        assert Action.TRIGGER_ESTOP in actions1
        assert Action.TRIGGER_ESTOP in actions2

    def test_no_duplicate_log_error_while_in_estop(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
        mon.tick(T0 + 0.5)
        actions_at_escalation = mon.tick(T0 + 2.0)   # LOG_ERROR here
        assert Action.LOG_ERROR in actions_at_escalation
        later_actions = mon.tick(T0 + 2.1)
        assert Action.LOG_ERROR not in later_actions   # not repeated


# ---------------------------------------------------------------------------
# VescMonitor — recovery from DEGRADED
# ---------------------------------------------------------------------------

class TestRecoveryFromDegraded:
    def test_on_frame_clears_degraded(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5)
        mon.tick(T0 + 0.5)   # → DEGRADED
        actions = mon.on_frame(T0 + 0.9)
        assert mon.state == VescHealthState.HEALTHY
        assert Action.LOG_RECOVERY in actions

    def test_on_frame_does_not_trigger_clear_estop_from_degraded(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5)
        mon.tick(T0 + 0.5)   # → DEGRADED (not ESTOP)
        actions = mon.on_frame(T0 + 0.9)
        assert Action.CLEAR_ESTOP not in actions


# ---------------------------------------------------------------------------
# VescMonitor — recovery from ESTOP
# ---------------------------------------------------------------------------

class TestRecoveryFromEstop:
    def test_on_frame_clears_estop(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
        mon.tick(T0 + 0.5)
        mon.tick(T0 + 2.0)   # → ESTOP
        actions = mon.on_frame(T0 + 2.5)
        assert mon.state == VescHealthState.HEALTHY
        assert Action.LOG_RECOVERY in actions
        assert Action.CLEAR_ESTOP in actions

    def test_tick_after_recovery_is_clean(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
        mon.tick(T0 + 0.5)
        mon.tick(T0 + 2.0)
        mon.on_frame(T0 + 2.5)
        actions = mon.tick(T0 + 2.6)
        assert actions == []


# ---------------------------------------------------------------------------
# VescMonitor — bus-off
# ---------------------------------------------------------------------------

class TestBusOff:
    def test_bus_off_triggers_estop(self):
        mon = alive_mon(last_ts=T0)
        actions = mon.on_bus_off(T0 + 1.0)
        assert mon.state == VescHealthState.BUS_OFF
        assert Action.TRIGGER_ESTOP in actions
        assert Action.LOG_ERROR in actions

    def test_bus_off_repeated_call_still_asserts_estop(self):
        mon = alive_mon(last_ts=T0)
        mon.on_bus_off(T0)
        actions = mon.on_bus_off(T0 + 0.5)
        assert Action.TRIGGER_ESTOP in actions

    def test_bus_ok_clears_bus_off(self):
        mon = alive_mon(last_ts=T0)
        mon.on_bus_off(T0)
        actions = mon.on_bus_ok(T0 + 1.0)
        assert mon.state == VescHealthState.HEALTHY
        assert Action.CLEAR_ESTOP in actions
        assert Action.LOG_RECOVERY in actions

    def test_bus_ok_when_not_bus_off_is_noop(self):
        mon = alive_mon(last_ts=T0)
        assert mon.on_bus_ok(T0) == []

    def test_bus_off_tick_keeps_asserting_estop(self):
        mon = alive_mon(last_ts=T0)
        mon.on_bus_off(T0)
        actions = mon.tick(T0 + 1.0)
        assert Action.TRIGGER_ESTOP in actions

    def test_on_frame_clears_bus_off(self):
        mon = alive_mon(last_ts=T0)
        mon.on_bus_off(T0)
        actions = mon.on_frame(T0 + 0.5)
        assert mon.state == VescHealthState.HEALTHY
        assert Action.CLEAR_ESTOP in actions


# ---------------------------------------------------------------------------
# VescMonitor — elapsed helper
# ---------------------------------------------------------------------------

class TestElapsed:
    def test_elapsed_after_frame(self):
        mon = alive_mon(last_ts=T0)
        assert mon.elapsed(T0 + 0.3) == pytest.approx(0.3)

    def test_elapsed_never_received(self):
        mon = fresh_mon()
        assert mon.elapsed(T0) == float("inf")


# ---------------------------------------------------------------------------
# VescMonitor — estop_active property
# ---------------------------------------------------------------------------

class TestEstopActive:
    def test_healthy_not_active(self):
        assert not alive_mon().estop_active

    def test_estop_state_is_active(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
        mon.tick(T0 + 0.5)
        mon.tick(T0 + 2.0)
        assert mon.estop_active

    def test_bus_off_is_active(self):
        mon = alive_mon(last_ts=T0)
        mon.on_bus_off(T0)
        assert mon.estop_active

    def test_degraded_not_active(self):
        mon = alive_mon(last_ts=T0, timeout_s=0.5)
        mon.tick(T0 + 0.5)
        assert not mon.estop_active


# ---------------------------------------------------------------------------
# HealthFsm — dual-VESC wrapper
# ---------------------------------------------------------------------------

class TestHealthFsm:
    def _make_fsm(self) -> HealthFsm:
        fsm = HealthFsm(left_id=61, right_id=79, timeout_s=0.5, estop_s=2.0)
        fsm.left.last_frame_ts  = T0
        fsm.right.last_frame_ts = T0
        return fsm

    def test_tick_returns_per_vesc_actions(self):
        fsm = self._make_fsm()
        result = fsm.tick(T0 + 0.1)
        assert 61 in result
        assert 79 in result

    def test_on_frame_updates_left(self):
        fsm = self._make_fsm()
        fsm.left.last_frame_ts = T0 - 1.0  # stale
        fsm.left.state = VescHealthState.DEGRADED
        actions = fsm.on_frame(61, T0 + 0.1)
        assert Action.LOG_RECOVERY in actions
        assert fsm.left.state == VescHealthState.HEALTHY

    def test_on_frame_unknown_id_is_noop(self):
        fsm = self._make_fsm()
        assert fsm.on_frame(99, T0) == []

    def test_any_estop_true_when_one_vesc_estop(self):
        fsm = self._make_fsm()
        fsm.left.last_frame_ts = T0 - 3.0
        fsm.tick(T0)   # should escalate left to ESTOP
        assert fsm.any_estop

    def test_any_estop_false_when_both_healthy(self):
        fsm = self._make_fsm()
        fsm.tick(T0 + 0.1)
        assert not fsm.any_estop

    def test_bus_off_propagates_to_both(self):
        fsm = self._make_fsm()
        actions = fsm.on_bus_off(T0)
        assert fsm.left.state  == VescHealthState.BUS_OFF
        assert fsm.right.state == VescHealthState.BUS_OFF
        assert any(a == Action.TRIGGER_ESTOP for a in actions)

    def test_bus_ok_clears_both(self):
        fsm = self._make_fsm()
        fsm.on_bus_off(T0)
        actions = fsm.on_bus_ok(T0 + 1.0)
        assert fsm.left.state  == VescHealthState.HEALTHY
        assert fsm.right.state == VescHealthState.HEALTHY
        assert any(a == Action.CLEAR_ESTOP for a in actions)

    def test_bus_ok_when_not_bus_off_is_noop(self):
        fsm = self._make_fsm()
        assert fsm.on_bus_ok(T0) == []

    def test_left_timeout_right_healthy(self):
        fsm = self._make_fsm()
        fsm.left.last_frame_ts = T0 - 1.0   # stale
        result = fsm.tick(T0)
        assert Action.SEND_ALIVE in result[61]
        assert result[79] == []


# ---------------------------------------------------------------------------
# _make_get_values_request (inline copy in node module)
# ---------------------------------------------------------------------------

class TestMakeGetValuesRequest:
    def test_arb_id_encodes_target(self):
        from saltybot_vesc_health.health_monitor_node import _make_get_values_request
        arb_id, _ = _make_get_values_request(127, 61)
        assert (arb_id & 0xFF) == 61

    def test_payload_has_sender_and_comm_id(self):
        from saltybot_vesc_health.health_monitor_node import (
            _make_get_values_request,
            _COMM_GET_VALUES,
        )
        _, payload = _make_get_values_request(42, 61)
        assert payload[0] == 42
        assert payload[2] == _COMM_GET_VALUES

    def test_arb_id_is_extended(self):
        from saltybot_vesc_health.health_monitor_node import _make_get_values_request
        arb_id, _ = _make_get_values_request(127, 61)
        assert arb_id > 0x7FF


if __name__ == "__main__":
    pytest.main([__file__, "-v"])
