#pragma once

#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include "mg_msgs/action/move_point.hpp"
#include "mg_msgs/action/move_straight.hpp"
#include "mg_msgs/action/look_at.hpp"
#include "mg_msgs/action/rotate.hpp"

#include "geometry_msgs/msg/twist_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"

#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

#include "mg_obstacles/mg_obstacles.hpp"

#include "path_buffer.hpp"

namespace mg {

    namespace Geometry = geometry_msgs::msg;

    class MgNavigationServer : public rclcpp::Node {
      public:
        RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer)
        using MovePoint    = mg_msgs::action::MovePoint;
        using MoveGlobal   = mg_msgs::action::MoveGlobal;
        using MoveStraight = mg_msgs::action::MoveStraight;
        using LookAt       = mg_msgs::action::LookAt;
        using Rotate       = mg_msgs::action::Rotate;

        rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;

        rclcpp_action::Server<MoveGlobal>::SharedPtr   sv_move_global;
        rclcpp_action::Server<MovePoint>::SharedPtr    sv_move_point;
        rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
        rclcpp_action::Server<LookAt>::SharedPtr       sv_look_at;
        rclcpp_action::Server<Rotate>::SharedPtr       sv_rotate;

        rclcpp::Subscription<Geometry::TransformStamped>::SharedPtr tf2_subscription_;
        tf2_ros::Buffer::SharedPtr                                  tf2_buffer;
        std::shared_ptr<tf2_ros::TransformListener>                 tf2_listener;
        std::mutex                                                  mtx;

        MgNavigationServer(rclcpp::NodeOptions& opts);

        std::shared_ptr<PathBuffer>       path_buffer() { return path_buffer_; }
        std::shared_ptr<ObstacleManager>& obstacle_manager() { return obstacle_manager_; }

      private:
        bool is_processing = false;

        std::shared_ptr<PathBuffer>      path_buffer_;
        std::shared_ptr<ObstacleManager> obstacle_manager_;

        template <typename T>
        rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
                                                typename T::Goal::ConstSharedPtr,
                                                const char*);
        template <typename T>
        rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >,
                                                    const char*);
        template <typename T>
        void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >, const char*);

        template <typename T>
        void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >);
    };

};