//
// 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",
            "motors":[{"joint_name":""},{"joint_name":""}]
        },
        "operate":{}

    **/
    std::string canGroupId;
    std::string protocolType;
    if (json.isMember("params"))
    {
        const Json::Value& params = json["params"];
        if (params.isMember("canGroupId"))
            canGroupId = params["canGroupId"].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"))
    {

    }
    //最后计算好了通过下面的接口调用电机通信
    std::string joint_name ="";
    auto motorInfo = HardWareManager::getInstance().getCanGroup(canGroupId)->getMotorInfo(joint_name);
    motorInfo.motorProtocol->setQ(motorInfo.node_id,0.1);

}

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


    state_ = ControllerState_Idle;
}

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