cmvr-es/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp

#include "manager/safety_manager/include/safety_manager.h"

#include <atomic>
#include <chrono>
#include <memory>
#include <string>

#include <gtest/gtest.h>

namespace cmvr::safety {
namespace {

DeviceSafetyDescriptor controlDescriptor(std::string device_id = "arm")
{
    DeviceSafetyDescriptor descriptor;
    descriptor.device_id = std::move(device_id);
    descriptor.kind = device::DeviceKind::Arm;
    descriptor.default_policy = SafetyPolicyFamily::Control;
    descriptor.maximum_snapshot_age = std::chrono::seconds(1);
    descriptor.requires_safe_stop = true;
    descriptor.supports_active_refresh = true;
    return descriptor;
}

class FakeEndpoint final : public DeviceSafetyEndpoint {
public:
    explicit FakeEndpoint(DeviceSafetyDescriptor descriptor)
        : descriptor_(std::move(descriptor))
    {
    }

    DeviceSafetyDescriptor descriptor() const override
    {
        return descriptor_;
    }

    void bindPublisher(SafetySnapshotPublisher publisher) override
    {
        publisher_ = std::move(publisher);
    }

    void requestSafetyRefresh() noexcept override
    {
        if (!publisher_ || !publish_on_refresh) {
            return;
        }
        DeviceSafetySnapshot snapshot;
        snapshot.device_id = descriptor_.device_id;
        snapshot.condition = condition;
        snapshot.device_generation = generation;
        snapshot.sample_sequence = ++sequence;
        snapshot.observed_at = SafetyClock::now();
        snapshot.connected = connected;
        snapshot.operational_ready = ready;
        snapshot.quiescent = quiescent;
        snapshot.motion_active =
            quiescent == TriState::True ? TriState::False : TriState::Unknown;
        snapshot.emergency_stop_active = emergency_stop;
        snapshot.protective_stop_active = protective_stop;
        snapshot.fault_active = fault;
        (void)publisher_(std::move(snapshot));
    }

    HardwareCheckResult validateBeforeDispatch(
        const AdmissionPermit&) override
    {
        ++hardware_checks;
        return final_check;
    }

    RecoveryCheckResult reconcileAdmissionState(
        const RecoveryContext&) override
    {
        ++recoveries;
        return recovery_check;
    }

    RecoveryCheckResult restoreOperationalState(
        const RecoveryContext&) override
    {
        ++operational_restores;
        if (operational_restore_check.reconciled &&
            restore_to_nominal_on_success) {
            condition = SafetyCondition::Nominal;
            connected = TriState::True;
            ready = TriState::True;
            quiescent = TriState::True;
            emergency_stop = TriState::False;
            protective_stop = TriState::False;
            fault = TriState::False;
        }
        return operational_restore_check;
    }

    DeviceSafetyDescriptor descriptor_;
    SafetySnapshotPublisher publisher_;
    SafetyCondition condition{SafetyCondition::Nominal};
    TriState connected{TriState::True};
    TriState ready{TriState::True};
    TriState quiescent{TriState::True};
    TriState emergency_stop{TriState::False};
    TriState protective_stop{TriState::False};
    TriState fault{TriState::False};
    HardwareCheckResult final_check{true, SafetyReason::None, {}};
    RecoveryCheckResult recovery_check{true, SafetyReason::None, {}};
    RecoveryCheckResult operational_restore_check{
        true, SafetyReason::None, {}};
    std::uint64_t generation{1};
    std::uint64_t sequence{0};
    bool publish_on_refresh{true};
    std::atomic<int> hardware_checks{0};
    std::atomic<int> recoveries{0};
    std::atomic<int> operational_restores{0};
    bool restore_to_nominal_on_success{true};
};

class FakeParticipant final : public SafetyParticipant {
public:
    ParticipantDescriptor descriptor() const override
    {
        return {"arm", ParticipantPhase::Actuator, true,
                std::chrono::milliseconds(100)};
    }

    BarrierToken beginBarrier(
        const SafetyOperationContext& context) override
    {
        ++barriers;
        return {"arm", context.operation_id, context.safety_epoch,
                static_cast<std::uint64_t>(barriers.load())};
    }

    ParticipantResult requestQuiesce(
        const BarrierToken&,
        const SafetyOperationContext&) override
    {
        ++stop_requests;
        return stop_result;
    }

    ParticipantResult verifyQuiescent(
        const BarrierToken&,
        const SafetyOperationContext&) override
    {
        ++verifications;
        return verify_result;
    }

    RecoveryCheckResult recoverAdmission(
        const BarrierToken&,
        const RecoveryContext&) override
    {
        ++recoveries;
        return recovery_result;
    }

    ParticipantResult releaseBarrier(
        const BarrierToken&) noexcept override
    {
        ++releases;
        return release_result;
    }

    ParticipantResult stop_result{true, SafetyReason::None, {}};
    ParticipantResult verify_result{true, SafetyReason::None, {}};
    RecoveryCheckResult recovery_result{true, SafetyReason::None, {}};
    ParticipantResult release_result{true, SafetyReason::None, {}};
    std::atomic<int> barriers{0};
    std::atomic<int> stop_requests{0};
    std::atomic<int> verifications{0};
    std::atomic<int> recoveries{0};
    std::atomic<int> releases{0};
};

AdmissionRequest actuateRequest()
{
    AdmissionRequest request;
    request.command = {
        "/cmvr.api.ArmService/moveJ",
        CommandIntent::Actuate,
        SafetyPolicyFamily::Control,
        true,
        false};
    request.device_id = "arm";
    request.command_id = "command-1";
    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
    return request;
}

TEST(SafetyManagerTest, ShadowReportsDenyWithoutChangingLegacyBehavior)
{
    SafetyManager coordinator;
    ASSERT_TRUE(coordinator.registerDevice({controlDescriptor(), {}, {}}));
    coordinator.markStartupComplete();

    const auto result = coordinator.admit(actuateRequest());
    EXPECT_TRUE(result.decision.allowed);
    EXPECT_FALSE(result.decision.policy_allowed);
    EXPECT_FALSE(result.decision.enforced);
    EXPECT_EQ(result.decision.reason, SafetyReason::SafetyStateMissing);
    EXPECT_TRUE(result.permit.has_value());
}

TEST(SafetyManagerTest,
     EnforceSelectedStartupRejectsEmptyOrUnknownCoverage)
{
    SafetyManagerConfig empty_config;
    empty_config.enforcement_mode = EnforcementMode::EnforceSelected;
    SafetyManager empty(empty_config);
    const auto empty_result = empty.validateStartupCoverage(
        SafetyClock::now() + std::chrono::milliseconds(10));
    ASSERT_FALSE(empty_result.ready);
    ASSERT_EQ(empty_result.issues.size(), 1U);
    EXPECT_EQ(empty_result.issues.front().reason, SafetyReason::InvalidArgument);

    SafetyManagerConfig missing_config;
    missing_config.enforcement_mode = EnforcementMode::EnforceSelected;
    missing_config.enforced_device_ids.insert("missing-arm");
    SafetyManager missing(missing_config);
    const auto missing_result = missing.validateStartupCoverage(
        SafetyClock::now() + std::chrono::milliseconds(10));
    ASSERT_FALSE(missing_result.ready);
    ASSERT_EQ(missing_result.issues.size(), 1U);
    EXPECT_EQ(missing_result.issues.front().target_id, "missing-arm");
    EXPECT_EQ(missing_result.issues.front().reason, SafetyReason::DeviceNotFound);
}

TEST(SafetyManagerTest,
     EnforceAllStartupRequiresEndpointParticipantAndFreshSnapshot)
{
    SafetyManagerConfig config;
    config.enforcement_mode = EnforcementMode::EnforceAll;

    SafetyManager missing_capability(config);
    ASSERT_TRUE(missing_capability.registerDevice(
        {controlDescriptor(), {}, {}}));
    const auto structural = missing_capability.validateStartupCoverage(
        SafetyClock::now() + std::chrono::milliseconds(10));
    EXPECT_FALSE(structural.ready);
    EXPECT_EQ(structural.issues.size(), 2U);

    SafetyManager missing_sample(config);
    auto silent_endpoint =
        std::make_shared<FakeEndpoint>(controlDescriptor());
    silent_endpoint->publish_on_refresh = false;
    ASSERT_TRUE(missing_sample.registerDevice({
        controlDescriptor(),
        silent_endpoint,
        std::make_shared<FakeParticipant>()}));
    const auto stale = missing_sample.validateStartupCoverage(
        SafetyClock::now() + std::chrono::milliseconds(10));
    ASSERT_FALSE(stale.ready);
    ASSERT_EQ(stale.issues.size(), 1U);
    EXPECT_EQ(stale.issues.front().reason, SafetyReason::SafetyStateMissing);
}

TEST(SafetyManagerTest,
     HardwareUnsafeSnapshotBlocksAdmissionButNotStructuralStartup)
{
    SafetyManagerConfig config;
    config.enforcement_mode = EnforcementMode::EnforceAll;
    SafetyManager coordinator(config);
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    endpoint->condition = SafetyCondition::Unsafe;
    endpoint->emergency_stop = TriState::True;
    ASSERT_TRUE(coordinator.registerDevice({
        controlDescriptor(), endpoint, std::make_shared<FakeParticipant>()}));
    coordinator.updateDeviceRuntimeState(
        "arm", device::ManagedDeviceState::Running,
        {device::DeviceHealthState::Healthy, {}});

    const auto coverage = coordinator.validateStartupCoverage(
        SafetyClock::now() + std::chrono::milliseconds(50));
    EXPECT_TRUE(coverage.ready);
    coordinator.markStartupComplete();
    const auto admission = coordinator.admit(actuateRequest());
    EXPECT_FALSE(admission.decision.allowed);
    EXPECT_EQ(
        admission.decision.reason, SafetyReason::EmergencyStopActive);
}

TEST(SafetyManagerTest, EnforceAllFailsClosedOnUnknownControlState)
{
    SafetyManagerConfig config;
    config.enforcement_mode = EnforcementMode::EnforceAll;
    SafetyManager coordinator(config);
    ASSERT_TRUE(coordinator.registerDevice({controlDescriptor(), {}, {}}));
    coordinator.markStartupComplete();

    const auto result = coordinator.admit(actuateRequest());
    EXPECT_FALSE(result.decision.allowed);
    EXPECT_FALSE(result.decision.policy_allowed);
    EXPECT_TRUE(result.decision.enforced);
    EXPECT_FALSE(result.permit.has_value());
}

TEST(SafetyManagerTest, ControlSafetyBitsMustBeExplicitlyFalse)
{
    SafetyManagerConfig config;
    config.enforcement_mode = EnforcementMode::EnforceAll;
    SafetyManager coordinator(config);
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    endpoint->protective_stop = TriState::Unknown;
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, {}}));
    coordinator.updateDeviceRuntimeState(
        "arm", device::ManagedDeviceState::Running,
        {device::DeviceHealthState::Healthy, {}});
    coordinator.markStartupComplete();

    const auto result = coordinator.admit(actuateRequest());
    EXPECT_FALSE(result.decision.allowed);
    EXPECT_EQ(result.decision.reason, SafetyReason::ProtectiveStopActive);
    ASSERT_EQ(coordinator.snapshot().devices.size(), 1U);
    EXPECT_EQ(
        coordinator.snapshot().devices.front().admission_state,
        DeviceAdmissionState::Blocked);
}

TEST(SafetyManagerTest, EnforcedDispatchRunsFinalHardwareCheck)
{
    SafetyManagerConfig config;
    config.enforcement_mode = EnforcementMode::EnforceAll;
    SafetyManager coordinator(config);
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, {}}));
    coordinator.updateDeviceRuntimeState(
        "arm", device::ManagedDeviceState::Running,
        {device::DeviceHealthState::Healthy, {}});
    coordinator.markStartupComplete();

    auto admission = coordinator.admit(actuateRequest());
    ASSERT_TRUE(admission.decision.allowed) << admission.decision.detail;
    ASSERT_TRUE(admission.permit.has_value());
    auto dispatch = coordinator.beginDispatch(*admission.permit);
    EXPECT_TRUE(dispatch.acquired());
    EXPECT_EQ(endpoint->hardware_checks.load(), 1);
}

TEST(SafetyManagerTest,
     StartActivityMayEnterFromRestrictedButActuationMayNot)
{
    SafetyManagerConfig config;
    config.enforcement_mode = EnforcementMode::EnforceAll;
    SafetyManager coordinator(config);
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    endpoint->condition = SafetyCondition::Restricted;
    endpoint->ready = TriState::False;
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, {}}));
    coordinator.updateDeviceRuntimeState(
        "arm", device::ManagedDeviceState::Running,
        {device::DeviceHealthState::Healthy, {}});
    coordinator.markStartupComplete();

    auto start = actuateRequest();
    start.command.intent = CommandIntent::StartActivity;
    const auto start_result = coordinator.admit(start);
    ASSERT_TRUE(start_result.decision.allowed)
        << start_result.decision.detail;
    ASSERT_TRUE(start_result.permit.has_value());
    EXPECT_TRUE(coordinator.beginDispatch(*start_result.permit).acquired());

    const auto actuation = coordinator.admit(actuateRequest());
    EXPECT_FALSE(actuation.decision.allowed);
    EXPECT_EQ(actuation.decision.reason, SafetyReason::HardwareUnsafe);
}

TEST(SafetyManagerTest, SuccessfulStopInvalidatesOldPermitAndReopens)
{
    SafetyManagerConfig config;
    config.enforcement_mode = EnforcementMode::EnforceAll;
    SafetyManager coordinator(config);
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    auto participant = std::make_shared<FakeParticipant>();
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, participant}));
    coordinator.updateDeviceRuntimeState(
        "arm", device::ManagedDeviceState::Running,
        {device::DeviceHealthState::Healthy, {}});
    coordinator.markStartupComplete();

    auto admission = coordinator.admit(actuateRequest());
    ASSERT_TRUE(admission.permit.has_value());
    const auto old_epoch = admission.permit->safety_epoch;
    const auto stopped = coordinator.stopAll(
        "stop-1", SafetyClock::now() + std::chrono::seconds(1));
    ASSERT_TRUE(stopped.success);
    EXPECT_EQ(stopped.system_state, SystemAdmissionState::Open);
    EXPECT_GT(stopped.current_safety_epoch, old_epoch);
    const auto revalidated =
        coordinator.revalidatePermit(*admission.permit);
    EXPECT_FALSE(revalidated.safe);
    EXPECT_EQ(revalidated.reason, SafetyReason::SafetyLatched);
    EXPECT_FALSE(coordinator.beginDispatch(*admission.permit).acquired());
    EXPECT_EQ(participant->stop_requests.load(), 1);
    EXPECT_EQ(participant->verifications.load(), 1);
    const auto snapshot = coordinator.snapshot();
    ASSERT_EQ(snapshot.participants.size(), 1U);
    EXPECT_EQ(snapshot.participants.front().descriptor.participant_id, "arm");
    EXPECT_FALSE(snapshot.participants.front().barrier_active);
    EXPECT_FALSE(snapshot.participants.front().barrier_retained);
    EXPECT_TRUE(snapshot.participants.front().last_request.recorded);
    EXPECT_TRUE(snapshot.participants.front().last_request.success);
    EXPECT_TRUE(snapshot.participants.front().last_verify.recorded);
    EXPECT_TRUE(snapshot.participants.front().last_verify.success);
    EXPECT_TRUE(snapshot.participants.front().last_release.recorded);
    EXPECT_TRUE(snapshot.participants.front().last_release.success);
}

TEST(SafetyManagerTest, FailedStopRequiresVerifiedRecovery)
{
    SafetyManagerConfig config;
    config.enforcement_mode = EnforcementMode::EnforceAll;
    SafetyManager coordinator(config);
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    auto participant = std::make_shared<FakeParticipant>();
    participant->verify_result = {
        false, SafetyReason::StopUnconfirmed, "motion not confirmed"};
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, participant}));
    coordinator.updateDeviceRuntimeState(
        "arm", device::ManagedDeviceState::Running,
        {device::DeviceHealthState::Healthy, {}});
    coordinator.markStartupComplete();

    const auto stopped = coordinator.stopAll(
        "stop-failed", SafetyClock::now() + std::chrono::seconds(1));
    ASSERT_FALSE(stopped.success);
    ASSERT_EQ(stopped.system_state, SystemAdmissionState::Latched);
    const auto latched = coordinator.snapshot();
    ASSERT_EQ(latched.participants.size(), 1U);
    EXPECT_TRUE(latched.participants.front().barrier_active);
    EXPECT_TRUE(latched.participants.front().barrier_retained);
    EXPECT_TRUE(latched.participants.front().last_verify.recorded);
    EXPECT_FALSE(latched.participants.front().last_verify.success);
    EXPECT_FALSE(latched.participants.front().last_release.recorded);

    participant->verify_result = {true, SafetyReason::None, {}};
    RecoveryRequest request;
    request.recovery_id = "recovery-1";
    request.all_devices = true;
    request.expected_safety_epoch = stopped.current_safety_epoch;
    request.verify_only = false;
    request.reason = "operator confirmed work cell is clear";
    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
    const auto recovered = coordinator.recover(request);
    EXPECT_EQ(recovered.result, RecoveryResultCode::Recovered);
    EXPECT_EQ(recovered.system_state, SystemAdmissionState::Open);
    EXPECT_EQ(endpoint->recoveries.load(), 1);
    EXPECT_EQ(participant->recoveries.load(), 1);
    const auto reopened = coordinator.snapshot();
    ASSERT_EQ(reopened.participants.size(), 1U);
    EXPECT_FALSE(reopened.participants.front().barrier_active);
    EXPECT_FALSE(reopened.participants.front().barrier_retained);
    EXPECT_TRUE(reopened.participants.front().last_release.recorded);
    EXPECT_TRUE(reopened.participants.front().last_release.success);
}

TEST(SafetyManagerTest, RecoveryCannotIgnoreEmergencyStop)
{
    SafetyManager coordinator;
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    auto participant = std::make_shared<FakeParticipant>();
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, participant}));
    coordinator.markStartupComplete();
    coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown);
    const auto before = coordinator.snapshot();

    endpoint->emergency_stop = TriState::True;
    RecoveryRequest request;
    request.recovery_id = "recovery-estop";
    request.all_devices = true;
    request.expected_safety_epoch = before.safety_epoch;
    request.verify_only = false;
    request.reason = "test";
    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
    const auto recovered = coordinator.recover(request);
    EXPECT_EQ(recovered.result, RecoveryResultCode::BlockerRemains);
    EXPECT_EQ(recovered.system_state, SystemAdmissionState::Latched);
    ASSERT_FALSE(recovered.targets.empty());
    EXPECT_EQ(
        recovered.targets.front().reason,
        SafetyReason::EmergencyStopActive);
    EXPECT_EQ(endpoint->recoveries.load(), 0);
}

TEST(SafetyManagerTest,
     OperationalRestoreClearsRecoverableHardwareAndSoftwareState)
{
    SafetyManager coordinator;
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    endpoint->condition = SafetyCondition::Unsafe;
    endpoint->ready = TriState::False;
    endpoint->protective_stop = TriState::True;
    endpoint->quiescent = TriState::True;
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, {}}));
    coordinator.markStartupComplete();
    coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown);
    const auto before = coordinator.snapshot();

    RecoveryRequest request;
    request.recovery_id = "restore-operational";
    request.all_devices = true;
    request.expected_safety_epoch = before.safety_epoch;
    request.verify_only = false;
    request.restore_operational_state = true;
    request.reason = "operator confirmed the work cell is clear";
    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
    request.authorize_clear = [] { return true; };

    const auto restored = coordinator.recover(request);

    EXPECT_EQ(restored.result, RecoveryResultCode::Recovered);
    EXPECT_EQ(restored.system_state, SystemAdmissionState::Open);
    ASSERT_EQ(restored.targets.size(), 1U);
    EXPECT_TRUE(restored.targets.front().success)
        << restored.targets.front().detail;
    EXPECT_EQ(endpoint->operational_restores.load(), 1);
    EXPECT_EQ(endpoint->recoveries.load(), 1);
    EXPECT_EQ(
        coordinator.snapshot().devices.front().admission_state,
        DeviceAdmissionState::Open);
}

TEST(SafetyManagerTest, OperationalRestoreFailsClosedOnActiveEmergencyStop)
{
    SafetyManager coordinator;
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    endpoint->condition = SafetyCondition::Unsafe;
    endpoint->ready = TriState::False;
    endpoint->emergency_stop = TriState::True;
    endpoint->quiescent = TriState::True;
    endpoint->operational_restore_check = {
        false,
        SafetyReason::EmergencyStopActive,
        "hardware emergency-stop input is still active"};
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, {}}));
    coordinator.markStartupComplete();
    coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown);
    const auto before = coordinator.snapshot();

    RecoveryRequest request;
    request.recovery_id = "restore-active-estop";
    request.all_devices = true;
    request.expected_safety_epoch = before.safety_epoch;
    request.verify_only = false;
    request.restore_operational_state = true;
    request.reason = "operator requested recovery";
    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
    request.authorize_clear = [] { return true; };

    const auto restored = coordinator.recover(request);

    EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains);
    EXPECT_EQ(restored.system_state, SystemAdmissionState::Latched);
    ASSERT_EQ(restored.targets.size(), 1U);
    EXPECT_FALSE(restored.targets.front().success);
    EXPECT_EQ(
        restored.targets.front().reason,
        SafetyReason::EmergencyStopActive);
    EXPECT_EQ(endpoint->operational_restores.load(), 0);
    EXPECT_EQ(endpoint->recoveries.load(), 0);
}

TEST(SafetyManagerTest, OperationalRestoreDoesNotRecoverAnActiveDevice)
{
    SafetyManager coordinator;
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    endpoint->condition = SafetyCondition::Unsafe;
    endpoint->ready = TriState::False;
    endpoint->quiescent = TriState::False;
    endpoint->fault = TriState::True;
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, {}}));
    coordinator.markStartupComplete();
    coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown);

    RecoveryRequest request;
    request.recovery_id = "restore-active-device";
    request.all_devices = true;
    request.expected_safety_epoch = coordinator.snapshot().safety_epoch;
    request.verify_only = false;
    request.restore_operational_state = true;
    request.reason = "operator requested recovery";
    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
    request.authorize_clear = [] { return true; };

    const auto restored = coordinator.recover(request);

    EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains);
    ASSERT_EQ(restored.targets.size(), 1U);
    EXPECT_EQ(
        restored.targets.front().reason,
        SafetyReason::DeviceStillMoving);
    EXPECT_EQ(endpoint->operational_restores.load(), 0);
    EXPECT_EQ(endpoint->recoveries.load(), 0);
}

TEST(SafetyManagerTest, OperationalRestoreAuditFailurePreventsDeviceRecovery)
{
    SafetyManager coordinator;
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    endpoint->condition = SafetyCondition::Unsafe;
    endpoint->ready = TriState::False;
    endpoint->protective_stop = TriState::True;
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, {}}));
    coordinator.markStartupComplete();
    coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown);

    RecoveryRequest request;
    request.recovery_id = "restore-audit-failure";
    request.all_devices = true;
    request.expected_safety_epoch = coordinator.snapshot().safety_epoch;
    request.verify_only = false;
    request.restore_operational_state = true;
    request.reason = "operator requested recovery";
    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
    request.authorize_clear = [] { return false; };

    const auto restored = coordinator.recover(request);

    EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains);
    ASSERT_EQ(restored.targets.size(), 1U);
    EXPECT_EQ(
        restored.targets.front().reason,
        SafetyReason::RecoveryAuditFailed);
    EXPECT_EQ(endpoint->operational_restores.load(), 0);
    EXPECT_EQ(endpoint->recoveries.load(), 0);
}

TEST(SafetyManagerTest, OperationalRestoreRequiresExplicitAuthorization)
{
    SafetyManager coordinator;
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, {}}));
    coordinator.markStartupComplete();

    RecoveryRequest request;
    request.recovery_id = "restore-without-authorization";
    request.all_devices = true;
    request.expected_safety_epoch = coordinator.snapshot().safety_epoch;
    request.verify_only = false;
    request.restore_operational_state = true;
    request.reason = "operator requested recovery";

    const auto restored = coordinator.recover(request);

    EXPECT_EQ(restored.result, RecoveryResultCode::Failed);
    ASSERT_EQ(restored.targets.size(), 1U);
    EXPECT_EQ(
        restored.targets.front().reason,
        SafetyReason::InvalidArgument);
    EXPECT_EQ(endpoint->operational_restores.load(), 0);
}

TEST(SafetyManagerTest,
     OperationalRestoreAuditFailureWithNoDevicesRemainsLatched)
{
    SafetyManager coordinator;
    coordinator.markStartupComplete();

    RecoveryRequest request;
    request.recovery_id = "empty-restore-audit-failure";
    request.all_devices = true;
    request.expected_safety_epoch = coordinator.snapshot().safety_epoch;
    request.verify_only = false;
    request.restore_operational_state = true;
    request.reason = "operator requested recovery";
    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
    request.authorize_clear = [] { return false; };

    const auto restored = coordinator.recover(request);

    EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains);
    EXPECT_EQ(restored.system_state, SystemAdmissionState::Latched);
    ASSERT_EQ(restored.targets.size(), 1U);
    EXPECT_EQ(restored.targets.front().target_id, "system");
    EXPECT_EQ(
        restored.targets.front().reason,
        SafetyReason::RecoveryAuditFailed);
}

TEST(SafetyManagerTest,
     OperationalRestoreDoesNotPartiallyReleaseSoftwareAdmission)
{
    SafetyManager coordinator;
    auto first = std::make_shared<FakeEndpoint>(
        controlDescriptor("arm-first"));
    first->condition = SafetyCondition::Unsafe;
    first->ready = TriState::False;
    first->protective_stop = TriState::True;
    auto second = std::make_shared<FakeEndpoint>(
        controlDescriptor("arm-second"));
    second->condition = SafetyCondition::Unsafe;
    second->ready = TriState::False;
    second->fault = TriState::True;
    second->operational_restore_check = {
        false, SafetyReason::DeviceFault, "fault reset failed"};
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor("arm-first"), first, {}}));
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor("arm-second"), second, {}}));
    coordinator.markStartupComplete();
    coordinator.quarantineDevice(
        "arm-first", SafetyReason::OutcomeUnknown);
    coordinator.quarantineDevice(
        "arm-second", SafetyReason::OutcomeUnknown);

    RecoveryRequest request;
    request.recovery_id = "restore-partial-failure";
    request.all_devices = true;
    request.expected_safety_epoch = coordinator.snapshot().safety_epoch;
    request.verify_only = false;
    request.restore_operational_state = true;
    request.reason = "operator requested recovery";
    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
    request.authorize_clear = [] { return true; };

    const auto restored = coordinator.recover(request);

    EXPECT_EQ(restored.result, RecoveryResultCode::BlockerRemains);
    EXPECT_EQ(restored.system_state, SystemAdmissionState::Latched);
    EXPECT_EQ(first->operational_restores.load(), 1);
    EXPECT_EQ(second->operational_restores.load(), 1);
    EXPECT_EQ(first->recoveries.load(), 0);
    EXPECT_EQ(second->recoveries.load(), 0);
    const auto snapshot = coordinator.snapshot();
    ASSERT_EQ(snapshot.devices.size(), 2U);
    for (const auto& device : snapshot.devices) {
        EXPECT_EQ(
            device.admission_state,
            DeviceAdmissionState::Quarantined)
            << device.descriptor.device_id;
    }
}

TEST(SafetyManagerTest, RecoveryAuditFailureCannotReleaseLatch)
{
    SafetyManager coordinator;
    auto endpoint = std::make_shared<FakeEndpoint>(controlDescriptor());
    auto participant = std::make_shared<FakeParticipant>();
    participant->verify_result = {
        false, SafetyReason::StopUnconfirmed, "motion not confirmed"};
    ASSERT_TRUE(coordinator.registerDevice(
        {controlDescriptor(), endpoint, participant}));
    coordinator.markStartupComplete();

    const auto stopped = coordinator.stopAll(
        "stop-audit", SafetyClock::now() + std::chrono::seconds(1));
    ASSERT_FALSE(stopped.success);
    participant->verify_result = {true, SafetyReason::None, {}};

    RecoveryRequest request;
    request.recovery_id = "recovery-audit";
    request.all_devices = true;
    request.expected_safety_epoch = stopped.current_safety_epoch;
    request.verify_only = false;
    request.reason = "operator inspected the work cell";
    request.deadline = SafetyClock::now() + std::chrono::seconds(1);
    request.authorize_clear = [] { return false; };
    const auto recovered = coordinator.recover(request);

    EXPECT_EQ(recovered.result, RecoveryResultCode::BlockerRemains);
    EXPECT_EQ(recovered.system_state, SystemAdmissionState::Latched);
    ASSERT_FALSE(recovered.targets.empty());
    EXPECT_EQ(
        recovered.targets.front().reason,
        SafetyReason::RecoveryAuditFailed);
    EXPECT_EQ(endpoint->recoveries.load(), 0);
    EXPECT_EQ(participant->recoveries.load(), 0);
    EXPECT_EQ(participant->releases.load(), 0);
}

} // namespace
} // namespace cmvr::safety