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

namespace  mkt_algorithm
{

void GoStraight::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.);
}

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

bool GoStraight::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;

  const double rb_direction_x  = goal_.x - pose.x;
  const double rb_direction_y  = goal_.y - pose.y;
  const double rb_angle = atan2(rb_direction_y, rb_direction_x);
  double rb_dir = cos(rb_angle- pose.theta);
  x_direction = y_direction = theta_direction = rb_dir;
  
  return  true;
}
            
robot_nav_2d_msgs::Twist2DStamped GoStraight::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 odom_alpha = steer_to_baselink_pose.theta;
  if(fabs(odom_alpha) > 0.03) return result;

  double delta_x = goal_.x - pose.x;
  double delta_y = goal_.y - pose.y;

  double d_rg = sqrt(pow(delta_x, 2) + pow(delta_y, 2)); 
  double d_reduce  = std::min(std::max(fabs(velocity.x), 0.6), 0.9);  
  double v_min = 0.03 * velocity.x / (fabs(velocity.x) + 1e-9); 

// Van toc robot
  double vr =  d_rg > d_reduce?  velocity.x : (velocity.x - v_min)*sin((d_rg / d_reduce) * (M_PI/2)) + v_min; 
  result.velocity.x = fabs(cos(fabs(0.0 - odom_alpha))) * vr / cos(std::min(M_PI/3,  fabs(odom_alpha)));
  // ROS_INFO("d_rg %f d_reduce %f V = %f %f", d_rg, d_reduce,velocity.x, vr);
  return result;
}

}; // namespace  mkt_algorithm

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