#include "toid_spinner_controller/path_handler.hpp"
#include "angles/angles.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_core/controller_exceptions.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2/LinearMath/Vector3.hpp"
#include "tf2/exceptions.hpp"

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <iterator>
#include <memory>
#include <numeric>

namespace toid {
PathHandler::PathHandler(
    tf2::Duration tranform_tolerance, std::shared_ptr<tf2_ros::Buffer> tf,
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
    : transform_tolerance_(tranform_tolerance), tf_(tf),
      costmap_ros_(costmap_ros) {}

nav_msgs::msg::Path
PathHandler::transformGlobalPlan(const geometry_msgs::msg::PoseStamped &pose,
                                 double max_robot_pose_search_dist,
                                 double angle_tolerance) {

  if (global_plan_.poses.empty()) {
    throw nav2_core::InvalidPath("Received plan with length of one");
  }

  geometry_msgs::msg::PoseStamped robot_pose;
  if (!tranformPose(global_plan_.header.frame_id, pose, robot_pose)) {
    throw nav2_core::ControllerTFError(
        "Unable to transform robot pose into global plan's frame");
  }

  auto closest_pose_upper_bound =
      nav2_util::geometry_utils::first_after_integrated_distance(
          global_plan_.poses.begin(), global_plan_.poses.end(),
          max_robot_pose_search_dist);

  const double robot_orientation = tf2::getYaw(robot_pose.pose.orientation);

  // Calculate the closeset pose and keep track of the next rotation point
  auto closest_pose = global_plan_.poses.begin();
  auto last_rotation = global_plan_.poses.end();
  auto previous_it = global_plan_.poses.begin();
  auto it = global_plan_.poses.begin();
  double closest_dist = INFINITY;

  for (; it != closest_pose_upper_bound; it++) {
    const double it_orientation = tf2::getYaw(it->pose.orientation);
    const double angle_diff = std::abs(
        angles::shortest_angular_distance(it_orientation, robot_orientation));

    if (previous_it->pose.position == it->pose.position) {
      last_rotation = it + 1;
    }

    if (angle_diff <= angle_tolerance) {
      double dist = dist2(robot_pose, *it);
      if (dist <= closest_dist) {
        closest_dist = dist;
        closest_pose = it;
        last_rotation = global_plan_.poses.end();
      }
    }
    previous_it = it;
  }
  if(last_rotation == closest_pose_upper_bound || last_rotation == global_plan_.poses.end()) {
    for (; it != global_plan_.poses.end(); it++) {
      if (previous_it->pose.position == it->pose.position) {
        last_rotation = it + 1;
      } else if (last_rotation != global_plan_.poses.end()) {
        break;
      }
      previous_it = it;
    }
  }

  auto transformGlobalPoseToLocal = [&](const auto &global_plan_pose) {
    geometry_msgs::msg::PoseStamped stamped_pose;
    stamped_pose.header.frame_id = global_plan_.header.frame_id;
    stamped_pose.header.stamp = robot_pose.header.stamp;
    stamped_pose.pose = global_plan_pose.pose;
    stamped_pose.pose.position.z = 0.0;
    return stamped_pose;
  };

  nav_msgs::msg::Path out_path;
  std::transform(closest_pose, last_rotation,
                 std::back_inserter(out_path.poses),
                 transformGlobalPoseToLocal);

  out_path.header.frame_id = global_plan_.header.frame_id;
  out_path.header.stamp = robot_pose.header.stamp;

  global_plan_.poses.erase(begin(global_plan_.poses), closest_pose);

  return out_path;
}

bool PathHandler::tranformPose(const std::string frame,
                               const geometry_msgs::msg::PoseStamped &in_pose,
                               geometry_msgs::msg::PoseStamped &out_pose) {
  if (in_pose.header.frame_id == frame) {
    out_pose = in_pose;
    return true;
  }

  try {
    tf_->transform(in_pose, out_pose, frame, transform_tolerance_);
    out_pose.header.frame_id = frame;
    return true;
  } catch (tf2::TransformException &ex) {
    RCLCPP_ERROR(logger_, "Exception in tranformPose: %s", ex.what());
  }
  return false;
}

double PathHandler::dist2(const geometry_msgs::msg::PoseStamped &p1,
                          const geometry_msgs::msg::PoseStamped &p2) {
  const double x = p1.pose.position.x - p2.pose.position.x;
  const double y = p1.pose.position.y - p2.pose.position.y;
  return x * x + y * y;
}

} // namespace toid