//
// Created by lgv on 11/10/25.
//

#ifndef CMVR_ES_MOTORS_INFO_H
#define CMVR_ES_MOTORS_INFO_H

#include "../../devices/canbus/abstract_canbus.h"
#include "canbus/can_comm/can_sender.h"
#include "canbus/can_comm/can_receiver.h"
#include "canbus/can_comm/message_manager.h"
#include "cmvr/msgs/robot_detail.pb.h"
#include "motor/motor_manager.h"

namespace cmvr {
    class MotorsInfo {
    private:
        MotorsInfo();

        MotorsInfo(const MotorsInfo &) = delete;

        MotorsInfo &operator=(const MotorsInfo &) = delete;

    public:
        ~MotorsInfo(){};

        void init(const XmlNode &cfg);

        static MotorsInfo *getInstance() {
            static MotorsInfo instance;
            return &instance;
        }

        bool getJointsQ(const std::vector<std::string> &joints_name,std::unordered_map<std::string, double> &joint_qs) const;
        bool getJointsQ(const std::vector<std::string> &joints_name,std::vector<double> &joint_qs) const;
        bool getJointsQ(std::unordered_map<std::string, double> &joint_qs) const;

        std::shared_ptr<device::MotorManager> getMotorManager() const {
            return motor_manager_;
        }





    private:
        std::vector<XmlNode>  l_motors_cfg_{};
        std::vector<XmlNode>  r_motors_cfg_{};
        std::vector<XmlNode>  waist_motors_cfg_{};
        std::vector<XmlNode>  head_motors_cfg_{};


        std::shared_ptr<device::AbstractCanbus> l_can_client_{nullptr};
        std::shared_ptr<device::AbstractCanbus> r_can_client_{nullptr};
        std::shared_ptr<device::AbstractCanbus> waist_can_client_{nullptr};
        std::shared_ptr<device::AbstractCanbus> head_can_client_{nullptr};

        std::shared_ptr<device::CanReceiver<msgs::RobotDetail>>  l_can_receiver_{nullptr};
        std::shared_ptr<device::CanReceiver<msgs::RobotDetail>>  r_can_receiver_{nullptr};
        std::shared_ptr<device::CanReceiver<msgs::RobotDetail>>  waist_can_receiver_{nullptr};
        std::shared_ptr<device::CanReceiver<msgs::RobotDetail>>  head_can_receiver_{nullptr};

        std::shared_ptr<device::CanSender<msgs::RobotDetail>>  l_can_sender_{nullptr};
        std::shared_ptr<device::CanSender<msgs::RobotDetail>>  r_can_sender_{nullptr};
        std::shared_ptr<device::CanSender<msgs::RobotDetail>>  waist_can_sender_{nullptr};
        std::shared_ptr<device::CanSender<msgs::RobotDetail>>  head_can_sender_{nullptr};

        std::shared_ptr<device::MessageManager<msgs::RobotDetail>>  l_message_manager_{nullptr};
        std::shared_ptr<device::MessageManager<msgs::RobotDetail>>  r_message_manager_{nullptr};
        std::shared_ptr<device::MessageManager<msgs::RobotDetail>>  waist_message_manager_{nullptr};
        std::shared_ptr<device::MessageManager<msgs::RobotDetail>>  head_message_manager_{nullptr};

        bool waist_enabled_{false};
        bool right_arm_enabled_{false};
        bool left_arm_enabled_{false};
        bool head_enabled_{false};

        std::shared_ptr<device::MotorManager>  motor_manager_{nullptr};

    };
}



#endif //CMVR_ES_MOTORS_INFO_H