// File: /workspace/github_cpp_makefile_star2_O2/cfiutak1[P]HPyC-KD-Tree/./utils/filewriter/../../src/kdtree/KDTree.hpp
// Function: void hpyc::KDTree<float>::nearestNeighborsSearch<true>(float const*, unsigned long, unsigned long, unsigned long, hpyc::KNNQueue<float>&) const
void KDTree<ItemT>::nearestNeighborsSearch(const ItemT* query_point, std::size_t begin, std::size_t end, std::size_t depth, KNNQueue<ItemT>& nearest_neighbors) const {
    std::size_t range = end - begin;
    std::size_t traverser_index = begin + (range / 2);

    this->readPointAt(nearest_neighbors.getPotentialNeighbor(), traverser_index);

    // Optimization: This templatization is done to prevent unnecessarily checking if the KNN queue is full. Once
    // the queue is full, it will never be not full again, so we can cut down on some work by only checking the fullness
    // until it becomes full.
    // TODO - Can probably cut down on code reuse by making this template argument a function pointer instead of a boolean.
    if constexpr (CheckIfFull) {
        if (nearest_neighbors.full()) {
            nearest_neighbors.heapify();
            return this->nearestNeighborsSearch<false>(query_point, begin, end, depth, nearest_neighbors);
        }

        nearest_neighbors.registerAsNeighbor(traverser_index);

        // Base case - If there's two elements left, the 2nd element has already been tested. Thus, we simply register the
        // 1st element and stop recursing.
        if (range == 2) {
            this->readPointAt(nearest_neighbors.getPotentialNeighbor(), traverser_index - 1);

            if (nearest_neighbors.full()) {
                nearest_neighbors.registerAsNeighborIfCloser(traverser_index);
            }

            else {
                nearest_neighbors.registerAsNeighbor(traverser_index);
            }

            return;
        }
    }

    else {
        nearest_neighbors.registerAsNeighborIfCloser(traverser_index);

        // Base case - If there's two elements left, the 2nd element has already been tested. Thus, we simply register the
        // 1st element and stop recursing.
        if (range == 2) {
            this->readPointAt(nearest_neighbors.getPotentialNeighbor(), traverser_index - 1);

            nearest_neighbors.registerAsNeighborIfCloser(traverser_index);
            return;
        }
    }

    // Base case - If there's one element left, it has already been tested so stop recursing.
    if (range == 1) { return; }

    // TODO - These used to be floats. If there's very very small distances, this might behave differently if ItemT is a double instead.
    ItemT query_at_current_dimension = query_point[depth];
    ItemT traverser_at_current_dimension = this->nodes[depth][traverser_index];
    ItemT difference_at_current_dimension = traverser_at_current_dimension - query_at_current_dimension;
    ItemT distance_from_query_at_current_dimension = difference_at_current_dimension * difference_at_current_dimension;

    unsigned int new_depth = (depth + 1) & (-1 + (depth == this->num_dimensions - 1));

    // If the query is less than the traverser at the current dimension,
    //     1. Traverse down the left subtree.
    //     2. After fully traversing the left subtree, determine if the right subtree can possibly have a closer neighbor.
    //     3. If the right subtree is not viable, return. Otherwise, traverse the right subtree.
    if (query_at_current_dimension < traverser_at_current_dimension) {
        this->nearestNeighborsSearch<CheckIfFull>(query_point, begin, traverser_index, new_depth, nearest_neighbors);

        double farthest_neighbor_distance = nearest_neighbors.top().distance_from_queried_point;

        if (farthest_neighbor_distance < distance_from_query_at_current_dimension) { return; }

        this->nearestNeighborsSearch<CheckIfFull>(query_point, traverser_index + 1, end, new_depth, nearest_neighbors);
    }

    // If the query is greater than or equal to the traverser at the current dimension,
    //     1. Traverse down the right subtree.
    //     2. After fully traversing the right subtree, determine if the left subtree can possibly have a closer neighbor.
    //     3. If the left subtree is not viable, return. Otherwise, traverse the left subtree.
    else {
        this->nearestNeighborsSearch<CheckIfFull>(query_point, traverser_index + 1, end, new_depth, nearest_neighbors);

        double farthest_neighbor_distance = nearest_neighbors.top().distance_from_queried_point;

        if (farthest_neighbor_distance < distance_from_query_at_current_dimension) { return; }

        this->nearestNeighborsSearch<CheckIfFull>(query_point, begin, traverser_index, new_depth, nearest_neighbors);
    }
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x18, %rsp
movq %r9, %rbx
movq %r8, %r12
movq %rcx, %rbp
movq %rdx, 0x8(%rsp)
movq %rsi, 0x10(%rsp)
movq %rdi, %r14
movq %rbp, %r15
movq 0x8(%rsp), %rax
subq %rax, %r15
movq %r15, %r13
shrq %r13
addq %rax, %r13
movq 0x8(%rbx), %rsi
movq %r14, %rdi
movq %r13, %rdx
callq 0x3d5e
movq 0x20(%rbx), %rax
cmpq 0x10(%rbx), %rax
je 0x3cbf
movq %rbx, %rdi
movq %r13, %rsi
callq 0x3edc
cmpq $0x1, %r15
je 0x3cb0
cmpq $0x2, %r15
je 0x3cf0
movq 0x10(%rsp), %rsi
movss (%rsi,%r12,4), %xmm0
movq 0x8(%r14), %rax
movq 0x18(%r14), %rcx
movq (%rax,%r12,8), %rax
movss (%rax,%r13,4), %xmm1
decq %rcx
leal 0x1(%r12), %eax
cmpq %r12, %rcx
movl $0x0, %ecx
cmovel %ecx, %eax
ucomiss %xmm0, %xmm1
subss %xmm0, %xmm1
mulss %xmm1, %xmm1
movss %xmm1, 0x4(%rsp)
movl %eax, %r12d
ja 0x3c7d
leaq 0x1(%r13), %rdx
movq %r14, %rdi
movq %rbp, %rcx
movq %r12, %r8
movq %rbx, %r9
callq 0x3b94
movq 0x28(%rbx), %rax
cvtss2sd 0x4(%rsp), %xmm0
ucomisd 0x8(%rax), %xmm0
movq %r13, %rbp
jbe 0x3bb8
jmp 0x3cb0
movq %r14, %rdi
movq 0x8(%rsp), %rdx
movq %r13, %rcx
movq %r12, %r8
movq %rbx, %r9
callq 0x3b94
movq 0x28(%rbx), %rax
cvtss2sd 0x4(%rsp), %xmm0
incq %r13
ucomisd 0x8(%rax), %xmm0
movq %r13, 0x8(%rsp)
jbe 0x3bb8
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rbx, %rdi
callq 0x3d82
movq %r14, %rdi
movq 0x10(%rsp), %rsi
movq 0x8(%rsp), %rdx
movq %rbp, %rcx
movq %r12, %r8
movq %rbx, %r9
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
jmp 0x3d96
movq 0x8(%rbx), %rsi
leaq -0x1(%r13), %rdx
movq %r14, %rdi
callq 0x3d5e
movq 0x20(%rbx), %rax
cmpq 0x10(%rbx), %rax
jne 0x3d23
movq %rbx, %rdi
movq %r13, %rsi
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
jmp 0x3f44
movq %rbx, %rdi
movq %r13, %rsi
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
jmp 0x3edc