#include <birabayin/birabayin.hpp>

using namespace casualty;

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

#define DEFAULT_MAINLOOP_DELAY	(25000)		// Main loop delay (usec)
#define DEFAULT_ROBOT_WIDTH	(0.5)		// Default robot width in metres
#define DEFAULT_ROBOT_DEPTH	(0.5)		// Default robot depth in metres
#define DEFAULT_WHEEL_DIAMETER	(0.25)		// Default wheel diameter in metres
#define DEFAULT_TRANSLATION_TWEAK (1.0)		// Translation velocity tweak
#define DEFAULT_ROTATION_TWEAK	  (1.0)		// Rotation velocity tweak
#define DEFAULT_WHEEL_TWEAKS	  (1.0)		// Wheel tweak factors
#define DEFAULT_COMMAND_TIMEOUT	  (1.0)		// Wheel tweak factors

// **********************************************************************
// Birabayin driver constructor
Birabayin::Birabayin(ros::NodeHandle* node) {

	// Read in the configuration file settings
	node->param<double>("robot_width", robot_width, DEFAULT_ROBOT_WIDTH);
	//robot_width = 0.42;
	node->param<double>("robot_depth", robot_depth, DEFAULT_ROBOT_DEPTH);
	//robot_depth = 0.7;
	node->param<double>("wheel_diameter", wheel_diameter, DEFAULT_WHEEL_DIAMETER);
//	wheel_diameter = 0.30;

	node->param<double>("translation_tweak", translation_tweak, DEFAULT_TRANSLATION_TWEAK);
	//translation_tweak = 1.0;
	node->param<double>("rotation_tweak", rotation_tweak, DEFAULT_ROTATION_TWEAK);
	//rotation_tweak = 1.0;

	node->param<double>("wheel_tweak_left_forward", wheel_tweak_left_forward, DEFAULT_WHEEL_TWEAKS);
	//wheel_tweak_left_forward = -1.0;
	node->param<double>("wheel_tweak_left_reverse", wheel_tweak_left_reverse, DEFAULT_WHEEL_TWEAKS);
	//wheel_tweak_left_reverse = -1.0;
	node->param<double>("wheel_tweak_right_forward", wheel_tweak_right_forward, DEFAULT_WHEEL_TWEAKS);
	//wheel_tweak_right_forward = 1.0;
	node->param<double>("wheel_tweak_right_reverse", wheel_tweak_right_reverse, DEFAULT_WHEEL_TWEAKS);
	//wheel_tweak_right_reverse = 1.0;

	int tempDelay;
	node->param<int>("mainloop_delay", tempDelay, DEFAULT_MAINLOOP_DELAY);
	mainloop_delay = tempDelay;
	if (mainloop_delay <= 0) {
		ROS_ERROR("mainloop_delay is out of range: %d "
		"(must be greater than 0)", mainloop_delay);
//		this->SetError(-1);
		return;
	}

	// Calculate appropriate values

	transl_mult = (2.0 * translation_tweak) / wheel_diameter;
	rotat_mult = (rotation_tweak * robot_width) / wheel_diameter;

	//TODO: Create the left motor device -- we need to pass it the port, and then set all the required variables.
	//left_motor = new Maxon4QEC(
	//		"/dev/serial/by-id/usb-FTDI_US232R_FTB3LB86-if00-port0");
	node->getParam("left_motor", left_motor_addr);
	left_motor = new Maxon4QEC(left_motor_addr.c_str(), node);

	//TODO: Create the right motor device
	node->getParam("right_motor", right_motor_addr);
	//right_motor = new Maxon4QEC(
	//		"/dev/serial/by-id/usb-FTDI_US232R_FTFBUJPN-if00-port0");
	right_motor = new Maxon4QEC(right_motor_addr.c_str(), node);

	// Timeout after which we stop the motors
	node->param<double>("timeout", commandTimeout, DEFAULT_COMMAND_TIMEOUT);
	return;
}

// **********************************************************************
// Birabayin driver destructor
Birabayin::~Birabayin(void) {

	// Stop the motors

	motors_enabled = false;
	req_left_speed = 0.0;
	req_right_speed = 0.0;

	SetMotorVals();

	delete (left_motor);

	delete (right_motor);

	ROS_INFO("Birabayin driver has been shut down");

	return;
}

// **********************************************************************
// Birabayin driver initialisation routine
int Birabayin::Setup() {

	// Initialise (by subscribing to) the left and right motor drivers
	ROS_INFO("Birabayin driver initialising");

	if (left_motor->MainSetup() == -1) {
		ROS_ERROR("Unable to setup left motor device");
		return -1;
	}

	if (right_motor->MainSetup() == -1) {
		ROS_ERROR("Unable to setup right motor device");
		return -1;
	}

	// Initialise variables

	motors_enabled = false;
	req_left_speed = 0.0;
	req_right_speed = 0.0;

	// Set the initial motor speeds

	if (!SetMotorVals()) {
		ROS_ERROR("Aborting Birabayin driver initialisation");
		return -1;
	}

	return 0;
}

// **********************************************************************
// Set the motor speeds and enabled bits on the Birabayin robot
bool Birabayin::SetMotorVals() {

	// Always enable or disable the motors

	left_motor->setMotorEnabled(motors_enabled);
	right_motor->setMotorEnabled(motors_enabled);

	// Set the requested motor speeds if the motors are enabled

	if (motors_enabled) {

		left_motor->setVelocity(req_left_speed);
		right_motor->setVelocity(req_right_speed);

	}

	return true;
}

// **********************************************************************
// Callback to process a new velocity message
void Birabayin::processCmdVel(const geometry_msgs::Twist::ConstPtr& cmd_vel) {
	lastCommandRecieved = ros::Time::now();
	double px = cmd_vel->linear.x;
	double py = 0.0;
	double pa = cmd_vel->angular.z;

	if (py != 0.0) {
		ROS_WARN("Birabayin driver received a non-zero Y velocity of "
		"py = %g; ignored", py);
	}

	// Calculate actual velocities for left and right motors (with tweaks)

	double transl_vel = px * transl_mult;
	double rotat_vel = sin(pa) * rotat_mult;

	req_left_speed = transl_vel - rotat_vel;
	req_right_speed = transl_vel + rotat_vel;

	if (req_left_speed >= 0.0) {
		req_left_speed *= wheel_tweak_left_forward;
	} else {
		req_left_speed *= wheel_tweak_left_reverse;
	}

	if (req_right_speed >= 0.0) {
		req_right_speed *= wheel_tweak_right_forward;
	} else {
		req_right_speed *= wheel_tweak_right_reverse;
	}

	motors_enabled = true;
	SetMotorVals();

}

void Birabayin::publishOdom(ros::Publisher* odom_pub, ros::Time current_time) {

	nav_msgs::Odometry odom;
	odom.header.stamp = current_time;
	odom.header.frame_id = "odom";

//    position_data.stall = last_left_data.stall | last_right_data.stall;

	//set the position
	odom.pose.pose.position.x = 0.0;
	odom.pose.pose.position.y = 0.0;
	odom.pose.pose.position.z = 0.0;
	// Position is NOT calculated at the moment as odometry on wheels
	// that slip is not all that good.  It is better to give a "known
	// wrong" result rather than a subtly-wrong one.  Later versions
	// of this driver may rectify this situation...

	// Calculate an estimate of the current speed and direction

	double last_left_speed = left_motor->getVel();
	double last_right_speed = right_motor->getVel();

	if (last_left_speed >= 0.0) {
		last_left_speed /= wheel_tweak_left_forward;
	} else {
		last_left_speed /= wheel_tweak_left_reverse;
	}

	if (last_right_speed >= 0.0) {
		last_right_speed /= wheel_tweak_right_forward;
	} else {
		last_right_speed /= wheel_tweak_right_reverse;
	}

	double transl_vel = (last_right_speed + last_left_speed)
			/ (2.0 * transl_mult);
	double rotat_vel = (last_right_speed - last_left_speed)
			/ (2.0 * rotat_mult);

	//set the velocity
	odom.child_frame_id = "base_link";
	odom.twist.twist.linear.x = transl_vel;
	odom.twist.twist.linear.y = 0.0;
	odom.twist.twist.angular.z = rotat_vel;

	//publish the message
	odom_pub->publish(odom);

}

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

	// Subscribe to a listener callback.
	ros::Subscriber cmd_vel_sub = node->subscribe<geometry_msgs::Twist>(
			"cmd_vel", 1, &Birabayin::processCmdVel, this);
	ros::Publisher odom_pub = node->advertise<nav_msgs::Odometry>("odom", 50);

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

//	gettimeofday(&lasttime, NULL); // time required for the motor timeout.

	ros::Time current_time;
	current_time = ros::Time::now();
	lastCommandRecieved = current_time - ros::Duration(commandTimeout + 20);
//	last_time = ros::Time::now();

	while (ros::ok()) {

		// Check to see if driver has timed out (ie, has not received
		// any messages during the last (curtime - lasttime) interval)
		left_motor->checkMotorTimeout();
		right_motor->checkMotorTimeout();

		ros::spinOnce(); // listen for new cmd_vel messages.

		current_time = ros::Time::now();
		/*if (motors_enabled && current_time - lastCommandRecieved > ros::Duration(commandTimeout)) {
			ROS_WARN("No commands recieved in %.1lf seconds, stopping motors.\n", (current_time-lastCommandRecieved).toSec());

			req_left_speed = 0.0;
			req_right_speed = 0.0;
			SetMotorVals();
			motors_enabled = false;
			SetMotorVals();
		}*/

		// check for errors and calc odom.
		left_motor->UpdateMotorData();
		right_motor->UpdateMotorData();

		publishOdom(&odom_pub, current_time);

//		last_time = current_time;

		loop_rate.sleep();
	}
}
