#ifndef FOLLOW_WAYPOINTS_H_
#define FOLLOW_WAYPOINTS_H_

#include <ros/ros.h>
#include <math.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib_msgs/GoalID.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <tf/transform_listener.h>
#include <std_srvs/Trigger.h>
#include <dynamic_reconfigure/server.h>
#include <follow_waypoints/FollowWayPointsConfig.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <boost/thread.hpp>

namespace follow_waypoints
{
  class FollowWayPoints 
  {
  public:
    FollowWayPoints(ros::NodeHandlePtr nh, std::string node_name);
    virtual ~FollowWayPoints();
    void run();
  private:
    void dyn_callback(follow_waypoints::FollowWayPointsConfig &config, int32_t level);
    bool make_path_ready(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
    bool do_path_reset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
    void point_cb(geometry_msgs::PointStamped point);
    void pub_waypoints_list();
    void send_move_base_goal(geometry_msgs::PoseWithCovarianceStamped pose);
    void run_once();
    geometry_msgs::PoseWithCovarianceStamped to_pose_with_Cov(geometry_msgs::PointStamped point);
    geometry_msgs::PoseArray toPoseArray(std::vector<geometry_msgs::PoseWithCovarianceStamped> waypoints);

    std::string frame_id_;
    std::string odom_frame_id_;
    std::string base_frame_id_;
    std::string waypoints_to_follow_topic_;
    std::string waypoints_list_topic_;
    double wait_duration_;
    double distance_tolerance_;
    bool path_ready_;
    bool waypoints_are_poses_;

    // TF Initialisation -------------------------------------------------------------------
    tf::TransformListener tf_;
    tf::TransformListener listener_;

    ros::NodeHandlePtr private_nh_;
    ros::ServiceServer path_ready_srv_;
    ros::ServiceServer path_reset_srv_;
    ros::Publisher pose_array_publisher_;
    ros::Subscriber waypoints_sub_;
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>* move_base_client_;
    std::vector<geometry_msgs::PoseWithCovarianceStamped> waypoints_;
    dynamic_reconfigure::Server<follow_waypoints::FollowWayPointsConfig> server;
    dynamic_reconfigure::Server<follow_waypoints::FollowWayPointsConfig>::CallbackType f;
    follow_waypoints::FollowWayPointsConfig dyn_config_;
    /* Create Thread */
    boost::thread lineThread; 
    ros::Rate* loop_rate_;
  };
} //follow_waypoints

#endif