#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float64MultiArray.h>
#include "DynamixelHandler.h"
#include <vector>

// Global variables for the Dynamixel handler and ROS publishers/subscribers
DynamixelHandler dxlHandler;
ros::Publisher jointPositionPublisher;
ros::Subscriber jointCommandSubscriber;

// Convert angles in degrees to Dynamixel motor commands
int convertAnglesToJointCmd(float fJointAngle) {
    // Placeholder for conversion logic
    // This should be replaced with the actual logic used in your system
    // Example: Linear mapping from angle to motor command
    const float minJointCmd = 0;
    const float maxJointCmd = 1023;
    const float minJointAngle = -180.0f;
    const float maxJointAngle = 180.0f;
    
    float a =  (maxJointCmd - minJointCmd) / (maxJointAngle - minJointAngle);
    float b = minJointCmd - a * minJointAngle;
    return static_cast<int>(a * fJointAngle + b);
}

// Callback function for joint commands
void jointCMDCallback(const std_msgs::Float64MultiArray& joint_cmd) {
    std::vector<uint16_t> target_positions;
    for (float angle : joint_cmd.data) {
        target_positions.push_back(static_cast<uint16_t>(convertAnglesToJointCmd(angle)));
    }
    
    // Send the target joint positions to the Dynamixel motors
    dxlHandler.sendTargetJointPosition(target_positions);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "poppy_ros");
    ros::NodeHandle nh;

    // Setup the DynamixelHandler
    std::string poppyDxlPortName = "/dev/ttyUSB0";
    float poppyDxlProtocol = 2.0;
    int poppyDxlBaudRate = 1000000;
    
    dxlHandler.setDeviceName(poppyDxlPortName);
    dxlHandler.setProtocolVersion(poppyDxlProtocol);
    dxlHandler.openPort();
    dxlHandler.setBaudRate(poppyDxlBaudRate);
    dxlHandler.enableTorque(true);

    // Create the ROS publisher and subscriber
    jointPositionPublisher = nh.advertise<geometry_msgs::Twist>("joint_position", 1);
    jointCommandSubscriber = nh.subscribe("joint_cmd", 1, jointCMDCallback);

    ros::Rate loop_rate(10); // Define a suitable rate

    while (ros::ok()) {
//        std::vector<uint16_t> current_positions = dxlHandler.readCurrentJointPosition();
  		std::vector<uint16_t> current_positions;
		bool bIsReadSuccessfull = dxlHandler.readCurrentJointPosition(current_positions);      
        // Publish the current joint positions as Twist messages
        geometry_msgs::Twist joint_positions_msg;
        for (uint16_t position : current_positions) {
            // Placeholder for converting Dynamixel units to degrees
            // Add conversion logic here
        }
        
        jointPositionPublisher.publish(joint_positions_msg);
        
        ros::spinOnce();
        loop_rate.sleep();
    }
    
    dxlHandler.closePort();
    
    return 0;
}