#ifndef _SCORE_GOAL_CHECKER_H_INCLUDED__
#define _SCORE_GOAL_CHECKER_H_INCLUDED__

#include <robot/robot.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <memory>

namespace score_algorithm
{

  /**
   * @class GoalChecker
   * @brief Function-object for checking whether a goal has been reached
   *
   * This class defines the plugin interface for determining whether you have reached
   * the goal state. This primarily consists of checking the relative positions of two poses
   * (which are presumed to be in the same frame). It can also check the velocity, as some
   * applications require that robot be stopped to be considered as having reached the goal.
   */
  class GoalChecker
  {
  public:
    using Ptr = std::shared_ptr<score_algorithm::GoalChecker>;

    virtual ~GoalChecker() {}

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

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

    /**
     * @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
     */
    virtual bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& query_pose, const robot_nav_2d_msgs::Pose2DStamped& goal_pose,
      const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D& velocity) = 0;
  };

}  // namespace score_algorithm

#endif  // _SCORE_GOAL_CHECKER_H_INCLUDED__