//
// 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)
{
    create(cfg);
}

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;
        }
    }
}

void ControllerManager::switchMode(ControlManagerState state)
{
    //无论当前切换成哪种模式都应该停止控制器操作？
    if (activeController_)
    {
        activeController_->stop();
    }
    //切换当前状态
    switch (state)
    {
    case ControlManagerState_Idle:
        {
            break;
        }
    case ControlManagerState_Command:
        {
            break;
        }
    case ControlManagerState_MinorFault:
        {
            break;
        }
    case ControlManagerState_MajorFault:
        {
            break;
        }
    case ControlManagerState_Teach:
        {
            break;
        }
    case ControlManagerState_EStop:
        {
            break;
        }
    default:
        break;
    }
    state_ = state;
}

ComponentGroup& ControllerManager::getComponentGroup(const std::string& id)
{
    if (componentGroups_.count(id))
        return componentGroups_[id];
    throw std::runtime_error("ComponentGroup not found");
}

std::shared_ptr<AbstractController> ControllerManager::getActiveController()
{
    return activeController_;
}