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

  // Adjust number of parts
  if (nparts > node->NBlocks()) nparts = node->NBlocks();
  if (nparts == 1) return 0;

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

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

  // Initialize kmeans centroids
  for (int i = 0; i < nparts; i++) {
    int block_index = i * node->NBlocks() / nparts;
    R3SurfelBlock *block = node->Block(block_index);
    centroids[i] = block->Centroid();
  }

  // Intialize kmeans membership
  for (int i = 0; i < node->NBlocks(); 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->NBlocks(); j++) {
      R3SurfelBlock *block = node->Block(j);
      R3Point block_centroid = block->Centroid();
      RNLength closest_distance = FLT_MAX;
      for (int k = 0; k < nparts; k++) {
        RNLength distance = R3SquaredDistance(centroids[k], block_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
      int weight = 0;
      centroids[j] = R3zero_point;
      for (int k = 0; k < node->NBlocks(); k++) {
        if (membership[k] != j) continue;
        R3SurfelBlock *block = node->Block(k);
        centroids[j] += block->NSurfels() * block->Centroid();
        weight += block->NSurfels();
      }
      if (weight > 0) centroids[j] /= weight;
    }
  }

  ////// CREATE PARTS AND REDISTRIBUTE BLOCKS ////// 

  // Create temporary array of blocks (so that can remove blocks from node while iterating)
  RNArray<R3SurfelBlock *> blocks; 
  for (int i = 0; i < node->NBlocks(); i++) {
    R3SurfelBlock *block = node->Block(i);
    blocks.Insert(block);
  }

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

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

    // Move blocks into part
    for (int j = 0; j < blocks.NEntries(); j++) {
      if (membership[j] != i) continue;
      R3SurfelBlock *block = blocks.Kth(j);
      node->RemoveBlock(block);
      part->InsertBlock(block);
    }

    // Check number of blocks in part
    if (part->NBlocks() == 0) {
      tree->RemoveNode(part);
      delete part;
      continue;
    }
  }

  // Just checking
  assert(node->NBlocks() == 0);

  // Check if result is only one part
  if (node->NParts() == 1) {
    R3SurfelNode *part = node->Part(0);
    for (int i = 0; i < blocks.NEntries(); i++) {
      R3SurfelBlock *block = blocks.Kth(i);
      part->RemoveBlock(block);
      node->InsertBlock(block);
    }
    tree->RemoveNode(part);
    delete part;
    status = 0;
  }


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

  delete [] membership;
  delete [] centroids;

  // Return whether anything was split
  return status;
}

// Assembly:
subq $0x208, %rsp            # imm = 0x208
movq %rdi, 0x1f8(%rsp)
movq %rsi, 0x1f0(%rsp)
movl %edx, 0x1ec(%rsp)
movl $0x1, 0x1e8(%rsp)
movl 0x1ec(%rsp), %eax
movl %eax, 0xac(%rsp)
movq 0x1f0(%rsp), %rdi
callq 0x186b0
movl %eax, %ecx
movl 0xac(%rsp), %eax
cmpl %ecx, %eax
jle 0x63415
movq 0x1f0(%rsp), %rdi
callq 0x186b0
movl %eax, 0x1ec(%rsp)
cmpl $0x1, 0x1ec(%rsp)
jne 0x6342f
movl $0x0, 0x204(%rsp)
jmp 0x63da1
movslq 0x1ec(%rsp), %rax
movq %rax, 0x98(%rsp)
movl $0x18, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
movq %rdi, 0x90(%rsp)
callq 0x13030
movq %rax, %rcx
movq 0x98(%rsp), %rax
movq %rcx, 0xa0(%rsp)
cmpq $0x0, %rax
je 0x634dd
movq 0xa0(%rsp), %rax
movq 0x98(%rsp), %rcx
imulq $0x18, %rcx, %rdx
movq %rax, %rcx
addq %rdx, %rcx
movq %rcx, 0x80(%rsp)
movq %rax, 0x88(%rsp)
movq 0x88(%rsp), %rdi
movq %rdi, 0x78(%rsp)
callq 0xf8340
jmp 0x634bf
movq 0x80(%rsp), %rcx
movq 0x78(%rsp), %rax
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0x88(%rsp)
jne 0x634ab
movq 0xa0(%rsp), %rax
movq %rax, 0x1e0(%rsp)
movq 0x1f0(%rsp), %rdi
callq 0x186b0
cltq
movl $0x4, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
callq 0x13030
movq %rax, 0x1c8(%rsp)
movl $0x0, 0x1c4(%rsp)
movl 0x1c4(%rsp), %eax
cmpl 0x1ec(%rsp), %eax
jge 0x6360d
movl 0x1c4(%rsp), %eax
movl %eax, 0x74(%rsp)
movq 0x1f0(%rsp), %rdi
callq 0x186b0
movl %eax, %ecx
movl 0x74(%rsp), %eax
imull %ecx, %eax
cltd
idivl 0x1ec(%rsp)
movl %eax, 0x1c0(%rsp)
movq 0x1f0(%rsp), %rdi
movl 0x1c0(%rsp), %esi
callq 0x186d0
movq %rax, 0x1b8(%rsp)
movq 0x1b8(%rsp), %rsi
leaq 0x1a0(%rsp), %rdi
callq 0x66340
movq 0x1e0(%rsp), %rdi
movslq 0x1c4(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0x1a0(%rsp), %rsi
callq 0xf89e0
movl 0x1c4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x1c4(%rsp)
jmp 0x63530
movq 0x90(%rsp), %rsi
movq 0xa0(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1d8(%rsp)
movl %eax, 0x1d4(%rsp)
callq 0x13a90
jmp 0x63db0
movl $0x0, 0x19c(%rsp)
movl 0x19c(%rsp), %eax
movl %eax, 0x70(%rsp)
movq 0x1f0(%rsp), %rdi
callq 0x186b0
movl %eax, %ecx
movl 0x70(%rsp), %eax
cmpl %ecx, %eax
jge 0x63664
movq 0x1c8(%rsp), %rax
movslq 0x19c(%rsp), %rcx
movl $0xffffffff, (%rax,%rcx,4) # imm = 0xFFFFFFFF
movl 0x19c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x19c(%rsp)
jmp 0x63618
movl $0x0, 0x198(%rsp)
movl $0x10, 0x194(%rsp)
movl $0x0, 0x190(%rsp)
cmpl $0x10, 0x190(%rsp)
jge 0x639bd
movl $0x1, 0x198(%rsp)
movl $0x0, 0x18c(%rsp)
movl 0x18c(%rsp), %eax
movl %eax, 0x6c(%rsp)
movq 0x1f0(%rsp), %rdi
callq 0x186b0
movl %eax, %ecx
movl 0x6c(%rsp), %eax
cmpl %ecx, %eax
jge 0x637f8
movq 0x1f0(%rsp), %rdi
movl 0x18c(%rsp), %esi
callq 0x186d0
movq %rax, 0x180(%rsp)
movq 0x180(%rsp), %rsi
leaq 0x168(%rsp), %rdi
callq 0x66340
movsd 0x140a88(%rip), %xmm0   # 0x1a4190
movsd %xmm0, 0x160(%rsp)
movl $0x0, 0x15c(%rsp)
movl 0x15c(%rsp), %eax
cmpl 0x1ec(%rsp), %eax
jge 0x637e0
movq 0x1e0(%rsp), %rdi
movslq 0x15c(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0x168(%rsp), %rsi
callq 0x8e7b0
movsd %xmm0, 0x150(%rsp)
movsd 0x150(%rsp), %xmm1
movsd 0x160(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x637c8
movsd 0x150(%rsp), %xmm0
movsd %xmm0, 0x160(%rsp)
movq 0x1c8(%rsp), %rax
movslq 0x18c(%rsp), %rcx
movl (%rax,%rcx,4), %eax
cmpl 0x15c(%rsp), %eax
je 0x637ae
movl $0x0, 0x198(%rsp)
movl 0x15c(%rsp), %edx
movq 0x1c8(%rsp), %rax
movslq 0x18c(%rsp), %rcx
movl %edx, (%rax,%rcx,4)
jmp 0x637ca
movl 0x15c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x15c(%rsp)
jmp 0x6371c
jmp 0x637e2
movl 0x18c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x18c(%rsp)
jmp 0x636a9
cmpl $0x0, 0x198(%rsp)
je 0x63807
jmp 0x639bd
movl $0x0, 0x14c(%rsp)
movl 0x14c(%rsp), %eax
cmpl 0x1ec(%rsp), %eax
jge 0x639a5
movl $0x0, 0x148(%rsp)
movq 0x1e0(%rsp), %rdi
movslq 0x14c(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0x59a861(%rip), %rsi    # 0x5fe0b0
callq 0xf89e0
movl $0x0, 0x144(%rsp)
movl 0x144(%rsp), %eax
movl %eax, 0x68(%rsp)
movq 0x1f0(%rsp), %rdi
callq 0x186b0
movl %eax, %ecx
movl 0x68(%rsp), %eax
cmpl %ecx, %eax
jge 0x6395e
movq 0x1c8(%rsp), %rax
movslq 0x144(%rsp), %rcx
movl (%rax,%rcx,4), %eax
cmpl 0x14c(%rsp), %eax
je 0x638a6
jmp 0x63948
movq 0x1f0(%rsp), %rdi
movl 0x144(%rsp), %esi
callq 0x186d0
movq %rax, 0x138(%rsp)
movq 0x138(%rsp), %rdi
callq 0x1c0e0
cvtsi2sd %eax, %xmm0
movsd %xmm0, 0x60(%rsp)
movq 0x138(%rsp), %rsi
leaq 0x108(%rsp), %rdi
callq 0x66340
movsd 0x60(%rsp), %xmm0
leaq 0x120(%rsp), %rdi
leaq 0x108(%rsp), %rsi
callq 0x66380
movq 0x1e0(%rsp), %rdi
movslq 0x14c(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0x120(%rsp), %rsi
callq 0xf9020
movq 0x138(%rsp), %rdi
callq 0x1c0e0
addl 0x148(%rsp), %eax
movl %eax, 0x148(%rsp)
movl 0x144(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x144(%rsp)
jmp 0x6385f
cmpl $0x0, 0x148(%rsp)
jle 0x6398d
cvtsi2sdl 0x148(%rsp), %xmm0
movq 0x1e0(%rsp), %rdi
movslq 0x14c(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
callq 0xf9160
jmp 0x6398f
movl 0x14c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x14c(%rsp)
jmp 0x63812
jmp 0x639a7
movl 0x190(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x190(%rsp)
jmp 0x63685
leaq 0xf8(%rsp), %rdi
callq 0x436b0
movl $0x0, 0xf4(%rsp)
movl 0xf4(%rsp), %eax
movl %eax, 0x5c(%rsp)
movq 0x1f0(%rsp), %rdi
callq 0x186b0
movl %eax, %ecx
movl 0x5c(%rsp), %eax
cmpl %ecx, %eax
jge 0x63a64
movq 0x1f0(%rsp), %rdi
movl 0xf4(%rsp), %esi
callq 0x186d0
movq %rax, 0x50(%rsp)
jmp 0x63a12
movq 0x50(%rsp), %rax
movq %rax, 0xe8(%rsp)
movq 0xe8(%rsp), %rsi
leaq 0xf8(%rsp), %rdi
callq 0x436c0
jmp 0x63a36
jmp 0x63a38
movl 0xf4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xf4(%rsp)
jmp 0x639d5
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1d8(%rsp)
movl %eax, 0x1d4(%rsp)
jmp 0x63d92
movl $0x0, 0xe4(%rsp)
movl 0xe4(%rsp), %eax
cmpl 0x1ec(%rsp), %eax
jge 0x63c21
movl $0xd0, %edi
callq 0x13570
movq %rax, 0x48(%rsp)
jmp 0x63a94
movq 0x48(%rsp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x5e770
jmp 0x63aa4
movq 0x48(%rsp), %rax
movq %rax, 0xd8(%rsp)
movq 0x1f8(%rsp), %rdi
movq 0xd8(%rsp), %rsi
movq 0x1f0(%rsp), %rdx
callq 0x620c0
jmp 0x63ad0
movl $0x0, 0xd4(%rsp)
movl 0xd4(%rsp), %eax
movl %eax, 0x44(%rsp)
leaq 0xf8(%rsp), %rdi
callq 0x16d50
movl %eax, %ecx
movl 0x44(%rsp), %eax
cmpl %ecx, %eax
jge 0x63bc0
movq 0x1c8(%rsp), %rax
movslq 0xd4(%rsp), %rcx
movl (%rax,%rcx,4), %eax
cmpl 0xe4(%rsp), %eax
je 0x63b4a
jmp 0x63baa
movq 0x48(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x1d8(%rsp)
movl %eax, 0x1d4(%rsp)
movl $0xd0, %esi
callq 0x13580
jmp 0x63d92
movl 0xd4(%rsp), %esi
leaq 0xf8(%rsp), %rdi
callq 0x436f0
movq %rax, 0x38(%rsp)
jmp 0x63b65
movq 0x38(%rsp), %rax
movq %rax, 0xc8(%rsp)
movq 0x1f0(%rsp), %rdi
movq 0xc8(%rsp), %rsi
movq (%rdi), %rax
movq 0x30(%rax), %rax
callq *%rax
jmp 0x63b8d
movq 0xd8(%rsp), %rdi
movq 0xc8(%rsp), %rsi
movq (%rdi), %rax
movq 0x28(%rax), %rax
callq *%rax
jmp 0x63ba8
jmp 0x63baa
movl 0xd4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xd4(%rsp)
jmp 0x63adb
movq 0xd8(%rsp), %rdi
callq 0x186b0
cmpl $0x0, %eax
jne 0x63c09
movq 0x1f8(%rsp), %rdi
movq 0xd8(%rsp), %rsi
callq 0x62370
jmp 0x63be9
movq 0xd8(%rsp), %rax
movq %rax, 0x30(%rsp)
cmpq $0x0, %rax
je 0x63c07
movq 0x30(%rsp), %rdi
movq (%rdi), %rax
callq *0x8(%rax)
jmp 0x63c0b
jmp 0x63c0b
movl 0xe4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xe4(%rsp)
jmp 0x63a6f
movq 0x1f0(%rsp), %rdi
callq 0x31660
cmpl $0x1, %eax
jne 0x63d3d
movq 0x1f0(%rsp), %rdi
xorl %esi, %esi
callq 0x43680
movq %rax, 0x28(%rsp)
jmp 0x63c4d
movq 0x28(%rsp), %rax
movq %rax, 0xc0(%rsp)
movl $0x0, 0xbc(%rsp)
movl 0xbc(%rsp), %eax
movl %eax, 0x24(%rsp)
leaq 0xf8(%rsp), %rdi
callq 0x16d50
movl %eax, %ecx
movl 0x24(%rsp), %eax
cmpl %ecx, %eax
jge 0x63cfd
movl 0xbc(%rsp), %esi
leaq 0xf8(%rsp), %rdi
callq 0x436f0
movq %rax, 0x18(%rsp)
jmp 0x63ca2
movq 0x18(%rsp), %rax
movq %rax, 0xb0(%rsp)
movq 0xc0(%rsp), %rdi
movq 0xb0(%rsp), %rsi
movq (%rdi), %rax
movq 0x30(%rax), %rax
callq *%rax
jmp 0x63cca
movq 0x1f0(%rsp), %rdi
movq 0xb0(%rsp), %rsi
movq (%rdi), %rax
movq 0x28(%rax), %rax
callq *%rax
jmp 0x63ce5
jmp 0x63ce7
movl 0xbc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xbc(%rsp)
jmp 0x63c65
movq 0x1f8(%rsp), %rdi
movq 0xc0(%rsp), %rsi
callq 0x62370
jmp 0x63d14
movq 0xc0(%rsp), %rax
movq %rax, 0x10(%rsp)
cmpq $0x0, %rax
je 0x63d32
movq 0x10(%rsp), %rdi
movq (%rdi), %rax
callq *0x8(%rax)
movl $0x0, 0x1e8(%rsp)
movq 0x1c8(%rsp), %rax
movq %rax, 0x8(%rsp)
cmpq $0x0, %rax
je 0x63d5a
movq 0x8(%rsp), %rdi
callq 0x13750
movq 0x1e0(%rsp), %rax
movq %rax, (%rsp)
cmpq $0x0, %rax
je 0x63d75
movq (%rsp), %rdi
callq 0x13750
movl 0x1e8(%rsp), %eax
movl %eax, 0x204(%rsp)
leaq 0xf8(%rsp), %rdi
callq 0x43730
jmp 0x63da1
leaq 0xf8(%rsp), %rdi
callq 0x43730
jmp 0x63db0
movl 0x204(%rsp), %eax
addq $0x208, %rsp            # imm = 0x208
retq
movq 0x1d8(%rsp), %rdi
callq 0x13970
nopl (%rax)