// File: /workspace/github_cpp_makefile_star2_O1/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 $0xe8, %rsp
movq %rdi, %r14
leaq 0x18(%rdi), %rbx
movl 0x24(%rdi), %eax
movl %eax, 0x14(%rsp)
leaq 0x40(%rsp), %rdi
callq 0xc4b50
cmpl $0x0, 0x24(%r14)
movq %rbx, 0x50(%rsp)
jle 0x2d6a8
xorl %ecx, %ecx
movq %r14, 0x20(%rsp)
movq (%rbx), %rax
movq %rcx, 0x30(%rsp)
movq (%rax,%rcx,8), %r12
movl 0x14(%r12), %eax
testl %eax, %eax
jle 0x2d689
movl %eax, %ecx
shrl $0x6, %ecx
cmpl $0x40, %eax
movl $0x1, %eax
cmovbl %eax, %ecx
movq %rcx, 0x38(%rsp)
xorl %r14d, %r14d
movq 0x8(%r12), %rax
movq (%rax,%r14,8), %r15
cmpl $0x0, 0xbc(%r15)
jle 0x2d679
xorl %ebx, %ebx
movq 0xb0(%r15), %rax
movq (%rax,%rbx,8), %rax
cmpq %rax, %r15
je 0x2d666
movq 0xc0(%rax), %r13
testq %r13, %r13
sete %al
cmpq %r13, %r12
sete %cl
orb %al, %cl
jne 0x2d666
movq %r12, %rdi
movq %r13, %rsi
callq 0x2dbad
testq %rax, %rax
jne 0x2d666
movq %r12, %rdi
movq %r13, %rsi
callq 0x2b954
movq 0x20(%rsp), %rax
movsd 0xb0(%rax), %xmm1
movsd %xmm0, 0x18(%rsp)
ucomisd %xmm0, %xmm1
jbe 0x2d634
movq 0x20(%rsp), %rax
movl 0x14(%rsp), %ecx
cmpl 0x34(%rax), %ecx
jg 0x2d634
movq 0x20(%rsp), %rax
cmpl $0x0, 0x38(%rax)
jne 0x2d634
movq 0x20(%rsp), %rax
movsd 0x40(%rax), %xmm0
ucomisd 0xdda28(%rip), %xmm0    # 0x10b058
jne 0x2d634
jnp 0x2d666
movl $0x28, %edi
callq 0x15610
movq %rax, %rbp
movq %rax, %rdi
movq %r12, %rsi
movq %r13, %rdx
movsd 0x18(%rsp), %xmm0
callq 0x2c168
movl 0x4c(%rsp), %edx
leaq 0x40(%rsp), %rdi
movq %rbp, %rsi
callq 0xc4be6
incq %rbx
movslq 0xbc(%r15), %rax
cmpq %rax, %rbx
jl 0x2d59e
addq 0x38(%rsp), %r14
cmpl %r14d, 0x14(%r12)
jg 0x2d585
movq 0x30(%rsp), %rcx
incq %rcx
movq 0x20(%rsp), %r14
movslq 0x24(%r14), %rax
cmpq %rax, %rcx
movq 0x50(%rsp), %rbx
jl 0x2d554
cmpl $0x0, 0x4c(%rsp)
je 0x2db01
xorpd %xmm0, %xmm0
movapd %xmm0, 0xc0(%rsp)
movupd %xmm0, 0xd8(%rsp)
movq $-0x1, 0xd0(%rsp)
movabsq $0x2000000000, %rax     # imm = 0x2000000000
movq %rax, 0x90(%rsp)
movapd %xmm0, 0x80(%rsp)
movupd %xmm0, 0x98(%rsp)
movupd %xmm0, 0xa4(%rsp)
movl $0x18, 0x90(%rsp)
cmpl $0x0, 0x4c(%rsp)
jle 0x2d73c
xorl %ebx, %ebx
leaq 0x80(%rsp), %r15
movq 0x40(%rsp), %rax
movq (%rax,%rbx,8), %rsi
movq %r15, %rdi
callq 0x2eb9e
incq %rbx
movslq 0x4c(%rsp), %rax
cmpq %rax, %rbx
jl 0x2d71e
movl $0x0, 0x30(%rsp)
leaq 0x80(%rsp), %r12
movl $0x0, 0x2c(%rsp)
cmpl $0x0, 0x88(%rsp)
je 0x2da0f
movq %r12, %rdi
callq 0x2ec9a
movsd 0x18(%rax), %xmm0
movsd 0xb0(%r14), %xmm2
ucomisd %xmm0, %xmm2
seta %dl
movl 0x34(%r14), %r8d
movl 0x38(%r14), %ecx
cmpl %r8d, 0x14(%rsp)
setle %sil
testl %ecx, %ecx
sete %dil
andb %sil, %dil
movsd 0x40(%r14), %xmm1
ucomisd 0xdd8b1(%rip), %xmm1    # 0x10b058
setnp %sil
sete %r15b
andb %sil, %r15b
andb %dil, %r15b
andb %dl, %r15b
jne 0x2d8b4
movq %r12, %rbx
movq (%rax), %rbp
movq %rax, 0x18(%rsp)
movq 0x8(%rax), %r12
cmpq $0x0, 0x18(%rbp)
jne 0x2d7e0
cmpq $0x0, 0x18(%r12)
je 0x2d8c2
movq %rbp, %r13
movq 0x18(%rbp), %rbp
testq %rbp, %rbp
jne 0x2d7e0
movq %r12, %rbp
movq 0x18(%r12), %r12
testq %r12, %r12
jne 0x2d7ec
cmpq %rbp, %r13
je 0x2d897
movq %r13, %rdi
movq %rbp, %rsi
callq 0x2dbad
testq %rax, %rax
movq %rbx, %r12
jne 0x2d89a
movq %r13, %rdi
movq %rbp, %rsi
callq 0x2b954
movsd %xmm0, 0x38(%rsp)
movl 0x14(%rsp), %eax
cmpl 0x34(%r14), %eax
movq 0x18(%rsp), %rbx
jg 0x2d861
cmpl $0x0, 0x38(%r14)
jg 0x2d861
movsd 0x40(%r14), %xmm0
ucomisd 0xdd80a(%rip), %xmm0    # 0x10b058
ja 0x2d861
movsd 0x38(%rsp), %xmm0
ucomisd 0xb0(%r14), %xmm0
jb 0x2d89f
movl $0x28, %edi
callq 0x15610
movq %rax, 0x68(%rsp)
movq %rax, %rdi
movq %r13, %rsi
movq %rbp, %rdx
movsd 0x38(%rsp), %xmm0
callq 0x2c168
movq %r12, %rdi
movq 0x68(%rsp), %rsi
callq 0x2eb9e
incl 0x30(%rsp)
jmp 0x2d89f
movq %rbx, %r12
movq 0x18(%rsp), %rbx
movq %rbx, %rdi
callq 0x2c1de
movl $0x28, %esi
movq %rbx, %rdi
callq 0x15620
testb %r15b, %r15b
je 0x2d754
jmp 0x2da0f
movapd %xmm0, %xmm3
cmpnltpd %xmm2, %xmm3
xorpd %xmm4, %xmm4
cmpnltpd %xmm2, %xmm4
orpd %xmm3, %xmm4
movd %xmm4, %edx
ucomisd 0xdd774(%rip), %xmm1    # 0x10b058
jbe 0x2d8f2
ucomisd 0xe8(%rbp), %xmm1
jbe 0x2d8f2
movb $0x1, %dl
ucomisd 0xdd75e(%rip), %xmm1    # 0x10b058
jbe 0x2d90a
ucomisd 0xe8(%r12), %xmm1
jbe 0x2d90a
movb $0x1, %dl
movzbl %dl, %edx
andl $0x1, %edx
testl %ecx, %ecx
jle 0x2d91f
cmpl %ecx, 0x14(%rbp)
movl $0x1, %esi
cmovll %esi, %edx
testl %ecx, %ecx
jle 0x2d930
cmpl %ecx, 0x14(%r12)
movl $0x1, %ecx
cmovll %ecx, %edx
cmpl %r8d, 0x14(%rsp)
setg %cl
testl %r8d, %r8d
setg %al
andb %cl, %al
testl %edx, %edx
setne %r13b
orb %al, %r13b
je 0x2d9df
cmpl $0x7b, 0xdc(%r14)
jne 0x2d9ba
movq 0x1434f0(%rip), %rax    # 0x170e50
leaq 0x1(%rax), %rcx
movq %rcx, 0x1434e5(%rip)    # 0x170e50
movabsq $0x1cac083126e978d5, %rcx # imm = 0x1CAC083126E978D5
imulq %rcx, %rax
rorq $0x3, %rax
movabsq $0x4189374bc6a7ef, %rcx # imm = 0x4189374BC6A7EF
cmpq %rcx, %rax
ja 0x2d9ba
movl 0x14(%rbp), %esi
movl 0x14(%r12), %edx
movl 0x88(%rsp), %ecx
movl 0x14(%rsp), %eax
movl %eax, (%rsp)
leaq 0xde923(%rip), %rdi     # 0x10c2cc
movl 0x2c(%rsp), %r8d
movl 0x30(%rsp), %r9d
movb $0x1, %al
callq 0x15080
movl 0x14(%rbp), %eax
cmpl 0x14(%r12), %eax
movq %r12, %rdi
cmovgq %rbp, %rdi
cmovgq %r12, %rbp
movq %rbp, %rsi
callq 0x2affa
decl 0x14(%rsp)
incl 0x2c(%rsp)
jmp 0x2d9f9
movq 0x18(%rsp), %r12
movq %r12, %rdi
callq 0x2c1de
movl $0x28, %esi
movq %r12, %rdi
callq 0x15620
movq %rbx, %r12
testb %r13b, %r13b
movq 0x18(%rsp), %rbx
jne 0x2d89f
jmp 0x2d8b4
leaq 0x58(%rsp), %r15
movq %r15, %rdi
callq 0xc4b50
leaq 0x70(%rsp), %rdi
movq 0x50(%rsp), %rbx
movq %rbx, %rsi
callq 0xc4b58
movq %rbx, %rdi
xorl %esi, %esi
callq 0xc4d90
cmpl $0x0, 0x7c(%rsp)
jle 0x2da94
xorl %ebx, %ebx
movq 0x70(%rsp), %rax
movq (%rax,%rbx,8), %r12
movq %r14, 0x130(%r12)
movl 0x24(%r14), %edx
movl %edx, 0x138(%r12)
movq 0x18(%r12), %rax
movq 0x50(%rsp), %rdi
testq %rax, %rax
je 0x2da7f
movq %rax, %rdi
movq %r12, %rsi
callq 0x2b372
movl 0x64(%rsp), %edx
movq %r15, %rdi
movq %r12, %rsi
callq 0xc4be6
incq %rbx
movslq 0x7c(%rsp), %rax
cmpq %rax, %rbx
jl 0x2da41
cmpl $0x0, 0x64(%rsp)
jle 0x2dace
xorl %r14d, %r14d
movq 0x58(%rsp), %rax
movq (%rax,%r14,8), %rbx
testq %rbx, %rbx
je 0x2dac1
movq %rbx, %rdi
callq 0x2ac1c
movl $0x140, %esi            # imm = 0x140
movq %rbx, %rdi
callq 0x15620
incq %r14
movslq 0x64(%rsp), %rax
cmpq %rax, %r14
jl 0x2da9e
leaq 0x70(%rsp), %rdi
callq 0xc4bba
leaq 0x58(%rsp), %rdi
callq 0xc4bba
movq 0x80(%rsp), %rdi
testq %rdi, %rdi
je 0x2daf4
callq 0x15820
leaq 0xc0(%rsp), %rdi
callq 0x2c1de
leaq 0x40(%rsp), %rdi
callq 0xc4bba
movl $0x1, %eax
addq $0xe8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2db47
movq %rax, %rbx
movl $0x28, %esi
movq 0x68(%rsp), %rdi
callq 0x15620
jmp 0x2db63
jmp 0x2db4c
movq %rax, %rbx
jmp 0x2db59
jmp 0x2db47
jmp 0x2db47
jmp 0x2db47
jmp 0x2db47
movq %rax, %rbx
jmp 0x2db63
movq %rax, %rbx
leaq 0x70(%rsp), %rdi
callq 0xc4bba
leaq 0x58(%rsp), %rdi
callq 0xc4bba
movq 0x80(%rsp), %rdi
testq %rdi, %rdi
je 0x2db75
callq 0x15820
leaq 0xc0(%rsp), %rdi
callq 0x2c1de
jmp 0x2db9b
movq %rax, %rbx
movl $0x28, %esi
movq %rbp, %rdi
callq 0x15620
jmp 0x2db9b
jmp 0x2db98
movq %rax, %rbx
leaq 0x40(%rsp), %rdi
callq 0xc4bba
movq %rbx, %rdi
callq 0x15ae0