// File: /workspace/github_cpp_makefile_star2_O1/tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelUtils.cpp
// Function: gaps::EstimateSupportPlane(gaps::R3SurfelPointSet*, double, double*)
R3Plane
EstimateSupportPlane(R3SurfelPointSet *pointset,
  RNLength accuracy, RNScalar *npoints)
{
  // Check point set
  if (pointset->NPoints() == 0) {
    if (npoints) *npoints = 0;
    return R3null_plane;
  }

  // Determine zres
  const R3Box& bbox = pointset->BBox();
  RNScalar zmin = bbox.ZMin();
  RNScalar zlength = bbox.ZLength();
  if (zlength == 0) return R3Plane(0, 0, 1, -(bbox.ZMin()));
  int zres = (int) (2 * zlength / accuracy) + 4;

  // Initialize votes
  RNScalar *votes = new RNScalar [zres];
  for (int i = 0; i < zres; i++) votes[i] = 0;
  
  // Cast votes
  int step = 10 * pointset->NPoints() / zres + 1;
  for (int j = 0; j < pointset->NPoints(); j += step) {
    const R3SurfelPoint *point = pointset->Point(j);
    R3Point position = point->Position();
    int iz = (int) (zres * (position.Z() - zmin) / zlength);
    if (iz >= zres) iz = zres - 1;
    for (int k = iz; k >= 0; k--) votes[k] += 0.01;
    votes[iz]++;
  }

  // Blur votes
  RNScalar *copy = new RNScalar [ zres ];
  for (int i = 0; i < zres; i++) copy[i] = votes[i];
  votes[0] = 0.75*copy[0] + 0.25*copy[1];
  votes[zres-1] = 0.75*copy[zres-1] + 0.25*copy[zres-2];
  for (int i = 1; i < zres-1; i++) {
    votes[i] = 0.5 * copy[i];
    votes[i] += 0.25 * copy[i-1];
    votes[i] += 0.25 * copy[i+1];
  }
  delete [] copy;

  // Find z with most votes
  RNScalar best_z = 0;
  RNScalar best_vote = 0;
  for (int i = 0; i < zres; i++) {
    if (votes[i] > best_vote) {
      best_vote = votes[i];
      best_z = zlength * i / zres + zmin;
    }
  }

  // Delete votes
  delete [] votes;

  // Fill in return value
  if (npoints) *npoints = step * best_vote;

  // Return ground plane
  return R3Plane(0, 0, 1, -best_z);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x78, %rsp
movq %rdi, %r14
movl 0x10(%rsi), %ebx
testl %ebx, %ebx
je 0x22579
movq %rsi, %r12
movsd 0x30(%rsi), %xmm3
movsd 0x48(%rsi), %xmm2
subsd %xmm3, %xmm2
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm2
jne 0x2259c
jp 0x2259c
xorpd 0xbbe3d(%rip), %xmm3    # 0xde3a0
movsd 0xbbbe5(%rip), %xmm2    # 0xde150
xorpd %xmm0, %xmm0
movq %r14, %rdi
callq 0x83894
jmp 0x22594
testq %rdx, %rdx
je 0x22585
movq $0x0, (%rdx)
leaq 0x513d14(%rip), %rsi    # 0x5362a0
movq %r14, %rdi
callq 0x83872
movq %r14, %rax
jmp 0x228c8
movapd %xmm3, 0x30(%rsp)
movq %rdx, 0x50(%rsp)
movq %r14, 0x58(%rsp)
movsd %xmm2, 0x20(%rsp)
movapd %xmm2, %xmm1
addsd %xmm2, %xmm1
divsd %xmm0, %xmm1
cvttsd2si %xmm1, %r13d
leal 0x4(%r13), %r14d
movslq %r14d, %rax
shlq $0x3, %rax
cmpl $-0x4, %r13d
movq $-0x1, %rdi
cmovgeq %rax, %rdi
movq %rdi, 0x48(%rsp)
callq 0x13030
movq %rax, %r15
movq %r13, 0x10(%rsp)
cmpl $-0x3, %r13d
jl 0x22610
cmpl $0x2, %r14d
movl $0x1, %edx
cmovgel %r14d, %edx
shlq $0x3, %rdx
movq %r15, %rdi
xorl %esi, %esi
callq 0x132a0
addl %ebx, %ebx
leal (%rbx,%rbx,4), %eax
cltd
idivl %r14d
incl %eax
movl %eax, 0xc(%rsp)
cmpl $0x0, 0x10(%r12)
xorps %xmm0, %xmm0
cvtsi2sd %r14d, %xmm0
movsd %xmm0, 0x18(%rsp)
jle 0x226fe
movq 0x10(%rsp), %rax
leal 0x3(%rax), %ebx
movslq 0xc(%rsp), %r13
xorl %ebp, %ebp
movq 0x8(%r12), %rax
movq %rbp, %rcx
shlq $0x4, %rcx
movq (%rax,%rcx), %rdx
movq 0x8(%rax,%rcx), %rax
xorps %xmm0, %xmm0
cvtss2sd (%rax), %xmm0
addsd 0x10(%rdx), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rax), %xmm1
addsd 0x18(%rdx), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rax), %xmm2
addsd 0x20(%rdx), %xmm2
leaq 0x60(%rsp), %rdi
callq 0x84d7e
movsd 0xbc44b(%rip), %xmm1    # 0xdeae0
movsd 0x70(%rsp), %xmm0
subsd 0x30(%rsp), %xmm0
mulsd 0x18(%rsp), %xmm0
divsd 0x20(%rsp), %xmm0
cvttsd2si %xmm0, %eax
cmpl %eax, %r14d
cmovlel %ebx, %eax
testl %eax, %eax
js 0x226d7
movl %eax, %ecx
incq %rcx
movsd -0x8(%r15,%rcx,8), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x8(%r15,%rcx,8)
decq %rcx
jg 0x226c0
cltq
movsd (%r15,%rax,8), %xmm0
addsd 0xbba69(%rip), %xmm0    # 0xde150
movsd %xmm0, (%r15,%rax,8)
addq %r13, %rbp
movslq 0x10(%r12), %rax
cmpq %rax, %rbp
jl 0x22648
movq 0x48(%rsp), %rdi
callq 0x13030
movq %rax, %r12
movq 0x10(%rsp), %rbx
cmpl $-0x3, %ebx
jl 0x22731
cmpl $0x2, %r14d
movl $0x1, %edx
cmovgel %r14d, %edx
shlq $0x3, %rdx
movq %r12, %rdi
movq %r15, %rsi
callq 0x133f0
movsd 0xbc3af(%rip), %xmm0    # 0xdeae8
movsd 0x8(%r12), %xmm1
mulsd %xmm0, %xmm1
movsd 0xbc3a4(%rip), %xmm2    # 0xdeaf0
movsd (%r12), %xmm3
mulsd %xmm2, %xmm3
addsd %xmm1, %xmm3
movsd %xmm3, (%r15)
movslq %ebx, %rax
movsd 0x10(%r12,%rax,8), %xmm1
mulsd 0x18(%r12,%rax,8), %xmm2
mulsd %xmm0, %xmm1
addsd %xmm1, %xmm2
movsd %xmm2, 0x18(%r15,%rax,8)
cmpl $-0x1, %ebx
jl 0x227e5
leal 0x3(%rbx), %ecx
cmpl $0x3, %ecx
movl $0x2, %eax
cmovgel %ecx, %eax
movl $0x1, %ecx
movsd 0xbb9a1(%rip), %xmm1    # 0xde140
movsd (%r12,%rcx,8), %xmm2
mulsd %xmm1, %xmm2
movsd %xmm2, (%r15,%rcx,8)
movsd -0x8(%r12,%rcx,8), %xmm3
mulsd %xmm0, %xmm3
addsd %xmm2, %xmm3
movsd %xmm3, (%r15,%rcx,8)
movsd 0x8(%r12,%rcx,8), %xmm2
mulsd %xmm0, %xmm2
addsd %xmm3, %xmm2
movsd %xmm2, (%r15,%rcx,8)
leaq 0x1(%rcx), %rdx
movq %rdx, %rcx
cmpq %rdx, %rax
jne 0x2279f
movq %r12, %rdi
callq 0x13680
xorpd %xmm6, %xmm6
xorpd %xmm7, %xmm7
cmpl $-0x3, %ebx
movapd 0x30(%rsp), %xmm3
movsd 0x20(%rsp), %xmm4
movsd 0x18(%rsp), %xmm5
jl 0x22865
cmpl $0x2, %r14d
movl $0x1, %eax
cmovgel %r14d, %eax
xorpd %xmm0, %xmm0
xorl %ecx, %ecx
xorpd %xmm1, %xmm1
movsd (%r15,%rcx,8), %xmm7
xorps %xmm2, %xmm2
cvtsi2sd %ecx, %xmm2
mulsd %xmm4, %xmm2
divsd %xmm5, %xmm2
addsd %xmm3, %xmm2
movapd %xmm0, %xmm6
cmpltsd %xmm7, %xmm6
andpd %xmm6, %xmm2
andnpd %xmm1, %xmm6
orpd %xmm2, %xmm6
maxsd %xmm0, %xmm7
incq %rcx
movapd %xmm7, %xmm0
movapd %xmm6, %xmm1
cmpq %rcx, %rax
jne 0x22823
movapd %xmm7, 0x20(%rsp)
movapd %xmm6, 0x30(%rsp)
movq %r15, %rdi
callq 0x13680
movq 0x50(%rsp), %rax
testq %rax, %rax
je 0x2289a
xorps %xmm0, %xmm0
cvtsi2sdl 0xc(%rsp), %xmm0
movapd 0x20(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, (%rax)
movapd 0x30(%rsp), %xmm3
xorpd 0xbbaf8(%rip), %xmm3    # 0xde3a0
movsd 0xbb8a0(%rip), %xmm2    # 0xde150
xorpd %xmm0, %xmm0
xorpd %xmm1, %xmm1
movq 0x58(%rsp), %rbx
movq %rbx, %rdi
callq 0x83894
movq %rbx, %rax
addq $0x78, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq