#ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
#define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_

#include <robot_nav_core/base_global_planner.h>
#include <pnkx_local_planner/pnkx_local_planner.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>

namespace pnkx_local_planner
{

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

    virtual ~PNKXDockingLocalPlanner();

    /**
     * @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
     */
    virtual 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.
     */
    virtual bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;

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

  protected:
    /**
     * @brief robot_nav_core2 initialization other
     */
    void initializeOthers() override;
    
    /**
     * @brief get docking maker
     */
    void getMaker();

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

    /**
     * @brief get parameters
     * @param nh NodeHandle to read parameters from
     */
    void getParams(robot::NodeHandle &nh) override;

    /**
     * @brief Helper method for preparing for the core scoring algorithm
     *
     * Runs the prepare method on all the critics with freshly transformed data
     */
    virtual 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
     */
    virtual robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;

  private:
    class DockingPlanner
    {
    public:
      DockingPlanner(const std::string &name);
      ~DockingPlanner();

      /**
       * @brief initialization
       * @param name Namespace for maker
       * @param tf TFListener pointer
       * @param costmap_robot Costmap pointer
       */
      void initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                      score_algorithm::TrajectoryGenerator::Ptr traj_generator);

      bool getMakerGoal(robot_nav_2d_msgs::Pose2DStamped &maker_goal);

      bool geLocalGoal(robot_nav_2d_msgs::Pose2DStamped &local_goal);

      bool getLocalPath(const robot_nav_2d_msgs::Pose2DStamped &local_pose,
                        const robot_nav_2d_msgs::Pose2DStamped &local_goal, robot_nav_msgs::Path &local_path);

      bool initialized_;
      bool is_detected_;
      bool is_goal_reached_;
      bool following_;
      bool allow_rotate_;

      double xy_goal_tolerance_;
      double yaw_goal_tolerance_;
      double min_lookahead_dist_;
      double max_lookahead_dist_;
      double lookahead_time_;
      double angle_threshold_;
      
      std::string name_;
      std::string docking_planner_name_;
      std::string docking_nav_name_;

      robot_nav_core::BaseGlobalPlanner::Ptr docking_planner_;
      score_algorithm::ScoreAlgorithm::Ptr docking_nav_;

      robot_geometry_msgs::Vector3 linear_;
      robot_geometry_msgs::Vector3 angular_;

    private:
      /**
       * @brief Callback wall timer for detect timeout
       */
      virtual void detectedTimeOutCb(const ::robot::WallTimerEvent &timer_event);

      /**
       * @brief Callback wall timer for delay
       */
      virtual void delayCb(const ::robot::WallTimerEvent &timer_event);

      robot::NodeHandle nh_, nh_priv_;
      TFListenerPtr tf_;
      robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
      score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
      std::function<robot_nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
      std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;

      robot::WallTimer detected_timeout_wt_, delayed_wt_;
      bool delayed_;
      bool detected_timeout_;
      std::string robot_base_frame_, maker_goal_frame_;
    };

    bool start_docking_;
    double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
    robot_xmlrpcpp::XmlRpcValue original_papams_;
    std::vector<DockingPlanner*> dkpl_;

    bool dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity);

  }; // PNKXDockingLocalPlanner

} // namespace pnkx_local_planner

#endif // _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_