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

#pragma once

#include "rapidxml/xml_parser.h"
#include "abstractmotorprotocol.h"
#include "cmvr/msgs/motor.pb.h"
#include "motor/motor_protocol_interface.h"
#include "devices/abstract_canbus.h"
#include "canbus/can_comm/can_receiver.h"
#include "canbus/can_comm/can_sender.h"
#include "canbus/can_comm/message_manager.h"
#include "cmvr/msgs/error_code.pb.h"
#include "cmvr/msgs/robot_detail.pb.h"

#include "canbus/canopen/sdo_request_protocol.h"
#include "canbus/canopen/sync_protocol.h"
#include "canbus/canopen/nmt_request_protocol.h"
#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h"
#include <cmath>
namespace cmvr::hardware{
    class Ti5MotorProtocol final : public AbstractMotorProtocol {
    public:
        explicit Ti5MotorProtocol(const XmlNode &cfg);
        Ti5MotorProtocol(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
                                std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager);
        ~Ti5MotorProtocol() override;

        bool initNode(uint8_t node_id) override;

        void setMode(uint8_t node_id, msgs::RunMode mode) override;

        void setQ(uint8_t node_id, double angle_rad) override;
        void setLimitQ(uint8_t node_id, double ub, double lb) override;
        void setLimitQd(uint8_t node_id, double qd) override;
        void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override;
        bool calibrateZeroQ(uint8_t node_id) override;
        void brake(uint8_t node_id) override;
        bool reachedTargetQ(uint8_t node_id) override;

        double getQ(uint8_t node_id) override;
        double getQd(uint8_t node_id) override;

        void setQd(uint8_t node_id, double qd) override;
        void setQdd(uint8_t node_id, double qdd) override;

        void torqueOff(uint8_t node_id) override;

        void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10);
        void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index,
                      msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10);

        void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
        void configPdo(uint8_t node_id);

        std::unique_ptr<msgs::RobotDetail> GetRobotDetail() {
            auto data_ptr = std::make_unique<msgs::RobotDetail>();
            message_manager_->GetSensorData(data_ptr.get());
            return data_ptr;
        }

        msgs::RunMode getMode(uint8_t node_id) override {
            return cur_mode_[node_id];
        }


    private:
        void setPPTargetPosBySdo(uint8_t node_id, int32_t pos);
        void setPPTargetPosByPdo(uint8_t node_id, int32_t pos);
        void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos);
        void configTPDO1(uint8_t node_id);
        void configTPDO2(uint8_t node_id);
        // 目标位置 607A + 控制字 6040
        void configRPDO1(uint8_t node_id, bool start);
        bool waitUntil(std::function<bool()> condition, int timeout_ms) {
            auto start = std::chrono::steady_clock::now();
            while (!condition()) {
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                if (std::chrono::steady_clock::now() - start > std::chrono::milliseconds(timeout_ms))
                    return false;
            }
            return true;
        }
    private:
        static constexpr double GearRatio = 101.0; // 电机减速比
        static constexpr double RADTODEG = 180.0 / M_PI;
        std::shared_ptr<device::AbstractCanbus> can_client_{nullptr};

        // key node_id
        std::unordered_map<uint8_t,msgs::RunMode> cur_mode_{};
        std::unordered_map<uint8_t,uint32_t> last_Qd_{};
        std::unordered_map<uint8_t,uint32_t> last_Qdd_{};
        std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
        std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};

        // nmt
        device::NmtRequestProtocol<msgs::RobotDetail> *nmt_command_{nullptr};

        //sync
        device::SyncProtocol<msgs::RobotDetail> *sync_command_{nullptr};

        // sdo
        std::map<uint8_t, device::SdoRequestProtocol<msgs::RobotDetail> *> sdo_commands_{};

        // rpdo1
        std::map<uint8_t, device::motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
    };
}