//
// Created by linbo on 2025/10/22.
//

#ifndef CMVR_ES_ABSTRACTMOTORPROTOCOL_H
#define CMVR_ES_ABSTRACTMOTORPROTOCOL_H
#include "rapidxml/xml_parser.h"
#include "cmvr/msgs/canopen.pb.h"
#include "cmvr/msgs/motor.pb.h"
namespace cmvr::hardware
{
    class AbstractMotorProtocol
    {

    public:
        enum class CommProto : uint8_t {
            CANOPEN = 1,
            CUSTOM = 2
        };
    public:
        explicit AbstractMotorProtocol(const XmlNode &cfg)
        {
            cfg_ = cfg;
            id_ = cfg.getAttrString("id");
            joint_name_ = cfg.getAttrString("joint_name");
            limitQLb_ = cfg.getAttrDefault("limitQLb_",3.14f);
            limitQUb_ = cfg.getAttrDefault("limitQUb_",3.14f);
            limitQd = cfg.getAttrDefault("limitQd",3.0f);
        }
        virtual ~AbstractMotorProtocol() = default;

        /**
         * @brief 用于初始化与通讯协议相关的设置
         * @param node_id 电机Id
         * @return
         */
        virtual bool initNode(uint8_t node_id) = 0;

        virtual void setQ(uint8_t node_id, double angle_rad) = 0;
        virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
        virtual msgs::RunMode getMode(uint8_t node_id) = 0;
        virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;
        virtual void setLimitQd(uint8_t node_id,double qd) = 0;
        virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0;
        virtual bool calibrateZeroQ(uint8_t node_id) = 0;
        virtual bool reachedTargetQ(uint8_t node_id) = 0;
        virtual void setQd(uint8_t node_id, double qd) = 0;
        virtual void setQdd(uint8_t node_id,double qdd) = 0;
        virtual void brake(uint8_t node_id) = 0;
        virtual void torqueOff(uint8_t node_id) = 0;

        virtual double getQ(uint8_t node_id) = 0;
        virtual double getQd(uint8_t node_id) = 0;
        CommProto comm_proto{CommProto::CANOPEN};
    protected:
        XmlNode cfg_;
        std::string id_;
        std::string joint_name_;
        double limitQLb_;
        double limitQUb_;
        double limitQd;
    };
}

#endif //CMVR_ES_ABSTRACTMOTORPROTOCOL_H