#ifndef _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__
#define _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__

#include <score_algorithm/goal_checker.h>
#include <mkt_algorithm/equation_line.h>

namespace mkt_algorithm
{
class GoalChecker : public score_algorithm::GoalChecker
{
public:

  /**
   * @brief Initialize any parameters from the NodeHandle
   * @param nh NodeHandle for grabbing parameters
   */
  virtual void initialize(const robot::NodeHandle& nh) override;

  /**
   * @brief Reset the state of the goal checker (if any)
   */
  virtual void reset() override;

  /**
   * @brief Check whether the goal should be considered reached
   * @param query_pose The pose to check
   * @param goal_pose The pose to check against
   * @param velocity The robot's current velocity
   * @return True if goal is reached
   */
  bool isGoalReached(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose, 
                             const std::vector<robot_geometry_msgs::PoseStamped>& path, const robot_nav_2d_msgs::Twist2D& velocity) override;

protected:

  boost::shared_ptr<mkt_algorithm::LineGenerator> line_generator_;
  double yaw_goal_tolerance_;
  double xy_goal_tolerance_;
  double old_xy_goal_tolerance_;
  bool setup_;
};

} // namespace mkt_algorithm

#endif // _NAV_ALGORITHM_CHECKER_GOAL_H_INCLUDED__