#ifndef _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__

#include <robot/robot.h>
#include <score_algorithm/score_algorithm.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PointStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
#include <angles/angles.h>

namespace mkt_algorithm
{
  namespace diff
  {
    class PurePursuit
    {
    public:
      void computePurePursuit(
        const score_algorithm::TrajectoryGenerator::Ptr &traj,
        const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
        const robot_nav_2d_msgs::Twist2D &velocity,
        const double &min_approach_linear_velocity,
        const double &journey_plan,
        const double &sign_x,
        const double &lookahead_dist_min,
        const double &lookahead_dist_max,
        const double &lookahead_dist,
        const double &lookahead_time,
        const double &dt,
        robot_nav_2d_msgs::Twist2D &drive_cmd
      );


    private:
      void applyConstraints(const double &dist_error, const double &lookahead_dist,
                            const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
                            const double &pose_cost, double &linear_vel, const double &sign_x);

      double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
                                                      const double &journey_plan);

      double computeDecelerationFactor(const double &effective_journey, const double &d_reduce);

      // properties
      double max_lateral_accel_;
    };
  }
}
#endif  