#ifndef GAZEBO_JACKING_LIFTER_PLUGIN_H_INCLUDED_
#define GAZEBO_JACKING_LIFTER_PLUGIN_H_INCLUDED_

#include <map>
// Gazebo
#include <gazebo_plugins/gazebo_ros_utils.h>

// ROS
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h>

// Custom Callback Queue
#include <ros/callback_queue.h>
#include <ros/advertise_options.h>

// Boost
#include <boost/thread.hpp>
#include <boost/bind.hpp>

namespace gazebo 
{
  // class Joint;
  // class Entity;


  class RosLiftLifter : public ModelPlugin
  {
  public:
    RosLiftLifter();
    ~RosLiftLifter();
    void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);

  protected:
    virtual void UpdateChild();
    virtual void FiniChild();

  private:
    GazeboRosPtr gazebo_ros_;
    physics::ModelPtr parent;
    //void publishOdometry(double step_time);
    void publishLiftSensorStates();
    void publishTF(physics::JointPtr joint); /// publishes the wheel tf's
    void publishJointState(physics::JointPtr joint);
    void effortController(physics::JointPtr joint, const double &error, const double &dt);
    void positionController(physics::JointPtr joint, const double &target, double &current , const double &dt);

    event::ConnectionPtr update_connection_;
    physics::JointPtr base_lifting_joint_, base_rotating_joint_;
    double lift_torque_, rotate_torque_;

    std::string robot_namespace_;
    std::string command_topic_;
    std::string robot_base_frame_;

    // ROS STUFF
    ros::Subscriber cmd_lift_subscriber_;
    boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
    ros::Publisher joint_state_publisher_;
    ros::Publisher state_lift_publisher_;

    boost::mutex lock;

    // Custom Callback Queue
    ros::CallbackQueue queue_;
    boost::thread callback_queue_thread_;
    void QueueThread();

    // DiffDrive stuff
    void cmdCallback(const geometry_msgs::Pose::ConstPtr& cmd_msg);
    /// updates the relative robot pose based on the wheel encoders
    void UpdateForkEncoder();

    double lift_height_cmd_, current_lift_position_, lift_step_;
    double lift_rotate_cmd_, current_lift_angle_, rotate_step_;
    double lift_height_maximum_;

    bool alive_;
    double lift_height_encoder_;
    common::Time last_encoder_update_;
    double lift_height_origin_;
    common::PID  joint_lift_pid_, joint_rotate_pid_;

    // Update Rate
    double update_rate_;
    double update_period_;
    common::Time last_actuator_update_;

    // Flags
    bool publishTF_;
    bool publishJointState_;

    // bool use_velocity_control_;
    // double max_velocity_;
  };
}

#endif

