#ifndef __ROS_KINEMATICS_STEER_DRIVE_PARAMETERS_H_INCLUDED_
#define __ROS_KINEMATICS_STEER_DRIVE_PARAMETERS_H_INCLUDED_

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

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

  inline double getOdomEncSteeringAngleOffset() { return odom_enc_steering_angle_offset_; }
  inline double getSteeringFixWheelDistanceX()  { return steering_fix_wheel_distance_x_; }
  inline double getSteeringFixWheelDistanceY()  { return steering_fix_wheel_distance_y_; }
  inline double getWheelAcceleration()          { return wheel_acceleration_; }
  inline double isSetup() {return setup_;}
  using Ptr = std::shared_ptr<KinematicSteerDriveParameters>;
protected:
  void reconfigureCB(SteerDriveParametersConfig &config, uint32_t level);
  void kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param);
  
  bool initialized_;
  bool setup_;
  // For parameter descriptions, see cfg/KinematicParams.cfg
  double odom_enc_steering_angle_offset_;
  double steering_fix_wheel_distance_x_;
  double steering_fix_wheel_distance_y_;
  double wheel_acceleration_;
  boost::mutex reconfig_mutex;
  ros::NodeHandle nh_;
  ros::Subscriber kinematics_sub_;
  std::shared_ptr<dynamic_reconfigure::Server<SteerDriveParametersConfig> > dsrv_;
};

}  // namespace ros_kinematics
#endif