/************************************************************************
 *									*
 *               Maxon 4QEC Motor Controller Ros Driver               *
 *									*
 *           Copyright (C) 2012, Centre for Autonomous Systems           *
 *           Copyright (C) 2012, University of New South Wales           *
 *									*
 ************************************************************************/

/*
 Author:  Bradford Heap <bradfordh@cse.unsw.edu.au>
 Date:    November 2012
 Version: 0.1

 $Id$

 This source file provides the ROS driver for one Maxon 4QEC motor
 controller connected to the server machine via an RS232 serial port.

 The driver is based off the existing Player driver.


 This program is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License along
 with this program; if not, write to the Free Software Foundation, Inc.,
 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

// **********************************************************************
// In-line Player-style documentation
/** @ingroup drivers */
/** @{ */
/** @defgroup driver_maxon4qec maxon4qec
 *  @brief Maxon 4QEC Motor Controller driver

 The maxon4qec driver controls one Maxon 4QEC motor controller.  This
 motor controller is connected to ROS server machine via an RS232
 serial port (including, of course, an RS232 to USB converter).

 Velocities are expressed in radians per second, odometry information (ie, current
 position) is returned in terms of radians (there are 2 * pi radians in a
 single revolution of the output shaft, AFTER the gearbox is taken into
 consideration).

 @par Compile-time dependencies

 - none

 @par Provides

 - @ref interface_position1d
 - Returns odometry and velocity data (in radians and radians per
 second, respectively).  Accepts velocity commands in radians per
 second.

 @par Configuration file options

 - port (string)
 - Default: "/dev/ttyS0"

 - driver_timeout (float)
 - Default: 10.0
 - If the Maxon4QEC driver does not receive a ROS message within
 driver_timeout seconds, it assumes communication with the ROS
 client has been lost.  To prevent a run-away robot in this case, the
 driver stops and brakes the motor (ie, sets the velocity to zero
 radians/sec).  Receiving a ROS message reenables the driver and
 restarts the motors with their old value.  You may want to tweak this
 value to a smaller number, such as 2.5 (seconds).

 - mainloop_delay (integer)
 - Default: 25000
 - The main loop Maxon4QEC driver consists of an infinite loop.
 You may want to tweak this value to a
 larger number, such as 100000 (ie, 0.1 seconds).

 - speedreggainp (integer)
 - Default: 682
 - P-gain for speed regulation.
 - The Maxon Communication Guide and Maxon Operating Guide are not
 particularly clear on how the P-gain and I-gain should be set.  The
 following procedure has been supplied directly by Maxon; it uses the
 Maxon-supplied DES User Interface program under Microsoft Windows:
 - 1. Set the P-gain and I-gain to zero.
 - 2. Increase P-gain until you get a stable oscillation.
 - 3. Set P-gain to 60% of the value derived in step 2.
 - 4. Use the Data Recorder tool in the DES User Interface program
 (Data -> Record Data -> Select Variable) with a Sampling period
 of 1.0ms and Variable "SpeedIs".
 - 5. Increase I-gain.
 - 6. Set Value to the desired speed
 - 7. Press the Jump button.  Warning: the motor will start moving!
 - 8. Set the speed to zero.
 - 9. Check the measured step response.  If the response is not good
 enough, return to step 5.

 - speedreggaini (integer)
 - Default: 220
 - I-gain for speed regulation.

 - peakcurrent (integer)
 - Default: 15000
 - Peak motor current in mA.  This must not be more than 30000 on the
 DES 70/10 controller, or 15000 on the DES 50/5.  It should be three
 times the value of maxcontcurrent.

 - maxcontcurrent (integer)
 - Default: 5000
 - Maximum continuous current in mA; should be the lesser of the motor's
 maximum continuous current or the controller's maximum.  On the DES
 70/10 controller, must not be more than 10000; on the DES 50/5, no
 more than 5000.

 - maxspeed (integer)
 - Default: 5000
 - Maximum idle (no load) speed of the motor in revolutions per minute
 (rpm).

 - acceleration (integer)
 - Default: 500
 - Acceleration and deceleration rates for the motor, in rpm per 128ms.

 - encresolution (integer)
 - Default: 500
 - Encoder resolution in counts per turn.

 - gearratio (float)
 - Default: 100.0
 - Gear reduction ratio on motor (ie, by default 100:1).

 - vel_tweak (float)
 - Default: 1.0
 - Velocity tweak factor, applied by multiplication when converting
 radians per second to revolutions per minute (rpm).  The appropriate
 formula is:
 - rpm = (60 * radpersec * gearratio * vel_tweak) / (2 * pi)

 - pos_tweak (float)
 - Default: 1.0
 - Position tweak factor, applied by multiplication when converting
 encoder tick counts to radians.  The appropriate formula is:
 - rad = (2 * pi * ticks * pos_tweak) / (encresolution * gearratio)

 - debug (boolean)
 - Default: false
 - Output debugging information (particularly results from the motor
 controller) if true.

 - test_mode (integer)
 - Default: 0
 - Enter a specific Maxon4QEC test mode if debug is true.  Currently not
 used.
 - bit 0: if set, test mode is on

 */
/** @} */

#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/Int32.h>

#include <boost/thread.hpp>

#include <maxon/desapi.h>
#include <maxon/maxon4qec.h>

// **********************************************************************
// Global constants

#define MAXON4QEC_PORT_SPEED	(B38400)	// Only 38400 baud supported
#define DEFAULT_MAXON4QEC_PORT	"/dev/ttyS0"	// Default serial port name
#define RETRY_COUNT		(1)		// Retry transmission count
#define DEFAULT_DRIVER_TIMEOUT	(10.0)		// Driver timeout in sec
#define DEFAULT_MAINLOOP_DELAY	(25000)		// Main loop delay (usec)
#define DEFAULT_SPEEDREGGAINP	(682)		// P-gain for speed regulation
#define DEFAULT_SPEEDREGGAINI	(220)		// I-gain for speed regulation
#define DEFAULT_PEAKCURRENT	(15000)		// Peak current (mA)
#define DEFAULT_MAXCONTCURRENT	(5000)		// Max. continuous current (mA)
#define DEFAULT_MAXSPEED	(5000)		// Max. idle speed (rpm)
#define DEFAULT_ACCELERATION	(500)		// Acceleration (rpm per 128ms)
#define DEFAULT_ENCRESOLUTION	(500)		// Encoder resolution (cpt)
#define DEFAULT_GEARRATIO	(100.0)		// Gear reduction ratio on motor
#define DEFAULT_VEL_TWEAK	(1.0)		// Velocity tweak factor
#define DEFAULT_POS_TWEAK	(1.0)		// Position tweak factor
// **********************************************************************
// Global macros

/*
 Retry a particular DES API function call.  First parameter is the function
 name as a string (eg, "des_reset"); second parameter is the function call
 itself with all parameters as required (eg, des_reset(port_fd)).  Sets
 retry_failed (which must be defined prior to macro invocation) to false if
 successful, true otherwise.  There does not seem to be an elegant way to do
 this in C or C++.
 */
#define HANDLE_RETRIES(des_func_name, des_func_call)			      \
    do {								      \
	retry_failed = true;						      \
	for (int tries = 0; tries < RETRY_COUNT; tries++) {		      \
									      \
	    if (des_func_call < 0) {					      \
		if (errno == EPROTO) {					      \
		    ROS_WARN("Communications (CRC) error detected; "      \
				 "possibly noisy cables on serial port %s.  " \
				 "Retrying...", port_name);		      \
		    usleep(DES_EPROTO_DELAY);				      \
		} else if (errno == ETIMEDOUT) {			      \
		    ROS_WARN("Time-out error; check serial port %s and "  \
				 "controller.  Retrying...", port_name);      \
		} else {						      \
		    break;						      \
		}							      \
	    } else {							      \
		retry_failed = false;					      \
		break;							      \
	    }								      \
	}								      \
									      \
	if (retry_failed) {						      \
	    ROS_ERROR("Failed on Maxon4QEC port %s: %s",		      \
			  port_name, strerror(errno));			      \
									      \
	    des_close(port_fd);						      \
	    port_fd = -1;						      \
	}								      \
    } while (0)

// **********************************************************************
// Constructor for the driver.
// TODO: take a file to parse as input.
Maxon4QEC::Maxon4QEC(const char* port, ros::NodeHandle* node) {
	// Setup the motor configuration.
	// TODO: Get these as actual message value

	//    port_name = cf->ReadString(section, "port", DEFAULT_MAXON4QEC_PORT);
	//port_name = "/dev/serial/by-id/usb-FTDI_US232R_FTB3LB86-if00-port0";
	port_name = port;
	port_fd = -1;

	node->param<int>("req_speedreggainp", req_speedreggainp, DEFAULT_SPEEDREGGAINP);
	//req_speedreggainp = 387;
	node->param<int>("req_speedreggaini", req_speedreggaini, DEFAULT_SPEEDREGGAINI);
	//req_speedreggaini = 62;
	node->param<int>("req_peakcurrent", req_peakcurrent, DEFAULT_PEAKCURRENT);
	//req_peakcurrent = 30000;
	node->param<int>("req_maxcontcurrent", req_maxcontcurrent, DEFAULT_MAXCONTCURRENT);
	//req_maxcontcurrent = 10000;
	node->param<int>("req_maxspeed", req_maxspeed, DEFAULT_MAXSPEED);
	//req_maxspeed = 9000;
	node->param<int>("req_acceleration", req_acceleration, DEFAULT_ACCELERATION);
	//req_acceleration = 500;
	node->param<int>("req_encresolution", req_encresolution, DEFAULT_ENCRESOLUTION);
	//req_encresolution = 500;

	node->param<double>("gearratio", gearratio, DEFAULT_GEARRATIO);
	//gearratio = 100.0;
	node->param<double>("vel_tweak", vel_tweak, DEFAULT_VEL_TWEAK);
	//vel_tweak = 1.0;
	node->param<double>("pos_tweak", pos_tweak, DEFAULT_POS_TWEAK);
	//pos_tweak = 1.0;

	double driver_timeout_f;
	node->param<double>("driver_timeout_f", driver_timeout_f, DEFAULT_DRIVER_TIMEOUT);
	//double driver_timeout_f = 1.5; // should be 10

	int tempDelay;
	node->param<int>("mainloop_delay", tempDelay, DEFAULT_MAINLOOP_DELAY);
	mainloop_delay = tempDelay;
	//mainloop_delay = 10000;

	// Calculate appropriate values

	double driver_timeout_sec_f = floor(driver_timeout_f);
	driver_timeout.tv_sec = lround(driver_timeout_sec_f);
	driver_timeout.tv_usec = lround(
			(driver_timeout_f - driver_timeout_sec_f) * 1000000);

	rad_rpm_factor = 30.0 * M_1_PI * gearratio * vel_tweak;
	rpm_rad_factor = M_PI / (30.0 * gearratio * vel_tweak);
	ticks_pos_factor = (2.0 * M_PI * pos_tweak)
			/ (req_encresolution * gearratio);

	return;
}

// **********************************************************************
// Maxon4QEC driver destructor
Maxon4QEC::~Maxon4QEC(void) {
	// Allow any outstanding DES commands to time out

	/* This timeout is necessary as Shutdown() is called in the
	 thread of the calling process / main server, NOT in the thread
	 that runs Main()... and Main() might be in the middle of
	 SetMotorVals() or UpdateMotorData().
	 */
	usleep(DES_TIMEOUT_DELAY * 3 / 2);

	// Stop the motors

	motor_enabled = false;
	req_motor_speed = 0;
	driver_enabled = true;

	if (!SetMotorVals()) {
		ROS_ERROR("Could not stop motor on Maxon4QEC port %s", port_name);
	}

	// Close the serial port

	if (port_fd >= 0) {
		des_close(port_fd);
		port_fd = -1;
	}

	ROS_INFO("Maxon4QEC has been shut down on port %s", port_name);

	return;
}

// **********************************************************************
// Set the motor speed and enabled bit on the Maxon4QEC controller
bool Maxon4QEC::SetMotorVals() {
	bool retry_failed;

	// Always enable or disable the motor

	HANDLE_RETRIES("des_enable", des_enable(port_fd, (int) motor_enabled));
	if (retry_failed) {
		return false;
	}

	// Set the requested motor speed if the motor is enabled

	if (motor_enabled) {
		HANDLE_RETRIES("des_setvelocity",
				des_setvelocity(port_fd, driver_enabled ? req_motor_speed : 0));
		if (retry_failed) {
			return false;
		}
	}

	return true;
}

// **********************************************************************
//
int Maxon4QEC::MainSetup() {
	// Maxon4QEC driver initialisation routine

	bool retry_failed;

	// Initialise the serial port

	ROS_INFO("Maxon4QEC initialising on serial port %s", port_name);

	port_fd = des_open(port_name, MAXON4QEC_PORT_SPEED);
	if (port_fd < 0) {
		ROS_ERROR(
				"Unable to open Maxon4QEC serial port %s: %s", port_name, strerror(errno));
		return -1;
	}

	// Reset the Maxon4QEC motor controller

	HANDLE_RETRIES("des_reset", des_reset(port_fd));
	if (retry_failed) {
		return -1;
	}

	usleep(DES_RESET_DELAY);

	// Read the controller's software and hardware version numbers

	HANDLE_RETRIES("des_readversion",
			des_readversion(port_fd, &software_version, &hardware_version, NULL, NULL));
	if (retry_failed) {
		return -1;
	}

	ROS_INFO("Maxon4QEC controller firmware version 0x%04X, hardware "
	"version 0x%04X", software_version, hardware_version);

	// Set the controller's parameters

	HANDLE_RETRIES("des_readalltempparam",
			des_readalltempparam(port_fd, &des_params));
	if (retry_failed) {
		return -1;
	}

	des_params.sysconfig &= ~DES_SP_SYSCONFIG_USESOFTWARE_MASK;
	des_params.sysconfig |= DES_SP_SYSCONFIG_USESOFTWARE;

	des_params.sysconfig &= ~DES_SP_SYSCONFIG_ACCELERATION_MASK;
	des_params.sysconfig &= ~DES_SP_SYSCONFIG_REGULATIONMODE_MASK;
	des_params.sysconfig |= DES_SP_SYSCONFIG_SPEEDREGULATION;

	des_params.speedreggainp = req_speedreggainp;
	des_params.speedreggaini = req_speedreggaini;
	des_params.peakcurrent = req_peakcurrent;
	des_params.maxcontcurrent = req_maxcontcurrent;
	des_params.maxspeed = req_maxspeed;
	des_params.acceleration = req_acceleration;
	des_params.encresolution = req_encresolution;

	HANDLE_RETRIES("des_setalltempparam",
			des_setalltempparam(port_fd, &des_params));
	if (retry_failed) {
		return -1;
	}

	// Initialise variables

	motor_enabled = false;
	req_motor_speed = 0;
	driver_enabled = true;

	cur_pos = 0.0;
	last_tick = 0;
	last_tick_valid = false;

	// Set the initial motor speeds
	if (!SetMotorVals()) {
		ROS_ERROR("Aborting Maxon4QEC initialisation on port %s", port_name);
		return -1;
	}

	return 0;
}

// **********************************************************************
// Set the motor state, only send the state if the state has changed.
void Maxon4QEC::setMotorEnabled(bool state) {

	if (state != motor_enabled) {
		motor_enabled = state;

		// spin the set motor vals into a new thread so we can control the two motors as evenly as possible.
		SetMotorVals();
		//boost::thread spin_thread = boost::thread(boost::bind(&Maxon4QEC::SetMotorVals, this));
	}

	return;
}

// **********************************************************************
// Set the motor velocity
void Maxon4QEC::setVelocity(const float vel) {
	int new_motor_speed;

	// Convert requested velocity vel (in radians per second) to a motor
	// speed (in revolutions per minute)

	new_motor_speed = lround(static_cast<double>(vel) * rad_rpm_factor);
	// 60 / (2 * pi) radians per second = 1 revolution per minute

	if (new_motor_speed > req_maxspeed) {
		ROS_WARN(
				"Requested velocity is too high on Maxon4QEC port %s: "
				"motor speed %d limited to %d", port_name, new_motor_speed, req_maxspeed);

		new_motor_speed = req_maxspeed;

	} else if (new_motor_speed < -req_maxspeed) {
		ROS_WARN(
				"Requested velocity is too high on Maxon4QEC port %s: "
				"motor speed %d limited to %d", port_name, new_motor_speed, -req_maxspeed);

		new_motor_speed = -req_maxspeed;
	}

	req_motor_speed = new_motor_speed;

	if (!driver_enabled) {
		ROS_DEBUG("Maxon4QEC driver received a message; "
		"enabling motor on port %s", port_name);

		driver_enabled = true;
		SetMotorVals();
	}
	gettimeofday(&lasttime, NULL);

	// spin up a new thread to set the motor running
	spin_thread = boost::thread(
			boost::bind(&Maxon4QEC::SetMotorVals, this));

	return;
}
// **********************************************************************
// check the time and if no messages recieved then stop motors.
void Maxon4QEC::checkMotorTimeout() {
	// Check to see if driver has timed out (ie, has not received
	// any messages during the last (curtime - lasttime) interval)
	gettimeofday(&curtime, NULL);
	timersub(&curtime, &lasttime, &t);
	// t = curtime - lasttime
	if (driver_enabled && timercmp(&t, &driver_timeout, >)) { // if (driver_enabled && (t > driver_timeout))
		ROS_DEBUG("Maxon4QEC driver timed out; "
		"disabling motor on port %s", port_name);

		driver_enabled = false;
		SetMotorVals();
	}

	return;
}

// **********************************************************************
// Read and calculate data values from Maxon4QEC controller
bool Maxon4QEC::UpdateMotorData() {
	static const int NPARAMS = 5;
	static const des_parameter_t params[NPARAMS] = { DES_SV_SYSOPSTATUS,
			DES_SV_STANDARDERROR, DES_SV_SPEEDDETVALUE, DES_SV_CURRENTDETQAXIS,
			DES_SV_CURRENTDETDAXIS };
	uint16_t data[NPARAMS];

	uint16_t data_sysopstatus;
	uint16_t data_standarderror;
	int16_t data_speeddetvalue;
	int32_t data_posdetvalue;

	bool retry_failed;

	// wait for any writes to the motor to finish.
	spin_thread.join();

	// Read data from DES

	HANDLE_RETRIES("des_readnvariables",
			des_readnvariables(port_fd, NPARAMS, params, data));
	if (retry_failed) {
		return false;
	}

	data_sysopstatus = data[0];
	data_standarderror = data[1];
	data_speeddetvalue = reinterpret_cast<int16_t*>(data)[2];

	HANDLE_RETRIES("des_readtempparam",
			des_readtempparam(port_fd, DES_SV_POSDETVALUE, &data_posdetvalue));
	if (retry_failed) {
		return false;
	}

	// Compute current velocity in radians per second

	position_data.vel =
			static_cast<double>(data_speeddetvalue * rpm_rad_factor);
	// (2 * pi) / 60 revolutions per minute = 1 radian per second

	// Compute current position in radians

	if (last_tick_valid) {
		/* The following formula assumes that data_posdetvalue uses the
		 full 32 bits before wrapping around (ie, 0x00000000 ->
		 0x7FFFFFFF -> 0x80000000 -> 0xFFFFFFFF -> 0x00000000 when
		 monotonically increasing, ie, rotating counter-clockwise)
		 */
		cur_pos = cur_pos + (data_posdetvalue - last_tick) * ticks_pos_factor;
	}

	last_tick = data_posdetvalue;
	last_tick_valid = true;

	position_data.pos = cur_pos;

	// Compute stall and status information

//	position_data.status &= ~(player_position1d_data_status_enabled_MASK
//			| player_position1d_data_status_over_current_MASK
//			| player_position1d_data_status_errors_MASK);

//	if (data_sysopstatus & DES_SYSSTATUS_POWERSTAGE1_MASK) {
//		position_data.status |= player_position1d_data_status_enabled_MASK;
//	}

	if (data_sysopstatus & DES_SYSSTATUS_ERRORS_MASK) {
		// DES indicates an error condition is present

		position_data.stall = ((data_standarderror & DES_ERROR_OVERCURRENT_MASK)
				!= 0);
		// @@@ Stall is not quite the same as being over-current: testing is needed!

//		if ((data_standarderror & DES_ERROR_OVERCURRENT_MASK) != 0) {
//			position_data.status |=
//					player_position1d_data_status_over_current_MASK;
//		}
//		if (data_standarderror) {
//			position_data.status |= player_position1d_data_status_errors_MASK;
//		}

		ROS_ERROR("Maxon4QEC on port %s returned error 0x%04x", port_name,
				data_standarderror);

		// Try to clear the error
		HANDLE_RETRIES("des_clearerror", des_clearerror(port_fd));
		if (retry_failed) {
			return false;
		}

	} else {
		// No error condition on the DES
		position_data.stall = false;
	}

	return true;
}

// **********************************************************************
// Returns the speed that the motor is reporting.
double Maxon4QEC::getVel() {
	return position_data.vel;
}

// **********************************************************************
// Velocity message callback.
//void Maxon4QEC::velocityCallback(const std_msgs::Int32::ConstPtr& msg) {
//	//msg->data;
//
//	/*int new_motor_speed;
//
//	 // Convert requested velocity vel (in radians per second) to a motor
//	 // speed (in revolutions per minute)
//
//	 new_motor_speed = lround(msg->data * rad_rpm_factor);
//	 // 60 / (2 * pi) radians per second = 1 revolution per minute
//
//	 if (new_motor_speed > req_maxspeed) {
//	 ROS_WARN("Requested velocity is too high on Maxon4QEC port %s: "
//	 "motor speed %d limited to %d", port_name, new_motor_speed,
//	 req_maxspeed);
//
//	 new_motor_speed = req_maxspeed;
//
//	 } else if (new_motor_speed < -req_maxspeed) {
//	 ROS_WARN("Requested velocity is too high on Maxon4QEC port %s: "
//	 "motor speed %d limited to %d", port_name, new_motor_speed,
//	 -req_maxspeed);
//
//	 new_motor_speed = -req_maxspeed;
//	 }
//
//	 req_motor_speed = new_motor_speed;
//
//	 if (!driver_enabled) {
//	 ROS_INFO("Maxon4QEC driver received a message; "
//	 "enabling motor on port %s", port_name);
//
//	 driver_enabled = true;
//	 SetMotorVals();
//	 }
//	 gettimeofday(&lasttime, NULL); // Reset the timeout counter
//	 if (driver_enabled) {
//	 SetMotorVals();
//	 }*/
//
//	return;
//}

// **********************************************************************
// Maxon4QEC driver main thread function
void Maxon4QEC::Main(ros::NodeHandle* node) {

	// Setup a subscriber to the callback.
	/*ros::Subscriber sub = node->subscribe("velocity", 1, &Maxon4QEC::velocityCallback, this);

	 // Setup the loop.
	 ros::Rate loop_rate( 1.0 / ( 100.0 / mainloop_delay) );	// needs to be in hertz not useconds

	 gettimeofday(&lasttime, NULL);

	 while (ros::ok()) {

	 // Check to see if driver has timed out (ie, has not received
	 // any messages during the last (curtime - lasttime) interval)
	 gettimeofday(&curtime, NULL);
	 timersub(&curtime, &lasttime, &t); // t = curtime - lasttime
	 if (driver_enabled && timercmp(&t, &driver_timeout, >)) { // if (driver_enabled && (t > driver_timeout))
	 ROS_INFO("Maxon4QEC driver timed out; "
	 "disabling motor on port %s", port_name);

	 driver_enabled = false;
	 SetMotorVals();
	 }

	 ros::spinOnce();
	 loop_rate.sleep();
	 }*/

	return;
}

// **********************************************************************

/*int main(int argc, char** argv) {
 ros::init(argc, argv, "maxon4qec");
 ros::NodeHandle node;

 ROS_INFO("CASWEEDER NODE STARTED");

 // Create the motor driver object:
 Maxon4QEC* driver = new Maxon4QEC();

 ROS_INFO("CASWEEDER CONFIGURATION SET");

 if (driver->MainSetup() == 0) { // setup the driver and enter into a loop listening for messages.
 driver->Main(&node);
 }

 delete (driver);

 return 0;
 }*/
