#include <iostream>
#include "ros/ros.h"
#include "poppy_ros/InverseKinematics.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char** argv)
{
    if (argc != 4)
    {
        std::cerr << "Usage: rosrun poppy_ros ik_client <x_target> <y_target> <z_target>" << std::endl;
        return 1;
    }

    ros::init(argc, argv, "ik_client");
    ros::NodeHandle nh;
    ros::ServiceClient ikClient = nh.serviceClient<poppy_ros::InverseKinematics>("ik");
    
    poppy_ros::InverseKinematics srv;
    srv.request.x = std::stof(argv[1]);
    srv.request.y = std::stof(argv[2]);
    srv.request.z = std::stof(argv[3]);

    if (ikClient.call(srv))
    {
        ROS_INFO("Received joint values from ik_server.");

        ros::Publisher jointCmdPublisher = nh.advertise<geometry_msgs::Twist>("/joint_cmd", 1);
        geometry_msgs::Twist jointCmdMsg;

        // Assign the received joint values to the Twist message
        jointCmdMsg.linear.x = srv.response.joint1;
        jointCmdMsg.linear.y = srv.response.joint2;
        jointCmdMsg.linear.z = srv.response.joint3;
        jointCmdMsg.angular.x = srv.response.joint4;
        jointCmdMsg.angular.y = srv.response.joint5;
        jointCmdMsg.angular.z = srv.response.joint6;

        // Publish joint values to the topic /joint_cmd
        jointCmdPublisher.publish(jointCmdMsg);

        ROS_INFO("Published joint values to /joint_cmd.");
    }
    else
    {
        ROS_ERROR("Failed to call ik service.");
        return 1;
    }

    return 0;
}