
#ifndef TWO_POINTS_PLANNER_H_INCLUDED
#define TWO_POINTS_PLANNER_H_INCLUDED

#include <iostream>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_nav_core/base_global_planner.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot/robot.h>

namespace two_points_planner
{

/**
 * @class TwoPointsPlanner
 * @brief Plugin-based flexible global_planner
 */
class TwoPointsPlanner : public robot_nav_core::BaseGlobalPlanner
{
public:
  /**
   * @brief  Default constructor for the NavFnROS object
   */
  TwoPointsPlanner();

  virtual ~TwoPointsPlanner(){};

  /**
   * @brief  Constructor for the TwoPointsPlanner object
   * @param  name The name of this planner
   * @param  costmap_robot A pointer to the ROS wrapper of the costmap to use
   */
  TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot);

  /**
   * @brief  Initialization function for the TwoPointsPlanner object
   * @param  name The name of this planner
   * @param  costmap_robot A pointer to the ROS wrapper of the costmap to use
   */
  virtual bool initialize(std::string name, 
                          robot_costmap_2d::Costmap2DROBOT* costmap_robot);
  
  /**
   * @brief Given a goal pose in the world, compute a plan
   * @param start The start pose 
   * @param goal The goal pose 
   * @param plan The plan... filled by the planner
   * @return True if a valid plan was found, false otherwise
   */
  virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start, 
                        const robot_geometry_msgs::PoseStamped& goal, 
                        std::vector<robot_geometry_msgs::PoseStamped>& plan);

  /**
   * @brief Create a new TwoPointsPlanner instance
   * @return A pointer to the new TwoPointsPlanner instance
   */
  static robot_nav_core::BaseGlobalPlanner::Ptr create();

protected:

  unsigned char computeCircumscribedCost();
  unsigned char costMapCostToSBPLCost(unsigned char newcost);

  bool initialized_;
  unsigned char lethal_obstacle_;
  unsigned char inscribed_inflated_obstacle_;
  unsigned char circumscribed_cost_;

  bool allow_unknown_;

  std::string name_;
  robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
  std::vector<robot_geometry_msgs::Point> footprint_;
  unsigned int current_env_width_;
  unsigned int current_env_height_; 

};

} // namespace two_points_planner

#endif // TWO_POINTS_PLANNER_H_INCLUDED