#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <ignition/math/Vector3.hh>
#include <thread>
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <geometry_msgs/Twist.h>

namespace gazebo
{
  class VtolForce : public ModelPlugin
  {
    public: void Load(physics::ModelPtr parent, sdf::ElementPtr _sdf)
    {
      //Store pointer pointing to the model
      this->model=parent;

      //store the name of propeller in the variable
      this->prop_name=_sdf->GetElement("prop_name")->Get<std::string>();

      //Listen to the pdate even this even is broadcast every simulation iteration
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
        std::bind(&VtolForce::OnUpdate, this));

      //create a test topic for cmd_vel
      std::string test_topic="/cmd_vel";

      // Initialize ros, if it has not already bee initialized.
      if (!ros::isInitialized())
      {
        int argc = 0;
        char **argv = NULL;
        ros::init(argc, argv, "vtol_force_rosnode",
            ros::init_options::NoSigintHandler);
      }

      //Create the ROS node. This acts in similiar manner to
      //Gazebo node
      this->rosNode.reset(new ros::NodeHandle("vtol_force_rosnode"));

      ros::SubscribeOptions options=
        ros::SubscribeOptions::create<geometry_msgs::Twist>(
          test_topic,
          1,
          boost::bind(&VtolForce::OnRosMsg, this, _1),
          ros::VoidPtr(), &this->rosQueue);
      this->rosSub=this->rosNode->subscribe(options);

      // Spin up the queue helper thread.
      this->rosQueueThread =
        std::thread(std::bind(&VtolForce::QueueThread, this));

      ROS_WARN("Loaded VtolForce Plugin with parent...%s", this->model->GetName().c_str());
    }

    public: void OnUpdate()
    {
      // ROS_WARN("%s", this->prop_name.c_str());
      ROS_WARN("%d", this->linearx);
      this->linearx=0;
    }

    public: void OnRosMsg(const geometry_msgs::Twist::ConstPtr &_msg)
    {
      ROS_WARN("OnRosMsg");
      linearx=_msg->linear.x;
    }

    private: void QueueThread()
    {
      static const double timeout = 0.01;
      while (this->rosNode->ok())
      {
        this->rosQueue.callAvailable(ros::WallDuration(timeout));
      }
    }

    // Pointer to the model
    private: physics::ModelPtr model;

    //variable to store link name of propeller
    private: std::string prop_name;

    //variable to store value in twist message
    private: int linearx=0;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    private: std::unique_ptr<ros::NodeHandle> rosNode;

    /// \brief A ROS subscriber
    private: ros::Subscriber rosSub;
    /// \brief A ROS callbackqueue that helps process messages
    private: ros::CallbackQueue rosQueue;
    /// \brief A thread the keeps running the rosQueue
    private: std::thread rosQueueThread;
  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(VtolForce)
}