// File: /workspace/github_c_makefile_star2_O3/zpole[P]IICV/vl/kdtree.c
// Function: vl_kdforest_query_recursively
vl_uindex
vl_kdforest_query_recursively (VlKDForestSearcher * searcher,
                               VlKDTree * tree,
                               vl_uindex nodeIndex,
                               VlKDForestNeighbor * neighbors,
                               vl_size numNeighbors,
                               vl_size * numAddedNeighbors,
                               double dist,
                               void const * query)
{

  VlKDTreeNode const * node = tree->nodes + nodeIndex ;
  vl_uindex i = node->splitDimension ;
  vl_index nextChild, saveChild ;
  double delta, saveDist ;
  double x ;
  double x1 = node->lowerBound ;
  double x2 = node->splitThreshold ;
  double x3 = node->upperBound ;
  VlKDForestSearchState * searchState ;

  searcher->searchNumRecursions ++ ;

  switch (searcher->forest->dataType) {
    case VL_TYPE_FLOAT :
      x = ((float const*) query)[i] ;
      break ;
    case VL_TYPE_DOUBLE :
      x = ((double const*) query)[i] ;
      break ;
    default :
      abort() ;
  }

  /* base case: this is a leaf node */
  if (node->lowerChild < 0) {

    vl_index begin = - node->lowerChild - 1 ;
    vl_index end   = - node->upperChild - 1 ;
    vl_index iter ;

    for (iter = begin ;
         iter < end &&
         (searcher->forest->searchMaxNumComparisons == 0 ||
          searcher->searchNumComparisons < searcher->forest->searchMaxNumComparisons) ;
         ++ iter) {

      vl_index di = tree->dataIndex [iter].index ;

      /* multiple KDTrees share the database points and we must avoid
       * adding the same point twice */
      if (searcher->searchIdBook[di] == searcher->searchId) continue ;
      searcher->searchIdBook[di] = searcher->searchId ;

      /* compare the query to this point */
      switch (searcher->forest->dataType) {
        case VL_TYPE_FLOAT:
          dist = ((VlFloatVectorComparisonFunction)searcher->forest->distanceFunction)
                 (searcher->forest->dimension,
                  ((float const *)query),
                  ((float const*)searcher->forest->data) + di * searcher->forest->dimension) ;
          break ;
        case VL_TYPE_DOUBLE:
          dist = ((VlDoubleVectorComparisonFunction)searcher->forest->distanceFunction)
                 (searcher->forest->dimension,
                  ((double const *)query),
                  ((double const*)searcher->forest->data) + di * searcher->forest->dimension) ;
          break ;
        default:
          abort() ;
      }
      searcher->searchNumComparisons += 1 ;

      /* see if it should be added to the result set */
      if (*numAddedNeighbors < numNeighbors) {
        VlKDForestNeighbor * newNeighbor = neighbors + *numAddedNeighbors ;
        newNeighbor->index = di ;
        newNeighbor->distance = dist ;
        vl_kdforest_neighbor_heap_push (neighbors, numAddedNeighbors) ;
      } else {
        VlKDForestNeighbor * largestNeighbor = neighbors + 0 ;
        if (largestNeighbor->distance > dist) {
          largestNeighbor->index = di ;
          largestNeighbor->distance = dist ;
          vl_kdforest_neighbor_heap_update (neighbors, *numAddedNeighbors, 0) ;
        }
      }
    } /* next data point */


    return nodeIndex ;
  }

#if 0
  assert (x1 <= x2 && x2 <= x3) ;
  assert (node->lowerChild >= 0) ;
  assert (node->upperChild >= 0) ;
#endif

  /*
   *   x1  x2 x3
   * x (---|---]
   *   (--x|---]
   *   (---|x--]
   *   (---|---] x
   */

  delta = x - x2 ;
  saveDist = dist + delta*delta ;

  if (x <= x2) {
    nextChild = node->lowerChild ;
    saveChild = node->upperChild ;
    if (x <= x1) {
      delta = x - x1 ;
      saveDist -= delta*delta ;
    }
  } else {
    nextChild = node->upperChild ;
    saveChild = node->lowerChild ;
    if (x > x3) {
      delta = x - x3 ;
      saveDist -= delta*delta ;
    }
  }

  if (*numAddedNeighbors < numNeighbors || neighbors[0].distance > saveDist) {
    searchState = searcher->searchHeapArray + searcher->searchHeapNumNodes ;
    searchState->tree = tree ;
    searchState->nodeIndex = saveChild ;
    searchState->distanceLowerBound = saveDist ;
    vl_kdforest_search_heap_push (searcher->searchHeapArray ,
                                  &searcher->searchHeapNumNodes) ;
  }

  return vl_kdforest_query_recursively (searcher,
                                        tree,
                                        nextChild,
                                        neighbors,
                                        numNeighbors,
                                        numAddedNeighbors,
                                        dist,
                                        query) ;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movq %rcx, %r15
movq %rdx, %r14
movq %rsi, %r12
movq %rdi, %r13
movq 0x70(%rsp), %rsi
movq (%r12), %rax
imulq $0x38, %r14, %rcx
movl 0x18(%rax,%rcx), %edx
movsd 0x28(%rax,%rcx), %xmm2
movsd 0x20(%rax,%rcx), %xmm5
movsd 0x30(%rax,%rcx), %xmm3
incq 0x30(%r13)
movq 0x20(%r13), %rdi
movl 0x10(%rdi), %edi
cmpl $0x2, %edi
je 0x1d978
cmpl $0x1, %edi
jne 0x1dcdb
xorps %xmm4, %xmm4
cvtss2sd (%rsi,%rdx,4), %xmm4
jmp 0x1d97d
movsd (%rsi,%rdx,8), %xmm4
addq %rcx, %rax
movq 0x8(%rax), %rbp
testq %rbp, %rbp
js 0x1da85
movaps %xmm4, %xmm1
subsd %xmm5, %xmm1
mulsd %xmm1, %xmm1
addsd %xmm0, %xmm1
ucomisd %xmm4, %xmm5
movq 0x10(%rax), %r14
jae 0x1d9bd
ucomisd %xmm3, %xmm4
jbe 0x1d9b8
subsd %xmm3, %xmm4
mulsd %xmm4, %xmm4
subsd %xmm4, %xmm1
movq %rbp, %rcx
jmp 0x1d9d5
ucomisd %xmm4, %xmm2
jb 0x1d9cf
subsd %xmm2, %xmm4
mulsd %xmm4, %xmm4
subsd %xmm4, %xmm1
movq %r14, %rcx
movq %rbp, %r14
cmpq %r8, (%r9)
jb 0x1d9e9
movsd (%r15), %xmm2
ucomisd %xmm1, %xmm2
jbe 0x1d937
movq 0x18(%r13), %rax
movq 0x40(%r13), %rdx
leaq (%rdx,%rdx,2), %rdx
movq %r12, (%rax,%rdx,8)
movq %rcx, 0x8(%rax,%rdx,8)
movsd %xmm1, 0x10(%rax,%rdx,8)
movq 0x40(%r13), %rcx
testq %rcx, %rcx
je 0x1da77
leaq -0x1(%rcx), %rdx
leaq (%rcx,%rcx,2), %rdi
movq %rdx, %rcx
shrq %rcx
leaq (%rcx,%rcx,2), %r11
movsd 0x10(%rax,%r11,8), %xmm1
ucomisd 0x10(%rax,%rdi,8), %xmm1
jbe 0x1da6e
movq 0x10(%rax,%rdi,8), %r10
movq %r10, 0x10(%rsp)
movups (%rax,%rdi,8), %xmm1
movaps %xmm1, (%rsp)
movq 0x10(%rax,%r11,8), %r10
movq %r10, 0x10(%rax,%rdi,8)
movups (%rax,%r11,8), %xmm1
movups %xmm1, (%rax,%rdi,8)
movq 0x10(%rsp), %rdi
movq %rdi, 0x10(%rax,%r11,8)
movapd (%rsp), %xmm1
movupd %xmm1, (%rax,%r11,8)
cmpq $0x1, %rdx
ja 0x1da0d
movq 0x40(%r13), %rax
incq %rax
jmp 0x1da7c
movl $0x1, %eax
movq %rax, 0x40(%r13)
jmp 0x1d937
movq 0x10(%rax), %r11
cmpq %r11, %rbp
jle 0x1dcc9
notq %r11
notq %rbp
movq %r9, 0x30(%rsp)
movq %r8, 0x28(%rsp)
movq %r11, 0x20(%rsp)
movq 0x20(%r13), %rax
movq 0xe0(%rax), %rcx
testq %rcx, %rcx
je 0x1dac1
cmpq %rcx, 0x28(%r13)
jae 0x1dcc9
movq 0x18(%r12), %rcx
movq %rbp, %rdx
shlq $0x4, %rdx
movq (%rcx,%rdx), %rbx
movq 0x10(%r13), %rcx
movq 0x48(%r13), %rdx
cmpq %rdx, (%rcx,%rbx,8)
je 0x1dc89
movq %rdx, (%rcx,%rbx,8)
movl 0x10(%rax), %ecx
cmpl $0x2, %ecx
je 0x1db13
cmpl $0x1, %ecx
jne 0x1dcdb
movq (%rax), %rdi
movq %rdi, %rdx
imulq %rbx, %rdx
shlq $0x2, %rdx
addq 0x18(%rax), %rdx
callq *0x30(%rax)
cvtss2sd %xmm0, %xmm0
jmp 0x1db28
movq (%rax), %rdi
movq %rdi, %rdx
imulq %rbx, %rdx
shlq $0x3, %rdx
addq 0x18(%rax), %rdx
callq *0x30(%rax)
incq 0x28(%r13)
movq 0x30(%rsp), %rdi
movq (%rdi), %rax
cmpq 0x28(%rsp), %rax
movq 0x20(%rsp), %r11
jae 0x1dbb5
shlq $0x4, %rax
movq %rbx, 0x8(%r15,%rax)
movsd %xmm0, (%r15,%rax)
movq (%rdi), %rax
testq %rax, %rax
movq 0x70(%rsp), %rsi
je 0x1dc81
movq %rax, %rdx
leaq -0x1(%rax), %rcx
movq %rcx, %rax
shrq %rax
movq %rax, %r8
shlq $0x4, %r8
shlq $0x4, %rdx
movsd (%r15,%rdx), %xmm0
ucomisd (%r15,%r8), %xmm0
jbe 0x1dbaa
addq %r15, %r8
addq %r15, %rdx
movups (%rdx), %xmm0
movaps %xmm0, (%rsp)
movups (%r8), %xmm0
movups %xmm0, (%rdx)
movapd (%rsp), %xmm0
movupd %xmm0, (%r8)
cmpq $0x1, %rcx
ja 0x1db60
movq (%rdi), %rax
incq %rax
jmp 0x1dc86
movsd (%r15), %xmm1
ucomisd %xmm0, %xmm1
movq 0x70(%rsp), %rsi
jbe 0x1dc89
movq %rbx, 0x8(%r15)
movsd %xmm0, (%r15)
movq (%rdi), %rax
cmpq $0x2, %rax
jb 0x1dc89
movl $0x1, %ecx
xorl %r10d, %r10d
xorl %edx, %edx
addq $0x2, %r10
cmpq %rax, %r10
jae 0x1dc97
movq %r10, %r8
shlq $0x4, %r8
movsd (%r15,%r8), %xmm0
movq %rcx, %r9
shlq $0x4, %r9
movsd (%r15,%r9), %xmm1
shlq $0x4, %rdx
leaq (%r15,%rdx), %rdi
ucomisd %xmm0, %xmm1
movsd (%r15,%rdx), %xmm2
jbe 0x1dc45
ucomisd %xmm2, %xmm1
jbe 0x1dc89
addq %r15, %r9
movups (%rdi), %xmm0
movaps %xmm0, (%rsp)
movups (%r9), %xmm0
movups %xmm0, (%rdi)
movaps (%rsp), %xmm0
movups %xmm0, (%r9)
jmp 0x1dc67
ucomisd %xmm2, %xmm0
jbe 0x1dc89
addq %r15, %r8
movups (%rdi), %xmm0
movaps %xmm0, (%rsp)
movups (%r8), %xmm0
movups %xmm0, (%rdi)
movaps (%rsp), %xmm0
movups %xmm0, (%r8)
movq %r10, %rcx
movq %rcx, %rdx
leaq (%rcx,%rcx), %r10
leaq 0x1(,%rcx,2), %rcx
cmpq %rax, %rcx
jb 0x1dbe9
jmp 0x1dc89
movl $0x1, %eax
movq %rax, (%rdi)
incq %rbp
cmpq %r11, %rbp
jl 0x1daa7
jmp 0x1dcc9
shlq $0x4, %rcx
movsd (%r15,%rcx), %xmm0
shlq $0x4, %rdx
ucomisd (%r15,%rdx), %xmm0
jbe 0x1dc89
addq %r15, %rcx
addq %r15, %rdx
movups (%rdx), %xmm0
movaps %xmm0, (%rsp)
movups (%rcx), %xmm0
movups %xmm0, (%rdx)
movaps (%rsp), %xmm0
movups %xmm0, (%rcx)
jmp 0x1dc89
movq %r14, %rax
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
callq 0x90b0