#include <mkt_algorithm/rotate_to_goal.h>
#include <robot_nav_2d_utils/parameters.h>
#include <pluginlib/class_list_macros.h>
#include <angles/angles.h>

namespace  mkt_algorithm
{

void RotateToGoal::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap)
{
  nh_       = robot::NodeHandle(nh, name);
  name_     = name;
  tf_       = tf;
  costmap_robot_ = costmap_robot;

  steering_fix_wheel_distance_x_    = robot_nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1);
  steering_fix_wheel_distance_y_    = robot_nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.);
  nh_.param("velocity_rotate_min", velocity_rotate_min_, 0.1);
  nh_.param("velocity_rotate_max", velocity_rotate_max_, 0.6);
  nh_.param("angle_pass_rotate", angle_threshold_, 0.02);
}

void RotateToGoal::reset()
{
  time_last_cmd_ = robot::Time::now();
}

bool RotateToGoal::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                       double& x_direction, double& y_direction, double& theta_direction) 
{
  goal_ = goal;
  double angle = angles::shortest_angular_distance(pose.theta, goal_.theta);

  if(angle > 0) current_direction_ = 1;
  else if(angle < 0) current_direction_ = -1;
  x_direction = y_direction = theta_direction = current_direction_;
  return  true;
}
            
robot_nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
{
  robot_nav_2d_msgs::Twist2DStamped result;
  result.header.stamp = robot::Time::now();

  robot_geometry_msgs::Pose2D steer_to_baselink_pose;
  
  if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
    return result;
  double actual_phi_steer = steer_to_baselink_pose.theta;

  double L = sqrt(pow(steering_fix_wheel_distance_x_, 2) + pow(steering_fix_wheel_distance_y_, 2));

  // robot::Time t = robot::Time::now();
  // double dt = time_last_cmd_.isZero() ? 0.0 : (t - time_last_cmd_).toSec();
  // time_last_cmd_ = robot::Time::now();

  double angle_rg = angles::shortest_angular_distance(pose.theta, goal_.theta);
  double angle_reduce  = std::min(std::max(fabs(velocity.theta), 0.15), velocity_rotate_max_);
  double w_min = velocity.theta / (fabs(velocity.theta) + 1e-9) * velocity_rotate_min_;
  double w_r =  fabs(angle_rg) > angle_reduce? velocity.theta : (velocity.theta - w_min)*sin((fabs(angle_rg) / angle_reduce) * (M_PI/2)) + w_min; 
  
  int direct_rotate = (angle_rg/fabs(angle_rg));
  double target_phi_steer = direct_rotate * M_PI/2;
  if( fabs(target_phi_steer - actual_phi_steer) <= angle_threshold_)
  {
    double target_v = fabs(w_r * L);
    result.velocity.x = std::min(velocity_rotate_max_ * L, fabs(target_v)); 
  }
  else result.velocity.x = 0.0;
  
  result.velocity.theta = target_phi_steer; 
  return result;
}

}; // namespace  mkt_algorithm

PLUGINLIB_EXPORT_CLASS(mkt_algorithm::RotateToGoal, score_algorithm::ScoreAlgorithm)