#include "ros/ros.h"
#include "poppy_ros/InverseKinematics.h"

bool inverseKinematicsService(poppy_ros::InverseKinematics::Request& req, poppy_ros::InverseKinematics::Response& res)
{
    
    res.joint1 = req.x;
    res.joint2 = req.y;
    res.joint3 = req.z;
    res.joint4 = 0.0;
    res.joint5 = 0.0;
    res.joint6 = 0.0;

    return true;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "ik_server");
    ros::NodeHandle nh;
    
    ros::ServiceServer server = nh.advertiseService("ik", inverseKinematicsService);

    ROS_INFO("ik_server is ready to receive requests.");

    ros::spin();

    return 0;
}