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

namespace ros_kinematics
{

  KinematicSteerDriveParameters::KinematicSteerDriveParameters()
    : odom_enc_steering_angle_offset_(0.0),
    steering_fix_wheel_distance_x_(0.0),
    steering_fix_wheel_distance_y_(0.0),
    wheel_acceleration_(0.0),
    setup_(false),
    dsrv_(nullptr)
  {
  }

  KinematicSteerDriveParameters::KinematicSteerDriveParameters(const ros::NodeHandle& nh)
    : odom_enc_steering_angle_offset_(0.0),
    steering_fix_wheel_distance_x_(0.0),
    steering_fix_wheel_distance_y_(0.0),
    wheel_acceleration_(0.0),
    setup_(false),
    dsrv_(nullptr)
  {
    initialize(nh);
  }

  void KinematicSteerDriveParameters::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<SteerDriveParametersConfig> >(nh_);
      dynamic_reconfigure::Server<SteerDriveParametersConfig>::CallbackType cb =
        boost::bind(&KinematicSteerDriveParameters::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, &KinematicSteerDriveParameters::kinematicsCallback, this);
      initialized_ = true;
    }
  }

  void KinematicSteerDriveParameters::reconfigureCB(SteerDriveParametersConfig& config, uint32_t level)
  {
    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
    // ROS_WARN("reconfigureCB");
    odom_enc_steering_angle_offset_ = config.odomEncSteeringAngleOffset;
    steering_fix_wheel_distance_x_ = config.steering_fix_wheel_distance_x;
    steering_fix_wheel_distance_y_ = config.steering_fix_wheel_distance_y;
    wheel_acceleration_ = config.wheelAcceleration;
    // reconfig_mutex.unlock();
  }

  void KinematicSteerDriveParameters::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("odomEncSteeringAngleOffset");
      if (it != param_m.end())
      {
        odom_enc_steering_angle_offset_ = param_m["odomEncSteeringAngleOffset"];
        ROS_INFO("[ros_kinematics]-odomEncSteeringAngleOffset %f", odom_enc_steering_angle_offset_);
      }
      else ROS_WARN("[ros_kinematics]-odomEncSteeringAngleOffset %f", odom_enc_steering_angle_offset_);

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

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

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

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


} // namespace ros_kinematics
