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

#ifndef CMVR_ES_ABSTRACT_MOTOR_H
#define CMVR_ES_ABSTRACT_MOTOR_H

#pragma once

#include "abstract_device.h"
#include "utils/dynamics/joint.h"
#include "motor/motor_protocol_interface.h"
#include <mutex>

namespace cmvr::device{

    class AbstractMotor: public AbstractDevice {

        typedef struct {


        } MotorProfile;

        // xml 使用
        struct MotorInfo {
            int id;
            std::string joint_name;
            float limitQ;
            float limitQd;
        };

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

    public:
        explicit AbstractMotor(const XmlNode &config): AbstractDevice(config) {}
        AbstractMotor(const XmlNode &config,uint8_t node_id): AbstractDevice(config) ,node_id_(node_id) {}
        ~AbstractMotor() override = default;

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

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

        void torqueOff() {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            protocol_->torqueOff(node_id_);
        }

        virtual void setLimitQ(double ub, double lb) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            protocol_->setLimitQ(node_id_, ub,lb);
        }
        virtual void setLimitQd(double qd) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            protocol_->setLimitQd(node_id_, qd);
        }
        virtual void setLimitQdd(double u_qdd,double l_qdd) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            protocol_->setLimitQdd(node_id_, u_qdd,l_qdd);
        }
        // virtual void setLimitTau(double tau) = 0;
        // virtual void setLimitCurrent(double tau) = 0;
        virtual void brake() {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            protocol_->brake(node_id_);
        }
        /**
         *
         * @param q unit : rad
         */
        virtual void setQ(double q) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            protocol_->setQ(node_id_, q);
        }

        virtual void set(double q,double qd) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            protocol_->set(node_id_,q, qd);
        }

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

        virtual bool reachedTargetQ() {
            if (!protocol_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            return  protocol_->reachedTargetQ(node_id_);
        }
        // rad /s
        virtual void setQd(double qd) {
            std::scoped_lock lock(mtx_);
            if (!protocol_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            return  protocol_->setQd(node_id_,qd);
        }
        // virtual void setQdd(double qdd) = 0;  // rad /s^2
        // 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_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            return  protocol_->getQ(node_id_);
        }

        virtual  double getQd() {
            if (!protocol_) {
                throw std::runtime_error("Protocol not set for motor");
            }
            return  protocol_->getQd(node_id_);
        }


        // 使用的通讯协议
        virtual void setProtocol(std::shared_ptr<MotorProtocolInterface> protocol) {
            std::scoped_lock lock(mtx_);
            protocol_ = std::move(protocol);
            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