#ifndef _NAV_ALGORITHM_BICYCLE_H_INCLUDED__
#define _NAV_ALGORITHM_BICYCLE_H_INCLUDED__

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

namespace mkt_algorithm
{

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

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

  unsigned int getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_geometry_msgs::Pose2D> &plan,
                            const double distance, unsigned int start_index, unsigned int last_valid_index) const;

  double journey(const std::vector<robot_geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;


  bool getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, 
                   const robot_nav_2d_msgs::Path2D &global_plan, robot_geometry_msgs::Pose2D &sub_goal, 
                   double &target_direction);
                         
  bool findSubGoalIndex(const std::vector<robot_geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);

  robot::NodeHandle nh_priv_, nh_;
  std::string frame_id_path_;
  robot::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_;

  std::vector<robot_geometry_msgs::Pose2D> reached_intermediate_goals_;
  robot_geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; 
  robot_geometry_msgs::Pose2D goal_;
  robot_nav_2d_msgs::Path2D global_plan_;

  unsigned int start_index_saved_;
  unsigned int sub_goal_index_saved_, sub_start_index_saved_;
  std::vector<unsigned int> start_index_saved_vt_;
  int index_samples_;

  double steering_fix_wheel_distance_x_;
  double steering_fix_wheel_distance_y_;
  double xy_local_goal_tolerance_;
  double xy_goal_tolerance_;
  double angle_threshold_;
  double acc_lim_theta_;
  double decel_lim_theta_;
  double x_direction_, y_direction_, theta_direction_;
  double k_d_reduce_, distance_rg_, k_ld_, ld_min_, ld_max_;
  double controller_frequency_param_name_;

  bool follow_step_path_;
  bool is_filter_;
  boost::shared_ptr<KalmanFilter> kf_;
  robot::Publisher cmd_raw_pub_;
  robot::Time last_actuator_update_;
}; // class Bicycle 

} // namespace mkt_algorithm


#endif //_NAV_ALGORITHM_BICYCLE_H_INCLUDED__