// File: tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelTree.cpp
// Function: gaps::SplitInteriorNodeWithKMeansClusteringOfParts(gaps::R3SurfelTree*, gaps::R3SurfelNode*, int)
static int
SplitInteriorNodeWithKMeansClusteringOfParts(R3SurfelTree *tree, R3SurfelNode *node, int nparts)
{
  // Just checking
  assert(node->NParts() > 0);
  assert(node->Tree() == tree);
  if (nparts >= node->NParts()) return 0;
  int status = 1;

  ////// INITIALIZE KMEANS ////// 

  // Allocate temporary data for kmeans clustering
  R3Point *centroids = new R3Point [ nparts ];
  int *membership = new int [ node->NParts() ];

  // Initialize kmeans centroids
  for (int i = 0; i < nparts; i++) {
    int part_index = i * node->NParts() / nparts;
    R3SurfelNode *part = node->Part(part_index);
    centroids[i] = part->Centroid();
  }

  // Intialize kmeans membership
  for (int i = 0; i < node->NParts(); i++) {
    membership[i] = -1;
  }

  ////// EXECUTE KMEANS ////// 

  // Interate K-means
  RNBoolean done = FALSE;
  const int max_iterations = 16;
  for (int i = 0; i < max_iterations; i++) {
    // Done when there are no changes in membership
    done = TRUE;

    // Update kmeans membership
    for (int j = 0; j < node->NParts(); j++) {
      R3SurfelNode *part = node->Part(j);
      R3Point part_centroid = part->Centroid();
      RNLength closest_distance = FLT_MAX;
      for (int k = 0; k < nparts; k++) {
        RNLength distance = R3SquaredDistance(centroids[k], part_centroid);
        if (distance < closest_distance) {
          closest_distance = distance;
          if (membership[j] != k) done = FALSE;
          membership[j] = k;
        }
      }
    }

    // Check if there were no membership changes 
    if (done) break;
    
    // Update kmeans centroids
    for (int j = 0; j < nparts; j++) {
      // Find centroid
      RNScalar weight = 0;
      centroids[j] = R3zero_point;
      for (int k = 0; k < node->NParts(); k++) {
        if (membership[k] != j) continue;
        R3SurfelNode *part = node->Part(k);
        centroids[j] += part->Complexity() * part->Centroid();
        weight += part->Complexity();
      }
      if (weight > 0) centroids[j] /= weight;
    }
  }


  ////// CREATE CHILDREN OBJECTS AND REDISTRIBUTE PARTS ////// 

  // Create temporary array of parts (so that can remove parts from node while iterating)
  RNArray<R3SurfelNode *> parts; 
  for (int i = 0; i < node->NParts(); i++) {
    R3SurfelNode *part = node->Part(i);
    parts.Insert(part);
  }

  // Create parts
  for (int i = 0; i < nparts; i++) {
    // Create child
    R3SurfelNode *child = new R3SurfelNode();

    // Insert child into tree
    tree->InsertNode(child, node);

    // Move parts into child
    for (int j = 0; j < parts.NEntries(); j++) {
      if (membership[j] != i) continue;
      R3SurfelNode *part = parts.Kth(j);
      tree->RemoveNode(part);
      tree->InsertNode(part, child);
    }

    // Check number of parts in child
    if (child->NParts() == 0) {
      tree->RemoveNode(child);
      delete child;
      continue;
    }
  }

  // Check if result is only one part
  if (node->NParts() == 1) {
    R3SurfelNode *part = node->Part(0);
    for (int i = 0; i < parts.NEntries(); i++) {
      R3SurfelNode *p = parts.Kth(i);
      tree->RemoveNode(p);
      tree->InsertNode(p, node);
    }
    tree->RemoveNode(part);
    delete part;
    status = 0;
  }


  ////// CLEAN UP ////// 

  delete [] membership;
  delete [] centroids;

  // Return whether anything was split
  return status;
}

// Assembly:
subq $0x218, %rsp            # imm = 0x218
movq %rdi, 0x208(%rsp)
movq %rsi, 0x200(%rsp)
movl %edx, 0x1fc(%rsp)
movl 0x1fc(%rsp), %eax
movl %eax, 0xb4(%rsp)
movq 0x200(%rsp), %rdi
callq 0x31660
movl %eax, %ecx
movl 0xb4(%rsp), %eax
cmpl %ecx, %eax
jl 0x63e16
movl $0x0, 0x214(%rsp)
jmp 0x647a4
movl $0x1, 0x1f8(%rsp)
movslq 0x1fc(%rsp), %rax
movq %rax, 0xa0(%rsp)
movl $0x18, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
movq %rdi, 0x98(%rsp)
callq 0x13030
movq %rax, %rcx
movq 0xa0(%rsp), %rax
movq %rcx, 0xa8(%rsp)
cmpq $0x0, %rax
je 0x63ed5
movq 0xa8(%rsp), %rax
movq 0xa0(%rsp), %rcx
imulq $0x18, %rcx, %rdx
movq %rax, %rcx
addq %rdx, %rcx
movq %rcx, 0x88(%rsp)
movq %rax, 0x90(%rsp)
movq 0x90(%rsp), %rdi
movq %rdi, 0x80(%rsp)
callq 0xf8340
jmp 0x63eb4
movq 0x88(%rsp), %rcx
movq 0x80(%rsp), %rax
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0x90(%rsp)
jne 0x63e9d
movq 0xa8(%rsp), %rax
movq %rax, 0x1f0(%rsp)
movq 0x200(%rsp), %rdi
callq 0x31660
cltq
movl $0x4, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
callq 0x13030
movq %rax, 0x1d8(%rsp)
movl $0x0, 0x1d4(%rsp)
movl 0x1d4(%rsp), %eax
cmpl 0x1fc(%rsp), %eax
jge 0x64005
movl 0x1d4(%rsp), %eax
movl %eax, 0x7c(%rsp)
movq 0x200(%rsp), %rdi
callq 0x31660
movl %eax, %ecx
movl 0x7c(%rsp), %eax
imull %ecx, %eax
cltd
idivl 0x1fc(%rsp)
movl %eax, 0x1d0(%rsp)
movq 0x200(%rsp), %rdi
movl 0x1d0(%rsp), %esi
callq 0x43680
movq %rax, 0x1c8(%rsp)
movq 0x1c8(%rsp), %rsi
leaq 0x1b0(%rsp), %rdi
callq 0x663c0
movq 0x1f0(%rsp), %rdi
movslq 0x1d4(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0x1b0(%rsp), %rsi
callq 0xf89e0
movl 0x1d4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x1d4(%rsp)
jmp 0x63f28
movq 0x98(%rsp), %rsi
movq 0xa8(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1e8(%rsp)
movl %eax, 0x1e4(%rsp)
callq 0x13a90
jmp 0x647b3
movl $0x0, 0x1ac(%rsp)
movl 0x1ac(%rsp), %eax
movl %eax, 0x78(%rsp)
movq 0x200(%rsp), %rdi
callq 0x31660
movl %eax, %ecx
movl 0x78(%rsp), %eax
cmpl %ecx, %eax
jge 0x6405c
movq 0x1d8(%rsp), %rax
movslq 0x1ac(%rsp), %rcx
movl $0xffffffff, (%rax,%rcx,4) # imm = 0xFFFFFFFF
movl 0x1ac(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x1ac(%rsp)
jmp 0x64010
movl $0x0, 0x1a8(%rsp)
movl $0x10, 0x1a4(%rsp)
movl $0x0, 0x1a0(%rsp)
cmpl $0x10, 0x1a0(%rsp)
jge 0x643be
movl $0x1, 0x1a8(%rsp)
movl $0x0, 0x19c(%rsp)
movl 0x19c(%rsp), %eax
movl %eax, 0x74(%rsp)
movq 0x200(%rsp), %rdi
callq 0x31660
movl %eax, %ecx
movl 0x74(%rsp), %eax
cmpl %ecx, %eax
jge 0x641f0
movq 0x200(%rsp), %rdi
movl 0x19c(%rsp), %esi
callq 0x43680
movq %rax, 0x190(%rsp)
movq 0x190(%rsp), %rsi
leaq 0x178(%rsp), %rdi
callq 0x663c0
movsd 0x140090(%rip), %xmm0   # 0x1a4190
movsd %xmm0, 0x170(%rsp)
movl $0x0, 0x16c(%rsp)
movl 0x16c(%rsp), %eax
cmpl 0x1fc(%rsp), %eax
jge 0x641d8
movq 0x1f0(%rsp), %rdi
movslq 0x16c(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0x178(%rsp), %rsi
callq 0x8e7b0
movsd %xmm0, 0x160(%rsp)
movsd 0x160(%rsp), %xmm1
movsd 0x170(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x641c0
movsd 0x160(%rsp), %xmm0
movsd %xmm0, 0x170(%rsp)
movq 0x1d8(%rsp), %rax
movslq 0x19c(%rsp), %rcx
movl (%rax,%rcx,4), %eax
cmpl 0x16c(%rsp), %eax
je 0x641a6
movl $0x0, 0x1a8(%rsp)
movl 0x16c(%rsp), %edx
movq 0x1d8(%rsp), %rax
movslq 0x19c(%rsp), %rcx
movl %edx, (%rax,%rcx,4)
jmp 0x641c2
movl 0x16c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x16c(%rsp)
jmp 0x64114
jmp 0x641da
movl 0x19c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x19c(%rsp)
jmp 0x640a1
cmpl $0x0, 0x1a8(%rsp)
je 0x641ff
jmp 0x643be
movl $0x0, 0x15c(%rsp)
movl 0x15c(%rsp), %eax
cmpl 0x1fc(%rsp), %eax
jge 0x643a6
xorps %xmm0, %xmm0
movsd %xmm0, 0x150(%rsp)
movq 0x1f0(%rsp), %rdi
movslq 0x15c(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0x599e68(%rip), %rsi    # 0x5fe0b0
callq 0xf89e0
movl $0x0, 0x14c(%rsp)
movl 0x14c(%rsp), %eax
movl %eax, 0x70(%rsp)
movq 0x200(%rsp), %rdi
callq 0x31660
movl %eax, %ecx
movl 0x70(%rsp), %eax
cmpl %ecx, %eax
jge 0x64357
movq 0x1d8(%rsp), %rax
movslq 0x14c(%rsp), %rcx
movl (%rax,%rcx,4), %eax
cmpl 0x15c(%rsp), %eax
je 0x6429f
jmp 0x64341
movq 0x200(%rsp), %rdi
movl 0x14c(%rsp), %esi
callq 0x43680
movq %rax, 0x140(%rsp)
movq 0x140(%rsp), %rdi
callq 0x5ecb0
movsd %xmm0, 0x68(%rsp)
movq 0x140(%rsp), %rsi
leaq 0x110(%rsp), %rdi
callq 0x663c0
movsd 0x68(%rsp), %xmm0
leaq 0x128(%rsp), %rdi
leaq 0x110(%rsp), %rsi
callq 0x66380
movq 0x1f0(%rsp), %rdi
movslq 0x15c(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0x128(%rsp), %rsi
callq 0xf9020
movq 0x140(%rsp), %rdi
callq 0x5ecb0
addsd 0x150(%rsp), %xmm0
movsd %xmm0, 0x150(%rsp)
movl 0x14c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x14c(%rsp)
jmp 0x64258
movsd 0x150(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x6438e
movsd 0x150(%rsp), %xmm0
movq 0x1f0(%rsp), %rdi
movslq 0x15c(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
callq 0xf9160
jmp 0x64390
movl 0x15c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x15c(%rsp)
jmp 0x6420a
jmp 0x643a8
movl 0x1a0(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x1a0(%rsp)
jmp 0x6407d
leaq 0x100(%rsp), %rdi
callq 0x1b960
movl $0x0, 0xfc(%rsp)
movl 0xfc(%rsp), %eax
movl %eax, 0x64(%rsp)
movq 0x200(%rsp), %rdi
callq 0x31660
movl %eax, %ecx
movl 0x64(%rsp), %eax
cmpl %ecx, %eax
jge 0x64465
movq 0x200(%rsp), %rdi
movl 0xfc(%rsp), %esi
callq 0x43680
movq %rax, 0x58(%rsp)
jmp 0x64413
movq 0x58(%rsp), %rax
movq %rax, 0xf0(%rsp)
movq 0xf0(%rsp), %rsi
leaq 0x100(%rsp), %rdi
callq 0x1bee0
jmp 0x64437
jmp 0x64439
movl 0xfc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xfc(%rsp)
jmp 0x643d6
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1e8(%rsp)
movl %eax, 0x1e4(%rsp)
jmp 0x64795
movl $0x0, 0xec(%rsp)
movl 0xec(%rsp), %eax
cmpl 0x1fc(%rsp), %eax
jge 0x64622
movl $0xd0, %edi
callq 0x13570
movq %rax, 0x50(%rsp)
jmp 0x64495
movq 0x50(%rsp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x5e770
jmp 0x644a5
movq 0x50(%rsp), %rax
movq %rax, 0xe0(%rsp)
movq 0x208(%rsp), %rdi
movq 0xe0(%rsp), %rsi
movq 0x200(%rsp), %rdx
callq 0x620c0
jmp 0x644d1
movl $0x0, 0xdc(%rsp)
movl 0xdc(%rsp), %eax
movl %eax, 0x4c(%rsp)
leaq 0x100(%rsp), %rdi
callq 0x16d50
movl %eax, %ecx
movl 0x4c(%rsp), %eax
cmpl %ecx, %eax
jge 0x645c1
movq 0x1d8(%rsp), %rax
movslq 0xdc(%rsp), %rcx
movl (%rax,%rcx,4), %eax
cmpl 0xec(%rsp), %eax
je 0x6454b
jmp 0x645ab
movq 0x50(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1e8(%rsp)
movl %eax, 0x1e4(%rsp)
movl $0xd0, %esi
callq 0x13580
jmp 0x64795
movl 0xdc(%rsp), %esi
leaq 0x100(%rsp), %rdi
callq 0x310e0
movq %rax, 0x40(%rsp)
jmp 0x64566
movq 0x40(%rsp), %rax
movq %rax, 0xd0(%rsp)
movq 0x208(%rsp), %rdi
movq 0xd0(%rsp), %rsi
callq 0x62370
jmp 0x6458a
movq 0x208(%rsp), %rdi
movq 0xd0(%rsp), %rsi
movq 0xe0(%rsp), %rdx
callq 0x620c0
jmp 0x645a9
jmp 0x645ab
movl 0xdc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xdc(%rsp)
jmp 0x644dc
movq 0xe0(%rsp), %rdi
callq 0x31660
cmpl $0x0, %eax
jne 0x6460a
movq 0x208(%rsp), %rdi
movq 0xe0(%rsp), %rsi
callq 0x62370
jmp 0x645ea
movq 0xe0(%rsp), %rax
movq %rax, 0x38(%rsp)
cmpq $0x0, %rax
je 0x64608
movq 0x38(%rsp), %rdi
movq (%rdi), %rax
callq *0x8(%rax)
jmp 0x6460c
jmp 0x6460c
movl 0xec(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xec(%rsp)
jmp 0x64470
movq 0x200(%rsp), %rdi
callq 0x31660
cmpl $0x1, %eax
jne 0x6473e
movq 0x200(%rsp), %rdi
xorl %esi, %esi
callq 0x43680
movq %rax, 0x30(%rsp)
jmp 0x6464e
movq 0x30(%rsp), %rax
movq %rax, 0xc8(%rsp)
movl $0x0, 0xc4(%rsp)
movl 0xc4(%rsp), %eax
movl %eax, 0x2c(%rsp)
leaq 0x100(%rsp), %rdi
callq 0x16d50
movl %eax, %ecx
movl 0x2c(%rsp), %eax
cmpl %ecx, %eax
jge 0x646fe
movl 0xc4(%rsp), %esi
leaq 0x100(%rsp), %rdi
callq 0x310e0
movq %rax, 0x20(%rsp)
jmp 0x646a3
movq 0x20(%rsp), %rax
movq %rax, 0xb8(%rsp)
movq 0x208(%rsp), %rdi
movq 0xb8(%rsp), %rsi
callq 0x62370
jmp 0x646c7
movq 0x208(%rsp), %rdi
movq 0xb8(%rsp), %rsi
movq 0x200(%rsp), %rdx
callq 0x620c0
jmp 0x646e6
jmp 0x646e8
movl 0xc4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xc4(%rsp)
jmp 0x64666
movq 0x208(%rsp), %rdi
movq 0xc8(%rsp), %rsi
callq 0x62370
jmp 0x64715
movq 0xc8(%rsp), %rax
movq %rax, 0x18(%rsp)
cmpq $0x0, %rax
je 0x64733
movq 0x18(%rsp), %rdi
movq (%rdi), %rax
callq *0x8(%rax)
movl $0x0, 0x1f8(%rsp)
movq 0x1d8(%rsp), %rax
movq %rax, 0x10(%rsp)
cmpq $0x0, %rax
je 0x6475b
movq 0x10(%rsp), %rdi
callq 0x13750
movq 0x1f0(%rsp), %rax
movq %rax, 0x8(%rsp)
cmpq $0x0, %rax
je 0x64778
movq 0x8(%rsp), %rdi
callq 0x13750
movl 0x1f8(%rsp), %eax
movl %eax, 0x214(%rsp)
leaq 0x100(%rsp), %rdi
callq 0x1b9a0
jmp 0x647a4
leaq 0x100(%rsp), %rdi
callq 0x1b9a0
jmp 0x647b3
movl 0x214(%rsp), %eax
addq $0x218, %rsp            # imm = 0x218
retq
movq 0x1e8(%rsp), %rdi
callq 0x13970