// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/R3Utils/R3Segmentation.cpp
// Function: gaps::R3Segmentation::MergeClusters()
int R3Segmentation::
MergeClusters(void)
{
  // Initialize statistics
  int cluster_count = clusters.NEntries();
  int merge_count = 0;
  int push_count = 0;

  //////////

  // Create pairs between clusters with neighbor points
  RNArray<R3SegmentationPair *> pairs;
  for (int i = 0; i < clusters.NEntries(); i++) {
    R3SegmentationCluster *cluster0 = clusters.Kth(i);

    // Sample points
    const int max_points = 64;
    int jstep = cluster0->points.NEntries() / max_points;
    if (jstep == 0) jstep = 1;
    for (int j = 0; j < cluster0->points.NEntries(); j += jstep) {
      R3SegmentationPoint *point0 = cluster0->points.Kth(j);

      // Check neighbors
      for (int k = 0; k < point0->neighbors.NEntries(); k++) {
        R3SegmentationPoint *point1 = point0->neighbors.Kth(k);
        if (point0 == point1) continue;
        R3SegmentationCluster *cluster1 = point1->cluster;
        if (!cluster1) continue;
        if (cluster0 == cluster1) continue;

        // Check if already have pair
        if (FindR3SegmentationPair(cluster0, cluster1)) continue;

        // Compute affinity
        RNScalar affinity = cluster0->Affinity(cluster1);
        if ((affinity < min_pair_affinity) && (cluster_count <= max_clusters) && (min_cluster_points == 0) && (min_cluster_area == 0)) continue;

        // Create pair
        R3SegmentationPair *pair = new R3SegmentationPair(cluster0, cluster1, affinity);
        if (!pair) continue;

        // Insert pair
        pairs.Insert(pair);
      }        
    }
  }

  // Check if there are any pairs
  if (pairs.IsEmpty()) return 1;

  //////////

  // Initialize heap
  R3SegmentationPair tmp;
  RNHeap<R3SegmentationPair *> heap(&tmp, &tmp.affinity, &tmp.heapentry, FALSE);
  for (int i = 0; i < pairs.NEntries(); i++) {
    R3SegmentationPair *pair = pairs.Kth(i);
    heap.Push(pair);
  }

  // Merge clusters hierarchically
  while (!heap.IsEmpty()) {
    // Get pair
    R3SegmentationPair *pair = heap.Pop();

    // Check if we are done
    if ((pair->affinity < min_pair_affinity) && (cluster_count <= max_clusters) && (min_cluster_points == 0) && (min_cluster_area == 0)) {
      break;
    }

    // Get clusters
    R3SegmentationCluster *cluster0 = pair->clusters[0];
    R3SegmentationCluster *cluster1 = pair->clusters[1];

    // Check if either cluster has already been merged
    if (cluster0->parent || cluster1->parent) {
      // Find ancestors
      R3SegmentationCluster *ancestor0 = cluster0;
      R3SegmentationCluster *ancestor1 = cluster1;
      while (ancestor0->parent) ancestor0 = ancestor0->parent;
      while (ancestor1->parent) ancestor1 = ancestor1->parent;
      if (ancestor0 != ancestor1) {
        if (!FindR3SegmentationPair(ancestor0, ancestor1)) {
          RNScalar affinity = ancestor0->Affinity(ancestor1);
          if ((cluster_count > max_clusters) || (min_cluster_points > 0) || (min_cluster_area > 0) || (affinity >= min_pair_affinity)) {
            // Create a pair between the ancestors
            R3SegmentationPair *pair = new R3SegmentationPair(ancestor0, ancestor1, affinity);
            heap.Push(pair);
            push_count++;
          }
        }
      }
    }
    else {
      // Check if should merge
      RNBoolean merge = TRUE;
      if ((min_pair_affinity > 0) && (pair->affinity < min_pair_affinity)) merge = FALSE;
      if ((min_cluster_area > 0) && (cluster0->area < min_cluster_area)) merge = TRUE;
      if ((min_cluster_area > 0) && (cluster1->area < min_cluster_area)) merge = TRUE;
      if ((min_cluster_points > 0) && (cluster0->points.NEntries() < min_cluster_points)) merge = TRUE;
      if ((min_cluster_points > 0) && (cluster1->points.NEntries() < min_cluster_points)) merge = TRUE;
      if ((max_clusters > 0) && (cluster_count > max_clusters)) merge = TRUE;
      if (!merge) { delete pair; continue; }

      // Print message (do not print, but keep so that compiler does not complain
      // about unused variables merge_count and push_count
      if (print_progress == 123) {
        static unsigned long count = 0;
        if ((count++ % 1000) == 0) {
          printf("        %15.12f : %9d %9d : %15d %15d %15d %15d\n", pair->affinity, 
                 cluster0->points.NEntries(), cluster1->points.NEntries(), 
                 heap.NEntries(), merge_count, push_count, cluster_count);
        }
      }
      
#if 0
      // Create merged cluster
      R3SegmentationCluster *cluster = new R3SegmentationCluster(cluster0, cluster1);
      clusters.Insert(cluster);
      cluster_count--;
      merge_count++;
#else
      // Merge smaller cluster into bigger one
      R3SegmentationCluster *parent = (cluster0->points.NEntries() > cluster1->points.NEntries()) ? cluster0 : cluster1;
      R3SegmentationCluster *child = (cluster0->points.NEntries() > cluster1->points.NEntries()) ? cluster1 : cluster0;
      parent->InsertChild(child);
      cluster_count--;
      merge_count++;
#endif
    }

    // Delete pair
    delete pair;
  }

  // Remove merged clusters
  RNArray<R3SegmentationCluster *> merged_clusters;
  RNArray<R3SegmentationCluster *> all_clusters = clusters;
  clusters.Empty();
  for (int i = 0; i < all_clusters.NEntries(); i++) {
    R3SegmentationCluster *cluster = all_clusters.Kth(i);
    cluster->segmentation = this;
    cluster->segmentation_index = clusters.NEntries();    
    if (!cluster->parent) { clusters.Insert(cluster); continue; }
    cluster->parent->RemoveChild(cluster);
    merged_clusters.Insert(cluster);
  }

  // Delete merged clusters
  for (int i = 0; i < merged_clusters.NEntries(); i++) {
    R3SegmentationCluster *cluster = merged_clusters.Kth(i);
    delete cluster;
  }

  // Return success
  return 1;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xd8, %rsp
movq %rdi, %rbx
leaq 0x18(%rdi), %rax
movq %rax, 0x30(%rsp)
movl 0x24(%rdi), %eax
movl %eax, 0x14(%rsp)
leaq 0x38(%rsp), %rdi
callq 0xb1eda
xorl %ecx, %ecx
movq %rbx, 0x18(%rsp)
movslq 0x24(%rbx), %rax
cmpq %rax, %rcx
jge 0x2b10f
movq 0x30(%rsp), %rax
movq (%rax), %rax
movq %rcx, 0x68(%rsp)
movq (%rax,%rcx,8), %r13
movl 0x14(%r13), %ecx
movl %ecx, %eax
cltd
pushq $0x40
popq %rsi
idivl %esi
leal 0x3f(%rcx), %edx
cmpl $0x7f, %edx
pushq $0x1
popq %rdx
cmovbl %edx, %eax
cltq
movq %rax, 0x20(%rsp)
xorl %r15d, %r15d
movslq %ecx, %rax
cmpq %rax, %r15
jge 0x2b0fd
movq 0x8(%r13), %rax
movq (%rax,%r15,8), %r12
xorl %ebx, %ebx
movslq 0xbc(%r12), %rax
cmpq %rax, %rbx
jge 0x2b0ef
movq 0xb0(%r12), %rax
movq (%rax,%rbx,8), %rax
cmpq %rax, %r12
je 0x2b0e7
movq 0xc0(%rax), %rbp
testq %rbp, %rbp
sete %al
cmpq %rbp, %r13
sete %cl
orb %al, %cl
jne 0x2b0e7
movq %r13, %rdi
movq %rbp, %rsi
callq 0x2b536
testq %rax, %rax
jne 0x2b0e7
movq %r13, %rdi
movq %rbp, %rsi
callq 0x29606
movq 0x18(%rsp), %rax
movsd 0xb0(%rax), %xmm1
movsd %xmm0, 0x28(%rsp)
ucomisd %xmm0, %xmm1
jbe 0x2b0bb
movq 0x18(%rsp), %rax
movl 0x14(%rsp), %ecx
cmpl 0x34(%rax), %ecx
jg 0x2b0bb
movq 0x18(%rsp), %rax
cmpl $0x0, 0x38(%rax)
jne 0x2b0bb
movq 0x18(%rsp), %rax
movsd 0x40(%rax), %xmm0
ucomisd 0xc2f91(%rip), %xmm0    # 0xee048
jne 0x2b0bb
jnp 0x2b0e7
pushq $0x28
popq %rdi
callq 0x15620
movq %rax, %r14
movq %rax, %rdi
movq %r13, %rsi
movq %rbp, %rdx
movsd 0x28(%rsp), %xmm0
callq 0x29d4e
leaq 0x38(%rsp), %rdi
movq %r14, %rsi
callq 0x1a04c
incq %rbx
jmp 0x2b017
addq 0x20(%rsp), %r15
movl 0x14(%r13), %ecx
jmp 0x2b001
movq 0x68(%rsp), %rcx
incq %rcx
movq 0x18(%rsp), %rbx
jmp 0x2afc1
movl 0x44(%rsp), %eax
testl %eax, %eax
je 0x2b49a
xorpd %xmm0, %xmm0
movapd %xmm0, 0xb0(%rsp)
movupd %xmm0, 0xc8(%rsp)
orq $-0x1, 0xc0(%rsp)
leaq 0x70(%rsp), %r14
movapd %xmm0, (%r14)
movupd %xmm0, 0x18(%r14)
movupd %xmm0, 0x24(%r14)
movabsq $0x2000000018, %rcx     # imm = 0x2000000018
movq %rcx, 0x10(%r14)
xorl %ebx, %ebx
cltq
cmpq %rax, %rbx
jge 0x2b181
movq 0x38(%rsp), %rax
movq (%rax,%rbx,8), %rsi
movq %r14, %rdi
callq 0x2c434
incq %rbx
movl 0x44(%rsp), %eax
jmp 0x2b160
xorl %r15d, %r15d
movl $0x0, 0x28(%rsp)
movq 0x30(%rsp), %rbx
cmpl $0x0, 0x78(%rsp)
je 0x2b3c0
leaq 0x70(%rsp), %rdi
callq 0x2c53c
movq %rax, %r13
movsd 0x18(%rax), %xmm0
movq 0x18(%rsp), %rdx
movsd 0xb0(%rdx), %xmm2
movl 0x34(%rdx), %eax
movl 0x38(%rdx), %ecx
movsd 0x40(%rdx), %xmm1
ucomisd %xmm0, %xmm2
jbe 0x2b1e6
cmpl %eax, 0x14(%rsp)
jg 0x2b1e6
testl %ecx, %ecx
jne 0x2b1e6
ucomisd 0xc2e6a(%rip), %xmm1    # 0xee048
jne 0x2b1e6
jnp 0x2b3c0
movq (%r13), %r14
movq 0x8(%r13), %r12
cmpq $0x0, 0x18(%r14)
jne 0x2b294
cmpq $0x0, 0x18(%r12)
jne 0x2b294
movapd %xmm0, %xmm3
cmpnltpd %xmm2, %xmm3
xorpd %xmm4, %xmm4
cmpnltpd %xmm2, %xmm4
orpd %xmm3, %xmm4
movd %xmm4, %edx
andl $0x1, %edx
ucomisd 0xc2e1e(%rip), %xmm1    # 0xee048
jbe 0x2b24a
ucomisd 0xe8(%r14), %xmm1
pushq $0x1
popq %rsi
cmoval %esi, %edx
ucomisd 0xe8(%r12), %xmm1
jbe 0x2b24a
pushq $0x1
popq %rdx
testl %ecx, %ecx
jle 0x2b260
cmpl %ecx, 0x14(%r14)
pushq $0x1
popq %rsi
cmovll %esi, %edx
cmpl %ecx, 0x14(%r12)
cmovll %esi, %edx
cmpl %eax, 0x14(%rsp)
setg %cl
testl %eax, %eax
setg %al
testb %cl, %al
jne 0x2b33f
testl %edx, %edx
jne 0x2b33f
movq %r13, %rdi
callq 0x29dbe
movq %r13, %rdi
pushq $0x28
popq %rsi
callq 0x15630
jmp 0x2b191
movq %r14, %rbp
movq 0x18(%r14), %r14
testq %r14, %r14
jne 0x2b294
movq %r12, %r14
movq 0x18(%r12), %r12
testq %r12, %r12
jne 0x2b2a0
cmpq %r14, %rbp
je 0x2b27c
movq %rbp, %rdi
movq %r14, %rsi
callq 0x2b536
testq %rax, %rax
jne 0x2b27c
movq %rbp, %rdi
movq %r14, %rsi
callq 0x29606
movsd %xmm0, 0x20(%rsp)
movq 0x18(%rsp), %rax
movl 0x14(%rsp), %ecx
cmpl 0x34(%rax), %ecx
jg 0x2b30a
cmpl $0x0, 0x38(%rax)
jg 0x2b30a
movsd 0x40(%rax), %xmm0
ucomisd 0xc2d54(%rip), %xmm0    # 0xee048
ja 0x2b30a
movsd 0x20(%rsp), %xmm0
ucomisd 0xb0(%rax), %xmm0
jb 0x2b27c
pushq $0x28
popq %rdi
callq 0x15620
movq %rax, %r12
movq %rax, %rdi
movq %rbp, %rsi
movq %r14, %rdx
movsd 0x20(%rsp), %xmm0
callq 0x29d4e
leaq 0x70(%rsp), %rdi
movq %r12, %rsi
callq 0x2c434
incl 0x28(%rsp)
jmp 0x2b27c
movq 0x18(%rsp), %rax
cmpl $0x7b, 0xdc(%rax)
jne 0x2b398
movq 0x130b0c(%rip), %rax    # 0x15be60
leaq 0x1(%rax), %rcx
movq %rcx, 0x130b01(%rip)    # 0x15be60
xorl %edx, %edx
movl $0x3e8, %ecx            # imm = 0x3E8
divq %rcx
testq %rdx, %rdx
jne 0x2b398
movl 0x14(%r14), %esi
movl 0x14(%r12), %edx
movl 0x78(%rsp), %ecx
movl 0x14(%rsp), %eax
movl %eax, (%rsp)
leaq 0xc3ec3(%rip), %rdi     # 0xef24c
movl %r15d, %r8d
movl 0x28(%rsp), %r9d
movb $0x1, %al
callq 0x15080
movl 0x14(%r14), %eax
cmpl 0x14(%r12), %eax
movq %r12, %rdi
cmovgq %r14, %rdi
cmovgq %r12, %r14
movq %r14, %rsi
callq 0x28dc2
decl 0x14(%rsp)
incl %r15d
jmp 0x2b27c
leaq 0x58(%rsp), %r15
movq %r15, %rdi
callq 0xb1eda
leaq 0x48(%rsp), %rdi
movq %rbx, %rsi
callq 0xb1ee2
movq 0x18(%rsp), %r12
movq %rbx, %rdi
xorl %esi, %esi
callq 0xb20f8
xorl %ebx, %ebx
movslq 0x54(%rsp), %rax
cmpq %rax, %rbx
jge 0x2b43a
movq 0x48(%rsp), %rax
movq (%rax,%rbx,8), %r14
movq %r12, 0x130(%r14)
movl 0x24(%r12), %eax
movl %eax, 0x138(%r14)
movq 0x18(%r14), %rax
movq 0x30(%rsp), %rdi
testq %rax, %rax
je 0x2b42d
movq %rax, %rdi
movq %r14, %rsi
callq 0x290e2
movq %r15, %rdi
movq %r14, %rsi
callq 0x1a04c
incq %rbx
jmp 0x2b3eb
xorl %r14d, %r14d
movslq 0x64(%rsp), %rax
cmpq %rax, %r14
jge 0x2b46f
movq 0x58(%rsp), %rax
movq (%rax,%r14,8), %rbx
testq %rbx, %rbx
je 0x2b45d
movq %rbx, %rdi
callq 0x28a24
movl $0x140, %esi            # imm = 0x140
movq %rbx, %rdi
callq 0x15630
incq %r14
jmp 0x2b43d
leaq 0x48(%rsp), %rdi
callq 0xb1f48
leaq 0x58(%rsp), %rdi
callq 0xb1f48
leaq 0x70(%rsp), %rdi
callq 0x2c5dc
leaq 0xb0(%rsp), %rdi
callq 0x29dbe
leaq 0x38(%rsp), %rdi
callq 0xb1f48
pushq $0x1
popq %rax
addq $0xd8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
pushq $0x28
popq %rsi
movq %r12, %rdi
callq 0x15630
jmp 0x2b4f6
jmp 0x2b4da
jmp 0x2b4da
jmp 0x2b4da
jmp 0x2b4df
movq %rax, %rbx
jmp 0x2b4ec
jmp 0x2b4da
jmp 0x2b4da
movq %rax, %rbx
jmp 0x2b4f6
movq %rax, %rbx
leaq 0x48(%rsp), %rdi
callq 0xb1f48
leaq 0x58(%rsp), %rdi
callq 0xb1f48
leaq 0x70(%rsp), %rdi
callq 0x2c5dc
leaq 0xb0(%rsp), %rdi
callq 0x29dbe
jmp 0x2b524
movq %rax, %rbx
pushq $0x28
popq %rsi
movq %r14, %rdi
callq 0x15630
jmp 0x2b524
jmp 0x2b521
movq %rax, %rbx
leaq 0x38(%rsp), %rdi
callq 0xb1f48
movq %rbx, %rdi
callq 0x15af0