#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__
#define _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__

#include <mkt_algorithm/diff/diff_predictive_trajectory.h>

namespace mkt_algorithm
{
  namespace diff
  {
    /**
     * @class GoStraight
     * @brief Standard Bicycle-like ScoreAlgorithm
     */
    class RotateToGoal : public mkt_algorithm::diff::PredictiveTrajectory
    {
    public:
      RotateToGoal();

      virtual ~RotateToGoal();

      /**
       * @brief Initialize parameters as needed
       * @param nh NodeHandle to read parameters from
       */
      virtual void initialize(
          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;

      /**
       * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
       *
       * Subclasses may overwrite. Return false in case there is any error.
       *
       * @param pose Current pose (costmap frame)
       * @param velocity Current velocity
       * @param goal The final goal (costmap frame)
       * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
       */
      virtual bool prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity,
                           const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan,
                           double &x_direction, double &y_direction, double &theta_direction) override;

      /**
       * @brief Reset all data
       */
      virtual void reset() override;

      /**
       * @brief Calculating algorithm
       * @param pose
       * @param velocity
       * @param traj
       */
      virtual mkt_msgs::Trajectory2D calculator(
          const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;

      /**
       * @brief Create a new RotateToGoal instance
       * @return A pointer to the new RotateToGoal instance
       */
      static score_algorithm::ScoreAlgorithm::Ptr create();

    }; // class RotateToGoalDiff

  } // namespace diff

} // namespace mkt_algorithm

#endif // _NAV_ALGORITHM_ROTATE_TO_GOAL_DIFF_H_INCLUDED__