#include "nearest_iterator.h"
#include "rtree.h"

#include <maps/libs/common/include/exception.h>

namespace maps::mrc::fb_rtree::impl {

NearestIterator::NearestIterator(const Rtree* rtree,
                                 const geolib3::Point2& point,
                                 DistanceToBox distanceToBox,
                                 DistanceToId distanceToId)
    : rtree_(rtree)
    , point_(point)
    , distanceToBox_(distanceToBox)
    , distanceToId_(distanceToId)
{
    onNewNode(rtree_->root());
    findNextResult();
}

bool NearestIterator::equal(const NearestIterator& other) const
{
    return queue_.empty() && other.queue_.empty();
}

Id NearestIterator::dereference() const
{
    ASSERT(hasObject());
    return std::get<Id>(queue_.top().object);
}

void NearestIterator::increment()
{
    ASSERT(hasObject());
    queue_.pop();
    findNextResult();
}

bool NearestIterator::hasObject() const
{
    return !queue_.empty() && std::get_if<Id>(&queue_.top().object);
}

void NearestIterator::findNextResult()
{
    while (!queue_.empty() && !hasObject()) {
        const auto entry = queue_.top();
        queue_.pop();
        const auto node = std::get<Node>(entry.object);
        if (const auto id = node.id) {
            queue_.push({{*id}, distanceToId_(point_, *id)});
        }
        else {
            rtree_->forEachDirectChild(
                node, [&](const Node& node) { onNewNode(node); });
        }
    }
}

void NearestIterator::onNewNode(const Node& node)
{
    queue_.push({{node}, distanceToBox_(point_, node.bbox)});
}

}  // namespace maps::mrc::fb_rtree::impl
