#include "math.h"

#include "ros/ros.h"
#include "advrobotics_lab3/ikpoppy.h"
#include "advrobotics_lab3/joints.h"
#include "advrobotics_lab3/gripper.h" // Include the gripper message header

#include "Kinematics.h"

float _fps = 10.0;

double deg2rad(double angle)
{
    return -angle / 180.0 * M_PI;
}

double rad2deg(double rad)
{
    return rad * 180.0 / M_PI;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "ik_client");

    if (argc != 4)
    {
        ROS_INFO("Usage: rosrun advrobotics_lab3 ik_client x y z");
        ROS_INFO("Example: rosrun advrobotics_lab3 ik_client 6 9 0");
        return 1;
    }

    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<advrobotics_lab3::ikpoppy>("ik");

    ros::Publisher jointCmdPublisher = nh.advertise<advrobotics_lab3::joints>("joint_cmd", 1);
    ros::Publisher gripperCmdPublisher = nh.advertise<advrobotics_lab3::gripper>("gripper_cmd", 1);
 // Add gripper publisher

    advrobotics_lab3::ikpoppy srv;
    srv.request.x = atof(argv[1]);
    srv.request.y = atof(argv[2]);
    srv.request.z = atof(argv[3]);

    // Send the target position
if (client.call(srv))
{
    ROS_INFO("IK: (x, y, z) --> (J1, J2, J3)");
    ROS_INFO("(%f, %f, %f) --> (%f, %f, %f)", atof(argv[1]), atof(argv[2]), atof(argv[3]),
             rad2deg(srv.response.q1), rad2deg(srv.response.q2), rad2deg(srv.response.q3));

    advrobotics_lab3::joints jointCmdMsg;
    jointCmdMsg.q1 = rad2deg(srv.response.q1);
    jointCmdMsg.q2 = rad2deg(srv.response.q2);
    jointCmdMsg.q3 = rad2deg(srv.response.q3);

    advrobotics_lab3::gripper gripperCmd;
    gripperCmd.gripper = -40.0; // Open the gripper

    ros::Rate loopRate(_fps); // Maintain loop rate for movement
    for (int i = 0; i < 10; ++i) // Assuming movement takes 5 seconds at _fps = 10
    {
        // Publish joint commands to move the robot
        jointCmdPublisher.publish(jointCmdMsg);

        // Continuously publish the gripper open command
        gripperCmdPublisher.publish(gripperCmd);

        ROS_INFO("Moving to target position... Step: %d", i + 1);

        ros::spinOnce();
        loopRate.sleep();
    }

    ROS_INFO("Robot has reached the target position. Gripper remains open.");
}
else
{
    ROS_ERROR("Failed to call the service ik!");
    return 1;
}


    return 0;
}