cmvr-es/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h

#ifndef CMVR_ES_AUBO_SAFETY_STATE_H
#define CMVR_ES_AUBO_SAFETY_STATE_H

#include <cstdint>
#include <mutex>
#include <optional>

namespace cmvr::device::aubo_internal {

// This is deliberately richer than RobotArm::SafetyMode. Recovery and
// Violation have no lossless public mapping, but both must remain fail-closed.
enum class SafetyCondition {
    Unknown,
    Normal,
    Reduced,
    Recovery,
    Violation,
    ProtectiveStop,
    SafeguardStop,
    SystemEmergencyStop,
    RobotEmergencyStop,
    SoftwareEmergencyStop,
    Fault,
};

inline bool isMotionSafe(const SafetyCondition condition) noexcept
{
    return condition == SafetyCondition::Normal ||
           condition == SafetyCondition::Reduced;
}

inline SafetyCondition effectiveSafetyCondition(
    const SafetyCondition reported_condition,
    const int robot_emergency_stop_source) noexcept
{
    if (robot_emergency_stop_source < 0) {
        return SafetyCondition::Unknown;
    }
    if (robot_emergency_stop_source != 0) {
        return SafetyCondition::RobotEmergencyStop;
    }
    return reported_condition;
}

inline bool needsProtectiveUnlock(
    const SafetyCondition condition) noexcept
{
    return condition == SafetyCondition::ProtectiveStop ||
           condition == SafetyCondition::Violation;
}

inline bool needsInterfaceBoardRestart(
    const SafetyCondition condition) noexcept
{
    return condition == SafetyCondition::SystemEmergencyStop ||
           condition == SafetyCondition::RobotEmergencyStop ||
           condition == SafetyCondition::Fault;
}

struct SafetyPermit {
    std::uint64_t epoch{0};

    bool valid() const noexcept { return epoch != 0; }
};

struct RecoveryToken {
    std::uint64_t epoch{0};

    bool valid() const noexcept { return epoch != 0; }
};

struct SafetySnapshot {
    SafetyCondition observed{SafetyCondition::Unknown};
    SafetyCondition latched_reason{SafetyCondition::Unknown};
    std::uint64_t epoch{0};
    bool latched{false};
    bool recovery_in_progress{false};
    bool software_emergency_stop_latched{false};
};

inline bool shouldAutoRecoverHardwareEmergencyStop(
    const SafetySnapshot& snapshot,
    const bool hardware_emergency_stop_was_observed,
    const int current_emergency_stop_source) noexcept
{
    return hardware_emergency_stop_was_observed && snapshot.latched &&
           !snapshot.recovery_in_progress &&
           !snapshot.software_emergency_stop_latched &&
           snapshot.latched_reason == SafetyCondition::RobotEmergencyStop &&
           isMotionSafe(snapshot.observed) &&
           current_emergency_stop_source == 0;
}

// Hardware safety is an event, not a level. Once an unsafe state has been
// observed, returning to Normal only changes the observed level. A separate,
// explicit recovery must prove that the old controller operation has been
// cancelled before new motion permits can be issued.
class SafetyState final {
public:
    SafetyState() = default;

    void observe(const SafetyCondition condition)
    {
        std::lock_guard lock(mutex_);
        const bool changed = observed_ != condition;
        observed_ = condition;
        if (condition == SafetyCondition::SoftwareEmergencyStop) {
            software_emergency_stop_latched_ = true;
        }
        if (isMotionSafe(condition)) {
            return;
        }

        if (!latched_ || recovery_in_progress_ || changed) {
            ++epoch_;
        }
        latched_ = true;
        recovery_in_progress_ = false;
        // A physical E-stop sample can continue arriving after a software
        // E-stop request. Keep the software stop independently latched so a
        // later physical-input release can never clear it automatically.
        latched_reason_ = software_emergency_stop_latched_
            ? SafetyCondition::SoftwareEmergencyStop
            : condition;
    }

    std::optional<SafetyPermit> tryPermit() const
    {
        std::lock_guard lock(mutex_);
        if (latched_ || !isMotionSafe(observed_)) {
            return std::nullopt;
        }
        return SafetyPermit{epoch_};
    }

    bool validate(const SafetyPermit permit) const
    {
        std::lock_guard lock(mutex_);
        return permit.valid() && permit.epoch == epoch_ && !latched_ &&
               isMotionSafe(observed_);
    }

    std::optional<RecoveryToken> beginRecovery(
        const std::uint64_t expected_epoch)
    {
        std::lock_guard lock(mutex_);
        if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ ||
            recovery_in_progress_ ||
            !isMotionSafe(observed_)) {
            return std::nullopt;
        }
        recovery_in_progress_ = true;
        return RecoveryToken{epoch_};
    }

    bool completeRecovery(
        const RecoveryToken token,
        const bool robot_running,
        const bool controller_idle,
        const bool cancellation_confirmed)
    {
        std::lock_guard lock(mutex_);
        if (!token.valid() || token.epoch != epoch_ || !latched_ ||
            !recovery_in_progress_ || !isMotionSafe(observed_) ||
            !robot_running || !controller_idle ||
            !cancellation_confirmed) {
            return false;
        }

        latched_ = false;
        recovery_in_progress_ = false;
        latched_reason_ = SafetyCondition::Unknown;
        software_emergency_stop_latched_ = false;
        ++epoch_;
        return true;
    }

    // Hardware E-stop release may clear only this software latch. It does not
    // power on, release brakes, resume runtime, or issue a motion command.
    bool completeHardwareEmergencyStopRecovery(
        const RecoveryToken token,
        const bool controller_idle,
        const bool cancellation_confirmed)
    {
        std::lock_guard lock(mutex_);
        if (!token.valid() || token.epoch != epoch_ || !latched_ ||
            !recovery_in_progress_ ||
            software_emergency_stop_latched_ ||
            latched_reason_ != SafetyCondition::RobotEmergencyStop ||
            !isMotionSafe(observed_) || !controller_idle ||
            !cancellation_confirmed) {
            return false;
        }

        latched_ = false;
        recovery_in_progress_ = false;
        latched_reason_ = SafetyCondition::Unknown;
        ++epoch_;
        return true;
    }

    void failRecovery(const RecoveryToken token)
    {
        std::lock_guard lock(mutex_);
        if (token.valid() && token.epoch == epoch_) {
            recovery_in_progress_ = false;
        }
    }

    SafetySnapshot snapshot() const
    {
        std::lock_guard lock(mutex_);
        return {
            observed_,
            latched_reason_,
            epoch_,
            latched_,
            recovery_in_progress_,
            software_emergency_stop_latched_};
    }

private:
    mutable std::mutex mutex_;
    SafetyCondition observed_{SafetyCondition::Unknown};
    SafetyCondition latched_reason_{SafetyCondition::Unknown};
    std::uint64_t epoch_{1};
    bool latched_{false};
    bool recovery_in_progress_{false};
    bool software_emergency_stop_latched_{false};
};

} // namespace cmvr::device::aubo_internal

#endif // CMVR_ES_AUBO_SAFETY_STATE_H