#pragma once

#include <memory>
#include <rclcpp/logger.hpp>
#include <tf2_ros/buffer.hpp>

#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_core/controller_exceptions.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "tf2/time.hpp"

namespace toid {

class PathHandler {
public:
  PathHandler(tf2::Duration tranform_tolerance,
              std::shared_ptr<tf2_ros::Buffer> tf,
              std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);

  ~PathHandler() = default;

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

  bool tranformPose(const std::string frame,
                    const geometry_msgs::msg::PoseStamped &in_pose,
                    geometry_msgs::msg::PoseStamped &out_pose);

  void setPlan(const nav_msgs::msg::Path &path) { global_plan_ = path; }

  nav_msgs::msg::Path getPlan() { return global_plan_; }

protected:

  double dist2(const geometry_msgs::msg::PoseStamped &p1, const geometry_msgs::msg::PoseStamped &p2);

  rclcpp::Logger logger_{rclcpp::get_logger("MultiPlexPathHandler")};
  tf2::Duration transform_tolerance_;
  std::shared_ptr<tf2_ros::Buffer> tf_;
  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
  nav_msgs::msg::Path global_plan_;
};

} // namespace toid
