#ifndef _ALFRED_PLUGIN_HH
#define _ALFRED_PLUGIN_HH

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <string>
#include <thread>
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/subscribe_options.h>
#include <geometry_msgs/Twist.h>

namespace gazebo
{
	class AlfredPlugin : public ModelPlugin
	{
		public: AlfredPlugin() {}

		public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
		{
			if(_model->GetJointCount()==0)
			{
				std::cerr<<"\nInvalid joint count, plugin not loaded.\n";
				return;
			}
			this->model = _model;
			this->leftJoint = this->model->GetJoint("left_wheel_joint");
			this->rightJoint = this->model->GetJoint("right_wheel_joint");
			this->pid = common::PID(10,2,0);
			this->model->GetJointController()->SetVelocityPID(this->leftJoint->GetScopedName(),this->pid);
			this->model->GetJointController()->SetVelocityPID(this->rightJoint->GetScopedName(),this->pid);
			if (!ros::isInitialized())
			{
			  int argc = 0;
			  char **argv = NULL;
			  ros::init(argc, argv, "alfred_simulator", ros::init_options::NoSigintHandler);
			}
			this->rosNode.reset(new ros::NodeHandle("alfred_simulator"));
			ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Twist>("/cmd_vel",1,boost::bind(&AlfredPlugin::cmdVelCallback, this, _1),ros::VoidPtr(), &this->rosQueue);
			this->cmdVelSub = this->rosNode->subscribe(so);
			this->rosQueueThread = std::thread(std::bind(&AlfredPlugin::QueueThread, this));
		}

		public: void cmdVelCallback(const geometry_msgs::TwistConstPtr &_msg)
		{
			double leftVel = _msg->linear.x - _msg->angular.z*0.5*0.34;
			double rightVel = _msg->linear.x + _msg->angular.z*0.5*0.34;
			this->model->GetJointController()->SetVelocityTarget(this->leftJoint->GetScopedName(), leftVel/0.05);
			this->model->GetJointController()->SetVelocityTarget(this->rightJoint->GetScopedName(), rightVel/0.05);
		}

		private: void QueueThread()
		{
		  static const double timeout = 0.01;
		  while (this->rosNode->ok())
		  {
		    this->rosQueue.callAvailable(ros::WallDuration(timeout));
		  }
		}
		private:
			physics::ModelPtr model;
			physics::JointPtr leftJoint,rightJoint;
			common::PID pid;
			std::unique_ptr<ros::NodeHandle> rosNode;
			ros::Subscriber cmdVelSub;
			ros::CallbackQueue rosQueue;
			std::thread rosQueueThread;

	};
	GZ_REGISTER_MODEL_PLUGIN(AlfredPlugin)
}
#endif
