#ifndef __ROS_DIFF_WHEEL_PLUGIN_H_INCLUDED_
#define __ROS_DIFF_WHEEL_PLUGIN_H_INCLUDED_

#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include "diff_wheel_plugin/command.h"
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>
// Boost
#include <boost/thread.hpp>
#include <boost/make_shared.hpp>
//plugin
#include <models/BaseSteerDrive.h>


namespace diff_wheel_plugin
{
/**
 * @class WheelPlugin
 */
class WheelPlugin : public models::BaseSteerDrive
{
public:

  /**
   * @brief Contructor
  */
  WheelPlugin();

  /**
   * @brief Contructor
   * @param[in] nh    NodeHandle 
   * @param[in] name  The name to give this instance of the model base
  */
  WheelPlugin(ros::NodeHandle & nh, const std::string &name);


  /**
   * @brief Decontructor 
  */
  virtual ~WheelPlugin();

  /**
   * @brief  Init 
   * @param[in] nh  NodeHandle 
   * @param name    The name to give this instance of the model base
  */
  virtual void initialize(ros::NodeHandle & nh, const std::string &name) override;

  /**
   * @brief  Constructs 
   * @param name Check model is deady
   * @return true, if model is ready, false otherwise
  */
  virtual bool Ready() override;

  /**
   * @brief Get the velocity limit on axis(index).
   * @return Velocity limit specified in SDF
  */ 
  virtual double GetVelocityLimit(unsigned int _index) override;

  /**
   * @brief Set the velocity limit on a joint axis.
   * @param[in] _velocity Velocity limit for the axis.
  */ 
  virtual void SetVelocityLimit(double _velocity) override;

  /**
   * @brief Get the rotation rate of an axis(index)
   * @return The rotaional velocity of the joint axis. (rad/s)
  */ 
  virtual double GetVelocity() override;

  /**
   * @brief Set the velocity of an axis(index).
   * In ODE and Bullet, the SetVelocityMaximal function is used to
   * set the velocity of the child link relative to the parent.
   * In Simbody and DART, this function updates the velocity state,
   * which has a recursive effect on the rest of the chain.
   * @param[in] _vel Velocity.
  */
  virtual void SetVelocity(double _vel) override;

  /**
   * @brief Get the position of an axis according to its index.
   *
   * For rotational axes, the value is in radians. For prismatic axes,
   * it is in meters.
   *
   * For static models, it returns the static joint position.
   *
   * It returns ignition::math::NAN_D in case the position can't be
   * obtained. For instance, if the index is invalid, if the joint is
   * fixed, etc.
   *
   * Subclasses can't override this method. See PositionImpl instead.
   *
   * @return Current position of the axis.
   * @sa PositionImpl
  */ 
  virtual double Position() override;

  /**
   * @brief The child links of this joint are updated based on desired
   * position.  And all the links connected to the child link of this joint
   * except through the parent link of this joint moves with the child
   * link.
   * @param[in] _position Position to set the joint to.
   * unspecified, pure kinematic teleportation.
   * link with respect to the world frame will remain the same after
   * setting the position. By default this is false, which means there are
   * no guarantees about what the child link's world velocity will be after
   * the position is changed (the behavior is determined by the underlying
   * physics engine).
   *
   * @note{Only ODE and Bullet support _preserveWorldVelocity being true.}
  */ 
  virtual void SetPosition(const double _position) override;

private:

  /**
   * @brief Brake by away SetVelocity(0.0)
   */
  void brake() { this->SetVelocity(0.0); }

  /**
   * @brief call back from a subscriber  
  */
  void DLS_Callback(const std_msgs::Float32::ConstPtr & msg);

  /**
   * @brief Do stuff when create boost::Thread
  */
  void DoStuff(void);

  ros::NodeHandle nh_;
  ros::NodeHandle nh_priv_;
  std::string name_;
  bool initialized_;
  bool is_ready_;
  int m_subscribe_queue_size_;
  double update_rate_;
  boost::thread * m_publish_thread_;
  boost::mutex callback_mutex_;
  std_msgs::Float32::ConstPtr m_measurement_;

  bool allow_multiple_command_publishers_;
  realtime_tools::RealtimeBuffer<diff_wheel_plugin::WheelCommand> command_;
  diff_wheel_plugin::WheelCommand command_struct_;
  double command_timeout_;
  ros::Subscriber mesurement_sub_;
  boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float32>> wheel_pub_;
};
  
} // namespace diff_wheel_plugin

#endif // __ROS_DIFF_WHEEL_PLUGIN_H_INCLUDED_