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

#include "jointpositioncontroller.h"
#include "hardware/can/can_manager.h"
#include "hardware_manager/hardware_manager.h"
using namespace std;
using namespace cmvr::device;
using namespace cmvr::hardware;

JointPositionController::JointPositionController(const XmlNode& cfg):AbstractController(cfg)
{

}

void JointPositionController::call(const Json::Value& json)
{
    if (state_ != ControllerState_Idle)
        return;


    //解析jason，执行算法
    /*
        "params":{
            “canGroupId”:"leftArm",
            "protocolType":"Ti5MotorProtocol",
            "motors":[{"joint_name":""},{"joint_name":""}]
        },
        "operate":{}

    **/
    std::string canGroupId;
    std::string protocolType;
    if (json.isMember("params"))
    {
        Json::Value params = json["params"];
        if (params.isMember("canGroupId"))
            canGroupId = params["canGroupId"].asString();
        if (params.isMember("protocolType"))
            protocolType = params["protocolType"].asString();
        if (params.isMember("motors"))
        {
            for (int i = 0; i < params["motors"].size(); i++)
            {
                std::string joint_name =  params["motors"][i]["joint_name"].asString();
            }
        }
    }

    if (json.isMember("operate"))
    {

    }

    //调用canmanager执行电机指令
    auto protocol = HardWareManager::getInstance().getCanGroup(canGroupId)->getMotorProtocol(protocolType);
    //这里通过joint_name获取node_id?
}

void JointPositionController::interrupt()
{
    state_ = ControllerState_Switching;


    state_ = ControllerState_Idle;
}

void JointPositionController::stop()
{
    state_ = ControllerState_Idle;
}