// File: 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
movq %rsp, %rbp
subq $0x70, %rsp
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movq %rcx, -0x20(%rbp)
movq %r8, -0x28(%rbp)
movq %r9, -0x30(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x70(%rbp)
movq -0x20(%rbp), %rax
subq -0x18(%rbp), %rax
movq %rax, -0x38(%rbp)
movq -0x18(%rbp), %rax
movq -0x38(%rbp), %rcx
shrq %rcx
addq %rcx, %rax
movq %rax, -0x40(%rbp)
movq -0x30(%rbp), %rdi
callq 0x5810
movq -0x70(%rbp), %rdi
movq %rax, %rsi
movq -0x40(%rbp), %rdx
callq 0x57a0
movq -0x30(%rbp), %rdi
callq 0x5830
testb $0x1, %al
jne 0x552e
jmp 0x5559
movq -0x30(%rbp), %rdi
callq 0x5850
movq -0x70(%rbp), %rdi
movq -0x10(%rbp), %rsi
movq -0x18(%rbp), %rdx
movq -0x20(%rbp), %rcx
movq -0x28(%rbp), %r8
movq -0x30(%rbp), %r9
callq 0x5880
jmp 0x572d
movq -0x30(%rbp), %rdi
movq -0x40(%rbp), %rsi
callq 0x5aa0
cmpq $0x2, -0x38(%rbp)
jne 0x55ba
movq -0x30(%rbp), %rdi
callq 0x5810
movq -0x70(%rbp), %rdi
movq %rax, %rsi
movq -0x40(%rbp), %rdx
subq $0x1, %rdx
callq 0x57a0
movq -0x30(%rbp), %rdi
callq 0x5830
testb $0x1, %al
jne 0x5599
jmp 0x55a8
movq -0x30(%rbp), %rdi
movq -0x40(%rbp), %rsi
callq 0x5b70
jmp 0x55b5
movq -0x30(%rbp), %rdi
movq -0x40(%rbp), %rsi
callq 0x5aa0
jmp 0x572d
cmpq $0x1, -0x38(%rbp)
jne 0x55c6
jmp 0x572d
movq -0x70(%rbp), %rdx
movq -0x10(%rbp), %rax
movq -0x28(%rbp), %rcx
movss (%rax,%rcx,4), %xmm0
movss %xmm0, -0x44(%rbp)
movq 0x8(%rdx), %rax
movq -0x28(%rbp), %rcx
movq (%rax,%rcx,8), %rax
movq -0x40(%rbp), %rcx
movss (%rax,%rcx,4), %xmm0
movss %xmm0, -0x48(%rbp)
movss -0x48(%rbp), %xmm0
subss -0x44(%rbp), %xmm0
movss %xmm0, -0x4c(%rbp)
movss -0x4c(%rbp), %xmm0
mulss -0x4c(%rbp), %xmm0
movss %xmm0, -0x50(%rbp)
movq -0x28(%rbp), %rax
addq $0x1, %rax
movq -0x28(%rbp), %rcx
movq 0x18(%rdx), %rdx
subq $0x1, %rdx
cmpq %rdx, %rcx
sete %cl
andb $0x1, %cl
movzbl %cl, %ecx
addl $0xffffffff, %ecx       # imm = 0xFFFFFFFF
movslq %ecx, %rcx
andq %rcx, %rax
movl %eax, -0x54(%rbp)
movss -0x44(%rbp), %xmm1
movss -0x48(%rbp), %xmm0
ucomiss %xmm1, %xmm0
jbe 0x56c2
movq -0x70(%rbp), %rdi
movq -0x10(%rbp), %rsi
movq -0x18(%rbp), %rdx
movq -0x40(%rbp), %rcx
movl -0x54(%rbp), %eax
movl %eax, %r8d
movq -0x30(%rbp), %r9
callq 0x54c0
movq -0x30(%rbp), %rdi
callq 0x5be0
movsd 0x8(%rax), %xmm0
movsd %xmm0, -0x60(%rbp)
movsd -0x60(%rbp), %xmm1
movss -0x50(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x569d
jmp 0x572d
movq -0x70(%rbp), %rdi
movq -0x10(%rbp), %rsi
movq -0x40(%rbp), %rdx
addq $0x1, %rdx
movq -0x20(%rbp), %rcx
movl -0x54(%rbp), %eax
movl %eax, %r8d
movq -0x30(%rbp), %r9
callq 0x54c0
jmp 0x572d
movq -0x70(%rbp), %rdi
movq -0x10(%rbp), %rsi
movq -0x40(%rbp), %rdx
addq $0x1, %rdx
movq -0x20(%rbp), %rcx
movl -0x54(%rbp), %eax
movl %eax, %r8d
movq -0x30(%rbp), %r9
callq 0x54c0
movq -0x30(%rbp), %rdi
callq 0x5be0
movsd 0x8(%rax), %xmm0
movsd %xmm0, -0x68(%rbp)
movsd -0x68(%rbp), %xmm1
movss -0x50(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x570e
jmp 0x572d
movq -0x70(%rbp), %rdi
movq -0x10(%rbp), %rsi
movq -0x18(%rbp), %rdx
movq -0x40(%rbp), %rcx
movl -0x54(%rbp), %eax
movl %eax, %r8d
movq -0x30(%rbp), %r9
callq 0x54c0
addq $0x70, %rsp
popq %rbp
retq
nopw %cs:(%rax,%rax)
nopl (%rax)