#ifndef _NAV_ALGORITHM_PTA_H_INCLUDED__
#define _NAV_ALGORITHM_PTA_H_INCLUDED__

#include <score_algorithm/score_algorithm.h>
#include <mkt_algorithm/equation_line.h>

namespace mkt_algorithm
{

/**
 * @class PTA
 * @brief Standard PTA-like ScoreAlgorithm
*/
class PTA : public score_algorithm::ScoreAlgorithm 
{
public:
  
  PTA() : line_generator_(nullptr) {};
  
  virtual ~PTA() {};

// Standard ScoreAlgorithm Interface
  /**
   * @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 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:

  boost::shared_ptr<mkt_algorithm::LineGenerator> line_generator_;
  double pta_amplitude_max_;  // (Default: 0.5 - Range: 0.1 -> 1.0) Cai dat bien do (mot phia - m) cho phep robot di chuyen trong do

}; // class PTA 

} // namespace mkt_algorithm


#endif //_NAV_ALGORITHM_PTA_H_INCLUDED__