/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2017, Locus Robotics
 *  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 <dwb_critics/metratronik_technik.h>
#include <nav_2d_utils/parameters.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>

const double EPSILON = 1E-5;

PLUGINLIB_EXPORT_CLASS(dwb_critics::MetratronikTechnikCritic, dwb_local_planner::TrajectoryCritic)

namespace dwb_critics
{
  inline double hypot_sq(double dx, double dy)
  {
    return dx * dx + dy * dy;
  }

  void MetratronikTechnikCritic::onInit()
  {
    xy_local_goal_tolerance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "xy_goal_tolerance", 0.25);
    xy_local_goal_tolerance_sq_ = xy_local_goal_tolerance_ * xy_local_goal_tolerance_;
    stopped_velocity_ = nav_2d_utils::searchAndGetParam(critic_nh_, "trans_stopped_velocity", 0.25);
    critic_nh_.param("sq_window", sq_window_, 1.0);
    critic_nh_.param("slowing_factor", slowing_factor_, 5.0);
    critic_nh_.param("angle_threshold", angle_threshold_, 0.436332313);
    critic_nh_.param("Lm", Lm_, 0.436332313);
    reset();
  }

  void MetratronikTechnikCritic::reset()
  {
    in_window_ = false;
    running_ = false;
  }

  bool MetratronikTechnikCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
                                         const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan)
  {
    if (!getPosition(pose, global_plan, current_position_))
    {
      return false;
    }
    // ROS_INFO("current_position_");
    dxy_sq_ = hypot_sq(pose.x - goal.x, pose.y - goal.y);
    in_window_ = in_window_ || dxy_sq_ <= sq_window_;
    current_speed_ = vel.x;
    running_ = running_ || (in_window_ && vel.x > 0.0 && vel.x <= stopped_velocity_);
    return true;
  }

  double MetratronikTechnikCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj)
  {
    if (!in_window_)
    {
      return 0.0;
    }
    else if (!running_)
    {
      return fabs(slowing_factor_ + scoreMetratronikTechnik(traj));
    }
    // If we're sufficiently close to the goal, any transforming velocity is invalid
    if (fabs(traj.velocity.x) > stopped_velocity_ || fabs(traj.velocity.y) > stopped_velocity_)
    {
      return fabs(traj.velocity.x + slowing_factor_ + scoreMetratronikTechnik(traj));
    }
    return 0.01 * scoreMetratronikTechnik(traj);
  }

  double MetratronikTechnikCritic::scoreMetratronikTechnik(const dwb_msgs::Trajectory2D &traj)
  {
    if (traj.poses.empty())
    {
      throw nav_core2::IllegalTrajectoryException(name_, "Empty trajectory.");
    }
    double k_reduce_v = cos(((M_PI / 2) / Lm_) * current_position_);
    double result_linear_x = traj.velocity.x * k_reduce_v;
    double k_reduce_w_v = cos(((M_PI / 2) / stopped_velocity_) * result_linear_x);
    double result_angular_z = atan(current_position_ / Lm_) * fabs(k_reduce_w_v);

    double running_time = traj.time_offsets.back().toSec();
    double score_linear_x = fabs(traj.velocity.x * running_time - sqrt(dxy_sq_));
    // ROS_INFO("%f %f %f %f", running_time,fabs(traj.velocity.x*running_time), sqrt(dxy_sq_), score_linear_x);
    double score_angular_z = fabs(traj.velocity.theta - result_angular_z);
    // ROS_INFO("%f %f %f %f %f %f %f", result_linear_x, traj.velocity.x, result_angular_z, traj.velocity.theta, score_linear_x, score_angular_z, score_angular_z + score_linear_x);
    return score_angular_z + score_linear_x;
  }

  bool MetratronikTechnikCritic::getPosition(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Path2D &global_plan, double &position)
  {
    const nav_core2::Costmap &costmap = *costmap_;
    const nav_grid::NavGridInfo &info = costmap.getInfo();

    if (global_plan.poses.empty())
    {
      ROS_ERROR_NAMED("MetratronikTechnikCritic", "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;
    // unsigned int start_index = 0;
    // double distance_to_start = std::numeric_limits<double>::infinity();
    // 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.
    //       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
    // }
    // // 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("MetratronikTechnikCritic", "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;
    //   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;
    //     }
    //   }

    //   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("MetratronikTechnikCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size());
    //     }
    //     else
    //     {
    //       // we've found a new goal!
    //       break;
    //     }
    //   }
    // }
    // if (start_index >= goal_index)
    //   return false;
    lineEquation leq;
    getLine(plan.front(), plan.back(), leq);
    position = calculateError(leq, Lm_, robot_pose, 0).error_line;
    return true;
  }

  unsigned int MetratronikTechnikCritic::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;
  }

  bool MetratronikTechnikCritic::getLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point, lineEquation &leq)
  {
    /**
     *  Phương trình đường thẳng có dạng:
     *  (y - yi)/(yj - yi) = (x - xi)/(xj - xi)
     */
    leq.Gi = start_point;
    leq.Gj = end_point;
    leq.a = (leq.Gj.y - leq.Gi.y);
    leq.b = (leq.Gi.x - leq.Gj.x);
    leq.c = (leq.Gj.x * leq.Gi.y - leq.Gi.x * leq.Gj.y);
    return leq.a != 0 || leq.b != 0;
  }

  presentPosition MetratronikTechnikCritic::calculateError(const lineEquation &leq, const float Lm, const geometry_msgs::Pose2D robot_pose, float pha)
  {
    presentPosition present_pose;
    float Yaw_rad = robot_pose.theta;
    float Yaw = ((Yaw_rad * 180) / M_PI);
    Yaw_rad = (Yaw - pha) * M_PI / 180;
    /*
     * Tính tọa độ điểm R
     */
    float G_xr = float(robot_pose.x);
    float G_yr = float(robot_pose.y);
    /*
     * Tính tọa độ điểm H
     */
    present_pose.Gxh = G_xr + Lm * cos(Yaw_rad);
    present_pose.Gyh = G_yr + Lm * sin(Yaw_rad);
    /**
     *  Phương trình đường thẳng (rh) có dạng:
     *  (y - yr)/(yh - yr) = (x - xr)/(xh - xr)
     */
    float A_rh = (present_pose.Gyh - G_yr);
    float B_rh = (G_xr - present_pose.Gxh);
    float C_rh = (present_pose.Gxh * G_yr - G_xr * present_pose.Gyh);
    /*
     * Phương trình đường thẳng vuông góc (y - yr)/(yh - yr) = (x - xr)/(xh - xr)
     * và đi qua Gh có dạng:
     * -B_rh*(x - Gxh) + Arh*(y - Gyh) = 0
     */
    float A_he = -B_rh;
    float B_he = A_rh;
    float C_he = (-B_rh) * (-present_pose.Gxh) + A_rh * (-present_pose.Gyh);
    /*
     * Đường thẳng (he) giao với đường thẳng (ij) tại e
     * vậy G_xe và G_ye là nghiệm của hệ phương trình 2 ẩn của 2 đường thẳng trên:
     */
    float D = A_he * leq.b - leq.a * B_he;
    float Dx = (-C_he) * leq.b - (-leq.c) * B_he;
    float Dy = A_he * (-leq.c) - leq.a * (-C_he);
    /*
     * Tính tọa độ điểm E
     */
    if (D == 0)
    {
      if (Dx + Dy == 0)
      {
        /*
         * Vì hai đường thẳng trùng nhau nên sai số giữa robot và đường đi là bằng 0
         */
        present_pose.error_line = 0;
      }
      else
        throw NoSolution();
    }
    else
    {
      float G_xe = Dx / D;
      float G_ye = Dy / D;
      /*
       * Tính khoảng cách từ điểm E đến đường thẳng (RH)
       */
      present_pose.error_line = -(A_rh * G_xe + B_rh * G_ye + C_rh) / (sqrt(pow(A_rh, 2) + pow(B_rh, 2)));
    }
    present_pose.distanceToEnd = sqrt(pow((leq.Gj.x - G_xr), 2) + pow((leq.Gj.y - G_yr), 2));
    present_pose.distanceToStart = sqrt(pow((leq.Gi.x - G_xr), 2) + pow((leq.Gi.y - G_yr), 2));

    return present_pose;
  }

} /* namespace dwb_critics */