#ifndef _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__
#define _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__

#include <robot/robot.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_utils/conversions.h>

#include <robot_nav_core2/common.h>
#include <robot_nav_core2/exceptions.h>

#include <tf3/buffer_core.h>
#include <tf3/convert.h>
#include <tf3/utils.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>

#include <robot_costmap_2d/costmap_2d_robot.h>

namespace pnkx_local_planner
{
  bool getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame,
               robot_nav_2d_msgs::Pose2DStamped &global_pose, double transform_tolerance = 2.0);

  bool transformGlobalPose(TFListenerPtr tf, const std::string &global_frame,
                           const robot_nav_2d_msgs::Pose2DStamped &stamped_pose, robot_nav_2d_msgs::Pose2DStamped &newer_pose);

  double getSquareDistance(const robot_geometry_msgs::Pose2D &pose_a, const robot_geometry_msgs::Pose2D &pose_b);

  /**
   * @brief  Transforms the global plan of the robot from the planner frame to the local frame (modified).
   *
   * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
   * such that the index of the current goal pose is returned as well as
   * the transformation between the global plan and the planning frame.
   * @param tf A reference to a tf buffer
   * @param global_plan The plan to be transformed
   * @param pose The pose of the robot
   * @param costmap A reference to the costmap being used so the window size for transforming can be computed
   * @param global_frame The frame to transform the plan to
   * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
   * @param[out] transformed_plan Populated with the transformed plan
   * @return \c true if the global plan is transformed, \c false otherwise
   */
  bool transformGlobalPlan(
      TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
      const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &global_frame, double max_plan_length,
      robot_nav_2d_msgs::Path2D &transformed_plan, const bool from_global_frame = false);

}

#endif