/*
 * birabayin.hpp
 *
 *  Created on: 30/05/2013
 *      Author: mmcgill
 */

#ifndef CASUALTY_BIRABAYIN_HPP_
#define CASUALTY_BIRABAYIN_HPP_

/************************************************************************
 *									*
 *                        Birabayin 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 merging two Maxon 4QEC
 motor controllers (using the Maxon4QEC driver).

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_birabayin birabayin
 *  @brief Birabayin driver

 The birabayin driver takes two position1d interfaces, usually provided by
 two instances of the Maxon4QEC driver, and merges them into a single
 position2d robot driver.

 This driver provides a Player/Stage 2.0 position2d interface.  Velocities
 are expressed in metres per second, odometry information (ie, current
 position) is returned in terms of metres.

 @par Compile-time dependencies

 - none

 @par Provides

 - @ref interface_position2d
 - Returns odometry and velocity data (in metres and meters per second,
 respectively).  Accepts velocity commands in metres per second.

 @par Requires

 - @ref interface_position1d
 - Two instances of position1d devices are required, "left" for the left
 motor and "right" for the right motor.  These devices are assumed to
 take velocity commands in radians per second, and are assumed to
 return odometry and velocity data in radians and radians per second
 respectively.

 @par Configuration file options

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

 - driver_timeout (float)
 - Default: 10.0
 - If the Birabayin driver does not receive a Player message within
 driver_timeout seconds, it assumes communication with the Player
 client has been lost.  To prevent a run-away robot in this case, the
 driver stops and brakes the motors (ie, sets the velocity of each
 motor to zero radians/sec).  Receiving a Player 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 Birabayin driver consists of an infinite loop running
 in its own thread.  This thread calls usleep(mainloop_delay) to
 prevent it from taking too much CPU and from filling up the Player
 message queue too quickly.  You may want to tweak this value to a
 larger number, such as 100000 (ie, 0.1 seconds).

 - robot_width (length)
 - Default: 0.5
 - Width of the robot (from side to side), including wheel thickness.

 - robot_depth (length)
 - Default: 0.5
 - Depth of the robot (from front to back), including wheel protrusions.

 - wheel_diameter (length)
 - Default: 0.25
 - Diameter of wheels/tyres on robot.

 - translation_tweak (float)
 - Default: 1.0
 - When converting Player's velocity values (in metres per second) to
 the underlying motors' values in radians per second, tweak the final
 translational (forward) value by multiplying it with translation_tweak.
 Thus, a value of 1.0 (the default) does nothing; a value of 0.5 will
 halve the resulting forward velocity, -2.0 will double it and reverse
 the direction.

 - rotation_tweak (float)
 - Default: 1.0
 - When converting Player's velocity values (in metres per second) to
 the underlying motors' values in radians per second, tweak the final
 rotational (side-to-side) value by multiplying it with rotation_tweak.
 Thus, a value of 1.0 (the default) does nothing; a value of 0.5 will
 halve the resulting side-ways velocity, -2.0 will double it and
 reverse the direction.

 - wheel_tweak_left_forward (float)
 - Default: 1.0
 - Tweak factor applied to the left wheel when moving forward; the
 forward left wheel velocity is multiplied by this amount.  For
 example, if wheel_tweak_left_forward is 0.98, a nominal velocity of
 3.14 rad/sec is tweaked to 3.14 * 0.98 = 3.08 (with rounding).

 - wheel_tweak_left_reverse (float)
 - Default: 1.0
 - Tweak factor (multiplier) applied to the left wheel when moving in
 reverse.

 - wheel_tweak_right_forward (float)
 - Default: 1.0
 - Tweak factor (multiplier) applied to the right wheel when moving
 forward.

 - wheel_tweak_right_reverse (float)
 - Default: 1.0
 - Tweak factor (multiplier) applied to the right wheel when moving
 in reverse.

 */
/** @} */

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

#include <nav_msgs/Odometry.h>

#include <maxon/maxon4qec.h>

namespace casualty {

 // **********************************************************************
 // Birabayin driver class

class Birabayin {

private:
	Maxon4QEC* left_motor; // Driver for left motor
	Maxon4QEC* right_motor; // Driver for right motor

	double robot_width; // Robot width in metres
	double robot_depth; // Robot depth in metres
	double wheel_diameter; // Diameter of wheels in metres

	bool motors_enabled; // Are motors enabled?
	double req_left_speed; // Requested left motor speed
	double req_right_speed; // Requested right motor speed

	useconds_t mainloop_delay; // Main loop delay (usec)

	double translation_tweak; // Tweak for translation velocity
	double rotation_tweak; // Tweak for rotation velocity
	double wheel_tweak_left_forward; // Tweak factor for left wheel
	double wheel_tweak_left_reverse; // Tweak factor for left wheel
	double wheel_tweak_right_forward; // Tweak factor for right wheel
	double wheel_tweak_right_reverse; // Tweak factor for right wheel

	double transl_mult; // Multiplier for translation velocity
	double rotat_mult; // Multiplier for rotation velocity

//	struct timeval lasttime, curtime, t;	// timing data for motor timeout.
	ros::Time lastCommandRecieved;
	double commandTimeout;
	std::string left_motor_addr; 	// port that the motors are on.
	std::string right_motor_addr;

	bool SetMotorVals(); // Set the motor speeds
	void publishOdom(ros::Publisher* odom_pub, ros::Time current_time); // publish the odom data
	void processCmdVel(const geometry_msgs::Twist::ConstPtr& cmd_vel);

public:
	Birabayin(ros::NodeHandle* node);
	~Birabayin(void);

	int Setup();

	void Main(ros::NodeHandle* node); // Main driver thread
};

} // namespace casualty

#endif /* CASUALTY_BIRABAYIN_HPP_ */
