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

#include "controller_manager.h"

#include "jointpositioncontroller.h"
#include "cartesiancontroller.h"
using namespace std;
using namespace cmvr::device;

ControllerManager::ControllerManager(const XmlNode& cfg):state_(ControlManagerState_Idle)
{

}

void ControllerManager::clearError()
{
    state_ = ControlManagerState_Idle;
}

void ControllerManager::create(const XmlNode& cfg)
{
    auto children = cfg.getChildren();
    for (auto& child : children)
    {
        if (child.getNodeName() == "ComponentGroup")
        {
            ComponentGroup group;
            group.id = child.getAttrString("id");
            // CanGroupID可能记录了多个canGroupID
            std::string canGroupIDs = child.getAttrString("CanGroupID");
            std::istringstream iss(canGroupIDs);
            std::string id;
            while (std::getline(iss, id, ',')) {
                group.CanGroupIDs.push_back(id);
            }
            auto controller_nodes = child.getChildren();
            for (auto& controller_node : controller_nodes)
            {
                if (controller_node.getNodeName() == "JointPositionCtrl")
                {
                    group.controllers_["JointPositionCtrl"] = std::make_shared<JointPositionController>(controller_node);
                }
                else if (controller_node.getNodeName() == "CartesianController")
                {
                    group.controllers_["CartesianController"] = std::make_shared<CartesianController>(controller_node);
                }
            }
            //

            componentGroups_[group.id] = group;
        }
    }
}