// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/R3Utils/R3SurfelViewer.cpp
// Function: gaps::R3SurfelViewer::PickNode(int, int, gaps::R3Point*, gaps::R3SurfelBlock**, int*, int)
R3SurfelNode *R3SurfelViewer::
PickNode(int x, int y, R3Point *picked_position, 
  R3SurfelBlock **picked_block, int *picked_surfel_index,
  RNBoolean exclude_nonobjects) 
{
  // How close the cursor has to be to a point (in pixels)
  int pick_tolerance = 0.005 * Viewport().Width() + 1;

  // Clear window 
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  // Set viewing transformation
  viewer.Camera().Load();

  // Set viewing extent
  EnableViewingExtent();

  // Set OpenGL stuff
  glPointSize(pick_tolerance);    

  // Draw surfels
  DrawSurfels(R3_SURFEL_VIEWER_COLOR_BY_PICK_INDEX);

  // Reset OpenGL stuff
  glPointSize(1);
  glFinish();

  // Reset viewing modes
  DisableViewingExtent();

  // Read color buffer at cursor position
  unsigned char rgba[4];
  glReadPixels(x, y, 1, 1, GL_RGBA, GL_UNSIGNED_BYTE, rgba);
  if (rgba[3] == 0) return NULL;

  // Determine node index
  int r = rgba[0] & 0xFF;
  int g = rgba[1] & 0xFF;
  int b = rgba[2] & 0xFF;
  // int a = rgba[3] & 0xFF;
  // if (a != 0xFE) return NULL;
  int node_index = (r << 16) | (g << 8) | b;
  if (node_index == 0) return NULL;
  node_index--;

  // Determine node
  R3SurfelTree *tree = scene->Tree();
  if (!tree) return NULL;
  if (node_index < 0) return NULL;
  if (node_index >= tree->NNodes()) return NULL;
  R3SurfelNode *hit_node = tree->Node(node_index);

  // Find node part of an object
  R3SurfelNode *picked_node = hit_node;
  if (exclude_nonobjects) {
    // Find node associated with object
    picked_node = NULL;

    // Check if hit node is part of an object
    if (hit_node->Object()) {
      picked_node = hit_node;
    }

    // Check if hit node has ancestor that is part of an object
    if (picked_node == NULL) {
      R3SurfelNode *ancestor = hit_node->Parent();
      while (ancestor) {
        if (ancestor->Object()) { picked_node = ancestor; break; }
        ancestor = ancestor->Parent();
      }
    }
    
    // Check if hit node has descendent that is part of an object
    if (picked_node == NULL) {
      R3Ray ray = viewer.WorldRay(x, y);
      RNScalar t, picked_t = FLT_MAX;
      RNArray<R3SurfelNode *> stack;
      stack.Insert(hit_node);
      while (!stack.IsEmpty()) {
        R3SurfelNode *node = stack.Tail();
        stack.RemoveTail();
        for (int i = 0; i < node->NParts(); i++) {
          stack.Insert(node->Part(i));
        }
        if (node->Object()) {
          if (R3Intersects(ray, node->BBox(), NULL, NULL, &t)) {
            if (t < picked_t) {
              picked_node = node;
              picked_t = t;
            }
          }
        }
      }
    }
  }
    
  // Find hit position
  GLfloat depth;
  GLdouble p[3];
  GLint viewport[4];
  GLdouble modelview_matrix[16];
  GLdouble projection_matrix[16];
  glGetIntegerv(GL_VIEWPORT, viewport);
  glGetDoublev(GL_MODELVIEW_MATRIX, modelview_matrix);
  glGetDoublev(GL_PROJECTION_MATRIX, projection_matrix);
  glReadPixels(x, y, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &depth);
  gluUnProject(x, y, depth, modelview_matrix, projection_matrix, viewport, &(p[0]), &(p[1]), &(p[2]));
  R3Point position(p[0], p[1], p[2]);
  if (picked_position) *picked_position = position;

  // Find hit surfel
  if (picked_block || picked_surfel_index) {
    // Create pointset in vicinity of picked position
    R3Point position(p[0], p[1], p[2]);
    RNScalar radius = 0.1 * R3Distance(position, viewer.Camera().Origin());
    R3SurfelSphereConstraint sphere_constraint(R3Sphere(position, radius));
    R3SurfelPointSet *pointset = CreatePointSet(scene, hit_node, &sphere_constraint);
    if (pointset) {
      // Find surfel point closest to picked position
      R3SurfelPoint *closest_point = NULL;
      RNLength closest_distance = FLT_MAX;
      for (int i = 0; i < pointset->NPoints(); i++) {
        R3SurfelPoint *point = pointset->Point(i);
        RNLength distance = R3SquaredDistance(point->Position(), position);
        if (distance < closest_distance) {
          closest_distance = distance;
          closest_point = point;
        }
      }

      // Return closest point
      if (closest_point) {
        if (picked_position) *picked_position = closest_point->Position();
        if (picked_block) *picked_block = closest_point->Block();
        if (picked_surfel_index) *picked_surfel_index = closest_point->BlockIndex();
      }

      // Delete point set
      delete pointset;
    }
  }

  // Return picked node
  return picked_node;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x238, %rsp            # imm = 0x238
movq %r9, 0x68(%rsp)
movq %r8, 0x60(%rsp)
movq %rcx, 0x20(%rsp)
movl %edx, %r14d
movl %esi, %r15d
movq %rdi, %r13
cvtsi2sdl 0x118(%rdi), %xmm0
mulsd 0x1053b3(%rip), %xmm0   # 0x138440
addsd 0x104f73(%rip), %xmm0   # 0x138008
movapd %xmm0, 0x30(%rsp)
movl $0x4100, %edi           # imm = 0x4100
callq 0x271f0
leaq 0x80(%r13), %rdi
xorl %ebp, %ebp
movq %rdi, 0x10(%rsp)
xorl %esi, %esi
callq 0x7a9a0
movq (%r13), %rax
movq %r13, %rdi
callq *0x230(%rax)
cvttpd2dq 0x30(%rsp), %xmm0
cvtdq2ps %xmm0, %xmm0
callq 0x270a0
movq (%r13), %rax
movq %r13, %rdi
movl $0xe, %esi
callq *0x270(%rax)
movss 0x104f51(%rip), %xmm0   # 0x138040
callq 0x270a0
callq 0x28370
movq (%r13), %rax
movq %r13, %rdi
callq *0x238(%rax)
leaq 0xc(%rsp), %rbx
movq %rbx, (%rsp)
movl %r15d, %r12d
movl %r15d, %edi
movl %r14d, 0x30(%rsp)
movl %r14d, %esi
movl $0x1, %edx
movl $0x1, %ecx
movl $0x1908, %r8d           # imm = 0x1908
movl $0x1401, %r9d           # imm = 0x1401
callq 0x273d0
cmpb $0x0, 0x3(%rbx)
je 0x3316d
movzbl 0xc(%rsp), %ecx
movzbl 0xd(%rsp), %eax
movzbl 0xe(%rsp), %edx
shll $0x10, %ecx
shll $0x8, %eax
orl %ecx, %eax
orl %edx, %eax
je 0x3316b
movq 0x8(%r13), %rcx
movq 0x8(%rcx), %rcx
testq %rcx, %rcx
je 0x3316b
cmpl 0x24(%rcx), %eax
jle 0x33182
xorl %ebp, %ebp
movq %rbp, %rax
addq $0x238, %rsp            # imm = 0x238
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x18(%rcx), %rcx
movl %eax, %eax
cmpl $0x0, 0x270(%rsp)
movl %r12d, 0x2c(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %r12d, %xmm0
movsd %xmm0, 0x58(%rsp)
movq -0x8(%rcx,%rax,8), %rbp
xorps %xmm0, %xmm0
cvtsi2sdl 0x30(%rsp), %xmm0
movsd %xmm0, 0x50(%rsp)
movq %rbp, 0x18(%rsp)
je 0x33328
movq 0x18(%rsp), %rbp
movq %rbp, %rdi
xorl %esi, %esi
xorl %edx, %edx
callq 0x401d4
testq %rax, %rax
jne 0x33328
leaq 0x78(%r13), %r15
movq 0x18(%rsp), %rbp
movq 0x28(%rbp), %rbp
testq %rbp, %rbp
je 0x33204
movq %rbp, %rdi
xorl %esi, %esi
xorl %edx, %edx
callq 0x401d4
testq %rax, %rax
je 0x331e5
jmp 0x33328
leaq 0x1b0(%rsp), %rdi
movq %r15, %rsi
movsd 0x58(%rsp), %xmm0
movsd 0x50(%rsp), %xmm1
callq 0x793f2
leaq 0x120(%rsp), %r15
movq %r15, %rdi
callq 0xe59c8
movl 0xc(%r15), %edx
movq %r15, %rdi
movq 0x18(%rsp), %rsi
callq 0xe5a5e
movl 0x12c(%rsp), %eax
testl %eax, %eax
je 0x33319
movsd 0x1050e0(%rip), %xmm0   # 0x138338
movsd %xmm0, 0x48(%rsp)
xorl %ebp, %ebp
leaq 0x120(%rsp), %r15
movq 0x120(%rsp), %rcx
movslq %eax, %rsi
movq -0x8(%rcx,%rsi,8), %r12
decl %esi
movq %r15, %rdi
callq 0xe5be8
cmpl $0x0, 0x3c(%r12)
jle 0x332b1
xorl %ebx, %ebx
movq 0x30(%r12), %rax
movq (%rax,%rbx,8), %rsi
movl 0x12c(%rsp), %edx
movq %r15, %rdi
callq 0xe5a5e
incq %rbx
movslq 0x3c(%r12), %rax
cmpq %rax, %rbx
jl 0x3328c
movq %r12, %rdi
xorl %esi, %esi
xorl %edx, %edx
callq 0x401d4
testq %rax, %rax
je 0x33308
movq %r12, %rdi
callq 0x4003c
leaq 0x1b0(%rsp), %rdi
movq %rax, %rsi
xorl %edx, %edx
xorl %ecx, %ecx
leaq 0x98(%rsp), %r8
callq 0x85ae9
testl %eax, %eax
je 0x33308
movsd 0x98(%rsp), %xmm0
movsd 0x48(%rsp), %xmm1
ucomisd %xmm0, %xmm1
jbe 0x33308
movsd %xmm0, 0x48(%rsp)
movq %r12, %rbp
movl 0x12c(%rsp), %eax
testl %eax, %eax
jne 0x33268
jmp 0x3331b
xorl %ebp, %ebp
leaq 0x120(%rsp), %rdi
callq 0xe5a32
leaq 0x1a0(%rsp), %r14
movl $0xba2, %edi            # imm = 0xBA2
movq %r14, %rsi
callq 0x278c0
leaq 0x1b0(%rsp), %r12
movl $0xba6, %edi            # imm = 0xBA6
movq %r12, %rsi
callq 0x27a90
leaq 0x120(%rsp), %r15
movl $0xba7, %edi            # imm = 0xBA7
movq %r15, %rsi
callq 0x27a90
leaq 0x94(%rsp), %rbx
movq %rbx, (%rsp)
movl 0x2c(%rsp), %edi
movl 0x30(%rsp), %esi
movl $0x1, %edx
movl $0x1, %ecx
movl $0x1902, %r8d           # imm = 0x1902
movl $0x1406, %r9d           # imm = 0x1406
callq 0x273d0
cvtss2sd (%rbx), %xmm2
leaq 0x78(%rsp), %r8
leaq 0x80(%rsp), %r9
leaq 0x70(%rsp), %rbx
movsd 0x58(%rsp), %xmm0
movsd 0x50(%rsp), %xmm1
movq %r12, %rdi
movq %r15, %rsi
movq %r14, %rdx
movq %rbx, %rcx
callq 0x28280
movsd (%rbx), %xmm0
movsd 0x8(%rbx), %xmm1
movsd 0x10(%rbx), %xmm2
leaq 0x108(%rsp), %rdi
callq 0xc3d32
movq 0x20(%rsp), %rdi
testq %rdi, %rdi
je 0x333fb
leaq 0x108(%rsp), %rsi
callq 0xc409a
movq 0x60(%rsp), %rax
orq 0x68(%rsp), %rax
je 0x3316d
movsd 0x70(%rsp), %xmm0
movsd 0x78(%rsp), %xmm1
movsd 0x80(%rsp), %xmm2
leaq 0xf0(%rsp), %rbx
movq %rbx, %rdi
callq 0xc3d32
movq %rbx, %rdi
movq 0x10(%rsp), %rsi
callq 0x8bb28
mulsd 0x104f0b(%rip), %xmm0   # 0x138350
leaq 0xc8(%rsp), %r14
movq %r14, %rdi
movq %rbx, %rsi
callq 0xaad4e
leaq 0x98(%rsp), %rdi
movq %r14, %rsi
callq 0x39e7c
leaq 0xc8(%rsp), %rdi
callq 0xae0e4
movq 0x8(%r13), %rdi
leaq 0x98(%rsp), %rdx
movq 0x18(%rsp), %rsi
callq 0x51eeb
testq %rax, %rax
je 0x335a8
movq %rax, 0x10(%rsp)
cmpl $0x0, 0x10(%rax)
jle 0x3359d
movsd 0x104e8d(%rip), %xmm0   # 0x138338
xorl %r13d, %r13d
leaq 0xc8(%rsp), %rbx
xorl %r12d, %r12d
xorl %r14d, %r14d
movq 0x10(%rsp), %rcx
movsd %xmm0, 0x30(%rsp)
movq 0x8(%rcx), %r15
movq (%r15,%r13), %rax
movq 0x8(%r15,%r13), %rcx
xorps %xmm0, %xmm0
cvtss2sd (%rcx), %xmm0
addsd 0x10(%rax), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rcx), %xmm1
addsd 0x18(%rax), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rcx), %xmm2
addsd 0x20(%rax), %xmm2
movq %rbx, %rdi
callq 0xc3d32
movq %rbx, %rdi
leaq 0xf0(%rsp), %rsi
callq 0x8baff
addq %r13, %r15
movsd 0x30(%rsp), %xmm1
ucomisd %xmm0, %xmm1
cmovaq %r15, %r14
minsd %xmm1, %xmm0
incq %r12
movq 0x10(%rsp), %rcx
movslq 0x10(%rcx), %rax
addq $0x10, %r13
cmpq %rax, %r12
jl 0x334c1
testq %r14, %r14
movq 0x68(%rsp), %r15
movq 0x60(%rsp), %r12
je 0x3359d
cmpq $0x0, 0x20(%rsp)
je 0x33575
leaq 0xc8(%rsp), %rdi
movq %r14, %rsi
callq 0x344fc
leaq 0xc8(%rsp), %rsi
movq 0x20(%rsp), %rdi
callq 0xc409a
testq %r12, %r12
je 0x33581
movq (%r14), %rax
movq %rax, (%r12)
testq %r15, %r15
je 0x3359d
movq (%r14), %rax
movq 0x8(%r14), %rcx
subq (%rax), %rcx
shrq $0x4, %rcx
imull $0xaaaaaaab, %ecx, %eax # imm = 0xAAAAAAAB
movl %eax, (%r15)
movq 0x10(%rsp), %rdi
movq (%rdi), %rax
callq *0x8(%rax)
leaq 0x180b09(%rip), %rax    # 0x1b40b8
addq $0x10, %rax
leaq 0xa0(%rsp), %rdi
movq %rax, -0x8(%rdi)
callq 0xae0e4
leaq 0x98(%rsp), %rdi
callq 0x3b726
jmp 0x3316d
jmp 0x3361e
jmp 0x335f0
jmp 0x335f0
movq %rax, %rbx
leaq 0xc8(%rsp), %rdi
callq 0xae0e4
jmp 0x3362e
jmp 0x3361e
movq %rax, %rbx
leaq 0x180abe(%rip), %rax    # 0x1b40b8
addq $0x10, %rax
leaq 0xa0(%rsp), %rdi
movq %rax, -0x8(%rdi)
callq 0xae0e4
leaq 0x98(%rsp), %rdi
callq 0x3b726
jmp 0x3362e
movq %rax, %rbx
leaq 0x120(%rsp), %rdi
callq 0xe5a32
movq %rbx, %rdi
callq 0x28120