#ifndef _PNKX_GO_STRAIGHT_LOCAL_PLANNER_H_INCLUDED_
#define _PNKX_GO_STRAIGHT_LOCAL_PLANNER_H_INCLUDED_

#include <pnkx_local_planner/pnkx_local_planner.h>

namespace pnkx_local_planner
{
  /**
   * @class PNKXGoStraightLocalPlanner
   * @brief Plugin-based flexible local_planner
   */
  class PNKXGoStraightLocalPlanner : public pnkx_local_planner::PNKXLocalPlanner
  {
  public:
    /**
     * @brief Constructor that brings up pluginlib loaders
     */
    PNKXGoStraightLocalPlanner();

    virtual ~PNKXGoStraightLocalPlanner();

    /**
     * @brief robot_nav_core2 initialization
     * @param name Namespace for this planner
     * @param tf TFListener pointer
     * @param costmap_robot Costmap pointer
     */
    void initialize(robot::NodeHandle &parent, const std::string &name,
                    TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;

    /**
     * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
     *
     * It is presumed that the global plan is already set.
     *
     * This is mostly a wrapper for the protected computeVelocityCommands
     * function which has additional debugging info.
     *
     * @param pose Current robot pose
     * @param velocity Current robot velocity
     * @return The best command for the robot to drive
     */
    robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
                                                        const robot_nav_2d_msgs::Twist2D &velocity) override;

    /**
     * @brief robot_nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
     *
     * The pose that it checks against is the last pose in the current global plan.
     * The calculation is delegated to the goal_checker plugin.
     *
     * @param pose Current pose
     * @param velocity Current velocity
     * @return True if the robot should be considered as having reached the goal.
     */
    bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;

    /**
     * @brief Create a new instance of the PNKXGoStraightLocalPlanner
     * @return A shared pointer to the new instance
     */
    static robot_nav_core2::LocalPlanner::Ptr create();

  protected:
    /**
     * @brief robot_nav_core2 reset other
     */
    virtual void reset() override;;

    /**
     * @brief Helper method for preparing for the core scoring algorithm
     *
     * Runs the prepare method on all the critics with freshly transformed data
     */
    void prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;

    /**
     * @brief Iterate through all the twists and find the best one
     */
    robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;

  };

} // namespace pnkx_local_planner
#endif // _PNKX_GO_STRAIGHT_LOCAL_PLANNER_H_INCLUDED_