#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>


mkt_algorithm::diff::RotateToGoal::RotateToGoal()
  : PredictiveTrajectory()
{
}

mkt_algorithm::diff::RotateToGoal::~RotateToGoal() {}

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;
    nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
    this->getParams();

    last_actuator_update_ = robot::Time::now();
    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::reset()
{
  robot::log_info("[%s:%d]\n RotateToGoal is reset", __FILE__, __LINE__);
  if (initialized_)
  {
    this->clear();
    x_direction_ = y_direction_ = theta_direction_ = 0;
    last_actuator_update_ = robot::Time::now();
  }
}

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;
  }
  if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2)
  {
    robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
    return false;
  }
  if(robot::Time::now() - last_actuator_update_ > robot::Duration(0.5))
  {
    last_actuator_update_ = robot::Time::now();
  }
  this->getParams();
  frame_id_path_ = global_plan.header.frame_id;
  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;

  robot::Time current_time = robot::Time::now();
  const double dt = (current_time - last_actuator_update_).toSec();
  last_actuator_update_ = current_time;
  double vel_yaw = velocity.theta;
  double ang_diff = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta);
  rotateToHeading(ang_diff, velocity, drive_cmd, dt);

  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)