//
// Created by lgv on 2025/8/1.
//


#pragma once

#include "abstract_motor.h"

#include <unordered_map>
#include <memory>
#include <string>
#include <mutex>

namespace cmvr {
    namespace device {

        class MotorManager {
        public:
            void addMotor(uint8_t node_id, std::shared_ptr<AbstractMotor> motor) {
                std::lock_guard<std::mutex> lock(mutex_);
                motors_[node_id] = std::move(motor);
            }

            void addMotor(std::shared_ptr<AbstractMotor> motor) {
                std::lock_guard<std::mutex> lock(mutex_);
                motors_map_[motor->jointName()] = std::move(motor);
            }

            std::shared_ptr<AbstractMotor> getMotor(uint8_t node_id) const {
                auto it = motors_.find(node_id);
                if (it != motors_.end()) {
                    return it->second;
                }
                return nullptr;
            }

            std::shared_ptr<AbstractMotor> getMotor(const std::string& joint_name) const {
                auto it = motors_map_.find(joint_name);
                if (it != motors_map_.end()) {
                    return it->second;
                }
                return nullptr;
            }

            const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& motorsMap() const {
                return motors_map_;
            }

        private:
            mutable std::mutex mutex_;
            std::unordered_map<uint8_t, std::shared_ptr<AbstractMotor>> motors_;
            std::unordered_map<std::string, std::shared_ptr<AbstractMotor>> motors_map_;
        };

    } // namespace device
} // namespace cmvr