#include <boost/thread.hpp>
//#include <ros/ros.h>
//#include <std_msgs/Int32.h>
#include <maxon/desapi.h>

class Maxon4QEC {

private:
	const char* port_name; // Serial port name
	int port_fd; // Serial port file descriptor

	bool motor_enabled; // Is motor enabled?
	int req_motor_speed; // Requested motor speed (rpm)

	useconds_t mainloop_delay; // Main loop delay (usec)
	bool driver_enabled; // Is ROS driver enabled?
	struct timeval driver_timeout; // Time out value for driver

	des_sysparam_t des_params; // Servoamplifier parameters

	int req_speedreggainp; // P-gain for speed regulation
	int req_speedreggaini; // I-gain for speed regulation
	int req_peakcurrent; // Peak current (mA)
	int req_maxcontcurrent; // Max. continuous current (mA)
	int req_maxspeed; // Max. idle speed (rpm)
	int req_acceleration; // Acceleration (rpm per 128ms)
	int req_encresolution; // Encoder resolution (cpt)

	double gearratio; // Gear reduction ratio on motor
	double vel_tweak; // Velocity tweak factor
	double pos_tweak; // Position tweak factor

	double rad_rpm_factor; // Radians/sec to rpm factor
	double rpm_rad_factor; // rpm to radians/sec factor
	double ticks_pos_factor; // ticks to position factor

	double cur_pos; // Current position (radians)
	int32_t last_tick; // Last encoder tick value
	bool last_tick_valid; // Is last_tick usable?

	uint16_t software_version; // Controller firmware "software" version
	uint16_t hardware_version; // Controller firmware "hardware" version

	struct timeval lasttime, curtime, t;

	bool SetMotorVals();

	boost::thread spin_thread;

	// position data struct, manually created as pos_1d data type doesn't exist in ROS.
	struct position1d_data_t {
		double vel;
		double pos;
		uint16_t stall;
	};

	position1d_data_t position_data;

public:
	Maxon4QEC(const char* port, ros::NodeHandle* node);
	~Maxon4QEC(void);
	int MainSetup();
	void Main(ros::NodeHandle* node);
//	void velocityCallback(const std_msgs::Int32::ConstPtr& msg);
	void setMotorEnabled(bool state);
	void setVelocity(const float vel);
	void checkMotorTimeout();
	bool UpdateMotorData();
	double getVel();
};
