#include "DynamixelHandler.h"

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>


// Global variables
float _fps = 10.0f; // Hz

DynamixelHandler _oDxlHandler;
ros::Publisher _jointPositionPublisher;
std::string _poppyDxlPortName = "/dev/ttyUSB0";
float _poppyDxlProtocol = 2.0;
int _poppyDxlBaudRate = 1000000;
int _nbJoints = 6;
float _minJointCmd = 0;
float _maxJointCmd = 1023;
float _minJointAngle = -180.0f;
float _maxJointAngle = 180.0f;

 

int convertAnglesToJointCmd(float fJointAngle)
{
	// y = ax + b
	float a =  (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
	float b = _minJointCmd - a * _minJointAngle;
	float jointCmd = a * fJointAngle + b;
	return (int)jointCmd;
}


int main(int argc, char** argv)
{
	// create a node called poppy_ros
	ros::init(argc, argv, "poppy_ros", ros::init_options::NoSigintHandler);
	//ros::init(argc, argv, "autopilot");
	
	// create a node handle
	ros::NodeHandle nh;
	
	// create a publisher to joint_position topic
	_jointPositionPublisher = nh.advertise<geometry_msgs::Twist>("joint_position", 1);
	
	// create a loop rate
	ros::Rate loopRate(_fps);
	
	// create a Twist message
	geometry_msgs::Twist jointPositionMsg;
			
	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
	_oDxlHandler.setDeviceName(_poppyDxlPortName);
	_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
	_oDxlHandler.openPort();
	_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
	_oDxlHandler.enableTorque(false);
	std::cout << std::endl;
	
	
	
	ROS_INFO("===Launching Poppy node===");
	
	// loop until Ctrl+C is pressed or ROS connectivity issues
	while(ros::ok())
	{
		//===RETRIEVE Dynamixel Motor positions====
		std::vector<uint16_t> l_vCurrentJointPosition;
		bool bIsReadSuccessfull = _oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
		
		// stores them into a msg
		if (bIsReadSuccessfull)
		{
			jointPositionMsg.linear.x = l_vCurrentJointPosition[0];
			jointPositionMsg.linear.y = l_vCurrentJointPosition[1];
			jointPositionMsg.linear.z = l_vCurrentJointPosition[2];
			jointPositionMsg.angular.x = l_vCurrentJointPosition[3];
			jointPositionMsg.angular.y = l_vCurrentJointPosition[4];
			jointPositionMsg.angular.z = l_vCurrentJointPosition[5];
		}
	
		// publish the Twist message to the joint_position topic
		_jointPositionPublisher.publish(jointPositionMsg);
		
		// spin once to let the process handle callback ad key stroke
		ros::spinOnce();
		
		// sleep the right amout of time to comply with _fps 
		loopRate.sleep();
	}
	
	_oDxlHandler.enableTorque(false);
	_oDxlHandler.closePort();
	
	return 0;
}