// File: tomfunkhouser[P]gaps/pkgs/R3Shapes/R3Align.cpp
// Function: gaps::R3EstimatePlaneWithRansac(gaps::RNArray<gaps::R3Point*> const&, double const*, double, int, double*, double*)
R3Plane
R3EstimatePlaneWithRansac(const RNArray<R3Point *>& points, const RNScalar *weights,
  RNScalar tolerance, int max_iterations,
  RNScalar *max_inlier_fraction, RNScalar *avg_inlier_fraction)
{
  // Check number of points
  if (points.NEntries() < 3) return R3null_plane;
  
  // Try PCA plane
  RNScalar variances[3];
  RNArray<R3Point *> inliers;
  RNScalar *w = (weights) ? new RNScalar [ points.NEntries() ] : NULL;
  R3Point centroid = R3Centroid(points);
  R3Triad axes = R3PrincipleAxes(centroid, points, weights, variances);
  if (RNIsZero(variances[1])) return R3null_plane;
  R3Plane plane(centroid, axes[2]);
  if (tolerance == 0) tolerance = sqrt(variances[2]);
  for (int i = 0; i < points.NEntries(); i++) {
    R3Point *point = points[i];
    RNScalar d = R3Distance(plane, *point);
    if (d > tolerance) continue; 
    if (w) w[inliers.NEntries()] = weights[i];
    inliers.Insert(point);
  }

  // Initialize best score
  R3Plane best_plane = plane;
  RNScalar score = (RNScalar) inliers.NEntries() / (RNScalar) points.NEntries();
  RNScalar best_score = score;
  RNScalar total_score = score;
  int nscores = 1;

  // Search for a best normal
  for (int i = 0; i < max_iterations; i++) {
    // Guess normal by selecting three random points
    R3Point *p0 = points[(int) (RNRandomScalar() * points.NEntries()) ];
    R3Point *p1 = points[(int) (RNRandomScalar() * points.NEntries()) ];
    R3Point *p2 = points[(int) (RNRandomScalar() * points.NEntries()) ];
    if (R3Contains(*p0, *p1) || R3Contains(*p1, *p2) || R3Contains(*p2, *p0)) continue;
    R3Plane plane(*p0, *p1, *p2);
    
    // Find inliers
    inliers.Empty();
    for (int i = 0; i < points.NEntries(); i++) {
      R3Point *point = points[i];
      RNScalar d = R3Distance(plane, *point);
      if (d > tolerance) continue; 
      if (w) w[inliers.NEntries()] = weights[i];
      inliers.Insert(point);
    }

    // Compute  score
    RNScalar score = (RNScalar) inliers.NEntries() / (RNScalar) points.NEntries();
    total_score += score;
    nscores++;

    // Check score
    if (score <= best_score) continue;
    
    // Recompute plane with just inliers
    plane = R3EstimatePlaneWithPCA(inliers, w);
    if (plane == R3null_plane) continue;
    
    // Update best stuff
    best_plane = plane;
    best_score = score;
  }

  // Update returned results
  if (max_inlier_fraction) *max_inlier_fraction = best_score;
  if (avg_inlier_fraction) *avg_inlier_fraction = total_score / nscores;
  
  // Return best plane
  return best_plane;
}

// Assembly:
subq $0x308, %rsp            # imm = 0x308
movq %rdi, 0x128(%rsp)
movq %rdi, %rax
movq %rax, 0x130(%rsp)
movq %rdi, 0x300(%rsp)
movq %rsi, 0x2f8(%rsp)
movq %rdx, 0x2f0(%rsp)
movsd %xmm0, 0x2e8(%rsp)
movl %ecx, 0x2e4(%rsp)
movq %r8, 0x2d8(%rsp)
movq %r9, 0x2d0(%rsp)
movq 0x2f8(%rsp), %rdi
callq 0x13600
cmpl $0x3, %eax
jge 0x8f4fd
movq 0x128(%rsp), %rdi
leaq 0x4e9bed(%rip), %rsi    # 0x5790e0
callq 0x6c140
jmp 0x8fdc2
leaq 0x2a0(%rsp), %rdi
callq 0x90640
movq 0x2f0(%rsp), %rax
movb $0x0, 0x277(%rsp)
cmpq $0x0, %rax
je 0x8f59e
movq 0x2f8(%rsp), %rdi
callq 0x13600
movl %eax, 0x124(%rsp)
jmp 0x8f536
movl 0x124(%rsp), %eax
cltq
movl $0x8, %ecx
mulq %rcx
movq %rax, %rdi
movq $-0x1, %rax
cmovoq %rax, %rdi
movq %rdi, 0x110(%rsp)
callq 0xf030
movq %rax, 0x118(%rsp)
jmp 0x8f56c
movq 0x118(%rsp), %rax
movq 0x110(%rsp), %rcx
movq %rax, 0x280(%rsp)
movq %rcx, 0x278(%rsp)
movb $0x1, 0x277(%rsp)
movq %rax, 0x108(%rsp)
jmp 0x8f5aa
xorl %eax, %eax
movq %rax, 0x108(%rsp)
jmp 0x8f5aa
movq 0x108(%rsp), %rax
movq %rax, 0x298(%rsp)
movq 0x2f8(%rsp), %rsi
xorl %eax, %eax
movl %eax, %edx
leaq 0x258(%rsp), %rdi
callq 0x8cd30
jmp 0x8f5d5
movq 0x2f8(%rsp), %rdx
movq 0x2f0(%rsp), %rcx
leaq 0x210(%rsp), %rdi
leaq 0x258(%rsp), %rsi
leaq 0x2b0(%rsp), %r8
callq 0x8ce50
jmp 0x8f604
movsd 0x2b8(%rsp), %xmm0
leaq 0xe8ecc(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm1
callq 0x36b80
movl %eax, 0x104(%rsp)
jmp 0x8f626
movl 0x104(%rsp), %eax
cmpl $0x0, %eax
je 0x8f67e
movq 0x128(%rsp), %rdi
leaq 0x4e9a9f(%rip), %rsi    # 0x5790e0
callq 0x6c140
jmp 0x8f648
movl $0x1, 0x20c(%rsp)
jmp 0x8fdb5
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x290(%rsp)
movl %eax, 0x28c(%rsp)
leaq 0x2a0(%rsp), %rdi
callq 0x906e0
jmp 0x8fdd2
leaq 0x210(%rsp), %rdi
movl $0x2, %esi
callq 0x39fb0
movq %rax, 0xf8(%rsp)
jmp 0x8f69a
movq 0xf8(%rsp), %rdx
leaq 0x1e8(%rsp), %rdi
leaq 0x258(%rsp), %rsi
callq 0x6c260
jmp 0x8f6b9
movsd 0x2e8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x8f6e4
jp 0x8f6e4
movsd 0x2c0(%rsp), %xmm0
callq 0xf390
movsd %xmm0, 0x2e8(%rsp)
movl $0x0, 0x1e4(%rsp)
movl 0x1e4(%rsp), %eax
movl %eax, 0xf0(%rsp)
movq 0x2f8(%rsp), %rdi
callq 0x13600
movl %eax, 0xf4(%rsp)
jmp 0x8f713
movl 0xf0(%rsp), %eax
movl 0xf4(%rsp), %ecx
cmpl %ecx, %eax
jge 0x8f840
movq 0x2f8(%rsp), %rdi
movl 0x1e4(%rsp), %esi
callq 0x6d3e0
movq %rax, 0xe8(%rsp)
jmp 0x8f747
movq 0xe8(%rsp), %rax
movq %rax, 0x1d8(%rsp)
movq 0x1d8(%rsp), %rsi
leaq 0x1e8(%rsp), %rdi
callq 0x3a820
movsd %xmm0, 0xe0(%rsp)
jmp 0x8f777
movsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0x1d0(%rsp)
movsd 0x1d0(%rsp), %xmm0
ucomisd 0x2e8(%rsp), %xmm0
jbe 0x8f7a2
jmp 0x8f82a
cmpq $0x0, 0x298(%rsp)
je 0x8f811
movq 0x2f0(%rsp), %rax
movslq 0x1e4(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
movsd %xmm0, 0xc8(%rsp)
movq 0x298(%rsp), %rax
movq %rax, 0xd0(%rsp)
leaq 0x2a0(%rsp), %rdi
callq 0x13600
movl %eax, 0xdc(%rsp)
jmp 0x8f7f1
movq 0xd0(%rsp), %rax
movsd 0xc8(%rsp), %xmm0
movl 0xdc(%rsp), %ecx
movslq %ecx, %rcx
movsd %xmm0, (%rax,%rcx,8)
movq 0x1d8(%rsp), %rsi
leaq 0x2a0(%rsp), %rdi
callq 0x90650
jmp 0x8f828
jmp 0x8f82a
movl 0x1e4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x1e4(%rsp)
jmp 0x8f6ef
movq 0x128(%rsp), %rdi
leaq 0x1e8(%rsp), %rsi
callq 0x6c140
jmp 0x8f857
leaq 0x2a0(%rsp), %rdi
callq 0x13600
movl %eax, 0xc4(%rsp)
jmp 0x8f86d
movl 0xc4(%rsp), %eax
cvtsi2sd %eax, %xmm0
movsd %xmm0, 0xb8(%rsp)
movq 0x2f8(%rsp), %rdi
callq 0x13600
movl %eax, 0xc0(%rsp)
jmp 0x8f897
movsd 0xb8(%rsp), %xmm0
movl 0xc0(%rsp), %eax
cvtsi2sd %eax, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, 0x1c8(%rsp)
movsd 0x1c8(%rsp), %xmm0
movsd %xmm0, 0x1c0(%rsp)
movsd 0x1c8(%rsp), %xmm0
movsd %xmm0, 0x1b8(%rsp)
movl $0x1, 0x1b4(%rsp)
movl $0x0, 0x1b0(%rsp)
movl 0x1b0(%rsp), %eax
cmpl 0x2e4(%rsp), %eax
jge 0x8fd5d
movq 0x2f8(%rsp), %rax
movq %rax, 0xa8(%rsp)
callq 0xd4710
movsd %xmm0, 0xb0(%rsp)
jmp 0x8f926
movq 0x2f8(%rsp), %rdi
callq 0x13600
movl %eax, 0xa4(%rsp)
jmp 0x8f93c
movq 0xa8(%rsp), %rdi
movsd 0xb0(%rsp), %xmm0
movl 0xa4(%rsp), %eax
cvtsi2sd %eax, %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %esi
callq 0x6d3e0
movq %rax, 0x98(%rsp)
jmp 0x8f96f
movq 0x98(%rsp), %rax
movq %rax, 0x1a8(%rsp)
movq 0x2f8(%rsp), %rax
movq %rax, 0x88(%rsp)
callq 0xd4710
movsd %xmm0, 0x90(%rsp)
jmp 0x8f99f
movq 0x2f8(%rsp), %rdi
callq 0x13600
movl %eax, 0x84(%rsp)
jmp 0x8f9b5
movq 0x88(%rsp), %rdi
movsd 0x90(%rsp), %xmm0
movl 0x84(%rsp), %eax
cvtsi2sd %eax, %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %esi
callq 0x6d3e0
movq %rax, 0x78(%rsp)
jmp 0x8f9e5
movq 0x78(%rsp), %rax
movq %rax, 0x1a0(%rsp)
movq 0x2f8(%rsp), %rax
movq %rax, 0x68(%rsp)
callq 0xd4710
movsd %xmm0, 0x70(%rsp)
jmp 0x8fa0c
movq 0x2f8(%rsp), %rdi
callq 0x13600
movl %eax, 0x64(%rsp)
jmp 0x8fa1f
movq 0x68(%rsp), %rdi
movsd 0x70(%rsp), %xmm0
movl 0x64(%rsp), %eax
cvtsi2sd %eax, %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %esi
callq 0x6d3e0
movq %rax, 0x58(%rsp)
jmp 0x8fa46
movq 0x58(%rsp), %rax
movq %rax, 0x198(%rsp)
movq 0x1a8(%rsp), %rdi
movq 0x1a0(%rsp), %rsi
callq 0x82180
movl %eax, 0x54(%rsp)
jmp 0x8fa6e
movl 0x54(%rsp), %eax
cmpl $0x0, %eax
jne 0x8fabf
movq 0x1a0(%rsp), %rdi
movq 0x198(%rsp), %rsi
callq 0x82180
movl %eax, 0x50(%rsp)
jmp 0x8fa92
movl 0x50(%rsp), %eax
cmpl $0x0, %eax
jne 0x8fabf
movq 0x198(%rsp), %rdi
movq 0x1a8(%rsp), %rsi
callq 0x82180
movl %eax, 0x4c(%rsp)
jmp 0x8fab6
movl 0x4c(%rsp), %eax
cmpl $0x0, %eax
je 0x8fac4
jmp 0x8fd47
movq 0x1a8(%rsp), %rsi
movq 0x1a0(%rsp), %rdx
movq 0x198(%rsp), %rcx
leaq 0x178(%rsp), %rdi
callq 0x6c5f0
jmp 0x8faeb
leaq 0x2a0(%rsp), %rdi
xorl %esi, %esi
callq 0xd1d30
jmp 0x8fafc
movl $0x0, 0x174(%rsp)
movl 0x174(%rsp), %eax
movl %eax, 0x44(%rsp)
movq 0x2f8(%rsp), %rdi
callq 0x13600
movl %eax, 0x48(%rsp)
jmp 0x8fb25
movl 0x44(%rsp), %eax
movl 0x48(%rsp), %ecx
cmpl %ecx, %eax
jge 0x8fc2b
movq 0x2f8(%rsp), %rdi
movl 0x174(%rsp), %esi
callq 0x6d3e0
movq %rax, 0x38(%rsp)
jmp 0x8fb50
movq 0x38(%rsp), %rax
movq %rax, 0x168(%rsp)
movq 0x168(%rsp), %rsi
leaq 0x178(%rsp), %rdi
callq 0x3a820
movsd %xmm0, 0x30(%rsp)
jmp 0x8fb7a
movsd 0x30(%rsp), %xmm0
movsd %xmm0, 0x160(%rsp)
movsd 0x160(%rsp), %xmm0
ucomisd 0x2e8(%rsp), %xmm0
jbe 0x8fb9f
jmp 0x8fc15
cmpq $0x0, 0x298(%rsp)
je 0x8fbfc
movq 0x2f0(%rsp), %rax
movslq 0x174(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
movsd %xmm0, 0x18(%rsp)
movq 0x298(%rsp), %rax
movq %rax, 0x20(%rsp)
leaq 0x2a0(%rsp), %rdi
callq 0x13600
movl %eax, 0x2c(%rsp)
jmp 0x8fbe5
movq 0x20(%rsp), %rax
movsd 0x18(%rsp), %xmm0
movl 0x2c(%rsp), %ecx
movslq %ecx, %rcx
movsd %xmm0, (%rax,%rcx,8)
movq 0x168(%rsp), %rsi
leaq 0x2a0(%rsp), %rdi
callq 0x90650
jmp 0x8fc13
jmp 0x8fc15
movl 0x174(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x174(%rsp)
jmp 0x8fb07
leaq 0x2a0(%rsp), %rdi
callq 0x13600
movl %eax, 0x14(%rsp)
jmp 0x8fc3e
movl 0x14(%rsp), %eax
cvtsi2sd %eax, %xmm0
movsd %xmm0, 0x8(%rsp)
movq 0x2f8(%rsp), %rdi
callq 0x13600
movl %eax, 0x10(%rsp)
jmp 0x8fc5f
movsd 0x8(%rsp), %xmm0
movl 0x10(%rsp), %eax
cvtsi2sd %eax, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, 0x158(%rsp)
movsd 0x158(%rsp), %xmm0
addsd 0x1b8(%rsp), %xmm0
movsd %xmm0, 0x1b8(%rsp)
movl 0x1b4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x1b4(%rsp)
movsd 0x158(%rsp), %xmm1
movsd 0x1c0(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jb 0x8fcc3
jmp 0x8fd47
movq 0x298(%rsp), %rdx
leaq 0x138(%rsp), %rdi
leaq 0x2a0(%rsp), %rsi
callq 0x8f3c0
jmp 0x8fce2
leaq 0x178(%rsp), %rdi
leaq 0x138(%rsp), %rsi
callq 0x3a4d0
jmp 0x8fcf9
leaq 0x4e93e0(%rip), %rsi    # 0x5790e0
leaq 0x178(%rsp), %rdi
callq 0x90680
movl %eax, 0x4(%rsp)
jmp 0x8fd13
movl 0x4(%rsp), %eax
cmpl $0x0, %eax
je 0x8fd1e
jmp 0x8fd47
movq 0x128(%rsp), %rdi
leaq 0x178(%rsp), %rsi
callq 0x3a4d0
jmp 0x8fd35
movsd 0x158(%rsp), %xmm0
movsd %xmm0, 0x1c0(%rsp)
movl 0x1b0(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x1b0(%rsp)
jmp 0x8f8f2
cmpq $0x0, 0x2d8(%rsp)
je 0x8fd7d
movsd 0x1c0(%rsp), %xmm0
movq 0x2d8(%rsp), %rax
movsd %xmm0, (%rax)
cmpq $0x0, 0x2d0(%rsp)
je 0x8fdaa
movsd 0x1b8(%rsp), %xmm0
cvtsi2sdl 0x1b4(%rsp), %xmm1
divsd %xmm1, %xmm0
movq 0x2d0(%rsp), %rax
movsd %xmm0, (%rax)
movl $0x1, 0x20c(%rsp)
leaq 0x2a0(%rsp), %rdi
callq 0x906e0
movq 0x130(%rsp), %rax
addq $0x308, %rsp            # imm = 0x308
retq
movq 0x290(%rsp), %rdi
callq 0xf890
nop