/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, DFKI GmbH
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
#include <mir_dwb_critics/path_progress.h>
#include <nav_grid/coordinate_conversion.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <vector>

namespace mir_dwb_critics
{
  bool PathProgressCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
                                   const geometry_msgs::Pose2D &goal,
                                   const nav_2d_msgs::Path2D &global_plan)
  {
    dwb_critics::MapGridCritic::reset();

    unsigned int local_goal_x, local_goal_y;
    if (!getGoalPose(pose, global_plan, local_goal_x, local_goal_y, desired_angle_))
    {
      return false;
    }
    // Enqueue just the last pose
    cell_values_.setValue(local_goal_x, local_goal_y, 0.0);
    queue_->enqueueCell(local_goal_x, local_goal_y);

    // MapGridCritic::propogateManhattanDistances
    propogateManhattanDistances();

    return true;
  }

  void PathProgressCritic::onInit()
  {
    dwb_critics::MapGridCritic::onInit();
    critic_nh_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.20);
    critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4);
    critic_nh_.param("heading_scale", heading_scale_, 1.0);
    reached_intermediate_goals_pub_ = critic_nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
    current_goal_pub_ = critic_nh_.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
    closet_robot_goal_pub_ = critic_nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
    // divide heading scale by position scale because the sum will be multiplied by scale again
    heading_scale_ /= getScale();
  }

  void PathProgressCritic::reset()
  {
    reached_intermediate_goals_.clear();
  }

  double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj)
  {
    double position_score = MapGridCritic::scoreTrajectory(traj);
    double heading_diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI));
    double heading_score = heading_diff * heading_diff;
    return position_score + heading_scale_ * heading_score;
  }

  bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Path2D &global_plan,
                                       unsigned int &x, unsigned int &y, double &desired_angle)
  {
    const 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 'nav_2d_msgs::Path2D' sang 'std::vector<geometry_msgs::Pose2D>'
    std::vector<geometry_msgs::Pose2D> plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses;

    // 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++)
    {
      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) != nav_core2::Costmap::NO_INFORMATION)
      { // chi phí phải khác -1
        // Still on the costmap. Continue.
        double distance = 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
        // {
        //   // 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.
        //   ss <<" index_max_ " << index_max_;
        //   break;
        // }
      }
      else if (started_path)
      {
        // Off the costmap after being on the costmap.
        break;
      }
      // else, we have not yet found a point on the costmap, so we just continue
    }

    geometry_msgs::PoseStamped pose;
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = global_plan.header.frame_id;
    pose.pose.position.x = plan[start_index].x;
    pose.pose.position.y = plan[start_index].y;
    tf2::Quaternion temp;
    temp.setRPY(0, 0, plan[start_index].theta);
    pose.pose.orientation.x = temp.getX();
    pose.pose.orientation.y = temp.getY();
    pose.pose.orientation.z = temp.getZ();
    pose.pose.orientation.w = temp.getW();

    closet_robot_goal_pub_.publish(pose);

    // 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;
    }
    // current_index_ = start_index;
    // 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
    unsigned int last_valid_index = start_index;
    for (unsigned int i = start_index + 1; i < plan.size(); 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) != 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;

    while (goal_index < last_valid_index && critic_nh_.ok())
    {
      // Tìm kiếm vị trí điểm mục tiêu phù hợp
      goal_index = getGoalIndex(robot_pose, plan, start_index, last_valid_index);

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

      geometry_msgs::PoseArray poseArrayMsg;
      poseArrayMsg.header.stamp = ros::Time::now();
      poseArrayMsg.header.frame_id = global_plan.header.frame_id;
      for (const auto &pose2D : reached_intermediate_goals_)
      {
        geometry_msgs::Pose pose;
        pose.position.x = pose2D.x;
        pose.position.y = pose2D.y;
        tf2::Quaternion temp;
        temp.setRPY(0, 0, pose2D.theta);
        pose.orientation.x = temp.getX();
        pose.orientation.y = temp.getY();
        pose.orientation.z = temp.getZ();
        pose.orientation.w = temp.getW();
        poseArrayMsg.poses.push_back(pose);
      }
      reached_intermediate_goals_pub_.publish(poseArrayMsg);

      for (auto &reached_intermediate_goal : reached_intermediate_goals_)
      {
        double distance = 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 = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]);
            if (distance >= xy_local_goal_tolerance_)
            {
              break;
            }
          }
          break;
        }
      }

      pose.header.stamp = ros::Time::now();
      pose.header.frame_id = global_plan.header.frame_id;
      pose.pose.position.x = plan[goal_index].x;
      pose.pose.position.y = plan[goal_index].y;
      tf2::Quaternion temp;
      temp.setRPY(0, 0, plan[goal_index].theta);
      pose.pose.orientation.x = temp.getX();
      pose.pose.orientation.y = temp.getY();
      pose.pose.orientation.z = temp.getZ();
      pose.pose.orientation.w = temp.getW();

      current_goal_pub_.publish(pose);

      if (!goal_already_reached)
      {
        // new goal not in reached_intermediate_goals_
        double distance = nav_2d_utils::poseDistance(plan[goal_index], robot_pose);
        if (distance < xy_local_goal_tolerance_)
        {
          // 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;
        }
      }
    }

    if (start_index > goal_index)
      start_index = goal_index;
    ROS_ASSERT(goal_index <= last_valid_index);

    // save goal in x, y
    worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y);
    desired_angle = plan[start_index].theta;
    return true;
  }

  unsigned int PathProgressCritic::getGoalIndex(const geometry_msgs::Pose2D &robot_pose,
                                                const std::vector<geometry_msgs::Pose2D> &plan,
                                                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;
    
    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;
      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(robot_pose.theta - current_angle, 2 * M_PI)) > angle_threshold_)
          break;

        goal_index = i;
      }
    }
    if (nav_2d_utils::poseDistance(plan[goal_index], plan[last_valid_index]) <= xy_local_goal_tolerance_)
      goal_index = last_valid_index;
    return goal_index;
  }

} // namespace mir_dwb_critics

PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathProgressCritic, dwb_local_planner::TrajectoryCritic)