#include <ros_kinematics/ros_diff_drive_parameters.h>
#include <memory>
#include <string>
#include <map>

namespace ros_kinematics
{

  KinematicDiffDriveParameters::KinematicDiffDriveParameters()
    : left_wheel_radius_multiplier_(0.0),
    right_wheel_radius_multiplier_(0.0),
    wheel_separation_multiplier_(0.0),
    publish_rate_(0.0),
    enable_odom_tf_(false),
    setup_(false),
    dsrv_(nullptr)
  {
  }

  KinematicDiffDriveParameters::KinematicDiffDriveParameters(const ros::NodeHandle& nh)
    : left_wheel_radius_multiplier_(0.0),
    right_wheel_radius_multiplier_(0.0),
    wheel_separation_multiplier_(0.0),
    publish_rate_(0.0),
    enable_odom_tf_(false),
    setup_(false),
    dsrv_(nullptr)
  {
    initialize(nh);
  }

  void KinematicDiffDriveParameters::initialize(const ros::NodeHandle& nh)
  {
    if (!initialized_)
    {
      nh_ = ros::NodeHandle(nh);
      // the rest of the initial values are loaded through the dynamic reconfigure mechanisms
      dsrv_ = std::make_shared<dynamic_reconfigure::Server<DiffDriveParametersConfig> >(nh_);
      dynamic_reconfigure::Server<DiffDriveParametersConfig>::CallbackType cb =
        boost::bind(&KinematicDiffDriveParameters::reconfigureCB, this, _1, _2);
      dsrv_->setCallback(cb);
      int m_subscribe_queue_size = 1000;
      kinematics_sub_ = nh_.subscribe<dynamic_reconfigure::Config>("/agv_dynparam/parameter_updates", m_subscribe_queue_size, &KinematicDiffDriveParameters::kinematicsCallback, this);
      initialized_ = true;
    }
  }

  void KinematicDiffDriveParameters::reconfigureCB(DiffDriveParametersConfig& config, uint32_t level)
  {
    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
    // ROS_WARN("reconfigureCB");
    left_wheel_radius_multiplier_   = config.left_wheel_radius_multiplier;
    right_wheel_radius_multiplier_  = config.right_wheel_radius_multiplier;
    wheel_separation_multiplier_    = config.wheel_separation_multiplier;
    publish_rate_                   = config.publish_rate;
    enable_odom_tf_                 = config.enable_odom_tf;
    // reconfig_mutex.unlock();
  }

  void KinematicDiffDriveParameters::kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param)
  {
    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
    // ROS_WARN("dynamic_reconfigure");
    if (param != nullptr)
    {
      std::map<std::string, double> param_m;
      for (auto db : param->doubles) param_m[db.name] = db.value;

      auto it = param_m.find("left_wheel_radius_multiplier");
      if (it != param_m.end())
      {
        left_wheel_radius_multiplier_ = param_m["left_wheel_radius_multiplier"];
        ROS_INFO("[ros_kinematics]-left_wheel_radius_multiplier %f", left_wheel_radius_multiplier_);
      }
      else ROS_WARN("[ros_kinematics]-left_wheel_radius_multiplier %f", left_wheel_radius_multiplier_);

      it = param_m.find("right_wheel_radius_multiplier");
      if (it != param_m.end())
      {
        right_wheel_radius_multiplier_ = param_m["right_wheel_radius_multiplier"];
        ROS_INFO("[ros_kinematics]-right_wheel_radius_multiplier %f", right_wheel_radius_multiplier_);
      }
      else ROS_WARN("[ros_kinematics]-right_wheel_radius_multiplier %f", right_wheel_radius_multiplier_);

      it = param_m.find("wheel_separation_multiplier");
      if (it != param_m.end())
      {
        wheel_separation_multiplier_ = param_m["wheel_separation_multiplier"];
        ROS_INFO("[ros_kinematics]-wheel_separation_multiplier %f", wheel_separation_multiplier_);
      }
      else ROS_WARN("[ros_kinematics]-wheel_separation_multiplier %f", wheel_separation_multiplier_);

      it = param_m.find("publish_rate");
      if (it != param_m.end())
      {
        publish_rate_ = param_m["publish_rate"];
        ROS_INFO("[ros_kinematics]-publish_rate %f", publish_rate_);
      }
      else ROS_WARN("[ros_kinematics]-publish_rate %f", publish_rate_);

      // it = param_m.find("enable_odom_tf");
      // if (it != param_m.end())
      // {
      //   enable_odom_tf_ = param_m["enable_odom_tf"];
      //   ROS_INFO("[ros_kinematics]-enable_odom_tf %x", enable_odom_tf_);
      // }
      // else ROS_WARN("[ros_kinematics]-enable_odom_tf %x", enable_odom_tf_);

      param_m.clear();
      setup_ = true;
    }
    else ROS_WARN("dynamic_reconfigure param is null");
    // reconfig_mutex.unlock();
  }


} // namespace ros_kinematics
