#pragma once

#include <vector>
#include <mutex>
#include <thread>
#include <glm/glm.hpp>

#include "mg_msgs/srv/calc_path.hpp"
#include "mg_msgs/action/move_global.hpp"
#include "rclcpp/rclcpp.hpp"

#define AREAX  3000
#define AREAY  2000
#define GRID_Y 66
#define GRID_X 106

#define MIN_X 175
#define MIN_Y 175

#define MAX_X 2825
#define MAX_Y 1825

#define GRID_X_SIZE ((MAX_X - MIN_X) / GRID_X)
#define GRID_Y_SIZE ((MAX_Y - MIN_Y) / GRID_Y)

namespace mg {
    inline glm::vec2 GridToCoords(const glm::ivec2 grid) {
        return glm::vec2{ grid.x, grid.y } * glm::vec2{ GRID_X_SIZE, GRID_Y_SIZE } + glm::vec2{ MIN_X, MIN_Y };
    }
    inline glm::ivec2 IdToGrid(const int id) { return { id % GRID_X, id / GRID_X }; }
    inline glm::vec2  IdToCoords(const int id) { return GridToCoords(IdToGrid(id)) / 1000.0F; }

    class PathBuffer {
        using PathService = mg_msgs::srv::CalcPath;
        using PathGoal    = mg_msgs::action::MoveGlobal::Goal::ConstSharedPtr;
        using PathFuture  = rclcpp::Client<PathService>::SharedFuture;

      public:
        PathBuffer(rclcpp::Node* node);
        // ~PathBuffer();

        glm::vec2 get_next(glm::vec2 pos, const double lookahead);

        bool update_path(PathGoal goal = nullptr);
        void update_path_block(PathGoal goal = nullptr);

      private:
        int current;

        rclcpp::Node* node_;

        PathGoal                              goal_;
        PathService::Response::ConstSharedPtr path_msg_;

        rclcpp::Client<PathService>::Client::SharedPtr client_;

        PathFuture resp_;
    };

}