// File: tomfunkhouser[P]gaps/pkgs/R3Utils/R3Segmentation.cpp
// Function: gaps::R3Segmentation::CreateNeighbors(int, double, double, double, double, double, double, double, int)
int R3Segmentation::
CreateNeighbors(
  int max_neighbor_count,
  double max_neighbor_distance,
  double max_neighbor_primitive_distance,
  double max_neighbor_normal_angle,
  double max_neighbor_color_difference,
  double max_neighbor_distance_factor,
  double max_neighbor_timestamp_difference,
  double max_neighbor_category_difference,
  RNBoolean partition_identifiers)
{
  // Create kdtree of points
  R3SegmentationPoint tmp; int position_offset = (unsigned char *) &(tmp.position) - (unsigned char *) &tmp;
  kdtree = new R3Kdtree<R3SegmentationPoint *>(points, position_offset);
  if (!kdtree) {
    RNFail("Unable to create kdtree\n");
    return 0;
  }
  
  // Create arrays of neighbor points
  for (int i = 0; i < points.NEntries(); i++) {
    R3SegmentationPoint *point = points.Kth(i);

    // Refine maximum distance for this point
    RNScalar max_d = max_neighbor_distance;
    if ((max_neighbor_distance_factor > 0) && (point->radius1 > 0)) 
      max_d = max_neighbor_distance_factor * point->radius1;
    if (max_d == 0) max_d = 10 * point->radius1;
    if (max_d == 0) max_d = 1;

    // Create neighbors
    double min_affinity = 0;
    RNArray<R3SegmentationPoint *> neighbors;
    if (kdtree->FindClosest(point, 0, max_d, max_neighbor_count, neighbors)) {
      for (int j = 0; j < neighbors.NEntries(); j++) {
        R3SegmentationPoint *neighbor = neighbors.Kth(j);
        if (neighbor == point) continue;
        
        // Check identifier
        if (partition_identifiers) {
          if (point->identifier != neighbor->identifier) continue;
        }

        // Compute affinity
        RNScalar affinity = R3SegmentationPointAffinity(point, neighbor,
          max_d, max_neighbor_primitive_distance,
          max_neighbor_normal_angle, max_neighbor_color_difference,
          max_neighbor_timestamp_difference,
          max_neighbor_category_difference, min_affinity);
        if (affinity <= min_affinity) continue;
        
        // Insert neighbor
        if ((max_neighbor_count > 0) &&
            (neighbors.NEntries() - j > max_neighbor_count - point->neighbors.NEntries())) {
          // Find slot for neighbor in list sorted by affinity
          int slot = point->neighbors.NEntries();
          while (slot > 0) {
            RNScalar s_affinity = point->neighbors[slot-1]->cluster_affinity;
            if (s_affinity > affinity) break;
            slot--;
          }
          if (slot < max_neighbor_count) {
            neighbor->cluster_affinity = affinity;
            point->neighbors.InsertKth(neighbor, slot);
            point->neighbors.Truncate(max_neighbor_count);
          }
          if (point->neighbors.NEntries() == max_neighbor_count) {
            min_affinity = point->neighbors[point->neighbors.NEntries()-1]->cluster_affinity;
          }
        }
        else {
          // Insert neighbor at end of unconstrained list
          point->neighbors.Insert(neighbor);
        }
      }
    }
  }

  // Reset cluster affinities
  for (int i = 0; i < points.NEntries(); i++) {
    R3SegmentationPoint *point = points.Kth(i);
    point->cluster_affinity = 0;
  }
  
  // Return success
  return 1;
}

// Assembly:
subq $0x248, %rsp            # imm = 0x248
movq %rdi, 0x238(%rsp)
movl %esi, 0x234(%rsp)
movsd %xmm0, 0x228(%rsp)
movsd %xmm1, 0x220(%rsp)
movsd %xmm2, 0x218(%rsp)
movsd %xmm3, 0x210(%rsp)
movsd %xmm4, 0x208(%rsp)
movsd %xmm5, 0x200(%rsp)
movsd %xmm6, 0x1f8(%rsp)
movl %edx, 0x1f4(%rsp)
movq 0x238(%rsp), %rax
movq %rax, 0x80(%rsp)
leaq 0x110(%rsp), %rdi
callq 0x3bea0
movl $0x8, 0x10c(%rsp)
movl $0x60, %edi
callq 0x16740
movq %rax, 0x88(%rsp)
jmp 0x42ad8
movq 0x80(%rsp), %rsi
movq 0x88(%rsp), %rdi
movl 0x10c(%rsp), %edx
callq 0x1f400
jmp 0x42af6
movq 0x80(%rsp), %rax
movq 0x88(%rsp), %rcx
movq %rcx, 0x10(%rax)
cmpq $0x0, 0x10(%rax)
jne 0x42b80
leaq 0x1ba95a(%rip), %rdi    # 0x1fd472
xorl %eax, %eax
callq 0x1779b0
jmp 0x42b21
movl $0x0, 0x244(%rsp)
movl $0x1, 0xf8(%rsp)
jmp 0x4312a
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x100(%rsp)
movl %eax, 0xfc(%rsp)
jmp 0x43146
movq 0x88(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x100(%rsp)
movl %eax, 0xfc(%rsp)
movl $0x60, %esi
callq 0x16750
jmp 0x43146
movl $0x0, 0xf4(%rsp)
movq 0x80(%rsp), %rdi
movl 0xf4(%rsp), %eax
movl %eax, 0x78(%rsp)
callq 0x1ecf0
movl %eax, 0x7c(%rsp)
jmp 0x42ba9
movl 0x78(%rsp), %eax
movl 0x7c(%rsp), %ecx
cmpl %ecx, %eax
jge 0x43091
movq 0x80(%rsp), %rdi
movl 0xf4(%rsp), %esi
callq 0x1f6a0
movq %rax, 0x70(%rsp)
jmp 0x42bd4
movq 0x70(%rsp), %rax
movq %rax, 0xe8(%rsp)
movsd 0x228(%rsp), %xmm0
movsd %xmm0, 0xe0(%rsp)
movsd 0x208(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x42c3a
movq 0xe8(%rsp), %rax
movsd 0x50(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x42c3a
movsd 0x208(%rsp), %xmm0
movq 0xe8(%rsp), %rax
mulsd 0x50(%rax), %xmm0
movsd %xmm0, 0xe0(%rsp)
movsd 0xe0(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x42c6c
jp 0x42c6c
movq 0xe8(%rsp), %rax
movsd 0x1bb652(%rip), %xmm0   # 0x1fe2b0
mulsd 0x50(%rax), %xmm0
movsd %xmm0, 0xe0(%rsp)
movsd 0xe0(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x42c91
jp 0x42c91
movsd 0x1ba398(%rip), %xmm0   # 0x1fd020
movsd %xmm0, 0xe0(%rsp)
movq $0x0, 0xd8(%rsp)
leaq 0xc8(%rsp), %rdi
callq 0x20260
jmp 0x42cac
movq 0x80(%rsp), %rax
movq 0x10(%rax), %rdi
movq 0xe8(%rsp), %rsi
movsd 0xe0(%rsp), %xmm1
movl 0x234(%rsp), %edx
xorl %eax, %eax
movl %eax, %r8d
xorps %xmm0, %xmm0
leaq 0xc8(%rsp), %rcx
callq 0x47c50
movl %eax, 0x6c(%rsp)
jmp 0x42ceb
movl 0x6c(%rsp), %eax
cmpl $0x0, %eax
je 0x4306e
movl $0x0, 0xc4(%rsp)
movl 0xc4(%rsp), %eax
movl %eax, 0x64(%rsp)
leaq 0xc8(%rsp), %rdi
callq 0x1ecf0
movl %eax, 0x68(%rsp)
jmp 0x42d21
movl 0x64(%rsp), %eax
movl 0x68(%rsp), %ecx
cmpl %ecx, %eax
jge 0x4306c
movl 0xc4(%rsp), %esi
leaq 0xc8(%rsp), %rdi
callq 0x1f6a0
movq %rax, 0x58(%rsp)
jmp 0x42d4c
movq 0x58(%rsp), %rax
movq %rax, 0xb8(%rsp)
movq 0xb8(%rsp), %rax
cmpq 0xe8(%rsp), %rax
jne 0x42d96
jmp 0x43056
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x100(%rsp)
movl %eax, 0xfc(%rsp)
leaq 0xc8(%rsp), %rdi
callq 0x1f730
jmp 0x43146
cmpl $0x0, 0x1f4(%rsp)
je 0x42dc5
movq 0xe8(%rsp), %rax
movl 0xa8(%rax), %eax
movq 0xb8(%rsp), %rcx
cmpl 0xa8(%rcx), %eax
je 0x42dc3
jmp 0x43056
jmp 0x42dc5
movq 0xe8(%rsp), %rdi
movq 0xb8(%rsp), %rsi
movsd 0xe0(%rsp), %xmm0
movsd 0x220(%rsp), %xmm1
movsd 0x218(%rsp), %xmm2
movsd 0x210(%rsp), %xmm3
movsd 0x200(%rsp), %xmm4
movsd 0x1f8(%rsp), %xmm5
movsd 0xd8(%rsp), %xmm6
callq 0x43160
movsd %xmm0, 0x50(%rsp)
jmp 0x42e21
movsd 0x50(%rsp), %xmm0
movsd %xmm0, 0xb0(%rsp)
movsd 0xb0(%rsp), %xmm1
movsd 0xd8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jb 0x42e4d
jmp 0x43056
cmpl $0x0, 0x234(%rsp)
jle 0x43034
leaq 0xc8(%rsp), %rdi
callq 0x1ecf0
movl %eax, 0x4c(%rsp)
jmp 0x42e6e
movl 0x4c(%rsp), %eax
movl 0xc4(%rsp), %ecx
subl %ecx, %eax
movl %eax, 0x40(%rsp)
movl 0x234(%rsp), %eax
movl %eax, 0x44(%rsp)
movq 0xe8(%rsp), %rdi
addq $0xb0, %rdi
callq 0x1ecf0
movl %eax, 0x48(%rsp)
jmp 0x42ea4
movl 0x40(%rsp), %eax
movl 0x48(%rsp), %edx
movl 0x44(%rsp), %ecx
subl %edx, %ecx
cmpl %ecx, %eax
jle 0x43034
movq 0xe8(%rsp), %rdi
addq $0xb0, %rdi
callq 0x1ecf0
movl %eax, 0x3c(%rsp)
jmp 0x42ed4
movl 0x3c(%rsp), %eax
movl %eax, 0xac(%rsp)
cmpl $0x0, 0xac(%rsp)
jle 0x42f4f
movq 0xe8(%rsp), %rdi
addq $0xb0, %rdi
movl 0xac(%rsp), %esi
decl %esi
callq 0x47770
movq %rax, 0x30(%rsp)
jmp 0x42f0d
movq 0x30(%rsp), %rax
movq (%rax), %rax
movsd 0xc8(%rax), %xmm0
movsd %xmm0, 0xa0(%rsp)
movsd 0xa0(%rsp), %xmm0
ucomisd 0xb0(%rsp), %xmm0
jbe 0x42f3c
jmp 0x42f4f
movl 0xac(%rsp), %eax
addl $-0x1, %eax
movl %eax, 0xac(%rsp)
jmp 0x42edf
movl 0xac(%rsp), %eax
cmpl 0x234(%rsp), %eax
jge 0x42fbc
movsd 0xb0(%rsp), %xmm0
movq 0xb8(%rsp), %rax
movsd %xmm0, 0xc8(%rax)
movq 0xe8(%rsp), %rdi
addq $0xb0, %rdi
movq 0xb8(%rsp), %rsi
movl 0xac(%rsp), %edx
callq 0x47cc0
jmp 0x42f9d
movq 0xe8(%rsp), %rdi
addq $0xb0, %rdi
movl 0x234(%rsp), %esi
callq 0x173ac0
jmp 0x42fba
jmp 0x42fbc
movq 0xe8(%rsp), %rdi
addq $0xb0, %rdi
callq 0x1ecf0
movl %eax, 0x2c(%rsp)
jmp 0x42fd6
movl 0x2c(%rsp), %eax
cmpl 0x234(%rsp), %eax
jne 0x43032
movq 0xe8(%rsp), %rdi
addq $0xb0, %rdi
movq %rdi, 0x20(%rsp)
callq 0x1ecf0
movl %eax, 0x28(%rsp)
jmp 0x43002
movq 0x20(%rsp), %rdi
movl 0x28(%rsp), %esi
decl %esi
callq 0x47770
movq %rax, 0x18(%rsp)
jmp 0x43019
movq 0x18(%rsp), %rax
movq (%rax), %rax
movsd 0xc8(%rax), %xmm0
movsd %xmm0, 0xd8(%rsp)
jmp 0x43054
movq 0xe8(%rsp), %rdi
addq $0xb0, %rdi
movq 0xb8(%rsp), %rsi
callq 0x1f3d0
jmp 0x43052
jmp 0x43054
jmp 0x43056
movl 0xc4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xc4(%rsp)
jmp 0x42d03
jmp 0x4306e
leaq 0xc8(%rsp), %rdi
callq 0x1f730
movl 0xf4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xf4(%rsp)
jmp 0x42b8b
movl $0x0, 0x9c(%rsp)
movq 0x80(%rsp), %rdi
movl 0x9c(%rsp), %eax
movl %eax, 0x10(%rsp)
callq 0x1ecf0
movl %eax, 0x14(%rsp)
jmp 0x430ba
movl 0x10(%rsp), %eax
movl 0x14(%rsp), %ecx
cmpl %ecx, %eax
jge 0x43114
movq 0x80(%rsp), %rdi
movl 0x9c(%rsp), %esi
callq 0x1f6a0
movq %rax, 0x8(%rsp)
jmp 0x430e1
movq 0x8(%rsp), %rax
movq %rax, 0x90(%rsp)
movq 0x90(%rsp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, 0xc8(%rax)
movl 0x9c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x9c(%rsp)
jmp 0x4309c
movl $0x1, 0x244(%rsp)
movl $0x1, 0xf8(%rsp)
leaq 0x110(%rsp), %rdi
callq 0x1f2b0
movl 0x244(%rsp), %eax
addq $0x248, %rsp            # imm = 0x248
retq
leaq 0x110(%rsp), %rdi
callq 0x1f2b0
movq 0x100(%rsp), %rdi
callq 0x16ca0