#ifndef __ROS_KINEMATICS_DIFF_DRIVE_PARAMETERS_H_INCLUDED_
#define __ROS_KINEMATICS_DIFF_DRIVE_PARAMETERS_H_INCLUDED_

#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <ros_kinematics/DiffDriveParametersConfig.h>
#include <memory>
#include <boost/thread.hpp>
#include <dynamic_reconfigure/Config.h>

namespace ros_kinematics
{
/**
 * @class KinematicDiffDriveParameters
 * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics
 */
class KinematicDiffDriveParameters
{
public:
  KinematicDiffDriveParameters();
  KinematicDiffDriveParameters(const ros::NodeHandle& nh);
  void initialize(const ros::NodeHandle& nh);

  inline double getLeftWheelRadiusMultiplier()    { return left_wheel_radius_multiplier_; }
  inline double getRightWheelRadiusMultiplier()   { return right_wheel_radius_multiplier_; }
  inline double getWheelSeparationMultiplier()    { return wheel_separation_multiplier_; }
  inline double getPublishRate()                  { return publish_rate_; }
  inline bool   getEnableOdomTf()                 { return enable_odom_tf_; }
  inline double isSetup() {return setup_;}
  using Ptr = std::shared_ptr<KinematicDiffDriveParameters>;
protected:
  void reconfigureCB(DiffDriveParametersConfig &config, uint32_t level);
  void kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param);
  
  bool initialized_;
  bool setup_;
  // For parameter descriptions, see cfg/KinematicParams.cfg
  double left_wheel_radius_multiplier_;
  double right_wheel_radius_multiplier_;
  double wheel_separation_multiplier_;
  double publish_rate_;
  bool   enable_odom_tf_;

  boost::mutex reconfig_mutex;
  ros::NodeHandle nh_;
  ros::Subscriber kinematics_sub_;
  std::shared_ptr<dynamic_reconfigure::Server<DiffDriveParametersConfig> > dsrv_;
};

}  // namespace ros_kinematics
#endif