cmvr-es/cmvr-es/service/grpc/server/src/grpc_system_service.cpp

//
// Created by xtkuang on 2025/6/6.
//

#include "../include/grpc_system_service.h"

#include <algorithm>
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <cstdint>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include "common/base/logging/logger.h"
#include "devices/agv/abstract_agv.h"
#include "devices/arm/robot_arm.h"
#include "devices/biohead/abstract_biohead.h"
#include "devices/camera/abstract_camera.h"
#include "devices/dexhand/abstract_dexhand.h"
#include "devices/gripper/abstract_gripper.h"
#include "devices/microphone/abstract_microphone.h"
#include "devices/speaker/abstract_speaker.h"
#include "manager/control_authority_manager/include/control_authority_manager.h"
#include "manager/media_source_manager/include/device_media_source_adapter.h"
#include "manager/task_manager/include/task_manager.h"
#include "service/grpc/action/include/action_queue_executor.h"
#include "service/grpc/server/include/camera_operational_activity_registry.h"
#include "service/grpc/server/include/camera_ptz_activity_registry.h"
#include "service/grpc/server/include/grpc_recovery_audit.h"
#include "service/grpc/server/include/grpc_safety_proto.h"
#include "service/grpc/server/include/grpc_safety_participants.h"
#include "service/grpc/server/include/grpc_security.h"
#include "service/grpc/server/include/media_activity_coordinator.h"
#include "service/grpc/server/include/motor_activity_coordinator.h"
#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
#include "service/grpc/stop_all/include/stop_operation_dispatcher.h"

using namespace cmvr::device;
using namespace cmvr::service;

namespace {

class ScopedControlBarrierSet final {
public:
    ~ScopedControlBarrierSet()
    {
        auto& authority =
            cmvr::control::ControlAuthorityManager::instance();
        for (const auto& barrier : barriers_) {
            if (barrier.release_on_destroy) {
                authority.release(barrier.token);
            } else {
                (void)authority.retireSafetyHolder(barrier.token);
            }
        }
    }

    bool acquire(const std::string& device_id, std::string& detail)
    {
        cmvr::control::ControlLeaseToken ignored;
        return acquire(device_id, detail, ignored);
    }

    bool acquire(
        const std::string& device_id,
        std::string& detail,
        cmvr::control::ControlLeaseToken& token)
    {
        static std::atomic<std::uint64_t> sequence{0};
        const std::string owner =
            "grpc-system:stop-all:" +
            std::to_string(
                sequence.fetch_add(1U, std::memory_order_relaxed) + 1U);
        const auto result =
            cmvr::control::ControlAuthorityManager::instance()
                .preemptAcquire(
                    device_id,
                    owner,
                    std::chrono::duration_cast<
                        cmvr::control::ControlAuthorityManager::Duration>(
                            std::chrono::hours(24)));
        if (!result.acquired) {
            detail = result.detail;
            return false;
        }
        // StopAll barriers default to fail-closed. If retaining the token in
        // this local vector throws, retire the manager-side holder so a later
        // successful StopAll can recover it without reopening control now.
        try {
            barriers_.push_back({result.token, false});
        } catch (...) {
            (void)cmvr::control::ControlAuthorityManager::instance()
                .retireSafetyHolder(result.token);
            throw;
        }
        token = result.token;
        return true;
    }

    bool waitForPreemptedRelease(
        const std::string& device_id,
        const std::chrono::milliseconds timeout)
    {
        const auto* barrier = find_(device_id);
        return barrier &&
            cmvr::control::ControlAuthorityManager::instance()
                .waitForPreemptedRelease(barrier->token, timeout);
    }

    bool recoverRetiredSafetyHolders(const std::string& device_id)
    {
        const auto* barrier = find_(device_id);
        return barrier &&
            cmvr::control::ControlAuthorityManager::instance()
                .recoverRetiredSafetyHolders(barrier->token);
    }

    void quarantine(const std::string& device_id)
    {
        auto& authority =
            cmvr::control::ControlAuthorityManager::instance();
        for (auto& barrier : barriers_) {
            if (barrier.token.resource_id == device_id) {
                (void)authority.retireSafetyHolder(barrier.token);
                barrier.release_on_destroy = false;
            }
        }
    }

    void quarantineAll()
    {
        auto& authority =
            cmvr::control::ControlAuthorityManager::instance();
        for (auto& barrier : barriers_) {
            (void)authority.retireSafetyHolder(barrier.token);
            barrier.release_on_destroy = false;
        }
    }

    void confirmSafeToReleaseAll()
    {
        for (auto& barrier : barriers_) {
            barrier.release_on_destroy = true;
        }
    }

    bool empty() const noexcept { return barriers_.empty(); }

    void recoverRetiredSafetyHoldersAll()
    {
        for (const auto& barrier : barriers_) {
            if (!recoverRetiredSafetyHolders(
                    barrier.token.resource_id)) {
                throw std::runtime_error(
                    "StopAll could not recover an earlier failed safety "
                    "barrier: " + barrier.token.resource_id);
            }
        }
    }

    void releaseAll() noexcept
    {
        auto& authority =
            cmvr::control::ControlAuthorityManager::instance();
        for (const auto& barrier : barriers_) {
            authority.release(barrier.token);
        }
        barriers_.clear();
    }

private:
    struct Barrier {
        cmvr::control::ControlLeaseToken token;
        bool release_on_destroy{false};
    };

    const Barrier* find_(const std::string& device_id) const noexcept
    {
        for (const auto& barrier : barriers_) {
            if (barrier.token.resource_id == device_id) {
                return &barrier;
            }
        }
        return nullptr;
    }

    std::vector<Barrier> barriers_;
};

class ScopedMotorStopAll final {
public:
    ScopedMotorStopAll()
        : coordinator_(globalMotorActivityCoordinator()),
          ticket_(coordinator_.beginStopAll(true))
    {
    }

    ~ScopedMotorStopAll()
    {
        if (ticket_.valid() && !completed_) {
            (void)coordinator_.finishStopAll(ticket_, false);
        }
    }

    bool valid() const noexcept { return ticket_.valid(); }

    bool stopAndWait(
        const std::chrono::milliseconds timeout,
        std::string* error)
    {
        return ticket_.valid() &&
            coordinator_.stopAndWait(ticket_, timeout, error);
    }

    bool requestStop(std::string* error)
    {
        return ticket_.valid() && coordinator_.requestStop(ticket_, error);
    }

    bool collectStopOperations(
        std::vector<cmvr::service::DeferredStopOperation>& operations,
        std::string* error)
    {
        return ticket_.valid() &&
            coordinator_.collectStopOperations(ticket_, operations, error);
    }

    bool waitForStopped(
        const std::chrono::milliseconds timeout,
        std::string* error)
    {
        return ticket_.valid() &&
            coordinator_.waitForStopped(ticket_, timeout, error);
    }

    bool complete(const bool all_motors_stopped)
    {
        if (!ticket_.valid() || completed_) {
            return false;
        }
        const bool completed = coordinator_.finishStopAll(
            ticket_, all_motors_stopped);
        completed_ = completed;
        return completed;
    }

private:
    MotorActivityCoordinator& coordinator_;
    MotorActivityCoordinator::StopAllTicket ticket_;
    bool completed_{false};
};

class ScopedActionQueueStopAll final {
public:
    explicit ScopedActionQueueStopAll(
        cmvr::service::ActionQueueExecutor& executor)
        : executor_(executor), ticket_(executor_.beginStopAll(true))
    {
    }

    ~ScopedActionQueueStopAll()
    {
        if (ticket_.valid() && !completed_) {
            (void)executor_.finishStopAll(ticket_, false);
        }
    }

    bool valid() const noexcept { return ticket_.valid(); }

    bool complete(const bool all_devices_stop_confirmed)
    {
        if (!ticket_.valid() || completed_) {
            return false;
        }
        const bool completed = executor_.finishStopAll(
            ticket_, all_devices_stop_confirmed);
        completed_ = completed;
        return completed;
    }

private:
    cmvr::service::ActionQueueExecutor& executor_;
    cmvr::service::ActionQueueExecutor::StopAllTicket ticket_;
    bool completed_{false};
};

class ScopedMediaStopAll final {
public:
    ScopedMediaStopAll()
        : coordinator_(cmvr::service::globalMediaActivityCoordinator()),
          ticket_(coordinator_.beginStopAll(true))
    {
    }

    ~ScopedMediaStopAll()
    {
        if (ticket_.valid() && !completed_) {
            (void)coordinator_.finishStopAll(ticket_, false);
        }
    }

    bool valid() const noexcept { return ticket_.valid(); }

    bool waitForStopped(const std::chrono::milliseconds timeout)
    {
        return ticket_.valid() &&
            coordinator_.waitForStopped(ticket_, timeout);
    }

    bool requestCancellation()
    {
        return ticket_.valid() && coordinator_.requestCancellation(ticket_);
    }

    bool collectCancellationOperations(
        std::vector<cmvr::service::DeferredStopOperation>& operations,
        std::string* error)
    {
        return ticket_.valid() &&
            coordinator_.collectCancellationOperations(
                ticket_, operations, error);
    }

    bool complete(const bool all_media_stopped)
    {
        if (!ticket_.valid() || completed_) {
            return false;
        }
        const bool completed =
            coordinator_.finishStopAll(ticket_, all_media_stopped);
        completed_ = completed;
        return completed;
    }

private:
    cmvr::service::MediaActivityCoordinator& coordinator_;
    cmvr::service::MediaActivityCoordinator::StopAllTicket ticket_;
    bool completed_{false};
};

class ScopedAdmissionStopAll final {
public:
    ScopedAdmissionStopAll()
        : gate_(globalStopAllAdmissionGate()),
          ticket_(gate_.beginStopAll())
    {
    }

    ~ScopedAdmissionStopAll()
    {
        if (ticket_.valid() && !completed_) {
            (void)gate_.finishStopAll(ticket_, false);
        }
    }

    bool valid() const noexcept { return ticket_.valid(); }

    bool complete(const bool all_domains_stop_confirmed)
    {
        if (!ticket_.valid() || completed_) {
            return false;
        }
        const auto result = gate_.finishStopAllDetailed(
            ticket_, all_domains_stop_confirmed);
        completed_ = result.ticket_consumed;
        return result.admission_reopened;
    }

private:
    StopAllAdmissionGate& gate_;
    StopAllAdmissionGate::StopAllTicket ticket_;
    bool completed_{false};
};

std::uint64_t unixTimeMs() noexcept
{
    const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch());
    return elapsed.count() > 0
               ? static_cast<std::uint64_t>(elapsed.count())
               : 0U;
}

std::chrono::milliseconds remainingStopBudget(
    const std::chrono::steady_clock::time_point deadline) noexcept
{
    const auto now = std::chrono::steady_clock::now();
    if (now >= deadline) {
        return std::chrono::milliseconds::zero();
    }
    return std::chrono::duration_cast<std::chrono::milliseconds>(
        deadline - now);
}

std::timed_mutex& processStopAllMutex()
{
    // SystemService is expected to be unique, but keeping the mutex at process
    // scope also protects test, reload, and accidental multi-instance paths
    // from issuing overlapping whole-device stop rounds.
    static std::timed_mutex mutex;
    return mutex;
}

using StopDispatcher = cmvr::service::StopOperationDispatcher;
using StopHandle = StopDispatcher::Handle;
using StopOutcome = StopDispatcher::OperationResult;

struct ProcessStopDispatcherRegistry final {
    std::mutex mutex;
    std::condition_variable available;
    std::weak_ptr<StopDispatcher> dispatcher;
    bool destroying{false};
};

ProcessStopDispatcherRegistry& processStopDispatcherRegistry()
{
    static ProcessStopDispatcherRegistry registry;
    return registry;
}

void acquireProcessStopDispatcher(
    std::shared_ptr<StopDispatcher>& owner)
{
    auto& registry = processStopDispatcherRegistry();
    std::unique_lock lock(registry.mutex);
    registry.available.wait(
        lock, [&registry] { return !registry.destroying; });
    if (auto existing = registry.dispatcher.lock()) {
        owner = std::move(existing);
        return;
    }

    owner = std::make_shared<StopDispatcher>();
    registry.dispatcher = owner;
}

void releaseProcessStopDispatcher(
    std::shared_ptr<StopDispatcher>& owner) noexcept
{
    if (!owner) {
        return;
    }

    auto& registry = processStopDispatcherRegistry();
    std::unique_lock lock(registry.mutex);
    if (owner.use_count() > 1) {
        owner.reset();
        return;
    }

    // Do not admit a replacement while the last dispatcher is joining a
    // worker which may still be inside a device driver.
    registry.destroying = true;
    registry.dispatcher.reset();
    registry.available.notify_all();
    auto last_owner = std::move(owner);
    lock.unlock();
    last_owner.reset();
    lock.lock();
    registry.destroying = false;
    lock.unlock();
    registry.available.notify_all();
}

class StopDeadlineDetail final {
public:
    void publishInitial(std::string detail)
    {
        std::lock_guard lock(mutex_);
        detail_ = std::move(detail);
    }

    void publishUntil(
        const StopDispatcher::Deadline deadline,
        std::string detail)
    {
        std::lock_guard lock(mutex_);
        if (StopDispatcher::Clock::now() < deadline) {
            detail_ = std::move(detail);
        }
    }

    std::string snapshot() const
    {
        std::lock_guard lock(mutex_);
        return detail_;
    }

private:
    mutable std::mutex mutex_;
    std::string detail_;
};

struct StopHandleEntry final {
    std::string device_id;
    StopHandle handle;
    bool control_resource{false};
    bool require_success{true};
};

std::string stopResourceKey(const std::string& device_id)
{
    return "device:" + device_id;
}

void appendFailure(
    std::vector<std::string>& failures,
    const std::string& subject,
    const std::string& detail)
{
    failures.push_back(
        subject + ": " +
        (detail.empty() ? "operational stop was not confirmed" : detail));
}

bool waitForStopOperations(
    const std::vector<StopHandleEntry>& operations,
    const StopDispatcher::Deadline deadline,
    ScopedControlBarrierSet& control_barriers,
    std::vector<std::string>& failures,
    std::unordered_set<std::string>* completed_devices = nullptr)
{
    bool all_succeeded = true;
    for (const auto& operation : operations) {
        const auto outcome = operation.handle.waitUntil(deadline);
        if (outcome.completed && completed_devices) {
            completed_devices->emplace(operation.device_id);
        }
        if (outcome.completed && outcome.result) {
            continue;
        }
        if (outcome.completed && !operation.require_success) {
            CMVR_LOG(WARNING)
                << "[gRPCSystemServiceImpl] (StopAll): initial stop was not "
                   "confirmed and will be retried, id="
                << operation.device_id << ", detail=" << outcome.detail;
            continue;
        }

        all_succeeded = false;
        if (operation.control_resource) {
            control_barriers.quarantine(operation.device_id);
        }
        appendFailure(
            failures,
            operation.device_id,
            outcome.detail.empty()
                ? "stop operation did not complete before the deadline"
                : outcome.detail);
    }
    return all_succeeded;
}

StopOutcome stopArm(
    const std::shared_ptr<cmvr::device::RobotArm>& arm,
    const bool final_confirmation)
{
    const auto result = arm->stopMotion();
    if (!result.ok()) {
        return {
            false,
            std::string(final_confirmation ? "final " : "initial ") +
                "RobotArm stop was not confirmed" +
                (result.message.empty() ? "" : ": " + result.message)};
    }
    if (final_confirmation) {
        try {
            if (arm->busy()) {
                return {false, "RobotArm remained busy after final stop"};
            }
        } catch (const std::exception& error) {
            return {
                false,
                std::string("RobotArm idle confirmation threw: ") +
                    error.what()};
        } catch (...) {
            return {
                false,
                "RobotArm idle confirmation threw an unknown exception"};
        }
    }
    return {true, {}};
}

bool agvStopResultAccepted(const cmvr::device::AgvResult& result) noexcept
{
    return result.ok() ||
        result.code == cmvr::device::AgvErrorCode::UnsupportedCommand;
}

StopOutcome stopAgv(
    const std::shared_ptr<cmvr::device::AbstractAGV>& agv,
    const bool final_confirmation)
{
    const auto cancel = agv->cancelNavigation();
    const auto velocity = agv->stopVelocityControl();
    const auto mapping = agv->stopMapping();
    const auto stopped = agv->confirmMotionStopped();

    if (final_confirmation &&
        (!agvStopResultAccepted(cancel) ||
         !agvStopResultAccepted(velocity) ||
         !agvStopResultAccepted(mapping) || !stopped.ok())) {
        return {false, "final AGV operational stop was not confirmed"};
    }
    if (!final_confirmation && !stopped.ok()) {
        return {
            false,
            "initial AGV stopped state was not confirmed" +
                (stopped.message.empty() ? "" : ": " + stopped.message)};
    }
    return {true, {}};
}

StopOutcome stopDexHand(
    const std::shared_ptr<cmvr::device::AbstractDexHand>& hand,
    const bool final_confirmation)
{
    if (!hand->stopOperationalActivity()) {
        return {
            false,
            std::string(final_confirmation ? "final " : "initial ") +
                "DexHand operational stop was not confirmed"};
    }
    return {true, {}};
}

template <typename Operation>
StopOutcome invokeStopOperation(
    Operation&& operation,
    const std::string& description) noexcept
{
    try {
        return std::forward<Operation>(operation)();
    } catch (const std::exception& error) {
        return {
            false,
            description + " threw: " + error.what()};
    } catch (...) {
        return {
            false,
            description + " threw an unknown exception"};
    }
}

template <typename InitialStop, typename FinalStop>
StopOutcome stopControlWithFence(
    const cmvr::control::ControlLeaseToken& barrier,
    const StopDispatcher::Deadline deadline,
    InitialStop&& initial_stop,
    FinalStop&& final_stop,
    const std::string& description,
    const std::shared_ptr<StopDeadlineDetail>& deadline_detail)
{
    deadline_detail->publishUntil(
        deadline,
        "initial " + description +
        " stop did not complete before the StopAll deadline");
    const auto initial = invokeStopOperation(
        std::forward<InitialStop>(initial_stop),
        "initial " + description + " stop");
    if (!initial.success) {
        CMVR_LOG(WARNING)
            << "[gRPCSystemServiceImpl] (StopAll): " << initial.detail;
    }

    if (!barrier.valid()) {
        return {
            false,
            description +
                " safety barrier was unavailable after the initial stop"};
    }

    const std::string handler_timeout_detail =
        "timed out waiting for the preempted " + description +
        " control handler to exit";
    deadline_detail->publishUntil(deadline, handler_timeout_detail);
    const bool handler_drained =
        cmvr::control::ControlAuthorityManager::instance()
            .waitForPreemptedRelease(
                barrier, remainingStopBudget(deadline));
    if (handler_drained) {
        deadline_detail->publishUntil(
            deadline,
            "final " + description +
            " stop did not complete before the StopAll deadline");
    }
    // This final typed stop remains mandatory even when the handler wait used
    // the entire RPC budget. The dispatcher owns this worker past the RPC
    // deadline, closing the race where an old handler resumes after the first
    // stop while the safety barrier remains fail-closed.
    const auto final = invokeStopOperation(
        std::forward<FinalStop>(final_stop),
        "final " + description + " stop");
    if (!handler_drained) {
        std::string detail = handler_timeout_detail;
        if (!final.success) {
            detail += "; " +
                (final.detail.empty()
                     ? "final " + description +
                           " stop was not confirmed"
                     : final.detail);
        }
        return {false, std::move(detail)};
    }
    return final;
}

StopOutcome stopCameraActivities(
    const std::string& device_id,
    const std::shared_ptr<cmvr::device::AbstractCamera>& camera)
{
    std::vector<std::string> failures;
    bool stopped = cmvr::media::globalMediaSourceManager()
        .stopSourcesForDevice(device_id, &failures);
    if (!globalCameraPtzActivityRegistry().stopActivitiesForDevice(
            device_id, &failures)) {
        stopped = false;
    }

    try {
        cmvr::device::CameraState state{};
        if (camera) {
            camera->getState(state);
        }
        if (camera && state.is_recording) {
            camera->stopRecording();
            camera->getState(state);
            if (state.is_recording) {
                stopped = false;
                failures.push_back(
                    device_id + ": camera recording did not stop");
            }
        }
    } catch (const std::exception& error) {
        stopped = false;
        failures.push_back(
            device_id + ": camera recording stop threw: " + error.what());
    } catch (...) {
        stopped = false;
        failures.push_back(
            device_id +
            ": camera recording stop threw an unknown exception");
    }

    if (!globalCameraOperationalActivityRegistry().stopActivitiesForDevice(
            device_id, camera, &failures)) {
        stopped = false;
    }
    return {
        stopped,
        failures.empty()
            ? std::string{}
            : "camera activities were not fully stopped: " +
                  failures.front()};
}

StopOutcome stopMicrophone(
    const std::string& device_id,
    const std::shared_ptr<cmvr::device::AbstractMicrophone>& microphone)
{
    std::vector<std::string> failures;
    bool stopped = cmvr::media::globalMediaSourceManager()
        .stopSourcesForDevice(device_id, &failures);
    try {
        cmvr::device::MicrophoneState state{};
        microphone->getState(state);
        if (state.is_recording) {
            microphone->stopRecording();
            microphone->getState(state);
            if (state.is_recording) {
                stopped = false;
                failures.push_back(
                    device_id + ": microphone recording did not stop");
            }
        }
    } catch (const std::exception& error) {
        stopped = false;
        failures.push_back(
            device_id + ": microphone recording stop threw: " +
            error.what());
    } catch (...) {
        stopped = false;
        failures.push_back(
            device_id +
            ": microphone recording stop threw an unknown exception");
    }
    return {
        stopped,
        failures.empty()
            ? std::string{}
            : "microphone activities were not fully stopped: " +
                  failures.front()};
}

StopOutcome stopSpeaker(
    const std::shared_ptr<cmvr::device::AbstractSpeaker>& speaker)
{
    return speaker->stopPlayback()
               ? StopOutcome{true, {}}
               : StopOutcome{false, "speaker playback did not stop"};
}

StopOutcome stopTrackedMediaActivities(const std::string& device_id)
{
    std::vector<std::string> failures;
    bool stopped = cmvr::media::globalMediaSourceManager()
        .stopSourcesForDevice(device_id, &failures);
    if (!globalCameraPtzActivityRegistry().stopActivitiesForDevice(
            device_id, &failures)) {
        stopped = false;
    }
    if (!globalCameraOperationalActivityRegistry().stopActivitiesForDevice(
            device_id, &failures)) {
        stopped = false;
    }
    return {
        stopped,
        failures.empty()
            ? std::string{}
            : "tracked media activities were not fully stopped: " +
                  failures.front()};
}

void mergeStopOutcome(
    const StopOutcome& outcome,
    bool& all_stopped,
    std::vector<std::string>& failures)
{
    if (outcome.success) {
        return;
    }
    all_stopped = false;
    failures.push_back(
        outcome.detail.empty()
            ? "operational stop was not confirmed"
            : outcome.detail);
}

template <typename DeviceType>
StopOutcome stopOperationalDevice(
    const std::shared_ptr<DeviceType>& device,
    const char* description);

StopOutcome stopOtherActivities(
    const std::string& device_id,
    const std::shared_ptr<cmvr::device::AbstractCamera>& camera,
    const std::shared_ptr<cmvr::device::AbstractMicrophone>& microphone,
    const std::shared_ptr<cmvr::device::AbstractSpeaker>& speaker,
    const std::shared_ptr<cmvr::device::AbstractBiohead>& head,
    const std::shared_ptr<cmvr::device::AbstractGripper>& gripper,
    const bool tracked_media)
{
    bool all_stopped = true;
    bool media_stopped_by_typed_device = false;
    std::vector<std::string> failures;

    if (camera) {
        mergeStopOutcome(
            invokeStopOperation(
                [&] { return stopCameraActivities(device_id, camera); },
                "camera activity stop"),
            all_stopped, failures);
        media_stopped_by_typed_device = true;
    }
    if (microphone) {
        mergeStopOutcome(
            invokeStopOperation(
                [&] { return stopMicrophone(device_id, microphone); },
                "microphone activity stop"),
            all_stopped, failures);
        media_stopped_by_typed_device = true;
    }
    if (tracked_media && !media_stopped_by_typed_device) {
        mergeStopOutcome(
            invokeStopOperation(
                [&] { return stopTrackedMediaActivities(device_id); },
                "tracked media activity stop"),
            all_stopped, failures);
    }
    if (speaker) {
        mergeStopOutcome(
            invokeStopOperation(
                [&] { return stopSpeaker(speaker); },
                "speaker activity stop"),
            all_stopped, failures);
    }
    if (head) {
        mergeStopOutcome(
            invokeStopOperation(
                [&] { return stopOperationalDevice(head, "BioHead"); },
                "BioHead activity stop"),
            all_stopped, failures);
    }
    if (gripper) {
        mergeStopOutcome(
            invokeStopOperation(
                [&] { return stopOperationalDevice(gripper, "gripper"); },
                "gripper activity stop"),
            all_stopped, failures);
    }

    return {
        all_stopped,
        failures.empty() ? std::string{} : failures.front()};
}

StopOutcome stopTaskActivity(
    const std::shared_ptr<cmvr::task::Task>& task)
{
    if (!task) {
        return {false, "task activity target was null"};
    }
    return task->stopActivity()
               ? StopOutcome{true, {}}
               : StopOutcome{
                     false,
                     "task " + task->id() +
                         " operational stop was not confirmed"};
}

void submitDeferredOperations(
    StopDispatcher& dispatcher,
    std::vector<cmvr::service::DeferredStopOperation> operations,
    std::vector<StopHandleEntry>& handles)
{
    handles.reserve(handles.size() + operations.size());
    for (auto& operation : operations) {
        const auto subject = operation.resource_key;
        auto callback = std::move(operation.operation);
        auto handle = dispatcher.submit(
            operation.resource_key,
            [callback = std::move(callback)]() mutable {
                const auto result = callback();
                return StopOutcome{result.success, result.detail};
            });
        handles.push_back(
            {subject, std::move(handle), false, true});
    }
}

template <typename DeviceType>
StopOutcome stopOperationalDevice(
    const std::shared_ptr<DeviceType>& device,
    const char* description)
{
    return device->stopOperationalActivity()
               ? StopOutcome{true, {}}
               : StopOutcome{
                     false,
                     std::string(description) +
                         " operational stop was not confirmed"};
}

cmvr::api::SystemDeviceType toApiDeviceType(
    const cmvr::device::DeviceKind kind) noexcept
{
    switch (kind) {
        case cmvr::device::DeviceKind::AGV:
            return cmvr::api::SYSTEM_DEVICE_TYPE_AGV;
        case cmvr::device::DeviceKind::Arm:
            return cmvr::api::SYSTEM_DEVICE_TYPE_ARM;
        case cmvr::device::DeviceKind::Battery:
            return cmvr::api::SYSTEM_DEVICE_TYPE_BATTERY;
        case cmvr::device::DeviceKind::BioHead:
            return cmvr::api::SYSTEM_DEVICE_TYPE_BIO_HEAD;
        case cmvr::device::DeviceKind::Camera:
            return cmvr::api::SYSTEM_DEVICE_TYPE_CAMERA;
        case cmvr::device::DeviceKind::CanBus:
            return cmvr::api::SYSTEM_DEVICE_TYPE_CAN_BUS;
        case cmvr::device::DeviceKind::DexHand:
            return cmvr::api::SYSTEM_DEVICE_TYPE_DEX_HAND;
        case cmvr::device::DeviceKind::Gripper:
            return cmvr::api::SYSTEM_DEVICE_TYPE_GRIPPER;
        case cmvr::device::DeviceKind::Microphone:
            return cmvr::api::SYSTEM_DEVICE_TYPE_MICROPHONE;
        case cmvr::device::DeviceKind::Motor:
            return cmvr::api::SYSTEM_DEVICE_TYPE_MOTOR;
        case cmvr::device::DeviceKind::MotorSystem:
            return cmvr::api::SYSTEM_DEVICE_TYPE_MOTOR_SYSTEM;
        case cmvr::device::DeviceKind::MujocoViewer:
            return cmvr::api::SYSTEM_DEVICE_TYPE_MUJOCO_VIEWER;
        case cmvr::device::DeviceKind::MujocoWorld:
            return cmvr::api::SYSTEM_DEVICE_TYPE_MUJOCO_WORLD;
        case cmvr::device::DeviceKind::Robot:
            return cmvr::api::SYSTEM_DEVICE_TYPE_ROBOT;
        case cmvr::device::DeviceKind::Speaker:
            return cmvr::api::SYSTEM_DEVICE_TYPE_SPEAKER;
        case cmvr::device::DeviceKind::Unknown:
            break;
    }
    return cmvr::api::SYSTEM_DEVICE_TYPE_UNSPECIFIED;
}

cmvr::api::SystemDeviceState toApiDeviceState(
    const cmvr::device::ManagedDeviceState state) noexcept
{
    switch (state) {
        case cmvr::device::ManagedDeviceState::Disabled:
            return cmvr::api::SYSTEM_DEVICE_STATE_DISABLED;
        case cmvr::device::ManagedDeviceState::Initializing:
            return cmvr::api::SYSTEM_DEVICE_STATE_INITIALIZING;
        case cmvr::device::ManagedDeviceState::Registered:
            return cmvr::api::SYSTEM_DEVICE_STATE_REGISTERED;
        case cmvr::device::ManagedDeviceState::Ready:
            return cmvr::api::SYSTEM_DEVICE_STATE_READY;
        case cmvr::device::ManagedDeviceState::Running:
            return cmvr::api::SYSTEM_DEVICE_STATE_RUNNING;
        case cmvr::device::ManagedDeviceState::Stopped:
            return cmvr::api::SYSTEM_DEVICE_STATE_STOPPED;
        case cmvr::device::ManagedDeviceState::Error:
            return cmvr::api::SYSTEM_DEVICE_STATE_ERROR;
        case cmvr::device::ManagedDeviceState::Unknown:
            break;
    }
    return cmvr::api::SYSTEM_DEVICE_STATE_UNSPECIFIED;
}

cmvr::api::SystemDeviceHealth toApiDeviceHealth(
    const cmvr::device::DeviceHealthState state) noexcept
{
    switch (state) {
        case cmvr::device::DeviceHealthState::Healthy:
            return cmvr::api::SYSTEM_DEVICE_HEALTH_HEALTHY;
        case cmvr::device::DeviceHealthState::Degraded:
            return cmvr::api::SYSTEM_DEVICE_HEALTH_DEGRADED;
        case cmvr::device::DeviceHealthState::Fault:
            return cmvr::api::SYSTEM_DEVICE_HEALTH_FAULT;
        case cmvr::device::DeviceHealthState::Unknown:
            break;
    }
    return cmvr::api::SYSTEM_DEVICE_HEALTH_UNSPECIFIED;
}

struct ParsedSafetyScope final {
    bool valid{false};
    bool all_devices{false};
    std::vector<std::string> device_ids;
    std::string error;
};

ParsedSafetyScope parseSafetyScope(
    const cmvr::api::SafetyScope& scope,
    const bool default_to_all)
{
    ParsedSafetyScope result;
    switch (scope.target_case()) {
    case cmvr::api::SafetyScope::kAllDevices:
        if (!scope.all_devices()) {
            result.error = "all_devices must be explicitly true";
            return result;
        }
        result.valid = true;
        result.all_devices = true;
        return result;
    case cmvr::api::SafetyScope::kDevices: {
        if (scope.devices().device_ids().empty()) {
            result.error = "device scope must contain at least one device ID";
            return result;
        }
        std::unordered_set<std::string> unique;
        result.device_ids.reserve(scope.devices().device_ids_size());
        for (const auto& id : scope.devices().device_ids()) {
            if (id.empty() || !unique.insert(id).second) {
                result.error =
                    "device scope IDs must be non-empty and unique";
                return result;
            }
            result.device_ids.push_back(id);
        }
        result.valid = true;
        return result;
    }
    case cmvr::api::SafetyScope::TARGET_NOT_SET:
        if (default_to_all) {
            result.valid = true;
            result.all_devices = true;
        } else {
            result.error = "an explicit recovery scope is required";
        }
        return result;
    }
    result.error = "invalid safety scope";
    return result;
}

void setSafetyHeaderFailure(
    cmvr::api::CommandHeader_Feedback* header,
    const cmvr::safety::SafetyReason reason,
    const std::string& detail)
{
    header->set_success(false);
    header->set_reason_code(toApiSafetyReason(reason));
    header->set_error_message(detail);
    header->set_execution_state(
        cmvr::api::COMMAND_EXECUTION_STATE_REJECTED_BEFORE_DISPATCH);
    setCurrentTimestamp(header->mutable_timestamp());
}

bool recoveryCompletedAsRequested(
    const cmvr::safety::RecoveryResultCode result) noexcept
{
    return result == cmvr::safety::RecoveryResultCode::Recovered ||
           result ==
               cmvr::safety::RecoveryResultCode::VerifiedButStillBlocked ||
           result == cmvr::safety::RecoveryResultCode::NothingToRecover;
}

} // namespace

gRPCSystemServiceImpl::gRPCSystemServiceImpl()
    : gRPCSystemServiceImpl(
          std::chrono::seconds(15), makeDefaultGrpcSecurityGateway())
{
}

gRPCSystemServiceImpl::gRPCSystemServiceImpl(
    const std::chrono::milliseconds stop_timeout)
    : gRPCSystemServiceImpl(stop_timeout, makeDefaultGrpcSecurityGateway())
{
}

gRPCSystemServiceImpl::gRPCSystemServiceImpl(
    std::shared_ptr<GrpcSecurityGateway> security_gateway)
    : gRPCSystemServiceImpl(
          std::chrono::seconds(15), std::move(security_gateway))
{
}

gRPCSystemServiceImpl::gRPCSystemServiceImpl(
    const std::chrono::milliseconds stop_timeout,
    std::shared_ptr<GrpcSecurityGateway> security_gateway)
    : gRPCSystemServiceImpl(
          stop_timeout, std::move(security_gateway), nullptr)
{
}

gRPCSystemServiceImpl::gRPCSystemServiceImpl(
    const std::chrono::milliseconds stop_timeout,
    std::shared_ptr<GrpcSecurityGateway> security_gateway,
    std::shared_ptr<RecoveryAuditSink> recovery_audit_sink)
    : dmgr_(DeviceManager::getInstance()),
      stop_timeout_(
          stop_timeout > std::chrono::milliseconds::zero()
              ? stop_timeout
              : std::chrono::seconds(15)),
      security_gateway_(
          security_gateway ? std::move(security_gateway)
                           : makeDefaultGrpcSecurityGateway()),
      recovery_audit_sink_(std::move(recovery_audit_sink)),
      action_queue_(std::make_shared<ActionQueueExecutor>(dmgr_))
{
    // Acquire only after ActionQueue construction succeeds. This ensures an
    // exception cannot release the last dispatcher outside the registry.
    acquireProcessStopDispatcher(stop_dispatcher_);
    try {
        safety_participant_registration_ = registerGrpcSafetyParticipants(
            dmgr_.safetyManager(), action_queue_, stop_dispatcher_);
    } catch (...) {
        releaseProcessStopDispatcher(stop_dispatcher_);
        throw;
    }
}

gRPCSystemServiceImpl::~gRPCSystemServiceImpl()
{
    // The dispatcher intentionally outlives ActionQueue, then joins any
    // deadline-overrunning stop workers before the last service disappears.
    safety_participant_registration_.reset();
    action_queue_.reset();
    releaseProcessStopDispatcher(stop_dispatcher_);
}

bool gRPCSystemServiceImpl::waitForStopDispatcherDestructionForTesting(
    const std::chrono::milliseconds timeout)
{
    auto& registry = processStopDispatcherRegistry();
    std::unique_lock lock(registry.mutex);
    return registry.available.wait_for(
        lock,
        timeout,
        [&registry] { return registry.destroying; });
}

void gRPCSystemServiceImpl::prepareForShutdown()
{
    // Do not wait for a business StopAll round here. StopAll may be blocked in
    // a device backend, while server shutdown must still invalidate queued and
    // active ActionQueue work promptly. ActionQueueExecutor owns the state lock
    // and generation fence needed to make this transition race-safe; an
    // in-flight StopAll ticket then becomes stale and fails closed.
    (void)action_queue_->disableForShutdown();
}

grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context,
    const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response)
{
    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
        security_gateway_, context,
        "/cmvr.api.SystemService/GetSystemInfo");
    try {
        response->set_version(dmgr_.version());
        response->set_system_name(dmgr_.name());
        response->set_action_service_instance_id(
            action_queue_->instanceId());
        const auto& security = security_gateway_->config();
        response->set_grpc_transport_security(toString(security.transport));
        response->set_grpc_authentication(toString(security.authentication));
        response->set_grpc_recovery_exposure(
            toString(security.recovery_exposure));
        response->set_grpc_insecure_non_loopback(
            security.insecure_non_loopback);
        const auto safety = dmgr_.safetyManager().snapshot();
        response->set_control_service_instance_id(
            safety.service_instance_id);
        response->set_safety_enforcement_mode(
            cmvr::safety::toString(safety.enforcement_mode));
        response->set_safety_schema_version(1);
        response->mutable_header()->set_success(true);
        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
        CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemInfo): success, name="
                        << response->system_name() << ", version=" << response->version();
        return grpc::Status::OK;
    }
    catch (std::exception& e) {
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(e.what());
        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
        return grpc::Status::OK;
    }
}

grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context,
    const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response)
{
    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
        security_gateway_, context,
        "/cmvr.api.SystemService/GetSystemStatus");
    try {
        std::list<std::pair<std::string, std::string>> dev_list;
        dmgr_.getDeviceList(dev_list);
        for (auto &pair: dev_list) {
            auto* dev = response->add_device_list();
            dev->set_device_id(pair.first);
            if (pair.second == "AGV") {
                dev->set_device_type(api::DeviceType::AGV);
            }
            else if (pair.second == "Battery") {
                dev->set_device_type(api::DeviceType::Battery);
            }
            else if (pair.second == "Camera") {
                dev->set_device_type(api::DeviceType::Camera);
            }
            else if (pair.second == "DexHand") {
                dev->set_device_type(api::DeviceType::DexHand);
            }
            else if (pair.second == "Gripper") {
                dev->set_device_type(api::DeviceType::Gripper);
            }
            else if (pair.second == "Microphone") {
                dev->set_device_type(api::DeviceType::Microphone);
            }
            else if (pair.second == "Robot") {
                dev->set_device_type(api::DeviceType::Robot);
            }
            else if (pair.second == "Speaker") {
                dev->set_device_type(api::DeviceType::Speaker);
            }
            else if (pair.second == "Unknown") {
                dev->set_device_type(api::DeviceType::Unknown);
            }
        }
        response->mutable_header()->set_success(true);
        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
        CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemStatus): success, devices="
                        << response->device_list_size();
        return grpc::Status::OK;
    }
    catch (const std::exception &e) {
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(e.what());
        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
        return grpc::Status::OK;
    }
}

grpc::Status gRPCSystemServiceImpl::GetDeviceList(
    grpc::ServerContext* context,
    const api::GetDeviceListCommand_Request* request,
    api::GetDeviceListCommand_Feedback* response)
{
    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
        security_gateway_, context,
        "/cmvr.api.SystemService/GetDeviceList");
    (void)request;
    try {
        const auto snapshot = dmgr_.snapshot();
        response->set_manager_name(snapshot.name);
        response->set_manager_version(snapshot.version);
        response->set_manager_description(snapshot.description);
        response->set_sampled_at_unix_ms(unixTimeMs());

        for (const auto& source : snapshot.devices) {
            // The public inventory contains enabled entries only. Keep enabled
            // devices visible even when their lifecycle or health is in error.
            if (!source.enabled) {
                continue;
            }

            auto* destination = response->add_device_list();
            destination->set_device_id(source.id);
            destination->set_device_type(toApiDeviceType(source.kind));
            destination->set_type_name(source.type_name);
            destination->set_enabled(true);
            destination->set_manager_state(toApiDeviceState(source.state));
            destination->set_health(toApiDeviceHealth(source.health.state));
            destination->set_has_error(source.abnormal);
            destination->set_error_message(source.error_message);
            destination->set_status_updated_at_unix_ms(
                source.status_updated_at_unix_ms);
        }

        response->mutable_header()->set_success(true);
        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
        CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetDeviceList): success, devices="
                        << response->device_list_size();
        return grpc::Status::OK;
    }
    catch (const std::exception& e) {
        response->Clear();
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(e.what());
        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
        return grpc::Status::OK;
    }
}

grpc::Status gRPCSystemServiceImpl::GetSafetyState(
    grpc::ServerContext* context,
    const cmvr::api::GetSafetyStateCommand_Request* request,
    cmvr::api::GetSafetyStateCommand_Feedback* response)
{
    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
        security_gateway_, context,
        "/cmvr.api.SystemService/GetSafetyState");
    if (!request || !response) {
        return grpc::Status(
            grpc::StatusCode::INVALID_ARGUMENT,
            "GetSafetyState request and response are required");
    }

    const auto scope = parseSafetyScope(request->scope(), true);
    if (!scope.valid) {
        setSafetyHeaderFailure(
            response->mutable_header(),
            cmvr::safety::SafetyReason::InvalidArgument,
            scope.error);
        return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, scope.error);
    }

    const auto snapshot = dmgr_.safetyManager().snapshot();
    response->set_system_state(
        toApiSystemAdmissionState(snapshot.system_state));
    response->set_safety_epoch(snapshot.safety_epoch);
    response->set_control_service_instance_id(snapshot.service_instance_id);
    response->set_enforcement_mode(
        cmvr::safety::toString(snapshot.enforcement_mode));
    response->set_active_operation_id(snapshot.active_operation_id);
    response->set_active_operation_phase(snapshot.active_operation_phase);
    response->set_sampled_at_unix_ms(unixTimeMs());

    std::unordered_set<std::string> requested_ids(
        scope.device_ids.begin(), scope.device_ids.end());
    for (const auto& device : snapshot.devices) {
        if (!scope.all_devices &&
            requested_ids.erase(device.descriptor.device_id) == 0U) {
            continue;
        }
        populateDeviceSafetyState(device, *response->add_devices());
    }
    for (const auto& participant : snapshot.participants) {
        populateSafetyParticipantState(
            participant, *response->add_participants());
    }
    if (!requested_ids.empty()) {
        const auto detail =
            "safety device is not registered: " + *requested_ids.begin();
        response->Clear();
        setSafetyHeaderFailure(
            response->mutable_header(),
            cmvr::safety::SafetyReason::DeviceNotFound,
            detail);
        return grpc::Status(grpc::StatusCode::NOT_FOUND, detail);
    }

    response->mutable_header()->set_success(true);
    response->mutable_header()->set_reason_code(
        cmvr::api::COMMAND_REASON_CODE_NONE);
    response->mutable_header()->set_service_instance_id(
        snapshot.service_instance_id);
    response->mutable_header()->set_safety_epoch(snapshot.safety_epoch);
    response->mutable_header()->set_execution_state(
        cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED);
    setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
    return grpc::Status::OK;
}

grpc::Status gRPCSystemServiceImpl::RecoverSafetyState(
    grpc::ServerContext* context,
    const cmvr::api::RecoverSafetyStateCommand_Request* request,
    cmvr::api::RecoverSafetyStateCommand_Feedback* response)
{
    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
        security_gateway_, context,
        "/cmvr.api.SystemService/RecoverSafetyState");
    if (!request || !response) {
        return grpc::Status(
            grpc::StatusCode::INVALID_ARGUMENT,
            "RecoverSafetyState request and response are required");
    }

    const auto scope = parseSafetyScope(request->scope(), false);
    if (!scope.valid || request->recovery_id().empty() ||
        request->reason().empty() || request->expected_safety_epoch() == 0 ||
        request->mode() ==
            cmvr::api::RecoverSafetyStateCommand::MODE_UNSPECIFIED) {
        std::string detail = scope.valid
            ? "recovery_id, reason, expected_safety_epoch, and mode are required"
            : scope.error;
        setSafetyHeaderFailure(
            response->mutable_header(),
            request->reason().empty()
                ? cmvr::safety::SafetyReason::RecoveryReasonRequired
                : cmvr::safety::SafetyReason::InvalidArgument,
            detail);
        return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, detail);
    }
    if (!recovery_audit_sink_) {
        const std::string detail =
            "persistent recovery audit is not configured";
        setSafetyHeaderFailure(
            response->mutable_header(),
            cmvr::safety::SafetyReason::RecoveryAuditFailed,
            detail);
        return grpc::Status(
            grpc::StatusCode::FAILED_PRECONDITION,
            "RECOVERY_AUDIT_FAILED");
    }

    const bool verify_only = request->mode() ==
        cmvr::api::RecoverSafetyStateCommand::VERIFY_ONLY;
    RecoveryAuditRecord audit;
    audit.occurred_at_unix_ms = unixTimeMs();
    audit.stage = "accepted";
    audit.correlation_id = cmvr_grpc_call_guard.context().correlation_id;
    audit.principal_id = cmvr_grpc_call_guard.context().principal.id;
    audit.peer = cmvr_grpc_call_guard.context().peer;
    audit.recovery_id = request->recovery_id();
    audit.reason = request->reason();
    audit.mode = verify_only ? "verify_only" : "clear_software_latch";
    audit.all_devices = scope.all_devices;
    audit.device_ids = scope.device_ids;
    audit.expected_safety_epoch = request->expected_safety_epoch();
    audit.result = "pending";
    std::string audit_error;
    if (!recovery_audit_sink_->append(audit, &audit_error)) {
        const auto detail = audit_error.empty()
            ? std::string("persistent recovery audit write failed")
            : audit_error;
        setSafetyHeaderFailure(
            response->mutable_header(),
            cmvr::safety::SafetyReason::RecoveryAuditFailed,
            detail);
        return grpc::Status(
            grpc::StatusCode::FAILED_PRECONDITION,
            "RECOVERY_AUDIT_FAILED");
    }

    auto deadline = cmvr_grpc_call_guard.context().deadline;
    const auto configured_deadline =
        cmvr::safety::SafetyClock::now() +
        dmgr_.safetyManager().config().recovery_timeout;
    if (deadline == cmvr::safety::SafetyClock::time_point::max() ||
        configured_deadline < deadline) {
        deadline = configured_deadline;
    }
    if (request->timeout_ms() != 0) {
        deadline = std::min(
            deadline,
            cmvr::safety::SafetyClock::now() +
                std::chrono::milliseconds(request->timeout_ms()));
    }

    cmvr::safety::RecoveryRequest coordinator_request;
    coordinator_request.recovery_id = request->recovery_id();
    coordinator_request.device_ids = scope.device_ids;
    coordinator_request.all_devices = scope.all_devices;
    coordinator_request.expected_safety_epoch =
        request->expected_safety_epoch();
    coordinator_request.verify_only = verify_only;
    coordinator_request.reason = request->reason();
    coordinator_request.deadline = deadline;
    if (!verify_only) {
        auto commit_audit = audit;
        commit_audit.stage = "clear_commit";
        commit_audit.result = "authorized";
        const auto sink = recovery_audit_sink_;
        coordinator_request.authorize_clear =
            [sink, commit_audit = std::move(commit_audit)]() mutable {
                commit_audit.occurred_at_unix_ms = unixTimeMs();
                std::string error;
                const bool persisted = sink->append(commit_audit, &error);
                if (!persisted) {
                    CMVR_LOG(ERROR)
                        << "[gRPCSystemServiceImpl] Recovery clear audit "
                           "failed: "
                        << error;
                }
                return persisted;
            };
    }

    const auto result =
        dmgr_.safetyManager().recover(coordinator_request);
    response->set_recovery_id(result.recovery_id);
    response->set_result(toApiRecoveryResult(result.result));
    response->set_previous_safety_epoch(result.previous_safety_epoch);
    response->set_current_safety_epoch(result.current_safety_epoch);
    response->set_system_state(
        toApiSystemAdmissionState(result.system_state));
    for (const auto& target : result.targets) {
        populateSafetyTargetResult(target, *response->add_targets());
    }

    const bool success = recoveryCompletedAsRequested(result.result);
    auto* header = response->mutable_header();
    header->set_success(success);
    header->set_command_id(request->recovery_id());
    header->set_service_instance_id(
        dmgr_.safetyManager().serviceInstanceId());
    header->set_safety_epoch(result.current_safety_epoch);
    header->set_execution_state(
        success ? cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED
                : cmvr::api::COMMAND_EXECUTION_STATE_FAILED);
    if (success) {
        header->set_reason_code(cmvr::api::COMMAND_REASON_CODE_NONE);
    } else if (!result.targets.empty()) {
        header->set_reason_code(
            toApiSafetyReason(result.targets.front().reason));
        header->set_error_message(result.targets.front().detail);
    } else {
        header->set_reason_code(
            cmvr::api::COMMAND_REASON_CODE_INTERNAL_ERROR);
        header->set_error_message("recovery failed without a target result");
    }
    setCurrentTimestamp(header->mutable_timestamp());

    audit.occurred_at_unix_ms = unixTimeMs();
    audit.stage = "completed";
    audit.previous_safety_epoch = result.previous_safety_epoch;
    audit.current_safety_epoch = result.current_safety_epoch;
    audit.result = cmvr::safety::toString(result.result);
    if (!recovery_audit_sink_->append(audit, &audit_error)) {
        CMVR_LOG(ERROR)
            << "[gRPCSystemServiceImpl] Recovery completion audit failed: "
            << audit_error;
    }
    return grpc::Status::OK;
}

grpc::Status gRPCSystemServiceImpl::RestoreOperationalState(
    grpc::ServerContext* context,
    const cmvr::api::RestoreOperationalStateCommand_Request* request,
    cmvr::api::RestoreOperationalStateCommand_Feedback* response)
{
    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
        security_gateway_, context,
        "/cmvr.api.SystemService/RestoreOperationalState");
    if (!request || !response) {
        return grpc::Status(
            grpc::StatusCode::INVALID_ARGUMENT,
            "RestoreOperationalState request and response are required");
    }
    if (request->recovery_id().empty() || request->reason().empty()) {
        const std::string detail =
            "recovery_id and reason are required";
        setSafetyHeaderFailure(
            response->mutable_header(),
            request->reason().empty()
                ? cmvr::safety::SafetyReason::RecoveryReasonRequired
                : cmvr::safety::SafetyReason::InvalidArgument,
            detail);
        return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, detail);
    }
    if (!recovery_audit_sink_) {
        const std::string detail =
            "persistent recovery audit is not configured";
        setSafetyHeaderFailure(
            response->mutable_header(),
            cmvr::safety::SafetyReason::RecoveryAuditFailed,
            detail);
        return grpc::Status(
            grpc::StatusCode::FAILED_PRECONDITION,
            "RECOVERY_AUDIT_FAILED");
    }

    const auto accepted_snapshot = dmgr_.safetyManager().snapshot();
    RecoveryAuditRecord audit;
    audit.occurred_at_unix_ms = unixTimeMs();
    audit.stage = "accepted";
    audit.correlation_id = cmvr_grpc_call_guard.context().correlation_id;
    audit.principal_id = cmvr_grpc_call_guard.context().principal.id;
    audit.peer = cmvr_grpc_call_guard.context().peer;
    audit.recovery_id = request->recovery_id();
    audit.reason = request->reason();
    audit.mode = "restore_operational_state";
    audit.all_devices = true;
    audit.expected_safety_epoch = accepted_snapshot.safety_epoch;
    audit.result = "pending";
    std::string audit_error;
    if (!recovery_audit_sink_->append(audit, &audit_error)) {
        const auto detail = audit_error.empty()
            ? std::string("persistent recovery audit write failed")
            : audit_error;
        setSafetyHeaderFailure(
            response->mutable_header(),
            cmvr::safety::SafetyReason::RecoveryAuditFailed,
            detail);
        return grpc::Status(
            grpc::StatusCode::FAILED_PRECONDITION,
            "RECOVERY_AUDIT_FAILED");
    }

    auto deadline = cmvr_grpc_call_guard.context().deadline;
    const auto configured_deadline =
        cmvr::safety::SafetyClock::now() +
        dmgr_.safetyManager().config().recovery_timeout;
    if (deadline == cmvr::safety::SafetyClock::time_point::max() ||
        configured_deadline < deadline) {
        deadline = configured_deadline;
    }
    if (request->timeout_ms() != 0) {
        deadline = std::min(
            deadline,
            cmvr::safety::SafetyClock::now() +
                std::chrono::milliseconds(request->timeout_ms()));
    }

    auto commit_audit = audit;
    commit_audit.stage = "restore_commit";
    commit_audit.result = "authorized";
    const auto sink = recovery_audit_sink_;

    cmvr::safety::RecoveryRequest coordinator_request;
    coordinator_request.recovery_id = request->recovery_id();
    coordinator_request.all_devices = true;
    coordinator_request.expected_safety_epoch =
        accepted_snapshot.safety_epoch;
    coordinator_request.verify_only = false;
    coordinator_request.restore_operational_state = true;
    coordinator_request.reason = request->reason();
    coordinator_request.deadline = deadline;
    coordinator_request.authorize_clear =
        [sink, commit_audit = std::move(commit_audit)]() mutable {
            commit_audit.occurred_at_unix_ms = unixTimeMs();
            std::string error;
            const bool persisted = sink->append(commit_audit, &error);
            if (!persisted) {
                CMVR_LOG(ERROR)
                    << "[gRPCSystemServiceImpl] Operational restore audit "
                       "failed: "
                    << error;
            }
            return persisted;
        };

    const auto result =
        dmgr_.safetyManager().recover(coordinator_request);
    response->set_recovery_id(result.recovery_id);
    response->set_result(toApiRecoveryResult(result.result));
    response->set_previous_safety_epoch(result.previous_safety_epoch);
    response->set_current_safety_epoch(result.current_safety_epoch);
    response->set_system_state(
        toApiSystemAdmissionState(result.system_state));
    for (const auto& target : result.targets) {
        populateSafetyTargetResult(target, *response->add_targets());
    }

    const bool success = recoveryCompletedAsRequested(result.result);
    auto* header = response->mutable_header();
    header->set_success(success);
    header->set_command_id(request->recovery_id());
    header->set_service_instance_id(
        dmgr_.safetyManager().serviceInstanceId());
    header->set_safety_epoch(result.current_safety_epoch);
    header->set_execution_state(
        success ? cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED
                : cmvr::api::COMMAND_EXECUTION_STATE_FAILED);
    if (success) {
        header->set_reason_code(cmvr::api::COMMAND_REASON_CODE_NONE);
    } else {
        const auto failed = std::find_if(
            result.targets.begin(), result.targets.end(),
            [](const auto& target) { return !target.success; });
        if (failed != result.targets.end()) {
            header->set_reason_code(toApiSafetyReason(failed->reason));
            header->set_error_message(failed->detail);
        } else {
            header->set_reason_code(
                cmvr::api::COMMAND_REASON_CODE_INTERNAL_ERROR);
            header->set_error_message(
                "operational restore failed without a target result");
        }
    }
    setCurrentTimestamp(header->mutable_timestamp());

    audit.occurred_at_unix_ms = unixTimeMs();
    audit.stage = "completed";
    audit.previous_safety_epoch = result.previous_safety_epoch;
    audit.current_safety_epoch = result.current_safety_epoch;
    audit.result = cmvr::safety::toString(result.result);
    if (!recovery_audit_sink_->append(audit, &audit_error)) {
        CMVR_LOG(ERROR)
            << "[gRPCSystemServiceImpl] Operational restore completion audit "
               "failed: "
            << audit_error;
    }
    return grpc::Status::OK;
}

grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response)
{
    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
        security_gateway_, context,
        "/cmvr.api.SystemService/UpdateParams");
    response->mutable_header()->set_success(false);
    response->mutable_header()->set_error_message("UpdateParams is no longer supported. Use typed device commands or reload configuration.");
    setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
    return grpc::Status::OK;
}

grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context,
    const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response)
{
    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
        security_gateway_, context, "/cmvr.api.SystemService/StopAll");
    if (!request || !response) {
        return grpc::Status(
            grpc::StatusCode::INVALID_ARGUMENT,
            "StopAll request and response are required");
    }
    if (dmgr_.safetyManager().config().enforcement_mode !=
        cmvr::safety::EnforcementMode::Legacy) {
        auto deadline = cmvr_grpc_call_guard.context().deadline;
        const auto configured_deadline =
            cmvr::safety::SafetyClock::now() + stop_timeout_;
        if (deadline == cmvr::safety::SafetyClock::time_point::max() ||
            configured_deadline < deadline) {
            deadline = configured_deadline;
        }
        if (request->timeout_ms() != 0) {
            deadline = std::min(
                deadline,
                cmvr::safety::SafetyClock::now() +
                    std::chrono::milliseconds(request->timeout_ms()));
        }

        std::string operation_id = request->operation_id();
        if (operation_id.empty() && request->has_header()) {
            operation_id = request->header().command_id();
        }
        const auto result = dmgr_.safetyManager().stopAll(
            std::move(operation_id), deadline);
        response->set_operation_id(result.operation_id);
        response->set_previous_safety_epoch(
            result.previous_safety_epoch);
        response->set_current_safety_epoch(
            result.current_safety_epoch);
        response->set_system_state(
            toApiSystemAdmissionState(result.system_state));
        for (const auto& target : result.targets) {
            populateSafetyTargetResult(target, *response->add_targets());
        }

        auto* header = response->mutable_header();
        header->set_success(result.success);
        header->set_command_id(result.operation_id);
        header->set_service_instance_id(
            dmgr_.safetyManager().serviceInstanceId());
        header->set_safety_epoch(result.current_safety_epoch);
        header->set_execution_state(
            result.success
                ? cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED
                : cmvr::api::COMMAND_EXECUTION_STATE_FAILED);
        if (result.success) {
            header->set_reason_code(cmvr::api::COMMAND_REASON_CODE_NONE);
        } else if (!result.targets.empty()) {
            const auto failed = std::find_if(
                result.targets.begin(), result.targets.end(),
                [](const auto& target) { return !target.success; });
            if (failed != result.targets.end()) {
                header->set_reason_code(toApiSafetyReason(failed->reason));
                header->set_error_message(failed->detail);
            }
        } else {
            header->set_reason_code(
                cmvr::api::COMMAND_REASON_CODE_STOP_UNCONFIRMED);
            header->set_error_message(
                "StopAll did not produce a participant result");
        }
        setCurrentTimestamp(header->mutable_timestamp());
        return grpc::Status::OK;
    }
    (void)request;
    const auto stop_deadline =
        std::chrono::steady_clock::now() + stop_timeout_;

    std::unique_lock<std::timed_mutex> stop_all_lock(
        processStopAllMutex(), std::defer_lock);
    while (!stop_all_lock.try_lock_for(std::min(
        std::chrono::milliseconds(50),
        remainingStopBudget(stop_deadline)))) {
        if (context && context->IsCancelled()) {
            return grpc::Status(
                grpc::StatusCode::CANCELLED,
                "StopAll was cancelled while waiting for another StopAll round");
        }
        if (remainingStopBudget(stop_deadline) ==
            std::chrono::milliseconds::zero()) {
            response->mutable_header()->set_success(false);
            response->mutable_header()->set_error_message(
                "StopAll timed out waiting for another StopAll round");
            setCurrentTimestamp(
                response->mutable_header()->mutable_timestamp());
            return grpc::Status::OK;
        }
    }

    try {
        // Close every admission domain before resolving devices or issuing
        // stops. The process remains alive; only new operational work pauses.
        ScopedAdmissionStopAll admission_stop;
        ScopedMediaStopAll media_stop;
        ScopedMotorStopAll motor_stop;
        ScopedActionQueueStopAll action_stop(*action_queue_);
        if (!admission_stop.valid()) {
            throw std::runtime_error(
                "StopAll could not establish the system admission barrier");
        }
        if (!media_stop.valid()) {
            throw std::runtime_error(
                "StopAll could not establish the media activity barrier");
        }
        if (!action_stop.valid()) {
            throw std::runtime_error(
                "StopAll cannot start because the service is shutting down");
        }
        if (!motor_stop.valid()) {
            throw std::runtime_error(
                "StopAll could not establish the motor activity barrier");
        }

        // This snapshot is intentionally metadata-only. A wedged health query
        // must not prevent physical stop requests from being dispatched.
        const auto inventory = dmgr_.inventorySnapshot();
        ScopedControlBarrierSet control_barriers;
        std::vector<std::string> unconfirmed_devices;

        struct ControlTarget final {
            std::string id;
            cmvr::device::DeviceKind kind{
                cmvr::device::DeviceKind::Unknown};
            std::shared_ptr<cmvr::device::RobotArm> arm;
            std::shared_ptr<cmvr::device::AbstractAGV> agv;
            std::shared_ptr<cmvr::device::AbstractDexHand> hand;
            cmvr::control::ControlLeaseToken barrier;
        };
        std::vector<ControlTarget> control_targets;
        control_targets.reserve(inventory.size());

        struct OtherTarget final {
            std::string id;
            std::shared_ptr<cmvr::device::AbstractCamera> camera;
            std::shared_ptr<cmvr::device::AbstractMicrophone> microphone;
            std::shared_ptr<cmvr::device::AbstractSpeaker> speaker;
            std::shared_ptr<cmvr::device::AbstractBiohead> head;
            std::shared_ptr<cmvr::device::AbstractGripper> gripper;
            bool tracked_media{false};
        };
        std::unordered_map<std::string, OtherTarget> other_targets;
        other_targets.reserve(inventory.size());

        for (const auto& device : inventory) {
            const bool is_control =
                device.kind == cmvr::device::DeviceKind::Arm ||
                device.kind == cmvr::device::DeviceKind::AGV ||
                device.kind == cmvr::device::DeviceKind::DexHand;
            if (!is_control) {
                OtherTarget target;
                target.id = device.id;
                switch (device.kind) {
                    case cmvr::device::DeviceKind::Camera:
                        target.camera = std::dynamic_pointer_cast<
                            cmvr::device::AbstractCamera>(device.device);
                        break;
                    case cmvr::device::DeviceKind::Microphone:
                        target.microphone = std::dynamic_pointer_cast<
                            cmvr::device::AbstractMicrophone>(device.device);
                        break;
                    case cmvr::device::DeviceKind::Speaker:
                        target.speaker = std::dynamic_pointer_cast<
                            cmvr::device::AbstractSpeaker>(device.device);
                        break;
                    case cmvr::device::DeviceKind::BioHead:
                        target.head = std::dynamic_pointer_cast<
                            cmvr::device::AbstractBiohead>(device.device);
                        break;
                    case cmvr::device::DeviceKind::Gripper:
                        target.gripper = std::dynamic_pointer_cast<
                            cmvr::device::AbstractGripper>(device.device);
                        break;
                    default:
                        continue;
                }
                if (!target.camera && !target.microphone &&
                    !target.speaker && !target.head && !target.gripper) {
                    appendFailure(
                        unconfirmed_devices, device.id,
                        "inventory type did not resolve to its typed device");
                    continue;
                }
                other_targets.emplace(device.id, std::move(target));
                continue;
            }

            ControlTarget target;
            target.id = device.id;
            target.kind = device.kind;
            if (device.kind == cmvr::device::DeviceKind::Arm) {
                target.arm = std::dynamic_pointer_cast<
                    cmvr::device::RobotArm>(device.device);
            } else if (device.kind == cmvr::device::DeviceKind::AGV) {
                target.agv = std::dynamic_pointer_cast<
                    cmvr::device::AbstractAGV>(device.device);
            } else {
                target.hand = std::dynamic_pointer_cast<
                    cmvr::device::AbstractDexHand>(device.device);
            }
            if (!target.arm && !target.agv && !target.hand) {
                appendFailure(
                    unconfirmed_devices, device.id,
                    "inventory type did not resolve to the typed control device");
                continue;
            }

            std::string detail;
            bool acquired = false;
            try {
                acquired = control_barriers.acquire(
                    device.id, detail, target.barrier);
            } catch (const std::exception& error) {
                detail = error.what();
            } catch (...) {
                detail = "unknown exception";
            }
            if (!acquired) {
                appendFailure(
                    unconfirmed_devices, device.id,
                    "could not establish the device safety barrier" +
                        (detail.empty() ? "" : ": " + detail));
                continue;
            }
            control_targets.push_back(std::move(target));
        }

        std::unordered_set<std::string> tracked_media_ids;
        const auto merge_tracked_ids = [&tracked_media_ids](
            const std::vector<std::string>& ids) {
            tracked_media_ids.insert(ids.begin(), ids.end());
        };
        merge_tracked_ids(
            cmvr::media::globalMediaSourceManager().trackedSourceIds());
        merge_tracked_ids(
            globalCameraPtzActivityRegistry().trackedDeviceIds());
        merge_tracked_ids(
            globalCameraOperationalActivityRegistry().trackedDeviceIds());
        for (const auto& id : tracked_media_ids) {
            auto [target, inserted] = other_targets.try_emplace(id);
            if (inserted) {
                target->second.id = id;
            }
            target->second.tracked_media = true;
        }

        // Every independent stop is submitted before waiting for any result.
        // A blocked backend therefore cannot delay peer motion or media stops.
        std::vector<StopHandleEntry> control_stops;
        control_stops.reserve(control_targets.size());
        for (const auto& target : control_targets) {
            StopHandle handle;
            auto deadline_detail = std::make_shared<StopDeadlineDetail>();
            if (target.arm) {
                const auto arm = target.arm;
                const auto barrier = target.barrier;
                deadline_detail->publishInitial(
                    "initial RobotArm stop did not complete before the "
                    "StopAll deadline");
                handle = stop_dispatcher_->submit(
                    "control:" + stopResourceKey(target.id),
                    [arm, barrier, stop_deadline, deadline_detail] {
                        return stopControlWithFence(
                            barrier, stop_deadline,
                            [arm] { return stopArm(arm, false); },
                            [arm] { return stopArm(arm, true); },
                            "RobotArm", deadline_detail);
                    },
                    [deadline_detail] {
                        return deadline_detail->snapshot();
                    });
            } else if (target.agv) {
                const auto agv = target.agv;
                const auto barrier = target.barrier;
                deadline_detail->publishInitial(
                    "initial AGV stop did not complete before the StopAll "
                    "deadline");
                handle = stop_dispatcher_->submit(
                    "control:" + stopResourceKey(target.id),
                    [agv, barrier, stop_deadline, deadline_detail] {
                        return stopControlWithFence(
                            barrier, stop_deadline,
                            [agv] { return stopAgv(agv, false); },
                            [agv] { return stopAgv(agv, true); },
                            "AGV", deadline_detail);
                    },
                    [deadline_detail] {
                        return deadline_detail->snapshot();
                    });
            } else {
                const auto hand = target.hand;
                const auto barrier = target.barrier;
                deadline_detail->publishInitial(
                    "initial DexHand stop did not complete before the "
                    "StopAll deadline");
                handle = stop_dispatcher_->submit(
                    "control:" + stopResourceKey(target.id),
                    [hand, barrier, stop_deadline, deadline_detail] {
                        return stopControlWithFence(
                            barrier, stop_deadline,
                            [hand] { return stopDexHand(hand, false); },
                            [hand] { return stopDexHand(hand, true); },
                            "DexHand", deadline_detail);
                    },
                    [deadline_detail] {
                        return deadline_detail->snapshot();
                    });
            }
            control_stops.push_back(
                {target.id, std::move(handle), true, true});
        }

        std::vector<StopHandleEntry> motor_operations;
        std::vector<cmvr::service::DeferredStopOperation>
            deferred_motor_operations;
        std::string motor_collection_error;
        if (!motor_stop.collectStopOperations(
                deferred_motor_operations, &motor_collection_error)) {
            appendFailure(
                unconfirmed_devices, "motors",
                motor_collection_error.empty()
                    ? "could not collect registered motor stop operations"
                    : motor_collection_error);
        } else {
            submitDeferredOperations(
                *stop_dispatcher_, std::move(deferred_motor_operations),
                motor_operations);
        }

        std::vector<StopHandleEntry> media_cancellations;
        std::vector<cmvr::service::DeferredStopOperation>
            deferred_media_cancellations;
        std::string media_collection_error;
        if (!media_stop.collectCancellationOperations(
                deferred_media_cancellations, &media_collection_error)) {
            appendFailure(
                unconfirmed_devices, "media",
                media_collection_error.empty()
                    ? "could not collect active media cancellations"
                    : media_collection_error);
        } else {
            submitDeferredOperations(
                *stop_dispatcher_, std::move(deferred_media_cancellations),
                media_cancellations);
        }

        std::vector<StopHandleEntry> task_stops;
        const auto task_targets =
            cmvr::task::TaskManager::activitySnapshotIfInitialized();
        task_stops.reserve(task_targets.size());
        for (const auto& task : task_targets) {
            const auto task_id = task ? task->id() : std::string{"unknown"};
            auto handle = stop_dispatcher_->submit(
                "task:" + task_id,
                [task] {
                    return invokeStopOperation(
                        [task] { return stopTaskActivity(task); },
                        "task activity stop");
                });
            task_stops.push_back(
                {task_id, std::move(handle), false, true});
        }

        std::vector<StopHandleEntry> other_stops;
        other_stops.reserve(other_targets.size());
        for (const auto& [id, target] : other_targets) {
            const auto camera = target.camera;
            const auto microphone = target.microphone;
            const auto speaker = target.speaker;
            const auto head = target.head;
            const auto gripper = target.gripper;
            const bool tracked_media = target.tracked_media;
            auto handle = stop_dispatcher_->submit(
                "activity:" + stopResourceKey(id),
                [id, camera, microphone, speaker, head, gripper,
                 tracked_media] {
                    return stopOtherActivities(
                        id, camera, microphone, speaker, head, gripper,
                        tracked_media);
                });
            other_stops.push_back(
                {id, std::move(handle), false, true});
        }

        std::unordered_set<std::string> completed_control_stops;
        (void)waitForStopOperations(
            control_stops, stop_deadline, control_barriers,
            unconfirmed_devices, &completed_control_stops);
        (void)waitForStopOperations(
            motor_operations, stop_deadline, control_barriers,
            unconfirmed_devices);
        (void)waitForStopOperations(
            media_cancellations, stop_deadline, control_barriers,
            unconfirmed_devices);
        (void)waitForStopOperations(
            task_stops, stop_deadline, control_barriers,
            unconfirmed_devices);
        std::unordered_set<std::string> completed_other_stops;
        (void)waitForStopOperations(
            other_stops, stop_deadline, control_barriers,
            unconfirmed_devices, &completed_other_stops);

        std::string motor_error;
        const bool motors_stopped = motor_stop.waitForStopped(
            remainingStopBudget(stop_deadline), &motor_error);
        if (!motors_stopped) {
            unconfirmed_devices.push_back(
                "motors: " + (motor_error.empty()
                    ? "operational stop was not confirmed"
                    : motor_error));
        }

        const bool media_stopped = media_stop.waitForStopped(
            remainingStopBudget(stop_deadline));
        if (!media_stopped) {
            unconfirmed_devices.push_back(
                "media: timed out waiting for active RPCs to stop");
        } else {
            // The first device stop runs in parallel with media cancellation so
            // motion stops are never delayed by a blocked media handler. A
            // handler already inside runIfCurrent(), however, can finish a
            // start/resume after that first stop. Once every old media session
            // has drained, repeat the typed operational stops while all
            // admission gates are still closed. Do not retry a device whose
            // first stop is still running; StopOperationDispatcher would only
            // join that old job and a concurrent driver stop would be unsafe.
            std::vector<StopHandleEntry> final_media_stops;
            final_media_stops.reserve(completed_other_stops.size());
            for (const auto& [id, target] : other_targets) {
                const bool needs_final_media_stop =
                    target.camera || target.microphone || target.speaker ||
                    target.tracked_media;
                if (!needs_final_media_stop ||
                    completed_other_stops.count(id) == 0U) {
                    continue;
                }

                const auto camera = target.camera;
                const auto microphone = target.microphone;
                const auto speaker = target.speaker;
                const bool tracked_media = target.tracked_media;
                auto handle = stop_dispatcher_->submit(
                    "activity:" + stopResourceKey(id),
                    [id, camera, microphone, speaker, tracked_media] {
                        return stopOtherActivities(
                            id, camera, microphone, speaker, nullptr, nullptr,
                            tracked_media);
                    });
                final_media_stops.push_back(
                    {id, std::move(handle), false, true});
            }

            // DexHand sensor RPCs also belong to the media coordinator and can
            // race their resumeOperationalActivity() with the initial control
            // stop. Their typed safety barrier is still held here, so a final
            // operational stop cannot admit a new hand command.
            for (const auto& target : control_targets) {
                if (!target.hand ||
                    completed_control_stops.count(target.id) == 0U) {
                    continue;
                }
                const auto hand = target.hand;
                auto handle = stop_dispatcher_->submit(
                    "final-media-control:" + stopResourceKey(target.id),
                    [hand] {
                        return invokeStopOperation(
                            [hand] { return stopDexHand(hand, true); },
                            "final DexHand media activity stop");
                    });
                final_media_stops.push_back(
                    {target.id, std::move(handle), true, true});
            }
            (void)waitForStopOperations(
                final_media_stops, stop_deadline, control_barriers,
                unconfirmed_devices);
        }

        if (!action_queue_->waitForIdle(
                remainingStopBudget(stop_deadline))) {
            unconfirmed_devices.push_back(
                "ActionQueue: timed out waiting for the execution queue to "
                "become idle");
        }
        if (!unconfirmed_devices.empty()) {
            control_barriers.quarantineAll();
            throw std::runtime_error(
                "StopAll could not confirm that every device stopped; "
                "control remains paused and affected resources remain "
                "quarantined: " +
                unconfirmed_devices.front());
        }
        control_barriers.recoverRetiredSafetyHoldersAll();
        if (!action_stop.complete(true)) {
            throw std::runtime_error(
                "StopAll stopped all devices but could not safely resume "
                "ActionQueue admission");
        }
        if (!media_stop.complete(true)) {
            throw std::runtime_error(
                "StopAll stopped all media but could not safely resume "
                "media activity admission");
        }
        if (!motor_stop.complete(true)) {
            throw std::runtime_error(
                "StopAll stopped all motors but could not safely resume "
                "motor command admission");
        }
        // Release the current round's typed control barriers while the global
        // gate is still closed. Retired barriers from earlier failed rounds
        // were recovered above; unrelated active safety holders are preserved.
        control_barriers.releaseAll();
        if (!admission_stop.complete(true)) {
            throw std::runtime_error(
                "StopAll stopped all activities but could not safely resume "
                "system admission");
        }
        response->mutable_header()->set_success(true);
        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
        CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (StopAll): success";
        return grpc::Status::OK;
    }
    catch (std::exception& e) {
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(e.what());
        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
        return grpc::Status::OK;
    }
    catch (...) {
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(
            "StopAll failed with an unknown exception");
        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
        return grpc::Status::OK;
    }
}

grpc::Status gRPCSystemServiceImpl::ExecuteActionQueue(
    grpc::ServerContext* context,
    const cmvr::api::ActionQueueCommand_Request* request,
    cmvr::api::ActionQueueCommand_Feedback* response)
{
    CMVR_GRPC_REQUIRE_REGISTERED_CALL(
        security_gateway_, context,
        "/cmvr.api.SystemService/ExecuteActionQueue");
    if (!request || !response) {
        return grpc::Status(
            grpc::StatusCode::INVALID_ARGUMENT,
            "ActionQueue request and response are required");
    }
    try {
        cmvr::safety::CommandActor actor;
        actor.principal_id =
            cmvr_grpc_call_guard.context().principal.id;
        actor.authenticated =
            cmvr_grpc_call_guard.context().principal.authenticated;
        for (const auto role :
             cmvr_grpc_call_guard.context().principal.roles) {
            actor.roles.emplace_back(cmvr::service::toString(role));
        }
        // The callback is consumed only on this synchronous handler stack. It
        // is never retained by the worker-owned action Record, so returning the
        // RPC cannot leave a dangling ServerContext reference.
        const auto wait_result = action_queue_->submitAndWait(
            *request,
            *response,
            [context]() {
                return context && context->IsCancelled();
            },
            std::move(actor));
        if (wait_result ==
            ActionQueueExecutor::WaitResult::CanceledBeforeAdmission) {
            return grpc::Status(
                grpc::StatusCode::CANCELLED,
                "ActionQueue RPC was canceled before admission");
        }
        if (wait_result ==
            ActionQueueExecutor::WaitResult::CanceledAfterAdmission) {
            return grpc::Status(
                grpc::StatusCode::CANCELLED,
                "ActionQueue RPC waiter was canceled after admission; the edge action continues and its result can be retrieved with the same action_id");
        }
        return grpc::Status::OK;
    } catch (const std::exception& error) {
        response->Clear();
        response->set_action_id(request->action_id());
        response->set_service_instance_id(action_queue_->instanceId());
        response->set_result(api::ACTION_RESULT_CODE_FAILED);
        response->mutable_header()->set_success(false);
        response->mutable_header()->set_error_message(error.what());
        setCurrentTimestamp(
            response->mutable_header()->mutable_timestamp());
        return grpc::Status::OK;
    }
}