// File: /workspace/github_cpp_makefile_star2_O3/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, %r14
leaq 0x18(%rdi), %rbx
movl 0x24(%rdi), %eax
movl %eax, 0x14(%rsp)
leaq 0x40(%rsp), %rdi
callq 0xc352c
movl 0x24(%r14), %ecx
testl %ecx, %ecx
movq %r14, 0x18(%rsp)
movq %rbx, 0x38(%rsp)
jle 0x2d2b3
xorl %edx, %edx
movq (%rbx), %rax
movq (%rax,%rdx,8), %r12
movl 0x14(%r12), %eax
testl %eax, %eax
jle 0x2d2a4
movq %rdx, 0x20(%rsp)
movl %eax, %edx
shrl $0x6, %edx
cmpl $0x40, %eax
movl $0x1, %ecx
cmovbl %ecx, %edx
movq %rdx, 0x28(%rsp)
xorl %r14d, %r14d
movq 0x8(%r12), %rcx
movq (%rcx,%r14,8), %r15
cmpl $0x0, 0xbc(%r15)
jle 0x2d288
xorl %ebx, %ebx
movq 0xb0(%r15), %rax
movq (%rax,%rbx,8), %rax
cmpq %rax, %r15
je 0x2d26b
movq 0xc0(%rax), %r13
testq %r13, %r13
sete %al
cmpq %r13, %r12
sete %cl
orb %al, %cl
jne 0x2d26b
movq %r12, %rdi
movq %r13, %rsi
callq 0x2d749
testq %rax, %rax
jne 0x2d26b
movq %r12, %rdi
movq %r13, %rsi
callq 0x2b606
movq 0x18(%rsp), %rax
movsd 0xb0(%rax), %xmm1
movsd %xmm0, 0x30(%rsp)
ucomisd %xmm0, %xmm1
jbe 0x2d239
movq 0x18(%rsp), %rax
movl 0x14(%rsp), %ecx
cmpl 0x34(%rax), %ecx
jg 0x2d239
movq 0x18(%rsp), %rax
cmpl $0x0, 0x38(%rax)
jne 0x2d239
movq 0x18(%rsp), %rax
movsd 0x40(%rax), %xmm0
ucomisd 0xdbe23(%rip), %xmm0    # 0x109058
jne 0x2d239
jnp 0x2d26b
movl $0x28, %edi
callq 0x15610
movq %rax, %rbp
movq %rax, %rdi
movq %r12, %rsi
movq %r13, %rdx
movsd 0x30(%rsp), %xmm0
callq 0x2bd4c
movl 0x4c(%rsp), %edx
leaq 0x40(%rsp), %rdi
movq %rbp, %rsi
callq 0xc35c2
incq %rbx
movslq 0xbc(%r15), %rax
cmpq %rax, %rbx
jl 0x2d1a3
movl 0x14(%r12), %eax
movq 0x38(%rsp), %rbx
addq 0x28(%rsp), %r14
cmpl %r14d, %eax
jg 0x2d18a
movq 0x18(%rsp), %r14
movl 0x24(%r14), %ecx
movq 0x20(%rsp), %rdx
incq %rdx
movslq %ecx, %rax
cmpq %rax, %rdx
jl 0x2d159
movl 0x4c(%rsp), %eax
testl %eax, %eax
je 0x2d6a2
xorpd %xmm0, %xmm0
movapd %xmm0, 0xb0(%rsp)
movupd %xmm0, 0xc8(%rsp)
movq $-0x1, 0xc0(%rsp)
movapd %xmm0, 0x70(%rsp)
movupd %xmm0, 0x88(%rsp)
movupd %xmm0, 0x94(%rsp)
movabsq $0x2000000018, %rax     # imm = 0x2000000018
movq %rax, 0x80(%rsp)
jle 0x2d5b8
xorl %ebx, %ebx
leaq 0x70(%rsp), %r15
movq 0x40(%rsp), %rax
movq (%rax,%rbx,8), %rsi
movq %r15, %rdi
callq 0x2e6ca
incq %rbx
movslq 0x4c(%rsp), %rax
cmpq %rax, %rbx
jl 0x2d318
cmpl $0x0, 0x78(%rsp)
movq 0x38(%rsp), %rbx
je 0x2d5b8
movl $0x0, 0x20(%rsp)
movl $0x0, 0x28(%rsp)
leaq 0x70(%rsp), %rdi
callq 0x2e7de
movq %rax, %r12
movsd 0x18(%rax), %xmm0
movsd 0xb0(%r14), %xmm2
movl 0x34(%r14), %eax
movl 0x38(%r14), %ecx
movsd 0x40(%r14), %xmm1
ucomisd %xmm0, %xmm2
jbe 0x2d39f
cmpl %eax, 0x14(%rsp)
jg 0x2d39f
testl %ecx, %ecx
jne 0x2d39f
ucomisd 0xdbcc1(%rip), %xmm1    # 0x109058
jne 0x2d39f
jnp 0x2d5b8
movq (%r12), %r13
movq 0x8(%r12), %r14
cmpq $0x0, 0x18(%r13)
jne 0x2d3ba
cmpq $0x0, 0x18(%r14)
je 0x2d472
movq %r13, %rbp
movq 0x18(%r13), %r13
testq %r13, %r13
jne 0x2d3ba
movq %r14, %r13
movq 0x18(%r14), %r14
testq %r14, %r14
jne 0x2d3c6
cmpq %r13, %rbp
je 0x2d593
movq %rbp, %rdi
movq %r13, %rsi
callq 0x2d749
testq %rax, %rax
movq 0x18(%rsp), %r14
jne 0x2d598
movq %rbp, %rdi
movq %r13, %rsi
callq 0x2b606
movsd %xmm0, 0x30(%rsp)
movl 0x14(%rsp), %eax
cmpl 0x34(%r14), %eax
jg 0x2d43b
cmpl $0x0, 0x38(%r14)
jg 0x2d43b
movsd 0x40(%r14), %xmm0
ucomisd 0xdbc34(%rip), %xmm0    # 0x109058
ja 0x2d43b
movsd 0x30(%rsp), %xmm0
ucomisd 0xb0(%r14), %xmm0
jb 0x2d598
movl $0x28, %edi
callq 0x15610
movq %rax, %r15
movq %rax, %rdi
movq %rbp, %rsi
movq %r13, %rdx
movsd 0x30(%rsp), %xmm0
callq 0x2bd4c
leaq 0x70(%rsp), %rdi
movq %r15, %rsi
callq 0x2e6ca
incl 0x28(%rsp)
jmp 0x2d598
movapd %xmm0, %xmm3
cmpnltpd %xmm2, %xmm3
xorpd %xmm4, %xmm4
cmpnltpd %xmm2, %xmm4
orpd %xmm3, %xmm4
movd %xmm4, %edx
andl $0x1, %edx
ucomisd 0xdbbc1(%rip), %xmm1    # 0x109058
jbe 0x2d4ba
ucomisd 0xe8(%r13), %xmm1
movl $0x1, %esi
cmoval %esi, %edx
ucomisd 0xe8(%r14), %xmm1
jbe 0x2d4ba
movl $0x1, %edx
testl %ecx, %ecx
jle 0x2d4d1
cmpl %ecx, 0x14(%r13)
movl $0x1, %esi
cmovll %esi, %edx
cmpl %ecx, 0x14(%r14)
cmovll %esi, %edx
cmpl %eax, 0x14(%rsp)
setg %cl
testl %eax, %eax
setg %al
testb %cl, %al
jne 0x2d504
testl %edx, %edx
jne 0x2d504
movq %r12, %rdi
callq 0x2bdc2
movl $0x28, %esi
movq %r12, %rdi
callq 0x15620
movq 0x18(%rsp), %r14
jmp 0x2d5ad
movq 0x18(%rsp), %rax
cmpl $0x7b, 0xdc(%rax)
jne 0x2d570
movq 0x141927(%rip), %rax    # 0x16ee40
leaq 0x1(%rax), %rcx
movq %rcx, 0x14191c(%rip)    # 0x16ee40
movabsq $0x1cac083126e978d5, %rcx # imm = 0x1CAC083126E978D5
imulq %rcx, %rax
rorq $0x3, %rax
movabsq $0x4189374bc6a7ef, %rcx # imm = 0x4189374BC6A7EF
cmpq %rcx, %rax
ja 0x2d570
movl 0x14(%r13), %esi
movl 0x14(%r14), %edx
movl 0x78(%rsp), %ecx
movl 0x14(%rsp), %eax
movl %eax, (%rsp)
leaq 0xdcd6d(%rip), %rdi     # 0x10a2cc
movl 0x20(%rsp), %r8d
movl 0x28(%rsp), %r9d
movb $0x1, %al
callq 0x15080
movl 0x14(%r13), %eax
cmpl 0x14(%r14), %eax
movq %r14, %rdi
cmovgq %r13, %rdi
cmovgq %r14, %r13
movq %r13, %rsi
callq 0x2acaa
decl 0x14(%rsp)
incl 0x20(%rsp)
movq 0x18(%rsp), %r14
movq %r12, %rdi
callq 0x2bdc2
movl $0x28, %esi
movq %r12, %rdi
callq 0x15620
cmpl $0x0, 0x78(%rsp)
jne 0x2d356
leaq 0x50(%rsp), %r15
movq %r15, %rdi
callq 0xc352c
leaq 0x60(%rsp), %rdi
movq %rbx, %rsi
callq 0xc3534
movq %rbx, %rdi
xorl %esi, %esi
callq 0xc378c
cmpl $0x0, 0x6c(%rsp)
jle 0x2d638
xorl %ebx, %ebx
movq 0x60(%rsp), %rax
movq (%rax,%rbx,8), %r12
movq %r14, 0x130(%r12)
movl 0x24(%r14), %edx
movl %edx, 0x138(%r12)
movq 0x18(%r12), %rax
movq 0x38(%rsp), %rdi
testq %rax, %rax
je 0x2d623
movq %rax, %rdi
movq %r12, %rsi
callq 0x2b03a
movl 0x5c(%rsp), %edx
movq %r15, %rdi
movq %r12, %rsi
callq 0xc35c2
incq %rbx
movslq 0x6c(%rsp), %rax
cmpq %rax, %rbx
jl 0x2d5e5
cmpl $0x0, 0x5c(%rsp)
jle 0x2d672
xorl %r14d, %r14d
movq 0x50(%rsp), %rax
movq (%rax,%r14,8), %rbx
testq %rbx, %rbx
je 0x2d658
movq %rbx, %rdi
callq 0x2a8cc
movl $0x140, %esi            # imm = 0x140
movq %rbx, %rdi
callq 0x15620
incq %r14
movslq 0x5c(%rsp), %rax
cmpq %rax, %r14
jl 0x2d642
leaq 0x60(%rsp), %rdi
callq 0xc3596
leaq 0x50(%rsp), %rdi
callq 0xc3596
movq 0x70(%rsp), %rdi
testq %rdi, %rdi
je 0x2d695
callq 0x15810
leaq 0xb0(%rsp), %rdi
callq 0x2bdc2
leaq 0x40(%rsp), %rdi
callq 0xc3596
movl $0x1, %eax
addq $0xd8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2d6e6
movq %rax, %rbx
movl $0x28, %esi
movq %r15, %rdi
callq 0x15620
jmp 0x2d702
jmp 0x2d6eb
movq %rax, %rbx
jmp 0x2d6f8
jmp 0x2d6e6
jmp 0x2d6e6
jmp 0x2d6e6
jmp 0x2d6e6
movq %rax, %rbx
jmp 0x2d702
movq %rax, %rbx
leaq 0x60(%rsp), %rdi
callq 0xc3596
leaq 0x50(%rsp), %rdi
callq 0xc3596
movq 0x70(%rsp), %rdi
testq %rdi, %rdi
je 0x2d711
callq 0x15810
leaq 0xb0(%rsp), %rdi
callq 0x2bdc2
jmp 0x2d737
movq %rax, %rbx
movl $0x28, %esi
movq %rbp, %rdi
callq 0x15620
jmp 0x2d737
jmp 0x2d734
movq %rax, %rbx
leaq 0x40(%rsp), %rdi
callq 0xc3596
movq %rbx, %rdi
callq 0x15ad0