cmvr-es/cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h

#ifndef CMVR_ES_HUAYAN_LIFECYCLE_STATE_H
#define CMVR_ES_HUAYAN_LIFECYCLE_STATE_H

#include <algorithm>
#include <chrono>
#include <condition_variable>
#include <cstdint>
#include <mutex>
#include <optional>

namespace cmvr::device::huayan_internal {

enum class MotionKind {
    None,
    Joint,
    Linear,
    SpeedJoint,
    SpeedLinear,
    Servo,
    Program,
};

struct MotionToken {
    std::uint64_t generation{0};
    MotionKind kind{MotionKind::None};

    bool valid() const noexcept
    {
        return generation != 0 && kind != MotionKind::None;
    }
};

enum class MotionStartStatus {
    Started,
    Invalid,
    Busy,
    Stopping,
    Blocked,
};

struct MotionStartResult {
    MotionStartStatus status{MotionStartStatus::Busy};
    MotionToken token;

    bool started() const noexcept
    {
        return status == MotionStartStatus::Started;
    }
};

enum class MotionFinishMode {
    RestorePrevious,
    Clear,
    Retain,
};

enum class StopStartStatus {
    Started,
    AlreadyStopping,
};

struct StopRequest {
    StopStartStatus status{StopStartStatus::AlreadyStopping};
    MotionKind kind{MotionKind::None};
    MotionToken active_token;
    bool tracked_motion{false};

    bool started() const noexcept
    {
        return status == StopStartStatus::Started;
    }
};

struct SafetyCancelResult {
    MotionKind kind{MotionKind::None};
    MotionToken active_token;
    bool tracked_motion{false};
};

struct MotionSnapshot {
    MotionKind active_kind{MotionKind::None};
    MotionKind retained_kind{MotionKind::None};
    std::uint64_t active_generation{0};
    bool owner_active{false};
    bool stop_in_progress{false};
    bool blocked{false};
};

// Tracks a single Huayan controller operation owner. Generation tokens make
// completion from an older RPC harmless after Stop or a safety event has
// cancelled it. Servo and program operations may retain their kind after the
// submitting RPC returns; begin(kind, true) supports same-kind updates while
// that retained controller mode remains active.
class MotionState final {
public:
    MotionStartResult begin(
        const MotionKind kind,
        const bool replace_retained_same_kind = false)
    {
        std::lock_guard lock(mutex_);
        if (kind == MotionKind::None) {
            return {MotionStartStatus::Invalid, {}};
        }
        if (stop_in_progress_) {
            return {MotionStartStatus::Stopping, {}};
        }
        if (blocked_) {
            return {MotionStartStatus::Blocked, {}};
        }
        if (owner_active_) {
            return {MotionStartStatus::Busy, {}};
        }
        if (retained_kind_ != MotionKind::None &&
            (!replace_retained_same_kind || retained_kind_ != kind)) {
            return {MotionStartStatus::Busy, {}};
        }

        const MotionToken token{++next_generation_, kind};
        owner_active_ = true;
        active_token_ = token;
        previous_kind_ = retained_kind_;
        return {MotionStartStatus::Started, token};
    }

    void finish(
        const MotionToken token,
        const MotionFinishMode mode = MotionFinishMode::RestorePrevious)
    {
        std::lock_guard lock(mutex_);
        if (!owner_active_ ||
            active_token_.generation != token.generation) {
            return;
        }

        owner_active_ = false;
        active_token_ = {};
        if (!stop_in_progress_ && !blocked_) {
            switch (mode) {
            case MotionFinishMode::RestorePrevious:
                retained_kind_ = previous_kind_;
                break;
            case MotionFinishMode::Clear:
                retained_kind_ = MotionKind::None;
                break;
            case MotionFinishMode::Retain:
                retained_kind_ = token.kind;
                break;
            }
        }
        previous_kind_ = MotionKind::None;
        owner_finished_cv_.notify_all();
    }

    void failMotion(const MotionToken token)
    {
        std::lock_guard lock(mutex_);
        if (!owner_active_ ||
            active_token_.generation != token.generation) {
            return;
        }

        owner_active_ = false;
        active_token_ = {};
        retained_kind_ = token.kind;
        previous_kind_ = MotionKind::None;
        blocked_ = true;
        owner_finished_cv_.notify_all();
    }

    StopRequest beginStop(
        const MotionKind requested_kind = MotionKind::None)
    {
        std::lock_guard lock(mutex_);
        if (stop_in_progress_) {
            return {};
        }

        stop_in_progress_ = true;
        const MotionToken active = owner_active_
            ? active_token_
            : MotionToken{};
        if (active.valid()) {
            cancelled_generation_ = std::max(
                cancelled_generation_, active.generation);
        }

        MotionKind kind = MotionKind::None;
        if (active.valid()) {
            kind = active.kind;
        } else if (retained_kind_ != MotionKind::None) {
            kind = retained_kind_;
        } else {
            kind = requested_kind;
        }
        if (kind != MotionKind::None) {
            retained_kind_ = kind;
        }

        return {
            StopStartStatus::Started,
            kind,
            active,
            active.valid() || kind != MotionKind::None};
    }

    SafetyCancelResult cancelActiveForSafety()
    {
        std::lock_guard lock(mutex_);
        const MotionToken active = owner_active_
            ? active_token_
            : MotionToken{};
        if (active.valid()) {
            cancelled_generation_ = std::max(
                cancelled_generation_, active.generation);
        }

        const MotionKind kind = active.valid()
            ? active.kind
            : retained_kind_;
        if (kind != MotionKind::None) {
            retained_kind_ = kind;
        }
        // A hardware safety transition is independent of a concurrent
        // software Stop. New controller operations remain rejected until
        // termination is positively confirmed.
        blocked_ = true;
        owner_finished_cv_.notify_all();
        return {kind, active, active.valid() || kind != MotionKind::None};
    }

    bool cancelled(const MotionToken token) const
    {
        std::lock_guard lock(mutex_);
        return token.valid() &&
               token.generation <= cancelled_generation_;
    }

    bool waitForOwnerExit(
        const MotionToken token,
        const std::chrono::milliseconds timeout)
    {
        if (!token.valid()) {
            return true;
        }
        std::unique_lock lock(mutex_);
        return owner_finished_cv_.wait_for(
            lock,
            timeout,
            [this, token]() {
                return !owner_active_ ||
                       active_token_.generation != token.generation;
            });
    }

    bool ownerActive(const MotionToken token) const
    {
        if (!token.valid()) {
            return false;
        }
        std::lock_guard lock(mutex_);
        return owner_active_ &&
               active_token_.generation == token.generation;
    }

    bool completeStop()
    {
        std::lock_guard lock(mutex_);
        if (owner_active_) {
            return false;
        }
        stop_in_progress_ = false;
        blocked_ = false;
        active_token_ = {};
        retained_kind_ = MotionKind::None;
        previous_kind_ = MotionKind::None;
        owner_finished_cv_.notify_all();
        return true;
    }

    void failStop()
    {
        std::lock_guard lock(mutex_);
        stop_in_progress_ = false;
        blocked_ = true;
        owner_finished_cv_.notify_all();
    }

    MotionSnapshot snapshot() const
    {
        std::lock_guard lock(mutex_);
        return {
            owner_active_ ? active_token_.kind : MotionKind::None,
            retained_kind_,
            owner_active_ ? active_token_.generation : 0,
            owner_active_,
            stop_in_progress_,
            blocked_};
    }

    bool busy() const
    {
        const auto state = snapshot();
        return state.owner_active || state.stop_in_progress ||
               state.blocked ||
               state.retained_kind != MotionKind::None;
    }

private:
    mutable std::mutex mutex_;
    std::condition_variable owner_finished_cv_;
    std::uint64_t next_generation_{0};
    std::uint64_t cancelled_generation_{0};
    MotionToken active_token_;
    MotionKind retained_kind_{MotionKind::None};
    MotionKind previous_kind_{MotionKind::None};
    bool owner_active_{false};
    bool stop_in_progress_{false};
    bool blocked_{false};
};

enum class SafetyCondition {
    Unknown,
    Normal,
    EmergencyStop,
    SafeguardStop,
    RobotFault,
    EmergencySignalFault,
    SafeguardSignalFault,
    SoftwareEmergencyStop,
    SoftwareProtectiveStop,
};

struct RawSafetyState {
    bool valid{false};
    int emergency_signal_fault{0};
    int emergency_stop{0};
    int safeguard_signal_fault{0};
    int safeguard_stop{0};
    int robot_fault{0};
    bool software_emergency_stop{false};
    bool software_protective_stop{false};
};

inline SafetyCondition classifySafetyCondition(
    const RawSafetyState& state) noexcept
{
    if (!state.valid) {
        return SafetyCondition::Unknown;
    }
    if (state.emergency_signal_fault != 0) {
        return SafetyCondition::EmergencySignalFault;
    }
    if (state.safeguard_signal_fault != 0) {
        return SafetyCondition::SafeguardSignalFault;
    }
    if (state.emergency_stop != 0) {
        return SafetyCondition::EmergencyStop;
    }
    if (state.safeguard_stop != 0) {
        return SafetyCondition::SafeguardStop;
    }
    if (state.robot_fault != 0) {
        return SafetyCondition::RobotFault;
    }
    if (state.software_emergency_stop) {
        return SafetyCondition::SoftwareEmergencyStop;
    }
    if (state.software_protective_stop) {
        return SafetyCondition::SoftwareProtectiveStop;
    }
    return SafetyCondition::Normal;
}

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

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};
};

// Safety inputs are events, not merely levels. Returning to Normal never
// clears a prior unsafe event. Explicit recovery is tied atomically to the
// event epoch, so a second event invalidates an older in-flight recovery.
class SafetyState final {
public:
    void observe(const RawSafetyState& raw_state)
    {
        observe(classifySafetyCondition(raw_state));
    }

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

        if (!latched_ || recovery_in_progress_ || changed) {
            ++epoch_;
        }
        latched_ = true;
        recovery_in_progress_ = false;
        latched_reason_ = 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_ready,
        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_ready || !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_};
    }

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};
};

} // namespace cmvr::device::huayan_internal

#endif // CMVR_ES_HUAYAN_LIFECYCLE_STATE_H