// File: tomfunkhouser[P]gaps/apps/msh2df/msh2df.cpp
// Function: RefineSignedDistanceUsingKdtree(gaps::R3Grid*, gaps::RNArray<Point*> const&, int)
static int
RefineSignedDistanceUsingKdtree(R3Grid *grid, const RNArray<Point *>& points, int refinement_radius)
{
  // Start statistics
  RNTime start_time;
  start_time.Read();
  int miss_count = 0;

  // Create kdtree for point samples 
  Point tmp; int position_offset = (unsigned char *) &(tmp.position) - (unsigned char *) &tmp;
  R3Kdtree<Point *> kdtree(points, position_offset);
  
  // Compute distance to closest point sample at every negative grid cell
  RNLength grid_spacing = grid->GridToWorldScaleFactor();
  RNScalar max_distance = refinement_radius * grid_spacing;
  if (max_distance > grid->WorldBox().DiagonalLength()) max_distance = grid->WorldBox().DiagonalLength();
  if (max_distance > truncation_distance) max_distance = truncation_distance;
  for (int iz = 0; iz < grid->ZResolution(); iz++) {
    for (int iy = 0; iy < grid->YResolution(); iy++) {
      for (int ix = 0; ix < grid->XResolution(); ix++) {
        // Get current distance
        RNScalar grid_distance = grid->GridValue(ix, iy, iz);
        RNScalar sign = (grid_distance < 0) ? -1 : 1;

        // Check if in swath on negative side
        if (grid_distance >= 0) continue;
        if (grid_distance < -max_distance) continue;

        // Get world position
        R3Point world_position = grid->WorldPosition(ix, iy, iz);

        // Find closest point sample
        Point *closest = kdtree.FindClosest(world_position, 0, fabs(grid_distance) + grid_spacing);
        if (!closest) {
          // Indicate should be interpolated later
          grid->SetGridValue(ix, iy, iz, -FLT_MAX);
          miss_count++;
          continue;
        }

        // Compute distance to closest point sample
        R3Point closest_position = closest->position;
        R3Plane plane(closest->position, closest->normal);
        R3Point projected_position = world_position; projected_position.Project(plane);
        R3Vector tangent_vector = projected_position - closest->position;
        RNLength tangent_distance = tangent_vector.Length();
        if (RNIsPositive(tangent_distance)) {
          tangent_vector /= tangent_distance;
          if (tangent_distance > closest->radius) tangent_distance = closest->radius;
          closest_position = closest->position + tangent_distance * tangent_vector;
        }
        
        // Set grid value
        RNScalar closest_distance = R3Distance(world_position, closest_position);
        grid->SetGridValue(ix, iy, iz, sign * closest_distance);
      }
    }
  }

  // Fill in missing values (-FLT_MAX)
  grid->Substitute(0, 0.000123456);
  grid->Substitute(-FLT_MAX, 0);
  grid->FillHoles(grid->NEntries());
  grid->Substitute(0.000123456, 0);
  
  // Print statistics
  if (print_verbose) {
    printf("Refined signed distances using kdtree ...\n");
    printf("  Time = %.2f seconds\n", start_time.Elapsed());
    printf("  Resolution = %d %d %d\n", grid->XResolution(), grid->YResolution(), grid->ZResolution());
    printf("  Spacing = %g\n", grid->GridToWorldScaleFactor());
    printf("  Cardinality = %d\n", grid->Cardinality());
    printf("  Volume = %g\n", grid->Volume());
    RNInterval grid_range = grid->Range();
    printf("  Minimum = %g\n", grid_range.Min());
    printf("  Maximum = %g\n", grid_range.Max());
    printf("  L1Norm = %g\n", grid->L1Norm());
    printf("  L2Norm = %g\n", grid->L2Norm());
    printf("  Refinement grid radius = %d\n", refinement_radius);
    printf("  Refinement world radius = %g\n", max_distance);
    printf("  Miss count = %d\n", miss_count);
    fflush(stdout);
  }

  // Return success
  return 1;
}

// Assembly:
subq $0x2e8, %rsp            # imm = 0x2E8
movq %rdi, 0x2e0(%rsp)
movq %rsi, 0x2d8(%rsp)
movl %edx, 0x2d4(%rsp)
leaq 0x2c0(%rsp), %rdi
movq %rdi, 0x80(%rsp)
callq 0xddf30
movq 0x80(%rsp), %rdi
callq 0x17860
movl $0x0, 0x2bc(%rsp)
leaq 0x280(%rsp), %rdi
callq 0x18570
movl $0x0, 0x27c(%rsp)
movq 0x2d8(%rsp), %rsi
movl 0x27c(%rsp), %edx
leaq 0x218(%rsp), %rdi
callq 0x185c0
movq 0x2e0(%rsp), %rdi
callq 0x17b20
movsd %xmm0, 0x210(%rsp)
cvtsi2sdl 0x2d4(%rsp), %xmm0
movsd 0x210(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x208(%rsp)
movsd 0x208(%rsp), %xmm0
movsd %xmm0, 0x88(%rsp)
movq 0x2e0(%rsp), %rsi
leaq 0x1d0(%rsp), %rdi
callq 0x1a2a0
jmp 0x14c4d
leaq 0x1d0(%rsp), %rdi
callq 0x685a0
movsd %xmm0, 0x78(%rsp)
jmp 0x14c62
leaq 0x1d0(%rsp), %rdi
callq 0x17670
movsd 0x78(%rsp), %xmm1
movsd 0x88(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x14d37
movq 0x2e0(%rsp), %rsi
leaq 0x188(%rsp), %rdi
callq 0x1a2a0
jmp 0x14c9f
leaq 0x188(%rsp), %rdi
callq 0x685a0
movsd %xmm0, 0x70(%rsp)
jmp 0x14cb4
movsd 0x70(%rsp), %xmm0
movsd %xmm0, 0x208(%rsp)
leaq 0x188(%rsp), %rdi
callq 0x17670
jmp 0x14d37
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1c8(%rsp)
movl %eax, 0x1c4(%rsp)
jmp 0x15405
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1c8(%rsp)
movl %eax, 0x1c4(%rsp)
leaq 0x1d0(%rsp), %rdi
callq 0x17670
jmp 0x15405
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1c8(%rsp)
movl %eax, 0x1c4(%rsp)
leaq 0x188(%rsp), %rdi
callq 0x17670
jmp 0x15405
movsd 0x208(%rsp), %xmm0
ucomisd 0x171898(%rip), %xmm0   # 0x1865e0
jbe 0x14d5b
movsd 0x17188e(%rip), %xmm0   # 0x1865e0
movsd %xmm0, 0x208(%rsp)
movl $0x0, 0x184(%rsp)
movl 0x184(%rsp), %eax
movl %eax, 0x6c(%rsp)
movq 0x2e0(%rsp), %rdi
callq 0x17b80
movl %eax, %ecx
movl 0x6c(%rsp), %eax
cmpl %ecx, %eax
jge 0x1517c
movl $0x0, 0x180(%rsp)
movl 0x180(%rsp), %eax
movl %eax, 0x68(%rsp)
movq 0x2e0(%rsp), %rdi
callq 0x17b60
movl %eax, %ecx
movl 0x68(%rsp), %eax
cmpl %ecx, %eax
jge 0x15164
movl $0x0, 0x17c(%rsp)
movl 0x17c(%rsp), %eax
movl %eax, 0x64(%rsp)
movq 0x2e0(%rsp), %rdi
callq 0x17b40
movl %eax, %ecx
movl 0x64(%rsp), %eax
cmpl %ecx, %eax
jge 0x1514c
movq 0x2e0(%rsp), %rdi
movl 0x17c(%rsp), %esi
movl 0x180(%rsp), %edx
movl 0x184(%rsp), %ecx
callq 0x17e80
movsd %xmm0, 0x170(%rsp)
movsd 0x170(%rsp), %xmm1
xorps %xmm0, %xmm0
movl $0x1, %eax
movl $0xffffffff, %ecx       # imm = 0xFFFFFFFF
ucomisd %xmm1, %xmm0
cmoval %ecx, %eax
cvtsi2sd %eax, %xmm0
movsd %xmm0, 0x168(%rsp)
movsd 0x170(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x14e5a
jmp 0x15136
movsd 0x170(%rsp), %xmm1
movsd 0x208(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x14e8e
jmp 0x15136
movq 0x2e0(%rsp), %rsi
cvtsi2sdl 0x17c(%rsp), %xmm0
cvtsi2sdl 0x180(%rsp), %xmm1
cvtsi2sdl 0x184(%rsp), %xmm2
leaq 0x150(%rsp), %rdi
callq 0x87100
jmp 0x14ec0
movaps 0x12b199(%rip), %xmm0   # 0x140060
movsd 0x170(%rsp), %xmm1
pand %xmm0, %xmm1
movsd 0x210(%rsp), %xmm0
addsd %xmm0, %xmm1
xorl %eax, %eax
movl %eax, %edx
leaq 0x218(%rsp), %rdi
leaq 0x150(%rsp), %rsi
xorps %xmm0, %xmm0
callq 0x1a360
movq %rax, 0x58(%rsp)
jmp 0x14f04
movq 0x58(%rsp), %rax
movq %rax, 0x148(%rsp)
cmpq $0x0, 0x148(%rsp)
jne 0x14f5e
movq 0x2e0(%rsp), %rdi
movl 0x17c(%rsp), %esi
movl 0x180(%rsp), %edx
movl 0x184(%rsp), %ecx
movsd 0x12b0cf(%rip), %xmm0   # 0x140010
callq 0x17ef0
jmp 0x14f48
movl 0x2bc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x2bc(%rsp)
jmp 0x15136
movq 0x148(%rsp), %rsi
leaq 0x130(%rsp), %rdi
callq 0x92f50
jmp 0x14f75
movq 0x148(%rsp), %rsi
movq %rsi, %rdx
addq $0x18, %rdx
leaq 0x110(%rsp), %rdi
callq 0x90650
jmp 0x14f93
leaq 0xf8(%rsp), %rdi
leaq 0x150(%rsp), %rsi
callq 0x92f50
jmp 0x14faa
leaq 0xf8(%rsp), %rdi
leaq 0x110(%rsp), %rsi
callq 0x935e0
jmp 0x14fc1
movq 0x148(%rsp), %rdx
leaq 0xe0(%rsp), %rdi
leaq 0xf8(%rsp), %rsi
callq 0x93270
jmp 0x14fe0
leaq 0xe0(%rsp), %rdi
callq 0x94250
movsd %xmm0, 0x50(%rsp)
jmp 0x14ff5
movsd 0x50(%rsp), %xmm0
movsd %xmm0, 0xd8(%rsp)
movsd 0xd8(%rsp), %xmm0
leaq 0x1714dc(%rip), %rax    # 0x1864f0
movsd (%rax), %xmm1
callq 0x1a130
cmpl $0x0, %eax
je 0x150ce
movsd 0xd8(%rsp), %xmm0
leaq 0xe0(%rsp), %rdi
callq 0x95280
jmp 0x1503e
movsd 0xd8(%rsp), %xmm0
movq 0x148(%rsp), %rax
ucomisd 0x30(%rax), %xmm0
jbe 0x1506c
movq 0x148(%rsp), %rax
movsd 0x30(%rax), %xmm0
movsd %xmm0, 0xd8(%rsp)
movq 0x148(%rsp), %rax
movq %rax, 0x48(%rsp)
movsd 0xd8(%rsp), %xmm0
leaq 0xa8(%rsp), %rdi
leaq 0xe0(%rsp), %rsi
callq 0x17f90
jmp 0x15099
movq 0x48(%rsp), %rsi
leaq 0xc0(%rsp), %rdi
leaq 0xa8(%rsp), %rdx
callq 0x934d0
jmp 0x150b5
leaq 0x130(%rsp), %rdi
leaq 0xc0(%rsp), %rsi
callq 0x935a0
jmp 0x150cc
jmp 0x150ce
leaq 0x150(%rsp), %rdi
leaq 0x130(%rsp), %rsi
callq 0x30500
movsd %xmm0, 0x40(%rsp)
jmp 0x150eb
movsd 0x40(%rsp), %xmm0
movsd %xmm0, 0xa0(%rsp)
movq 0x2e0(%rsp), %rdi
movl 0x17c(%rsp), %esi
movl 0x180(%rsp), %edx
movl 0x184(%rsp), %ecx
movsd 0x168(%rsp), %xmm0
movsd 0xa0(%rsp), %xmm1
mulsd %xmm1, %xmm0
callq 0x17ef0
jmp 0x15134
jmp 0x15136
movl 0x17c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x17c(%rsp)
jmp 0x14dc8
jmp 0x1514e
movl 0x180(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x180(%rsp)
jmp 0x14d97
jmp 0x15166
movl 0x184(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x184(%rsp)
jmp 0x14d66
movq 0x2e0(%rsp), %rdi
xorps %xmm0, %xmm0
movsd 0x12aea1(%rip), %xmm1   # 0x140030
callq 0x80b10
jmp 0x15196
movq 0x2e0(%rsp), %rdi
movsd 0x12ae6a(%rip), %xmm0   # 0x140010
xorps %xmm1, %xmm1
callq 0x80b10
jmp 0x151b0
movq 0x2e0(%rsp), %rdi
movq %rdi, 0x38(%rsp)
callq 0x184a0
movq 0x38(%rsp), %rdi
movl %eax, %esi
callq 0x80320
jmp 0x151d0
movq 0x2e0(%rsp), %rdi
movsd 0x12ae50(%rip), %xmm0   # 0x140030
xorps %xmm1, %xmm1
callq 0x80b10
jmp 0x151ea
cmpl $0x0, 0x171407(%rip)    # 0x1865f8
je 0x153eb
leaq 0x12b29c(%rip), %rdi    # 0x14049a
xorl %eax, %eax
callq 0xf070
jmp 0x15207
leaq 0x2c0(%rsp), %rdi
callq 0xddff0
movsd %xmm0, 0x30(%rsp)
jmp 0x1521c
movsd 0x30(%rsp), %xmm0
leaq 0x12affa(%rip), %rdi    # 0x140223
movb $0x1, %al
callq 0xf070
jmp 0x15232
movq 0x2e0(%rsp), %rdi
callq 0x17b40
movl %eax, 0x28(%rsp)
movq 0x2e0(%rsp), %rdi
callq 0x17b60
movl %eax, 0x2c(%rsp)
movq 0x2e0(%rsp), %rdi
callq 0x17b80
movl 0x28(%rsp), %esi
movl 0x2c(%rsp), %edx
movl %eax, %ecx
leaq 0x12b01e(%rip), %rdi    # 0x140290
xorl %eax, %eax
callq 0xf070
jmp 0x1527b
movq 0x2e0(%rsp), %rdi
callq 0x17b20
leaq 0x12b01a(%rip), %rdi    # 0x1402a9
movb $0x1, %al
callq 0xf070
jmp 0x15298
movq 0x2e0(%rsp), %rdi
callq 0x7b1f0
movl %eax, 0x24(%rsp)
jmp 0x152ab
movl 0x24(%rsp), %esi
leaq 0x12b003(%rip), %rdi    # 0x1402b9
xorl %eax, %eax
callq 0xf070
jmp 0x152bf
movq 0x2e0(%rsp), %rdi
callq 0x17ba0
movsd %xmm0, 0x18(%rsp)
jmp 0x152d4
movsd 0x18(%rsp), %xmm0
leaq 0x12afec(%rip), %rdi    # 0x1402cd
movb $0x1, %al
callq 0xf070
jmp 0x152ea
movq 0x2e0(%rsp), %rsi
leaq 0x90(%rsp), %rdi
callq 0x7aff0
jmp 0x15301
leaq 0x90(%rsp), %rdi
callq 0x17be0
leaq 0x12afc7(%rip), %rdi    # 0x1402dc
movb $0x1, %al
callq 0xf070
jmp 0x1531e
leaq 0x90(%rsp), %rdi
callq 0x17bf0
leaq 0x12afba(%rip), %rdi    # 0x1402ec
movb $0x1, %al
callq 0xf070
jmp 0x1533b
movq 0x2e0(%rsp), %rdi
callq 0x7b0e0
movsd %xmm0, 0x10(%rsp)
jmp 0x15350
movsd 0x10(%rsp), %xmm0
leaq 0x12af9f(%rip), %rdi    # 0x1402fc
movb $0x1, %al
callq 0xf070
jmp 0x15366
movq 0x2e0(%rsp), %rdi
callq 0x17c00
movsd %xmm0, 0x8(%rsp)
jmp 0x1537b
movsd 0x8(%rsp), %xmm0
leaq 0x12af83(%rip), %rdi    # 0x14030b
movb $0x1, %al
callq 0xf070
jmp 0x15391
movl 0x2d4(%rsp), %esi
leaq 0x12af9e(%rip), %rdi    # 0x14033d
xorl %eax, %eax
callq 0xf070
jmp 0x153a8
movsd 0x208(%rsp), %xmm0
leaq 0x12afa4(%rip), %rdi    # 0x14035c
movb $0x1, %al
callq 0xf070
jmp 0x153c1
movl 0x2bc(%rsp), %esi
leaq 0x12b0f6(%rip), %rdi    # 0x1404c5
xorl %eax, %eax
callq 0xf070
jmp 0x153d8
movq 0x167bb1(%rip), %rax    # 0x17cf90
movq (%rax), %rdi
callq 0xf5d0
jmp 0x153e9
jmp 0x153eb
leaq 0x218(%rsp), %rdi
callq 0x18a40
movl $0x1, %eax
addq $0x2e8, %rsp            # imm = 0x2E8
retq
leaq 0x218(%rsp), %rdi
callq 0x18a40
movq 0x1c8(%rsp), %rdi
callq 0xf890
nop