#ifndef _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__
#define _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__

#include <robot/robot.h>
#include <score_algorithm/goal_checker.h>

namespace mkt_plugins
{

  /**
   * @class SimpleGoalChecker
   * @brief Goal Checker plugin that only checks the position difference
   *
   * This class can be stateful if the stateful parameter is set to true (which it is by default).
   * This means that the goal checker will not check if the xy position matches again once it is found to be true.
   */
  class SimpleGoalChecker : public score_algorithm::GoalChecker
  {
  public:
    /**
     * @brief Default constructor
     * Initializes default values for goal tolerances and stateful behavior
     */
    SimpleGoalChecker();
    
    /**
     * @brief Initialize the goal checker with parameters from NodeHandle
     * @param nh NodeHandle for loading parameters (xy_goal_tolerance, yaw_goal_tolerance, stateful, check_xy)
     */
    void initialize(const robot::NodeHandle &nh) override;
    
    /**
     * @brief Reset the state of the goal checker
     * Clears any cached state to allow re-checking of goal conditions
     */
    void reset() override;
    
    /**
     * @brief Check if the goal has been reached
     * @param query_pose Current pose of the robot
     * @param goal_pose Target pose to reach
     * @param path Path to follow (not used in simple checker)
     * @param velocity Current velocity of the robot (not used in simple checker)
     * @return True if the robot is within xy_goal_tolerance and yaw_goal_tolerance of the goal
     * 
     * If stateful is true, once xy position matches, it will not check xy again.
     * This prevents oscillation when the robot is close to the goal.
     */
    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 SimpleGoalChecker instance
     * @return Shared pointer to a new SimpleGoalChecker 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, yaw_goal_tolerance, stateful, and check_xy parameters
     */
    void intParam(const robot::NodeHandle& nh);

    robot::NodeHandle nh_;
    double xy_goal_tolerance_, yaw_goal_tolerance_;
    bool stateful_, check_xy_;

    // Cached squared xy_goal_tolerance_
    double xy_goal_tolerance_sq_;
  };

} // namespace mkt_plugins

#endif // _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__