cmvr-es/cmvr-es/devices/dexhand/abstract_dexhand.h

//
// Created by xtkuang on 2025/5/8.
//

#ifndef CMVR_ES_ABSTRACT_DEXHAND_H
#define CMVR_ES_ABSTRACT_DEXHAND_H
#pragma once

#include <cmath>
#include <cstdint>
#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "devices/abstract_device.h"
#include "common/base/logging/logger.h"

namespace cmvr::device {
    class AbstractDexHand : public AbstractDevice {
    public:
        DeviceKind kind() const noexcept override { return DeviceKind::DexHand; }

        struct TactilePoint {
            int32_t fx{0};
            int32_t fy{0};
            int32_t fz{0};

            static TactilePoint fromFz(const int32_t value) {
                return TactilePoint{0, 0, value};
            }

            double magnitude() const {
                return std::sqrt(static_cast<double>(fx) * static_cast<double>(fx) +
                                 static_cast<double>(fy) * static_cast<double>(fy) +
                                 static_cast<double>(fz) * static_cast<double>(fz));
            }

            bool isZero() const {
                return fx == 0 && fy == 0 && fz == 0;
            }
        };

        using ResultantForce = TactilePoint;

        enum class FingerType {
            PINKY,
            RING,
            MIDDLE,
            INDEX,
            THUMB,
            PALM
        };

        enum class TactileRegion {
            TIP,
            FINGER,
            PAD,
            THUMB_MIDDLE,
            PALM_PAD
        };

        using TactileRegionKey = std::pair<FingerType, TactileRegion>;

        struct TactileMatrixView {
            const TactilePoint* data{nullptr};
            int rows{0};
            int cols{0};

            bool valid() const {
                return data != nullptr && rows > 0 && cols > 0;
            }

            int pointCount() const {
                return rows * cols;
            }

            const TactilePoint* rowData(int row) const {
                return data + row * cols;
            }

            TactilePoint at(int row, int col) const {
                return data[row * cols + col];
            }
        };

        class TactileRegionData {
        public:
            TactileRegionData() = default;

            TactileRegionData(FingerType finger_in,
                              TactileRegion region_in,
                              TactileMatrixView view_in,
                              const char* name_in,
                              std::shared_ptr<const void> lifetime = {})
                : finger(finger_in),
                  region(region_in),
                  view(view_in),
                  name(name_in),
                  lifetime_(std::move(lifetime)) {}

            bool valid() const {
                return view.valid();
            }

            int byteSize() const {
                return view.pointCount() * static_cast<int>(sizeof(TactilePoint));
            }

            FingerType finger{FingerType::PINKY};
            TactileRegion region{TactileRegion::TIP};
            TactileMatrixView view{};
            const char* name{nullptr};

        private:
            std::shared_ptr<const void> lifetime_;
        };

        enum class Status {
            CREATED,
            INITIALIZED,
            STREAMING,
            STOPPED,
            FAULT
        };

        AbstractDexHand() = default;
        ~AbstractDexHand() override = default;

        virtual Status state() const = 0;
        virtual std::string lastError() const = 0;

        DeviceHealthSnapshot healthSnapshot() override {
            const auto lifecycle = state();
            const auto error = lastError();

            DeviceHealthSnapshot health;
            health.error_message = error;
            if (lifecycle == Status::FAULT) {
                health.state = DeviceHealthState::Fault;
            } else if (!error.empty()) {
                health.state = DeviceHealthState::Degraded;
            } else if (lifecycle == Status::INITIALIZED ||
                       lifecycle == Status::STREAMING ||
                       lifecycle == Status::STOPPED) {
                health.state = DeviceHealthState::Healthy;
            }
            return health;
        }

        virtual void getState(DexHandState& state) {
            state = DexHandState{};
            const auto lifecycle = this->state();
            state.is_initialized =
                lifecycle == Status::INITIALIZED ||
                lifecycle == Status::STREAMING;
        }

        // Stops command-driven activity without changing the device lifecycle
        // or closing its transport. Implementations must return true only after
        // no pre-stop activity can continue. Motion-capable hands without a
        // reliable hold/idle command deliberately fail closed.
        virtual bool stopOperationalActivity() { return false; }

        // Restores an activity paused by stopOperationalActivity(). This is
        // called only after a new command has crossed the system admission
        // boundary. Most motion-capable hands need no separate resume command.
        virtual bool resumeOperationalActivity() { return true; }

        virtual void setAngles(const std::vector<int>& finger_joint_angles) = 0;
        virtual void setTactilePollingRegion(FingerType finger, TactileRegion region) {
            setTactilePollingRegions({TactileRegionKey{finger, region}});
        }

        virtual void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) = 0;
        virtual std::vector<TactileRegionData> getSensorData() = 0;
        virtual TactileRegionData getSensorData(FingerType finger, TactileRegion region) = 0;
        virtual ResultantForce getResultantForce(FingerType finger, TactileRegion region) = 0;

        virtual void setPositions(const std::vector<int>&) {
            CMVR_LOG(ERROR) << "[AbstractDexHand] setPositions is not supported by this dexhand abstraction.";
        }

        virtual void setVelocities(const std::vector<int>&) {
            CMVR_LOG(ERROR) << "[AbstractDexHand] setVelocities is not supported by this dexhand abstraction.";
        }

        virtual void setPresetAct(int) {
            CMVR_LOG(ERROR) << "[AbstractDexHand] setPresetAct is not supported by this dexhand abstraction.";
        }

        virtual void execPresetAct(int) {
            CMVR_LOG(ERROR) << "[AbstractDexHand] execPresetAct is not supported by this dexhand abstraction.";
        }

        virtual void setForce(const std::vector<int>&) {
            CMVR_LOG(ERROR) << "[AbstractDexHand] setForce is not supported by this dexhand abstraction.";
        }
    };
}

#endif //CMVR_ES_ABSTRACT_DEXHAND_H