// File: /workspace/github_cpp_makefile_star2_O3/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:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x138, %rsp            # imm = 0x138
movq %r9, 0x58(%rsp)
movq %r8, 0x50(%rsp)
movl %ecx, 0x14(%rsp)
movsd %xmm0, 0x8(%rsp)
movq %rdi, %rbx
cmpl $0x2, 0xc(%rsi)
jg 0x4d604
leaq 0x499099(%rip), %rsi    # 0x4e6690
movq %rbx, %rdi
callq 0x3b026
jmp 0x4d9fb
movq %rdx, %r15
movq %rsi, %r13
leaq 0x18(%rsp), %rdi
callq 0x6f80c
testq %r15, %r15
je 0x4d63d
movslq 0xc(%r13), %rax
leaq (,%rax,8), %rcx
testq %rax, %rax
movq $-0x1, %rdi
cmovnsq %rcx, %rdi
callq 0xe030
movq %rax, %r14
jmp 0x4d640
xorl %r14d, %r14d
leaq 0x98(%rsp), %rdi
movq %r13, %rsi
xorl %edx, %edx
callq 0x4c1ec
leaq 0xf0(%rsp), %rdi
leaq 0x98(%rsp), %rsi
leaq 0x80(%rsp), %r8
movq %r13, %rdx
movq %r15, %rcx
callq 0x4c299
movsd 0x88(%rsp), %xmm0
leaq 0x98513(%rip), %rax     # 0xe5b98
movsd (%rax), %xmm1
movapd 0x5498f(%rip), %xmm2    # 0xa2020
xorpd %xmm1, %xmm2
ucomisd %xmm2, %xmm0
jb 0x4d6b5
ucomisd %xmm0, %xmm1
jb 0x4d6b5
leaq 0x498fe8(%rip), %rsi    # 0x4e6690
movq %rbx, %rdi
callq 0x3b026
jmp 0x4d9f1
leaq 0x120(%rsp), %rdx
leaq 0xb0(%rsp), %rdi
leaq 0x98(%rsp), %rsi
callq 0x3b0b2
movq %rbx, 0x28(%rsp)
xorpd %xmm1, %xmm1
movsd 0x8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jne 0x4d709
jp 0x4d709
movsd 0x90(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jb 0x4d6fe
sqrtsd %xmm0, %xmm0
jmp 0x4d703
callq 0xe340
movsd %xmm0, 0x8(%rsp)
movq %r15, 0x48(%rsp)
cmpl $0x0, 0xc(%r13)
jle 0x4d770
xorl %ebp, %ebp
leaq 0xb0(%rsp), %rbx
leaq 0x18(%rsp), %r15
movq (%r13), %rax
movq (%rax,%rbp,8), %r12
movq %r12, %rdi
movq %rbx, %rsi
callq 0x495be
ucomisd 0x8(%rsp), %xmm0
ja 0x4d764
movslq 0x24(%rsp), %rdx
testq %r14, %r14
je 0x4d759
movq 0x48(%rsp), %rax
movsd (%rax,%rbp,8), %xmm0
movsd %xmm0, (%r14,%rdx,8)
movq %r15, %rdi
movq %r12, %rsi
callq 0x6f8a2
incq %rbp
movslq 0xc(%r13), %rax
cmpq %rax, %rbp
jl 0x4d724
leaq 0xb0(%rsp), %rsi
movq 0x28(%rsp), %rbx
movq %rbx, %rdi
callq 0x3b026
xorps %xmm1, %xmm1
cvtsi2sdl 0x24(%rsp), %xmm1
xorps %xmm0, %xmm0
cvtsi2sdl 0xc(%r13), %xmm0
divsd %xmm0, %xmm1
cmpl $0x0, 0x14(%rsp)
movsd %xmm1, 0x38(%rsp)
jle 0x4d9b7
movl $0x1, 0x10(%rsp)
xorl %ebp, %ebp
movsd %xmm1, 0x30(%rsp)
callq 0x715e8
xorps %xmm1, %xmm1
cvtsi2sdl 0xc(%r13), %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movq (%r13), %rcx
cltq
movq (%rcx,%rax,8), %rbx
callq 0x715e8
xorps %xmm1, %xmm1
cvtsi2sdl 0xc(%r13), %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movq (%r13), %rcx
cltq
movq (%rcx,%rax,8), %r15
callq 0x715e8
xorps %xmm1, %xmm1
cvtsi2sdl 0xc(%r13), %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movq (%r13), %rcx
cltq
movq (%rcx,%rax,8), %r12
movq %rbx, %rdi
movq %r15, %rsi
callq 0x4652b
testl %eax, %eax
jne 0x4d99b
movq %r15, %rdi
movq %r12, %rsi
callq 0x4652b
testl %eax, %eax
jne 0x4d99b
movq %r12, %rdi
movq %rbx, %rsi
callq 0x4652b
testl %eax, %eax
jne 0x4d99b
leaq 0x60(%rsp), %rdi
movq %rbx, %rsi
movq %r15, %rdx
movq %r12, %rcx
callq 0x3b21e
leaq 0x18(%rsp), %rdi
xorl %esi, %esi
callq 0x6fa6c
movl 0xc(%r13), %eax
testl %eax, %eax
jle 0x4d8d0
xorl %r15d, %r15d
movq (%r13), %rax
movq (%rax,%r15,8), %rbx
movq %rbx, %rdi
leaq 0x60(%rsp), %rsi
callq 0x495be
ucomisd 0x8(%rsp), %xmm0
ja 0x4d8c4
movslq 0x24(%rsp), %rdx
testq %r14, %r14
je 0x4d8b7
movq 0x48(%rsp), %rax
movsd (%rax,%r15,8), %xmm0
movsd %xmm0, (%r14,%rdx,8)
leaq 0x18(%rsp), %rdi
movq %rbx, %rsi
callq 0x6f8a2
incq %r15
movslq 0xc(%r13), %rax
cmpq %rax, %r15
jl 0x4d87f
xorps %xmm1, %xmm1
cvtsi2sdl 0x24(%rsp), %xmm1
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
divsd %xmm0, %xmm1
movsd 0x38(%rsp), %xmm0
movsd %xmm1, 0x40(%rsp)
ucomisd %xmm1, %xmm0
jae 0x4d985
leaq 0xd0(%rsp), %r15
movq %r15, %rdi
leaq 0x18(%rsp), %rsi
movq %r14, %rdx
callq 0x4d51f
leaq 0x60(%rsp), %rbx
movq %rbx, %rdi
movq %r15, %rsi
callq 0x3d752
movsd 0xe8(%rsp), %xmm0
movsd %xmm0, 0x78(%rsp)
movq %rbx, %rdi
leaq 0x498d55(%rip), %rsi    # 0x4e6690
callq 0x3d278
testl %eax, %eax
je 0x4d95a
movsd 0x78(%rsp), %xmm0
leaq 0x498d3f(%rip), %rax    # 0x4e6690
ucomisd 0x18(%rax), %xmm0
jne 0x4d95a
jnp 0x4d985
movq 0x28(%rsp), %rdi
leaq 0x60(%rsp), %rsi
callq 0x3d752
movsd 0x78(%rsp), %xmm0
movq 0x28(%rsp), %rax
movsd %xmm0, 0x18(%rax)
movsd 0x40(%rsp), %xmm0
movsd %xmm0, 0x38(%rsp)
movsd 0x30(%rsp), %xmm0
addsd 0x40(%rsp), %xmm0
movsd %xmm0, 0x30(%rsp)
incl 0x10(%rsp)
incl %ebp
cmpl 0x14(%rsp), %ebp
jne 0x4d7bc
xorps %xmm0, %xmm0
cvtsi2sdl 0x10(%rsp), %xmm0
movq 0x28(%rsp), %rbx
jmp 0x4d9c5
movsd 0x54679(%rip), %xmm0    # 0xa2038
movsd %xmm1, 0x30(%rsp)
movq 0x50(%rsp), %rax
testq %rax, %rax
je 0x4d9d9
movsd 0x38(%rsp), %xmm1
movsd %xmm1, (%rax)
movq 0x58(%rsp), %rax
testq %rax, %rax
je 0x4d9f1
movsd 0x30(%rsp), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, (%rax)
leaq 0x18(%rsp), %rdi
callq 0x6f876
movq %rbx, %rax
addq $0x138, %rsp            # imm = 0x138
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x4da26
jmp 0x4da26
jmp 0x4da26
jmp 0x4da26
jmp 0x4da26
jmp 0x4da26
jmp 0x4da26
jmp 0x4da26
jmp 0x4da26
jmp 0x4da26
jmp 0x4da26
movq %rax, %rbx
leaq 0x18(%rsp), %rdi
callq 0x6f876
movq %rbx, %rdi
callq 0xe7e0