#ifndef _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__
#define _NAV_ALGORITHM_ROTATE_TO_GOAL_H_INCLUDED__

#include <score_algorithm/score_algorithm.h>
#include <pluginlib/class_loader.h>
#include <robot_geometry_msgs/PoseArray.h>
#include <robot_geometry_msgs/PoseStamped.h>

namespace mkt_algorithm
{

/**
 * @class GoStraight
 * @brief Standard Bicycle-like ScoreAlgorithm
*/
class RotateToGoal : public score_algorithm::ScoreAlgorithm
{
public:
  
  RotateToGoal() {};
  
  virtual ~RotateToGoal() {};

  /**
   * @brief Initialize parameters as needed
   * @param nh NodeHandle to read parameters from
   */
  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) 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_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
                       const robot_geometry_msgs::Pose2D& 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 
  */
  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;


protected:
  robot::NodeHandle nh_;
  robot::NodeHandle nh_kinematics_;
  robot_geometry_msgs::Pose2D sub_goal_;

  double steering_fix_wheel_distance_x_;
  double steering_fix_wheel_distance_y_;
  double angle_threshold_;
  double current_direction_;
  double velocity_rotate_min_, velocity_rotate_max_;
  robot_geometry_msgs::Pose2D goal_;
  robot::Time time_last_cmd_;

}; // class RotateToGoal 

} // namespace mkt_algorithm

#endif