#ifndef _MKT_PLUGINS_CHECKER_GOAL_H_INCLUDED__
#define _MKT_PLUGINS_CHECKER_GOAL_H_INCLUDED__

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

namespace mkt_plugins
{
  class GoalChecker : public score_algorithm::GoalChecker
  {
  public:
    GoalChecker();
    /**
     * @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_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) override;

    /**
     * @brief Factory function to create a GoalChecker instance
     * @return Shared pointer to a new GoalChecker instance
     */
    static score_algorithm::GoalChecker::Ptr create();
  protected:
    /**
     * @brief Internal method to initialize parameters from NodeHandle
     * @param nh NodeHandle to load parameters from
     * 
     * Loads xy_goal_tolerance and yaw_goal_tolerance parameters
     */
    void intParam(const robot::NodeHandle &nh);

    robot::NodeHandle nh_;
    std::shared_ptr<mkt_plugins::LineGenerator> line_generator_;
    double yaw_goal_tolerance_;
    double xy_goal_tolerance_;
    double old_xy_goal_tolerance_;
  };

} // namespace mkt_plugins

#endif // _MKT_PLUGINS_CHECKER_GOAL_H_INCLUDED__
