#ifndef TANGENT_PLANNER_TANGENT_PLANNER_NODE_H
#define TANGENT_PLANNER_TANGENT_PLANNER_NODE_H

#include "tangent_planner.h"
#include "memory.h"
#include "nav_msgs/Odometry.h"
#include "geometry_msgs/Pose2D.h"
#include "ros/ros.h"
#include "sensor_msgs/Range.h"
#include "read.hpp"
#include <tf/transform_datatypes.h>
#include "thread"



namespace Planner {
    class TangentPlannerNode : public TangentPlanner {
    public:
        TangentPlannerNode();

        ~TangentPlannerNode();

        void set_publisher(const ros::Publisher &publisher);

        void odometry_subscriber_callback(const nav_msgs::Odometry &msg);

        void tof_subscriber_callback(const sensor_msgs::Range &msg);

        void twist_publisher_callback();

        void computeVelocity();

        bool is_started();

        bool going_home = false;
        ros::ServiceServer start_patrolling;
        ros::ServiceServer stop_patrolling;
        ros::ServiceServer get_patrol_path_name;
        ros::ServiceServer get_current_status;
        ros::ServiceServer low_battery;
        std::string path_file_name;
        void start_thread();
    private:
        void read_path_file(const char* path){
            std::shared_ptr<DATA_FILE<float,10>> file = std::make_shared<DATA_FILE<float, 10>>(path);
            std::array<float,10> in={0.f};
            while(file->read(in)){
                tf::Quaternion q(in[4],in[5],in[6], in[3]);
                tf::Matrix3x3 m(q);
                double roll, pitch, yaw;
                m.getRPY(roll, pitch, yaw);
                std::shared_ptr<geometry_msgs::Pose2D> pose = std::make_shared<geometry_msgs::Pose2D>();
                pose->x = in[1];
                pose->y = in[0];
                pose->theta = yaw;
                global_plan_.insert(global_plan_.begin(), pose);
            }
            setGoal();
            file.reset();
        }

        void read_path_file(std::string path);

        ros::Publisher twist_publisher;
        bool started_ = false;
    };
}

#endif //TANGENT_PLANNER_TANGENT_PLANNER_NODE_H