#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")
    {
        return KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(req_param.dump());
    }

    if (req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
    {
        return KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(req_param.dump());
    }

    return createUnknownCommandResponse(req_cmd, req_param);
}