cmvr-es/cmvr-es/devices/motor/abstract_motor.h

//
// Created by xtkuang on 2025/7/30.
//

#ifndef CMVR_ES_ABSTRACT_MOTOR_H
#define CMVR_ES_ABSTRACT_MOTOR_H

#pragma once

#include "devices/abstract_device.h"
#include "common/base/logging/logger.h"
#include "motor/motor_protocol_interface.h"
#include <mutex>

namespace cmvr::device{

    class AbstractMotor: public AbstractDevice {

        typedef struct {


        } MotorProfile;

        struct MotorInfo {
            int id;
            std::string joint_name;
            double limit_q_lb;
            double limit_q_ub;
            double limit_qd;
            double limit_qdd;
        };

        typedef struct {
            double position;
            double velocity;
            double acceleration;
            double torque;
            double temperature;
            double voltage;
            double current;
            std::string error_msg;
        } JointStatus;

    public:
        AbstractMotor() = default;
        explicit AbstractMotor(uint8_t node_id): node_id_(node_id) {}
        ~AbstractMotor() override = default;

        DeviceKind kind() const noexcept override { return DeviceKind::Motor; }

        virtual void setMode(msgs::RunMode mode) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return;
            }
            protocol_->setMode(node_id_, mode);
        }

        virtual msgs::RunMode getMode() {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return msgs::RUN_MODE_UNSPECIFIED;
            }
            return protocol_->getMode(node_id_);
        }

        virtual bool torqueOn() {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return protocol_->torqueOn(node_id_);
        }

        virtual bool torqueOff() {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return protocol_->torqueOff(node_id_);
        }

        virtual void setLimitQ(double ub, double lb) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return;
            }
            protocol_->setLimitQ(node_id_, ub,lb);
        }
        virtual void setLimitQd(double qd) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return;
            }
            protocol_->setLimitQd(node_id_, qd);
        }
        virtual void setLimitQdd(double u_qdd,double l_qdd) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return;
            }
            protocol_->setLimitQdd(node_id_, u_qdd,l_qdd);
        }
        // virtual void setLimitTau(double tau) = 0;
        // virtual void setLimitCurrent(double tau) = 0;
        virtual bool brakeRelease() {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return protocol_->brakeRelease(node_id_);
        }

        virtual bool quickStop() {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return protocol_->quickStop(node_id_);
        }
        virtual bool commandProfilePosition(double target_q,
                                            double max_qd = 0.0,
                                            double max_qdd = 0.0) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return protocol_->commandProfilePosition(node_id_, target_q, max_qd, max_qdd);
        }

        virtual bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return protocol_->commandProfileVelocity(node_id_, target_qd, max_qdd);
        }

        virtual bool commandCyclicPosition(double target_q,
                                           double target_qd = 0.0) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return protocol_->commandCyclicPosition(node_id_, target_q, target_qd);
        }

        virtual bool commandCyclicVelocity(double target_qd) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return protocol_->commandCyclicVelocity(node_id_, target_qd);
        }

        virtual bool commandCyclicTorque(double target_tau) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return protocol_->commandCyclicTorque(node_id_, target_tau);
        }

        virtual bool calibrateZeroQ() {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return  protocol_->calibrateZeroQ(node_id_);
        }

        virtual bool reachedTargetQ() {
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return false;
            }
            return  protocol_->reachedTargetQ(node_id_);
        }
        // virtual void setTau(double tau) = 0;   // N m
        // virtual void clear_err() = 0;
        // virtual void getStatus() = 0;
        // virtual MotorProfile getProfile() = 0;

        virtual  double getQ() {
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return 0.0;
            }
            return  protocol_->getQ(node_id_);
        }

        virtual  double getQd() {
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return 0.0;
            }
            return  protocol_->getQd(node_id_);
        }


        // 使用的通讯协议
        virtual void setProtocol(std::shared_ptr<MotorProtocolInterface> protocol) {
            std::scoped_lock lock(mtx_);
            protocol_ = std::move(protocol);
            if (!protocol_) {
                CMVR_LOG(ERROR) << "Protocol not set for motor";
                return;
            }
            protocol_->initNode(node_id_);
        }

        uint8_t id() const {
            return node_id_;
        }
        std::string jointName() const {
            return info_.joint_name;
        }
    protected:
        mutable std::mutex mtx_;
        MotorInfo info_{};
        uint8_t node_id_;
        // 使用的通讯协议
        std::shared_ptr<MotorProtocolInterface> protocol_;
    };
}
#endif //CMVR_ES_ABSTRACT_MOTOR_H