#include <mkt_plugins/kinematic_parameters.h>
#include <robot_nav_2d_utils/parameters.h>
#include <memory>
#include <string>
#include <cmath>


using robot_nav_2d_utils::moveDeprecatedParameter;

namespace mkt_plugins
{

  const double EPSILON = 1E-5;

  /**
   * @brief Helper function to set the deceleration to the negative acceleration if it was not already set.
   * @param nh NodeHandle
   * @param dimension String representing the dimension, used in constructing parameter names
   */
  void setDecelerationAsNeeded(const robot::NodeHandle &nh, const std::string dimension)
  {
    std::string decel_param = "decel_lim_" + dimension;
    if (nh.hasParam(decel_param))
      return;

    std::string accel_param = "acc_lim_" + dimension;
    if (!nh.hasParam(accel_param))
      return;

    double accel;
    nh.getParam(accel_param, accel);
    nh.setParam(decel_param, -accel);
  }

  KinematicParameters::KinematicParameters()
    : xytheta_direct_(),
      min_vel_x_(0.0),
      min_vel_y_(0.0),
      max_vel_x_(0.0),
      max_vel_y_(0.0),
      max_vel_theta_(0.0),
      min_speed_xy_(0.0),
      max_speed_xy_(0.0),
      min_speed_theta_(0.0),
      acc_lim_x_(0.0),
      acc_lim_y_(0.0),
      acc_lim_theta_(0.0),
      decel_lim_x_(0.0),
      decel_lim_y_(0.0),
      decel_lim_theta_(0.0),
      min_speed_xy_sq_(0.0),
      max_speed_xy_sq_(0.0),
      original_min_vel_x_(0.0),
      original_min_vel_y_(0.0),
      original_max_vel_x_(0.0),
      original_max_vel_y_(0.0),
      original_max_vel_theta_(0.0),
      original_min_speed_xy_(0.0),
      original_max_speed_xy_(0.0),
      original_min_speed_theta_(0.0),
      original_acc_lim_x_(0.0),
      original_acc_lim_y_(0.0),
      original_acc_lim_theta_(0.0),
      original_decel_lim_x_(0.0),
      original_decel_lim_y_(0.0),
      original_decel_lim_theta_(0.0)
  {
  }

  KinematicParameters::~KinematicParameters()
  {
  }

  void KinematicParameters::initialize(const robot::NodeHandle &nh)
  {
    // Special handling for renamed parameters
    moveDeprecatedParameter<double>(nh, "max_vel_theta", "max_rot_vel");
    moveDeprecatedParameter<double>(nh, "min_speed_xy", "min_trans_vel");
    moveDeprecatedParameter<double>(nh, "max_speed_xy", "max_trans_vel");
    moveDeprecatedParameter<double>(nh, "min_speed_theta", "min_rot_vel");

    // Set the deceleration parameters to negative the acceleration if the deceleration not already specified
    setDecelerationAsNeeded(nh, "x");
    setDecelerationAsNeeded(nh, "y");
    setDecelerationAsNeeded(nh, "theta");

    configure(nh);
    xytheta_direct_[0] = xytheta_direct_[1] = xytheta_direct_[2] = 1;
  }


  void KinematicParameters::configure(const robot::NodeHandle &nh)
  {
    nh.param("max_vel_x", original_max_vel_x_, 0.0);
    nh.param("max_vel_y", original_max_vel_y_, 0.0);
    nh.param("max_vel_theta", original_max_vel_theta_, 0.0);
    nh.param("min_vel_x", original_min_vel_x_, 0.0);
    nh.param("min_vel_y", original_min_vel_y_, 0.0);
    nh.param("min_speed_xy", original_min_speed_xy_, 0.0);
    nh.param("max_speed_xy", original_max_speed_xy_, 0.0);
    nh.param("min_speed_theta", original_min_speed_theta_, 0.0);
    nh.param("acc_lim_x", original_acc_lim_x_, 0.0);
    nh.param("acc_lim_y", original_acc_lim_y_, 0.0);
    nh.param("acc_lim_theta", original_acc_lim_theta_, 0.0);
    nh.param("decel_lim_x", original_decel_lim_x_, 0.0);
    nh.param("decel_lim_y", original_decel_lim_y_, 0.0);
    nh.param("decel_lim_theta", original_decel_lim_theta_, 0.0);
    reconfigure(nh);
  }

  void KinematicParameters::reconfigure(const robot::NodeHandle &nh)
  {
    nh.param("max_vel_x", max_vel_x_, 0.0);
    nh.param("max_vel_y", max_vel_y_, 0.0);
    nh.param("max_vel_theta", max_vel_theta_, 0.0);
    nh.param("min_vel_x", min_vel_x_, 0.0);
    nh.param("min_vel_y", min_vel_y_, 0.0);
    nh.param("min_speed_xy", min_speed_xy_, 0.0);
    nh.param("max_speed_xy", max_speed_xy_, 0.0);
    nh.param("min_speed_theta", min_speed_theta_, 0.0);
    nh.param("acc_lim_x", acc_lim_x_, 0.0);
    nh.param("acc_lim_y", acc_lim_y_, 0.0);
    nh.param("acc_lim_theta", acc_lim_theta_, 0.0);
    nh.param("decel_lim_x", decel_lim_x_, 0.0);
    nh.param("decel_lim_y", decel_lim_y_, 0.0);
    nh.param("decel_lim_theta", decel_lim_theta_, 0.0);
  }

  void KinematicParameters::setDirect(int *xytheta_direct)
  {
    xytheta_direct_[0] = xytheta_direct[0];
    xytheta_direct_[1] = xytheta_direct[1];
    xytheta_direct_[2] = xytheta_direct[2];
  }

  bool KinematicParameters::isValidSpeed(double x, double y, double theta)
  {
    double vmag_sq = x * x + y * y;
    if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON)
      return false;
    if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ &&
        min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_)
      return false;
    if (vmag_sq == 0.0 && theta == 0.0)
      return false;
    return true;
  }

  int *KinematicParameters::getDirect() { return xytheta_direct_; }

  double KinematicParameters::getMinX() { return min_vel_x_; }

  double KinematicParameters::getMaxX() { return max_vel_x_; }

  double KinematicParameters::getAccX() { return acc_lim_x_; }

  double KinematicParameters::getDecelX() { return decel_lim_x_; }

  double KinematicParameters::getMinY() { return min_vel_y_; }

  double KinematicParameters::getMaxY() { return max_vel_y_; }

  double KinematicParameters::getAccY() { return acc_lim_y_; }

  double KinematicParameters::getDecelY() { return decel_lim_y_; }

  double KinematicParameters::getMinSpeedXY() { return min_speed_xy_; }

  double KinematicParameters::getMinTheta() { return -max_vel_theta_; }

  double KinematicParameters::getMaxTheta() { return max_vel_theta_; }

  double KinematicParameters::getAccTheta() { return acc_lim_theta_; }

  double KinematicParameters::getDecelTheta() { return decel_lim_theta_; }

  double KinematicParameters::getMinSpeedTheta() { return min_speed_theta_; }

  void KinematicParameters::setMinX(double value)
  {
    min_vel_x_ = fabs(value) < fabs(original_min_vel_x_) + EPSILON ? value : min_vel_x_;
  }

  void KinematicParameters::setMaxX(double value)
  {
    max_vel_x_ = fabs(value) <= fabs(original_max_vel_x_) + EPSILON ? value : original_max_vel_x_;
  }

  void KinematicParameters::setAccX(double value)
  {
    acc_lim_x_ = fabs(value) <= fabs(original_acc_lim_x_) + EPSILON ? value : acc_lim_x_;
  }

  void KinematicParameters::setDecelX(double value)
  {
    decel_lim_x_ = fabs(value) <= fabs(original_decel_lim_x_) + EPSILON ? value : decel_lim_x_;
  }

  void KinematicParameters::setMinY(double value)
  {
    min_vel_y_ = fabs(value) <= fabs(original_min_vel_y_) + EPSILON ? value : min_vel_y_;
  }

  void KinematicParameters::setMaxY(double value)
  {
    max_vel_y_ = fabs(value) <= fabs(original_max_vel_y_) + EPSILON ? value : max_vel_y_;
  }

  void KinematicParameters::setAccY(double value)
  {
    acc_lim_y_ = fabs(value) <= fabs(original_acc_lim_y_) + EPSILON ? value : acc_lim_y_;
  }

  void KinematicParameters::setDecelY(double value)
  {
    decel_lim_y_ = fabs(value) <= fabs(original_decel_lim_y_) + EPSILON ? value : decel_lim_y_;
  }

  void KinematicParameters::setMinSpeedXY(double value)
  {
    min_speed_xy_ = fabs(value) <= fabs(original_min_speed_xy_) + EPSILON ? value : min_speed_xy_;
  }

  void KinematicParameters::setMaxTheta(double value)
  {
    max_vel_theta_  = fabs(value) <= fabs(original_max_vel_theta_) + EPSILON? value : original_max_vel_theta_;
  }

  void KinematicParameters::setAccTheta(double value)
  {
    acc_lim_theta_ = fabs(value) <= fabs(original_acc_lim_theta_) + EPSILON ? value : acc_lim_theta_;
  }

  void KinematicParameters::setDecelTheta(double value)
  {
    decel_lim_theta_ = fabs(value) <= fabs(original_decel_lim_theta_) + EPSILON ? value : decel_lim_theta_;
  }

  void KinematicParameters::setMinSpeedTheta(double value) 
  { 
    min_speed_theta_ = fabs(value) <= fabs(original_min_speed_theta_) + EPSILON ? value : min_speed_theta_;
  }
} // namespace mkt_plugins
