#include <mkt_algorithm/bicycle.h>
#include <pluginlib/class_list_macros.h>
#include <nav_grid/coordinate_conversion.h>
#include <robot_nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <angles/angles.h>


namespace  mkt_algorithm
{

void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap)
{
  nh_priv_  = robot::NodeHandle(nh, name);
  nh_       = robot::NodeHandle("~");
  name_     = name;
  tf_       = tf;
  costmap_robot_ = costmap_robot;
  last_actuator_update_ = robot::Time(0);

  nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
  nh_priv_.param("xy_local_goal_tolerance_offset", xy_local_goal_tolerance_offset_, 0.5);
  nh_priv_.param("angle_threshold", angle_threshold_, M_PI/8);
  nh_priv_.param("index_samples", index_samples_, 0);
  nh_priv_.param("filter", is_filter_, false);

  steering_fix_wheel_distance_x_    = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_x", 1.1);
  steering_fix_wheel_distance_y_    = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_y", 0.);
  xy_goal_tolerance_                = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "xy_goal_tolerance", 0.1);
  acc_lim_theta_                    = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "acc_lim_theta", 0.1);
  decel_lim_theta_                  = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "decel_lim_theta", 0.1);
  controller_frequency_param_name_  = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
  if(controller_frequency_param_name_ <= 0.0) 
  {
    ROS_WARN("controller_frequency is not setting lower 0.0 so will set to 1.0");
    controller_frequency_param_name_ = 1.0;
  }

  reached_intermediate_goals_pub_   = nh_.advertise<robot_geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
  current_goal_pub_                 = nh_.advertise<robot_geometry_msgs::PoseStamped>("current_goal", 1);
  closet_robot_goal_pub_            = nh_.advertise<robot_geometry_msgs::PoseStamped>("closet_robot_goal", 1);
  cmd_raw_pub_                      = nh_.advertise<robot_geometry_msgs::Twist>("cmd_raw", 1);
  x_direction_ = y_direction_= theta_direction_ = 0;

// kalman
  int n = 3; // Number of states
  int m = 1; // Number of measurements
  double dt = 1.0/controller_frequency_param_name_; // Time step
  // ROS_INFO("controller_frequency_param_name %f", controller_frequency_param_name);
  Eigen::MatrixXd A(n, n); // System dynamics matrix
  Eigen::MatrixXd C(m, n); // Output matrix
  Eigen::MatrixXd Q(n, n); // Process noise covariance
  Eigen::MatrixXd R(m, m); // Measurement noise covariance
  Eigen::MatrixXd P(n, n); // Estimate error covariance

  // Discrete LTI projectile motion, measuring position only
  A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
  C << 1, 0, 0;

  // Reasonable covariance matrices
  double q = 0.1;
  double r = 0.6;
  double p = 0.2;
  Q << q, q, 0.0, q, q, 0.0, 0.0, 0.0, 0.0;
  R << r;
  P << p, p, p, p, 10.0, 1, p, 1, 10.0;

  // Construct the filter
  kf_ = boost::make_shared<KalmanFilter>(dt,A, C, Q, R, P); 
  // Best guess of initial states
  Eigen::VectorXd x0(n);
  x0 << 0.0, 0.0, 0.0;
  kf_->init(robot::Time::now().toSec(), x0);
}

void Bicycle::reset()
{
  reached_intermediate_goals_.clear();
  start_index_saved_vt_.clear();
  start_index_saved_ = 0;
  sub_goal_index_saved_ = 0;
  sub_start_index_saved_ = 0;
  x_direction_ = y_direction_= theta_direction_ = 0;
  follow_step_path_ = false;
  last_actuator_update_ = robot::Time(0);
}

bool Bicycle::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) 
{
  nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
  nh_priv_.param("xy_local_goal_tolerance_offset", xy_local_goal_tolerance_offset_, 0.5);
  nh_priv_.param("angle_threshold", angle_threshold_, M_PI/8);
  nh_priv_.param("index_samples", index_samples_, 0);
  nh_priv_.param("distance_rg", distance_rg_, 0.1);
  nh_priv_.param("k_ld", k_ld_, 1.0);
  nh_priv_.param("k_d_reduce", k_d_reduce_, 1.0);
  nh_priv_.param("ld_min", ld_min_, 0.2);
  nh_priv_.param("ld_max", ld_max_, 1.1);
  nh_priv_.param("follow_step_path", follow_step_path_, false);

  // ROS_INFO("%f %f %f %d", xy_local_goal_tolerance_, xy_local_goal_tolerance_offset_, angle_threshold_, index_samples_);
  frame_id_path_ = global_plan.header.frame_id;
  goal_ = goal;
  global_plan_ = global_plan;
  double rb_direction;
  if(!getGoalPose(pose, velocity,global_plan, sub_goal_, rb_direction))
  {
    ROS_ERROR("getGoalPose Failed");
    return false;
  }
  x_direction_= fabs(rb_direction)/rb_direction;
  x_direction = x_direction_;
  y_direction = y_direction_ = 0;
  theta_direction = theta_direction_;
  return true;
}
            
robot_nav_2d_msgs::Twist2DStamped Bicycle::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;

  const double L = sqrt(pow(steering_fix_wheel_distance_x_, 2) + pow(steering_fix_wheel_distance_y_, 2));
  const double delta_x = sub_goal_.x - pose.x;
  const double delta_y = sub_goal_.y - pose.y;
  const double d_rg = sqrt(pow(delta_x, 2) + pow(delta_y, 2)); 

// Van toc robot
  double d_reduce  = std::min(std::max(fabs(velocity.x) * k_d_reduce_, 0.6), 3.0);  
  double v_min = 0.05 * velocity.x / (fabs(velocity.x) + 1e-9); 
  double vr =  d_rg > d_reduce?  velocity.x : (velocity.x - v_min)*sin((d_rg / d_reduce) * (M_PI/2)) + v_min; 

// Góc lái của bánh lái là: α(t)
  double alpha =  atan2(delta_y,delta_x) - pose.theta;
  double ld = std::min(std::max(fabs(velocity.x)*k_ld_, ld_min_), ld_max_);
  // double ld = std::min(std::max(d_rg*k_ld_, ld_min_), ld_max_);

  double phi_steer = atan2(2 * L * sin(alpha), ld);
  if(d_rg <= distance_rg_ && global_plan_.poses.size() > 1) 
  {
    const double d_x = goal_.x - sub_goal_.x;
    const double d_y = goal_.y - sub_goal_.y;
    const double dd  = sqrt(d_x * d_x + d_y * d_y);
    const double d_theta = goal_.theta - sub_goal_.theta;
    if(dd < 1e-5)
    {
      robot_geometry_msgs::Pose2D p = global_plan_.poses[global_plan_.poses.size() - 2];
      alpha = velocity.x / (fabs(velocity.x) + 1e-9)*(p.theta - pose.theta);
      phi_steer = atan2(2 * L * sin(alpha), 1.0);
    }
  }

  double alpha_reduce  = std::min(std::max(fabs(vr), 0.6), 1.0); 
  double cos_actual_phi_steer = cos( std::min(M_PI/2.3, fabs(actual_phi_steer)));

  // limit W
  double result_W_max = 0.25;
  // double result_W_min = 0.13;
  double result_W = vr * fabs(cos(alpha)) * tan(actual_phi_steer) / L;
  double V_s = std::max(alpha_reduce, fabs(cos(alpha))) * vr / cos_actual_phi_steer;
  std::stringstream ss;
  if(fabs(result_W) > 0.02)
  {
    double d_w = fabs(result_W) - fabs(velocity.theta);
    double a = fabs(velocity.theta) > fabs(result_W) ? fabs(acc_lim_theta_/controller_frequency_param_name_) : fabs(decel_lim_theta_/controller_frequency_param_name_);
    // ss << d_w << " " << a << " ";
    double result;
    if(fabs(d_w) >= a) 
      result =  fabs(velocity.theta) + d_w / fabs(d_w + 1e-9) * a;
    else if (fabs(d_w) < fabs(a))
      result = fabs(velocity.theta);
    // ss << result_W << " " << velocity.theta << " ";
    result_W = result_W / (fabs(result_W) + 1e-9) * result;
    // ss << result_W << " ";
    // ROS_INFO_STREAM(ss.str());

    double d_v = fabs(result_W * L / tan(actual_phi_steer)) - fabs(vr);
    if(fabs(d_v) >= a) 
      result =  velocity.x / (fabs(velocity.x) + 1e-9)* (fabs(vr) + d_v / fabs(d_v + 1e-9) * a);
    else if (fabs(d_v) < fabs(a))
      result = velocity.x / (fabs(velocity.x) + 1e-9) * fabs(vr);

    V_s = std::max(alpha_reduce, fabs(cos(alpha)))*result/ cos_actual_phi_steer;
  }
  
  if(fabs(result_W) > result_W_max) 
    V_s = ( result_W / (fabs(result_W) + 1e-9) * result_W_max * L / tan(actual_phi_steer)) / cos_actual_phi_steer;

  if(is_filter_)
  {
    robot_geometry_msgs::Twist msg;
    msg.linear.x = result.velocity.x;
    msg.angular.z = phi_steer;
    cmd_raw_pub_.publish(msg);
   
    Eigen::VectorXd y(1);
    y << phi_steer;
    robot::Time current_time = robot::Time::now();
    double dt = robot::Duration(current_time - last_actuator_update_).toSec();
    last_actuator_update_ = current_time;
    Eigen::MatrixXd A(3, 3); // System dynamics matrix
    A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
    kf_->update(y, dt, A);
    phi_steer = kf_->state().transpose()[0];
  }
  theta_direction_ = phi_steer !=0 ? phi_steer/(fabs(phi_steer) + 1e-9) * velocity.x/((fabs(velocity.x) + 1e-9)) : 0;

  result.velocity.theta = phi_steer;
  result.velocity.x = fabs(cos(phi_steer - actual_phi_steer)) * V_s;
  return result;
}

unsigned int Bicycle::getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_geometry_msgs::Pose2D> &plan,
                                   const double distance, unsigned int start_index, unsigned int last_valid_index) const
{
  if (start_index >= last_valid_index)
    return last_valid_index;

  unsigned int goal_index = start_index;
  const double start_direction_x = plan[start_index + 1].x - plan[start_index].x;
  const double start_direction_y = plan[start_index + 1].y - plan[start_index].y;
  if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) {  // make sure that atan2 is defined
    double start_angle = atan2(start_direction_y, start_direction_x);
    double S = sqrt(pow(start_direction_x, 2) + pow(start_direction_y, 2));
    // double angle_threshold = 0.0;
    for (unsigned int i = start_index + 1; i <= last_valid_index; ++i)
    {
      const double current_direction_x = plan[i].x - plan[i - 1].x;
      const double current_direction_y = plan[i].y - plan[i - 1].y;
      const double dd = sqrt(pow(current_direction_x, 2) + pow(current_direction_y, 2));
      S += dd;
      if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
      {
        double current_angle = atan2(current_direction_y, current_direction_x);
        if(fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= angle_threshold_ || S >= distance)
        {
          break;
        }
        goal_index = i;
      }
    }
  }
  return goal_index;
}

bool Bicycle::getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, 
                          const robot_nav_2d_msgs::Path2D &global_plan, robot_geometry_msgs::Pose2D &sub_goal, 
                          double &target_direction)
{
  const robot_nav_core2::Costmap &costmap = *costmap_;
  const nav_grid::NavGridInfo &info = costmap.getInfo();

  if (global_plan.poses.empty())
  {
    ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty.");
    return false;
  }
  // Chuyển đổi dữ liệu kế hoạch đường đi 'robot_nav_2d_msgs::Path2D' sang 'std::vector<robot_geometry_msgs::Pose2D>'
  std::vector<robot_geometry_msgs::Pose2D> plan = global_plan.poses;

  std::vector<unsigned int> index_s;
  if(!findSubGoalIndex(plan, index_s))
  {
    ROS_ERROR_NAMED("PathProgressCritic", "Can not find SubGoal Index");
    return false;
  }
  // else 
  // {
  //   std::stringstream ss;
  //   for(auto i : index_s) ss << i << " ";
  //   ROS_INFO_STREAM(ss.str());
  // }

  unsigned int sub_goal_index = sub_goal_index_saved_;
  unsigned int sub_start_index = sub_start_index_saved_;
  if(index_s.empty() || index_s.size() < 1)
  {
    sub_goal_index = (unsigned int)plan.size();
    sub_goal_index_saved_ = sub_goal_index;
  }
  else
  {
    if(index_s.front() > sub_goal_index_saved_) 
    {
      sub_goal_index = (unsigned int)(index_s.front()) + 1;
      sub_goal_index_saved_ = sub_goal_index;
    }
    if(index_s.size() > 1)
    {
      for(int i = 1; i < index_s.size(); ++i)
      {
        // double tolerance = line_generator->calculateTolerance(robot_pose, plan[index_s[i-1]]);
        double distance = fabs(robot_nav_2d_utils::poseDistance(robot_pose, plan[index_s[i-1]]));
        // double dx           = plan[index_s[i-1]].x - robot_pose.x;
        // double dy           = plan[index_s[i-1]].y - robot_pose.y;
        // if(std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_goal_tolerance_ + 0.15)
        if (distance < xy_local_goal_tolerance_offset_)
        {
          if(index_s[i] > sub_goal_index_saved_) 
          {
            if(i < index_s.size()-1)
            {
              sub_goal_index = index_s[i] + 1;
            }
            else
            {
              sub_goal_index = (unsigned int)plan.size();
            }
            if(follow_step_path_) 
            {
              sub_start_index = index_s[i-1];
              sub_start_index_saved_ = sub_start_index;
            }
            ROS_INFO("Tang nac path tu %d den %d boi khoang cach", sub_goal_index_saved_, sub_goal_index);
            // ROS_WARN("i %d follow_step_path_ = %x start = %d sub_goal = %d size = %d", i, follow_step_path_, sub_start_index, sub_goal_index, (unsigned int)plan.size());
            sub_goal_index_saved_ = sub_goal_index;
            break;
          }
        }
      }
    }
    else
    {
      double distance = fabs(robot_nav_2d_utils::poseDistance(plan[sub_goal_index], robot_pose)); 
      if(distance <= xy_local_goal_tolerance_ )
      {
        sub_goal_index = (unsigned int)plan.size();
        sub_goal_index_saved_ = sub_goal_index;
      }
    }
    
  }

  // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map
  // Tìm điểm pose bắt đầu trên kế hoạch đường đi gần nhất với robot đồng thời có tồn tại trong local map
  unsigned int start_index = 0;                      // số vị trí pose trong kế hoạch đường đi
  double distance_to_start = std::numeric_limits<double>::infinity(); // Xét giá trị là vô cùng oo
  bool started_path = false; 
  // for (unsigned int i = 0; i < plan.size(); i++)
  // ROS_INFO_THROTTLE(1.0, "follow_step_path_ = %x start = %d sub_goal = %d size = %d", follow_step_path_, sub_start_index, sub_goal_index, (unsigned int)plan.size());
  for (unsigned int i = sub_start_index; i < sub_goal_index; i++)
  {
    
    // double g_x = plan[i].x;
    // double g_y = plan[i].y;
    // unsigned int map_x, map_y;
    // if (worldToGridBounded(info, g_x, g_y, map_x, map_y) // chuyển tọa độ từ đơn vị mét sang pixel
    //     && costmap(map_x, map_y) != robot_nav_core2::Costmap::NO_INFORMATION)
    // { 
      // Still on the costmap. Continue.
      double distance = fabs(robot_nav_2d_utils::poseDistance(plan[i], robot_pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét
      if (distance_to_start > distance) 
      {
        start_index = i;
        distance_to_start = distance;
        started_path = true;
      }
      // else
      // { 
      //   break;
      // }
    // else if (started_path)
    // {
    //   // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
    //   // even closer to the robot, but then we would skip over parts of the plan.
    //   break;
    // }
    // else, we have not yet found a point on the costmap, so we just continue
  }
  // Nếu khồng tìm được điểm gần với robot nhất thì trả về false
  if (!started_path)
  {
    ROS_ERROR_NAMED("PathProgressCritic", "None of the points of the global plan were in the local costmap.");
    return false;
  }

  // find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
  // Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map
  if(start_index < 1) start_index == 1;
  // unsigned int last_valid_index = plan.size() - 1;
  unsigned int last_valid_index = sub_goal_index - 1;
  // for (unsigned int i = start_index + 1; i < sub_goal_index; i++)
  // {
  //   double g_x = plan[i].x;
  //   double g_y = plan[i].y;
  //   unsigned int map_x, map_y;
  //   if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != robot_nav_core2::Costmap::NO_INFORMATION)
  //   {
  //     // Still on the costmap. Continue.
  //     last_valid_index = i;
  //   }
  //   else
  //   {
  //     // Off the costmap after being on the costmap.
  //     break;
  //   }
  // }
    
  // find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
  // i.e. is within angle_threshold_ of the starting direction.
  // Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan

  unsigned int goal_index = start_index;
  robot::Rate r(30);
  while (goal_index < last_valid_index && robot::ok())
  {
    // Tìm kiếm vị trí điểm mục tiêu phù hợp
    double S;
    if(velocity.x > 0) S = std::min(4.0, std::max(0.9, fabs(velocity.x)*k_d_reduce_));
    // else if(velocity.x < 0) S = 0.5;
    else if(velocity.x < 0) S = std::min(0.7, std::max(0.5, fabs(velocity.x)*k_d_reduce_));
    else S = 0.5;
    goal_index = getGoalIndex(robot_pose, plan, S, start_index, last_valid_index);

    // check if goal already reached
    bool goal_already_reached = false;

    robot_geometry_msgs::PoseArray poseArrayMsg;
    poseArrayMsg.header.stamp = robot::Time::now();
    poseArrayMsg.header.frame_id = frame_id_path_;
	  // std::stringstream ss;
    for (auto &reached_intermediate_goal : reached_intermediate_goals_)
    {
      double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]));
      // ss << distance << " ";
      robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal);
      poseArrayMsg.poses.push_back(pose);
    }
    reached_intermediate_goals_pub_.publish(poseArrayMsg);
    // if(!reached_intermediate_goals_.empty()) 
    // {
    //   ROS_INFO_STREAM(ss.str());
    // }

    for (auto &reached_intermediate_goal : reached_intermediate_goals_)
    {
      double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]));
      if (distance < xy_local_goal_tolerance_)
      {
        goal_already_reached = true;
        // choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
        // (start_index is now > goal_index)
        for (start_index = goal_index; start_index <= last_valid_index; ++start_index)
        {
          distance = robot_nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]);
          if ( distance >= xy_local_goal_tolerance_offset_)
          {
            break;
          }
        }
        // start_index_saved_ = start_index;
        break;
      }
    }

  //try hiep
    unsigned num = 0;
    start_index_saved_vt_.insert(start_index_saved_vt_.begin(),start_index);
    if(start_index_saved_vt_.size() > index_samples_) start_index_saved_vt_.pop_back();
    std::stringstream ss;
    for(int i = 0; i < start_index_saved_vt_.size() - 1; i++)
    {
      double edge = 0;
      if(start_index_saved_vt_[i] != start_index_saved_vt_[i+1]) 
        edge = journey(plan, start_index_saved_vt_[i], start_index_saved_vt_[i+1]);
      if(edge < xy_local_goal_tolerance_) num++;
      ss << start_index_saved_vt_[i] << " ";
    }
    // ss << start_index << " ";

    if(num < index_samples_ - 1 && !start_index_saved_vt_.empty())
    {
      auto max_iter = *(std::max_element(start_index_saved_vt_.begin(), start_index_saved_vt_.end()));
      start_index_saved_ = start_index_saved_ >= max_iter? start_index_saved_ : max_iter;
      double S1 = journey(plan, start_index, plan.size()-1 );
      double S2 = journey(plan, start_index_saved_, plan.size()-1 );

      // ROS_INFO_STREAM(ss.str());
      if(S2 - S1 < -fabs(xy_local_goal_tolerance_))
      {
        start_index = start_index_saved_;
      }
      else start_index_saved_ = start_index;
      // ROS_INFO("  %d %d %f %d %f %f", max_iter, start_index, S1, start_index_saved_, S2, S2 - S1);
    } 
    else start_index_saved_ = start_index;

    if(start_index >= goal_index) goal_index = getGoalIndex(robot_pose, plan, S,start_index, last_valid_index);

    if (!goal_already_reached)
    {
      // new goal not in reached_intermediate_goals_
      double distance = fabs(robot_nav_2d_utils::poseDistance(plan[goal_index], plan[start_index]));
      if (distance < xy_local_goal_tolerance_)
      {
        // ROS_INFO("goal_index %d distance %f", goal_index, distance);
        // the robot has currently reached the goal
        reached_intermediate_goals_.push_back(plan[goal_index]);
        ROS_DEBUG_NAMED("PathProgressCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size());
      }
      else
      {
        // we've found a new goal!
        break;
      }
    }

    r.sleep();
    robot::spinOnce();
  }

  if (start_index > goal_index)
    start_index = goal_index;
  ROS_ASSERT(goal_index <= last_valid_index);
  if((unsigned int)plan.size() == 2 && !plan.empty()) 
  {
    start_index = 0;
    goal_index = 1;
  }

  // Tham chiếu lại đối số đầu vao
  sub_goal = plan[goal_index];
  if(goal_index > 0) rev_sub_goal_ = plan[goal_index - 1];
  else rev_sub_goal_ = plan[start_index];

  target_direction = x_direction_;
  if(goal_index > 0 && (unsigned int)plan.size() > 1)
  {
    if(goal_index < (unsigned int)(plan.size() -1))
    {
      const robot_geometry_msgs::Pose2D p1 = plan[start_index];
      int index;
      for(index = start_index+1; index < goal_index; index++)
      {
        robot_geometry_msgs::Pose2D pose = plan[index];
        if(robot_nav_2d_utils::poseDistance(p1, pose) > xy_goal_tolerance_)  break;
      }      
      const robot_geometry_msgs::Pose2D p2 = plan[index];
      if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
      {
        const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x);
        const double dir_path = cos(fabs(path_angle - p2.theta));
        if(fabs(dir_path) > 1e-9) 
          target_direction = dir_path; 
        // ROS_INFO("%f %f %f %f", p1.x ,p1.y, p2.x ,p2.y);
        // ROS_INFO("%d  %0.2f %0.2f %0.2f", goal_index, path_angle, p2.theta, target_direction);
      }
    }
    else 
    {
      const double rb_direction_x  = sub_goal.x - robot_pose.x;
      const double rb_direction_y  = sub_goal.y - robot_pose.y;
      const double rb_angle = atan2(rb_direction_y, rb_direction_x);
      double dir_rg = cos(rb_angle - robot_pose.theta);
      const double angle_threshold = 0.6;
      if(fabs(dir_rg) > angle_threshold) 
        target_direction = dir_rg; 
    }
  }

  // Publish start_index
  robot_geometry_msgs::PoseStamped pose_st = robot_nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, robot::Time::now());
  closet_robot_goal_pub_.publish(pose_st);

  // Publish goal_index
  robot_geometry_msgs::PoseStamped pose_g = robot_nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, robot::Time::now());
  current_goal_pub_.publish(pose_g);
 
  return true;
}

bool Bicycle::findSubGoalIndex(const std::vector<robot_geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s)
{
  if (plan.empty())
  {
    return false;
  }
  double x_direction_saved = 0.0;
  robot_geometry_msgs::PoseArray poseArrayMsg;
  poseArrayMsg.header.stamp = robot::Time::now();
  poseArrayMsg.header.frame_id = frame_id_path_;

  for (unsigned int i = 1; i < (unsigned int)plan.size(); i++)
  {
    robot_geometry_msgs::Pose2D p1 = plan[i];
    robot_geometry_msgs::Pose2D p2 = plan[i+1];
    // robot_geometry_msgs::Pose2D p3 = plan[i+2];
    double angle1;
    if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
      angle1 = atan2(p2.y - p1.y, p2.x - p1.x);
    else continue;
    // const double angle2 = atan2(p3.y - p2.y, p3.x - p2.x);
    const double target_direction = cos(fabs(angle1 - p2.theta));

    if(target_direction * x_direction_saved < 0.0)
    {
      index_s.push_back(i);
      robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(plan[i]);
      poseArrayMsg.poses.push_back(pose);
    }
    x_direction_saved = target_direction;
  }

  if(index_s.empty()) 
    index_s.push_back((unsigned int)plan.size() - 1);
  else
  {
    if(index_s.back() < (unsigned int)plan.size() - 1)
      index_s.push_back((unsigned int)plan.size() - 1);
  }
  reached_intermediate_goals_pub_.publish(poseArrayMsg);
  return true;
}

double Bicycle::journey(const std::vector<robot_geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
{
  double S = 0; 
  if(last_valid_index - start_index > 1)
  {
    for(int i= start_index; i < last_valid_index; i++)
    {
      S += robot_nav_2d_utils::poseDistance(plan[i], plan[i+1]);
    }
  }
  return S;
}


} // namespace  mkt_algorithm

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