#include <robot/robot.h>
#include <mkt_algorithm/diff/diff_rotate_to_goal.h>
#include <robot_nav_2d_utils/parameters.h>
#include <boost/dll/alias.hpp>
#include <angles/angles.h>

void mkt_algorithm::diff::RotateToGoal::initialize(
    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
{
  if (!initialized_)
  {
    nh_ = robot::NodeHandle(nh, name);
    name_ = name;
    tf_ = tf;
    traj_ = traj;
    costmap_robot_ = costmap_robot;
    this->getParams();

    x_direction_ = y_direction_ = theta_direction_ = 0;
    this->initialized_ = true;
    robot::log_info("[%s:%d]\n RotateToGoal Initialized successfully", __FILE__, __LINE__);
  }
}

void mkt_algorithm::diff::RotateToGoal::getParams()
{
  robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
  nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
  nh_priv_.param<double>("angle_threshold", angle_threshold_, M_PI / 8);
  nh_priv_.param<int>("index_samples", index_samples_, 0);
  nh_priv_.param<bool>("follow_step_path", follow_step_path_, false);

  nh_priv_.param<bool>("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false);
  nh_priv_.param<double>("lookahead_dist", lookahead_dist_, 0.25);
  nh_priv_.param<double>("min_lookahead_dist", min_lookahead_dist_, 0.3);
  nh_priv_.param<double>("max_lookahead_dist", max_lookahead_dist_, 0.9);
  nh_priv_.param<double>("lookahead_time", lookahead_time_, 1.5);
  nh_priv_.param<double>("min_journey_squared", min_journey_squared_, std::numeric_limits<double>::infinity());
  nh_priv_.param<double>("max_journey_squared", max_journey_squared_, std::numeric_limits<double>::infinity());

  nh_priv_.param<bool>("use_rotate_to_heading", use_rotate_to_heading_, false);
  nh_priv_.param<double>("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785);

  nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.0);
  nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.0);
  nh_priv_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);

  // Regulated linear velocity scaling
  nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
  nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
  nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);

  // Inflation cost scaling (Limit velocity by proximity to obstacles)
  nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
  nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
  nh_priv_.param<double>("cost_scaling_dist", cost_scaling_dist_, 0.6);
  nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
  if (inflation_cost_scaling_factor_ <= 0.0)
  {
    robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
    use_cost_regulated_linear_velocity_scaling_ = false;
  }
  double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
  control_duration_ = 1.0 / control_frequency;
  if (traj_)
  {
    traj_.get()->getNodeHandle().param<double>("max_vel_x", max_vel_x_, 0.0);
    traj_.get()->getNodeHandle().param<double>("min_vel_x", min_vel_x_, 0.0);
    traj_.get()->getNodeHandle().param<double>("acc_lim_x", acc_lim_x_, 0.0);
    traj_.get()->getNodeHandle().param<double>("decel_lim_x", decel_lim_x_, 0.0);

    traj_.get()->getNodeHandle().param<double>("max_vel_y", max_vel_y_, 0.0);
    traj_.get()->getNodeHandle().param<double>("min_vel_y", min_vel_y_, 0.0);
    traj_.get()->getNodeHandle().param<double>("acc_lim_y", acc_lim_y_, 0.0);
    traj_.get()->getNodeHandle().param<double>("decel_lim_y", decel_lim_y_, 0.0);

    traj_.get()->getNodeHandle().param<double>("max_vel_theta", max_vel_theta_, 0.0);
    traj_.get()->getNodeHandle().param<double>("min_vel_theta", min_vel_theta_, 0.0);
    traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
    traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
  }
}

void mkt_algorithm::diff::RotateToGoal::reset()
{
  this->clear();
}

bool mkt_algorithm::diff::RotateToGoal::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity,
                                                const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan,
                                                double &x_direction, double &y_direction, double &theta_direction)
{

  if (!initialized_)
  {
    robot::log_error("This planner has not been initialized, please call initialize() before using this planner");
    return false;
  }
  this->getParams();
  global_plan_ = global_plan;

  goal_ = goal;
  double angle = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta);
  x_direction = y_direction = theta_direction = sign(angle);
  ;
  return true;
}

mkt_msgs::Trajectory2D mkt_algorithm::diff::RotateToGoal::calculator(
    const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{
  mkt_msgs::Trajectory2D result;
  robot_nav_2d_msgs::Twist2D drive_cmd;
  result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
  if (!traj_)
    return result;

  const double dt = control_duration_;
  double vel_yaw = velocity.theta;
  double ang_diff = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta);
  rotateToHeading(ang_diff, velocity, drive_cmd);
  drive_cmd.x = 0.0;
  drive_cmd.y = 0.0;
  result.velocity = drive_cmd;
  return result;
}

score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::RotateToGoal::create()
{
  return std::make_shared<mkt_algorithm::diff::RotateToGoal>();
}

BOOST_DLL_ALIAS(mkt_algorithm::diff::RotateToGoal::create, MKTAlgorithmDiffRotateToGoal)