// File: 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:
subq $0x398, %rsp            # imm = 0x398
movl 0x3a0(%rsp), %eax
movq %rdi, 0x388(%rsp)
movl %esi, 0x384(%rsp)
movl %edx, 0x380(%rsp)
movq %rcx, 0x378(%rsp)
movq %r8, 0x370(%rsp)
movq %r9, 0x368(%rsp)
movq 0x388(%rsp), %rdi
movq %rdi, 0x98(%rsp)
callq 0x3df40
movq %rax, %rdi
callq 0x3df10
cvtsi2sd %eax, %xmm0
movsd 0x2067d3(%rip), %xmm1   # 0x2415b8
mulsd %xmm1, %xmm0
movsd 0x20621f(%rip), %xmm1   # 0x241010
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x364(%rsp)
movl $0x4100, %edi           # imm = 0x4100
callq 0x28230
movq 0x98(%rsp), %rdi
addq $0x78, %rdi
callq 0x2f6d0
movq %rax, %rdi
xorl %esi, %esi
callq 0xd1a70
movq 0x98(%rsp), %rdi
movq (%rdi), %rax
callq *0x230(%rax)
cvtsi2ssl 0x364(%rsp), %xmm0
callq 0x280b0
movq 0x98(%rsp), %rdi
movq (%rdi), %rax
movl $0xe, %esi
callq *0x270(%rax)
movss 0x206bca(%rip), %xmm0   # 0x241a2c
callq 0x280b0
callq 0x29580
movq 0x98(%rsp), %rdi
movq (%rdi), %rax
callq *0x238(%rax)
movl 0x384(%rsp), %edi
movl 0x380(%rsp), %esi
leaq 0x360(%rsp), %rax
movl $0x1, %ecx
movl $0x1908, %r8d           # imm = 0x1908
movl $0x1401, %r9d           # imm = 0x1401
movl %ecx, %edx
movq %rax, (%rsp)
callq 0x28450
movzbl 0x363(%rsp), %eax
cmpl $0x0, %eax
jne 0x3aecd
movq $0x0, 0x390(%rsp)
jmp 0x3b748
movzbl 0x360(%rsp), %eax
andl $0xff, %eax
movl %eax, 0x35c(%rsp)
movzbl 0x361(%rsp), %eax
andl $0xff, %eax
movl %eax, 0x358(%rsp)
movzbl 0x362(%rsp), %eax
andl $0xff, %eax
movl %eax, 0x354(%rsp)
movl 0x35c(%rsp), %eax
shll $0x10, %eax
movl 0x358(%rsp), %ecx
shll $0x8, %ecx
orl %ecx, %eax
orl 0x354(%rsp), %eax
movl %eax, 0x350(%rsp)
cmpl $0x0, 0x350(%rsp)
jne 0x3af48
movq $0x0, 0x390(%rsp)
jmp 0x3b748
movq 0x98(%rsp), %rax
movl 0x350(%rsp), %ecx
addl $-0x1, %ecx
movl %ecx, 0x350(%rsp)
movq 0x8(%rax), %rdi
callq 0x2f020
movq %rax, 0x348(%rsp)
cmpq $0x0, 0x348(%rsp)
jne 0x3af8e
movq $0x0, 0x390(%rsp)
jmp 0x3b748
cmpl $0x0, 0x350(%rsp)
jge 0x3afa9
movq $0x0, 0x390(%rsp)
jmp 0x3b748
movl 0x350(%rsp), %eax
movl %eax, 0x94(%rsp)
movq 0x348(%rsp), %rdi
callq 0x2f030
movl %eax, %ecx
movl 0x94(%rsp), %eax
cmpl %ecx, %eax
jl 0x3afe2
movq $0x0, 0x390(%rsp)
jmp 0x3b748
movq 0x348(%rsp), %rdi
movl 0x350(%rsp), %esi
callq 0x2f1d0
movq %rax, 0x340(%rsp)
movq 0x340(%rsp), %rax
movq %rax, 0x338(%rsp)
cmpl $0x0, 0x3a0(%rsp)
je 0x3b2f0
movq $0x0, 0x338(%rsp)
movq 0x340(%rsp), %rdi
xorl %edx, %edx
movl %edx, %esi
callq 0x58f70
cmpq $0x0, %rax
je 0x3b04f
movq 0x340(%rsp), %rax
movq %rax, 0x338(%rsp)
cmpq $0x0, 0x338(%rsp)
jne 0x3b0bc
movq 0x340(%rsp), %rdi
callq 0x3e7e0
movq %rax, 0x330(%rsp)
cmpq $0x0, 0x330(%rsp)
je 0x3b0ba
movq 0x330(%rsp), %rdi
xorl %edx, %edx
movl %edx, %esi
callq 0x58f70
cmpq $0x0, %rax
je 0x3b0a3
movq 0x330(%rsp), %rax
movq %rax, 0x338(%rsp)
jmp 0x3b0ba
movq 0x330(%rsp), %rdi
callq 0x3e7e0
movq %rax, 0x330(%rsp)
jmp 0x3b06f
jmp 0x3b0bc
cmpq $0x0, 0x338(%rsp)
jne 0x3b2ee
movq 0x98(%rsp), %rsi
addq $0x78, %rsi
cvtsi2sdl 0x384(%rsp), %xmm0
cvtsi2sdl 0x380(%rsp), %xmm1
leaq 0x300(%rsp), %rdi
callq 0xceda0
movabsq $0x47efffffe0000000, %rax # imm = 0x47EFFFFFE0000000
movq %rax, 0x2f0(%rsp)
leaq 0x2e0(%rsp), %rdi
movq %rdi, 0x88(%rsp)
callq 0x3eda0
movq 0x88(%rsp), %rdi
movq 0x340(%rsp), %rsi
callq 0x3edb0
jmp 0x3b134
jmp 0x3b136
leaq 0x2e0(%rsp), %rdi
callq 0x3ede0
movl %eax, 0x84(%rsp)
jmp 0x3b14c
movl 0x84(%rsp), %eax
cmpl $0x0, %eax
setne %al
xorb $-0x1, %al
testb $0x1, %al
jne 0x3b164
jmp 0x3b2e1
leaq 0x2e0(%rsp), %rdi
callq 0x3ee00
movq %rax, 0x78(%rsp)
jmp 0x3b178
movq 0x78(%rsp), %rax
movq %rax, 0x2c8(%rsp)
leaq 0x2e0(%rsp), %rdi
callq 0x3ee10
jmp 0x3b194
movl $0x0, 0x2c4(%rsp)
movl 0x2c4(%rsp), %eax
movl %eax, 0x70(%rsp)
movq 0x2c8(%rsp), %rdi
callq 0x2f260
movl %eax, 0x74(%rsp)
jmp 0x3b1bd
movl 0x70(%rsp), %eax
movl 0x74(%rsp), %ecx
cmpl %ecx, %eax
jge 0x3b233
movq 0x2c8(%rsp), %rdi
movl 0x2c4(%rsp), %esi
callq 0x3eba0
movq %rax, 0x68(%rsp)
jmp 0x3b1e4
movq 0x68(%rsp), %rsi
leaq 0x2e0(%rsp), %rdi
callq 0x3edb0
jmp 0x3b1f8
jmp 0x3b1fa
movl 0x2c4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x2c4(%rsp)
jmp 0x3b19f
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
leaq 0x2e0(%rsp), %rdi
callq 0x3ee30
jmp 0x3b758
movq 0x2c8(%rsp), %rdi
xorl %edx, %edx
movl %edx, %esi
callq 0x58f70
movq %rax, 0x60(%rsp)
jmp 0x3b24b
movq 0x60(%rsp), %rax
cmpq $0x0, %rax
je 0x3b2dc
movq 0x2c8(%rsp), %rdi
callq 0x58ae0
movq %rax, 0x58(%rsp)
jmp 0x3b26e
movq 0x58(%rsp), %rsi
xorl %eax, %eax
movl %eax, %ecx
leaq 0x300(%rsp), %rdi
leaq 0x2f8(%rsp), %r8
movq %rcx, %rdx
callq 0xe56a0
movl %eax, 0x54(%rsp)
jmp 0x3b295
movl 0x54(%rsp), %eax
cmpl $0x0, %eax
je 0x3b2da
movsd 0x2f8(%rsp), %xmm1
movsd 0x2f0(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x3b2d8
movq 0x2c8(%rsp), %rax
movq %rax, 0x338(%rsp)
movsd 0x2f8(%rsp), %xmm0
movsd %xmm0, 0x2f0(%rsp)
jmp 0x3b2da
jmp 0x3b2dc
jmp 0x3b136
leaq 0x2e0(%rsp), %rdi
callq 0x3ee30
jmp 0x3b2f0
leaq 0x290(%rsp), %rsi
movl $0xba2, %edi            # imm = 0xBA2
callq 0x289d0
leaq 0x210(%rsp), %rsi
movl $0xba6, %edi            # imm = 0xBA6
callq 0x28bf0
leaq 0x190(%rsp), %rsi
movl $0xba7, %edi            # imm = 0xBA7
callq 0x28bf0
movl 0x384(%rsp), %edi
movl 0x380(%rsp), %esi
movl $0x1, %ecx
movl $0x1902, %r8d           # imm = 0x1902
movl $0x1406, %r9d           # imm = 0x1406
leaq 0x2c0(%rsp), %rax
movl %ecx, %edx
movq %rax, (%rsp)
callq 0x28450
cvtsi2sdl 0x384(%rsp), %xmm0
cvtsi2sdl 0x380(%rsp), %xmm1
movss 0x2c0(%rsp), %xmm2
cvtss2sd %xmm2, %xmm2
leaq 0x210(%rsp), %rdi
leaq 0x190(%rsp), %rsi
leaq 0x290(%rsp), %rdx
leaq 0x2a0(%rsp), %rcx
leaq 0x2a0(%rsp), %r8
addq $0x8, %r8
leaq 0x2a0(%rsp), %r9
addq $0x10, %r9
callq 0x29460
movsd 0x2a0(%rsp), %xmm0
movsd 0x2a8(%rsp), %xmm1
movsd 0x2b0(%rsp), %xmm2
leaq 0x178(%rsp), %rdi
callq 0x1622c0
cmpq $0x0, 0x378(%rsp)
je 0x3b3fc
movq 0x378(%rsp), %rdi
leaq 0x178(%rsp), %rsi
callq 0x162950
cmpq $0x0, 0x370(%rsp)
jne 0x3b416
cmpq $0x0, 0x368(%rsp)
je 0x3b738
movsd 0x2a0(%rsp), %xmm0
movsd 0x2a8(%rsp), %xmm1
movsd 0x2b0(%rsp), %xmm2
leaq 0x160(%rsp), %rdi
movq %rdi, 0x40(%rsp)
callq 0x1622c0
movq 0x98(%rsp), %rdi
addq $0x78, %rdi
callq 0x2f6d0
movq %rax, %rdi
callq 0x2ee60
movq 0x40(%rsp), %rdi
movq %rax, %rsi
callq 0xf0ed0
movq 0x40(%rsp), %rsi
movsd 0x206072(%rip), %xmm1   # 0x2414e8
mulsd %xmm1, %xmm0
movsd %xmm0, 0x158(%rsp)
movsd 0x158(%rsp), %xmm0
leaq 0x100(%rsp), %rdi
movq %rdi, 0x48(%rsp)
callq 0x12f8e0
movq 0x48(%rsp), %rsi
leaq 0x128(%rsp), %rdi
callq 0x4c330
jmp 0x3b4b2
leaq 0x100(%rsp), %rdi
callq 0x3e1c0
movq 0x98(%rsp), %rax
movq 0x8(%rax), %rdi
movq 0x340(%rsp), %rsi
leaq 0x128(%rsp), %rdx
callq 0x7e060
movq %rax, 0x38(%rsp)
jmp 0x3b4e7
movq 0x38(%rsp), %rax
movq %rax, 0xf8(%rsp)
cmpq $0x0, 0xf8(%rsp)
je 0x3b72b
movq $0x0, 0xf0(%rsp)
movsd 0x205fb9(%rip), %xmm0   # 0x2414d0
movsd %xmm0, 0xe8(%rsp)
movl $0x0, 0xe4(%rsp)
movl 0xe4(%rsp), %eax
movl %eax, 0x30(%rsp)
movq 0xf8(%rsp), %rdi
callq 0x3ea50
movl %eax, 0x34(%rsp)
jmp 0x3b549
movl 0x30(%rsp), %eax
movl 0x34(%rsp), %ecx
cmpl %ecx, %eax
jge 0x3b664
movq 0xf8(%rsp), %rdi
movl 0xe4(%rsp), %esi
callq 0x3ea60
movq %rax, 0x28(%rsp)
jmp 0x3b574
movq 0x28(%rsp), %rax
movq %rax, 0xd8(%rsp)
movq 0xd8(%rsp), %rsi
leaq 0xb8(%rsp), %rdi
callq 0x3e1d0
jmp 0x3b598
leaq 0xb8(%rsp), %rdi
leaq 0x160(%rsp), %rsi
callq 0xf0ea0
movsd %xmm0, 0x20(%rsp)
jmp 0x3b5b5
movsd 0x20(%rsp), %xmm0
movsd %xmm0, 0xd0(%rsp)
movsd 0xd0(%rsp), %xmm1
movsd 0xe8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x3b64c
movsd 0xd0(%rsp), %xmm0
movsd %xmm0, 0xe8(%rsp)
movq 0xd8(%rsp), %rax
movq %rax, 0xf0(%rsp)
jmp 0x3b64c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
leaq 0x100(%rsp), %rdi
callq 0x3e1c0
jmp 0x3b758
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x2d8(%rsp)
movl %eax, 0x2d4(%rsp)
leaq 0x128(%rsp), %rdi
callq 0x3ee40
jmp 0x3b758
jmp 0x3b64e
movl 0xe4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xe4(%rsp)
jmp 0x3b52b
cmpq $0x0, 0xf0(%rsp)
je 0x3b70b
cmpq $0x0, 0x378(%rsp)
je 0x3b6ae
movq 0xf0(%rsp), %rsi
leaq 0xa0(%rsp), %rdi
callq 0x3e1d0
jmp 0x3b695
movq 0x378(%rsp), %rdi
leaq 0xa0(%rsp), %rsi
callq 0x162950
jmp 0x3b6ac
jmp 0x3b6ae
cmpq $0x0, 0x370(%rsp)
je 0x3b6dd
movq 0xf0(%rsp), %rdi
callq 0x3ea80
movq %rax, 0x18(%rsp)
jmp 0x3b6cd
movq 0x18(%rsp), %rcx
movq 0x370(%rsp), %rax
movq %rcx, (%rax)
cmpq $0x0, 0x368(%rsp)
je 0x3b709
movq 0xf0(%rsp), %rdi
callq 0x3ea90
movl %eax, 0x14(%rsp)
jmp 0x3b6fb
movl 0x14(%rsp), %ecx
movq 0x368(%rsp), %rax
movl %ecx, (%rax)
jmp 0x3b70b
movq 0xf8(%rsp), %rax
movq %rax, 0x8(%rsp)
cmpq $0x0, %rax
je 0x3b729
movq 0x8(%rsp), %rdi
movq (%rdi), %rax
callq *0x8(%rax)
jmp 0x3b72b
leaq 0x128(%rsp), %rdi
callq 0x3ee40
movq 0x338(%rsp), %rax
movq %rax, 0x390(%rsp)
movq 0x390(%rsp), %rax
addq $0x398, %rsp            # imm = 0x398
retq
movq 0x2d8(%rsp), %rdi
callq 0x292e0
nopw %cs:(%rax,%rax)