//
// 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)
{
    defaultSpeed_ = cfg.getAttrDefault("defaultSpeed",0.5f);
    defaultAcc_ = cfg.getAttrDefault("defaultAcc",0.5f);
}

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


        //解析json
        /*
         *  直接设置位置
            "params":{
                "canGroupId":["leftArm","rightArm"],
                "motors":[{"joint_name":"","position":0.01,"velocity"：0.8}]
            },
        **/
        std::string protocolType;
        if (json.isMember("params"))
        {
            std::vector<std::string> canGroupIds;
            const Json::Value& params = json["params"];
            if (params.isMember("canGroupId"))
            {
                for (const auto & canGroupId : params["canGroupId"])
                {
                    canGroupIds.emplace_back(canGroupId.asString());
                }
            }
            //先把所有电机信息都找到
            if (params.isMember("motors"))
            {
                for (int i = 0; i < params["motors"].size(); i++)
                {
                    const Json::Value& motorJson = params["motors"][i];
                    std::string joint_name = motorJson["joint_name"].asString();
                    double pos = motorJson["position"].asDouble();
                    double vel = motorJson["velocity"].asDouble();
                    for (const auto& canGroupId : canGroupIds)
                    {
                        try
                        {
                            auto canGroup = HardWareManager::getInstance().getCanGroup(canGroupId);
                            auto motorInfo = canGroup->getMotorInfo(joint_name);
                            motorInfo.motorProtocol->setMode(motorInfo.node_id, msgs::RUN_MODE_PROFILE_POSITION);
                            motorInfo.motorProtocol->setQd(motorInfo.node_id, vel);
                            //考虑限位
                            if (pos > motorInfo.limitQUb)
                            {
                                pos = motorInfo.limitQUb;
                                LOG(WARNING) << "JointPositionController[call]: Joint [" << joint_name
                                             << "] target position exceeds upper limit! Current target: " << pos
                                             << ", upper limit: " << motorInfo.limitQUb;
                            }
                            else if (pos < motorInfo.limitQLb)  // Assume lower limit field exists (limitQLb)
                            {
                                pos = motorInfo.limitQLb;
                                LOG(WARNING) << "JointPositionController[call]: Joint [" << joint_name
                                             << "] target position exceeds lower limit! Current target: " << pos
                                             << ", lower limit: " << motorInfo.limitQLb;
                            }
                            motorInfo.motorProtocol->setQ(motorInfo.node_id, pos);
                            break;
                        }
                        catch (const std::exception& e)
                        {
                            continue;  // 捕获异常，继续尝试下一个 canGroup
                        }
                    }
                }
            }
        }
        state_ = ControllerState_Executing;
    }
    catch (const std::exception& e)
    {
        LOG(ERROR)<<"[JointPositionController](call):" <<e.what();
        throw runtime_error(e.what());
    }

}

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


    state_ = ControllerState_Idle;
}

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