#include "path_buffer.hpp"

#include <glm/gtx/norm.hpp>

#include <chrono>
using namespace std::chrono_literals;

namespace mg {

    PathBuffer::PathBuffer(rclcpp::Node* node) : current(-1), node_(node) {
        client_ = node_->create_client<PathService>("GeneratePath");
    }

    glm::vec2 PathBuffer::get_next(glm::vec2 pos, const double lookahead) {
        if (current < 0) {
            current++;
            while (current < (int)path_msg_->indicies.size()
                   && glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) > lookahead * lookahead) {
                current++;
            }
        }
        while (current < (int)path_msg_->indicies.size()
               && glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) < lookahead * lookahead) {
            current++;
        }
        if (current == (int)path_msg_->indicies.size()) {
            double best = 10000000;
            int    id   = 0;

            for (int i = 0; i < (int)goal_->x.size(); i++) {
                const double dist2 = glm::distance2({ goal_->x[i], goal_->y[i] }, pos);
                if (dist2 < best) {
                    best = dist2;
                    id   = i;
                }
            }
            return { goal_->x[id], goal_->y[id] };
        }
        return IdToCoords(path_msg_->indicies[current]);
    }

    bool PathBuffer::update_path(PathGoal goal) {
        if (goal) {
            goal_ = goal;
            client_->prune_pending_requests();
            auto req = std::make_shared<PathService::Request>();
            req->x   = goal_->x;
            req->y   = goal_->y;
            resp_    = client_->async_send_request(req).share();
        }
        if (resp_.wait_for(0s) == std::future_status::ready) {

            path_msg_ = resp_.get();
            auto req  = std::make_shared<PathService::Request>();
            req->x    = goal_->x;
            req->y    = goal_->y;
            resp_     = client_->async_send_request(req).share();
            current   = -1;
            return true;
        }
        return false;
    }

    void PathBuffer::update_path_block(PathGoal goal) {
        if (goal) {
            goal_ = goal;
            client_->prune_pending_requests();
            auto req = std::make_shared<PathService::Request>();
            req->x   = goal_->x;
            req->y   = goal_->y;
            resp_    = client_->async_send_request(req).share();
        }

        resp_.wait();
        path_msg_ = resp_.get();
        auto req  = std::make_shared<PathService::Request>();
        req->x    = goal_->x;
        req->y    = goal_->y;
        resp_     = client_->async_send_request(req).share();
        current   = -1;
    }
}