#include "KinematicsWebAPI.h"
#include "QuadrupedRobotSimulation/KinematicsHelper.h"

#include <iostream>

json KinematicsWebAPI::handleQuadrupedCommand(const std::string &req_cmd, const json &req_param)
{
    if (req_cmd == "Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles")
    {
        std::cout << "[DEBUG] Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles: 开始计算点位" << std::endl;
        return KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(req_param.dump());
    }

    if (req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
    {
        std::cout << "[DEBUG] Cmd_QuadrupedRobot_PerformForwardKinematics: 开始执行正运动学" << std::endl;
        return KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(req_param.dump());
    }

    return createUnknownCommandResponse(req_cmd, req_param);
}
