// File: /workspace/github_cpp_makefile_star2_O2/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, %rbx
movl 0x10(%rsi), %ebp
testl %ebp, %ebp
je 0x21095
movq %rsi, %r12
movsd 0x30(%rsi), %xmm3
movsd 0x48(%rsi), %xmm2
subsd %xmm3, %xmm2
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm2
jne 0x210b2
jp 0x210b2
xorpd 0xa731c(%rip), %xmm3    # 0xc83a0
movsd 0xa70bc(%rip), %xmm2    # 0xc8148
xorpd %xmm0, %xmm0
jmp 0x21344
testq %rdx, %rdx
je 0x2109e
andq $0x0, (%rdx)
leaq 0x50521b(%rip), %rsi    # 0x5262c0
movq %rbx, %rdi
callq 0x76d90
jmp 0x2134c
movapd %xmm3, 0x20(%rsp)
movq %rdx, 0x50(%rsp)
movq %rbx, 0x58(%rsp)
movsd %xmm2, 0x18(%rsp)
movapd %xmm2, %xmm1
addsd %xmm2, %xmm1
divsd %xmm0, %xmm1
cvttsd2si %xmm1, %ecx
leal 0x4(%rcx), %r13d
movslq %r13d, %rbx
leaq (,%rbx,8), %rax
movq %rcx, 0x10(%rsp)
cmpl $-0x4, %ecx
pushq $-0x1
popq %rdi
cmovgeq %rax, %rdi
movq %rdi, 0x48(%rsp)
callq 0x13030
movq %rax, %r15
xorl %eax, %eax
testl %ebx, %ebx
movl $0x0, %r14d
movl %r13d, 0x8(%rsp)
cmovgl %r13d, %r14d
cmpq %rax, %r14
je 0x21125
andq $0x0, (%r15,%rax,8)
incq %rax
jmp 0x21116
imull $0xa, %ebp, %eax
cltd
movl 0x8(%rsp), %ecx
idivl %ecx
incl %eax
cvtsi2sd %ecx, %xmm0
movsd %xmm0, 0x30(%rsp)
movq 0x10(%rsp), %rcx
addl $0x3, %ecx
movl %ecx, 0x4(%rsp)
movl %eax, 0xc(%rsp)
movslq %eax, %rbx
xorl %r13d, %r13d
movslq %ebp, %rax
cmpq %rax, %r13
jge 0x211df
movq %r13, %rsi
shlq $0x4, %rsi
addq 0x8(%r12), %rsi
leaq 0x60(%rsp), %rdi
callq 0x16310
movsd 0xa7975(%rip), %xmm1    # 0xc8af0
movsd 0x70(%rsp), %xmm0
subsd 0x20(%rsp), %xmm0
mulsd 0x30(%rsp), %xmm0
divsd 0x18(%rsp), %xmm0
cvttsd2si %xmm0, %eax
cmpl %eax, 0x8(%rsp)
cmovlel 0x4(%rsp), %eax
movl %eax, %ecx
testl %ecx, %ecx
js 0x211bc
movl %ecx, %edx
movsd (%r15,%rdx,8), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, (%r15,%rdx,8)
decl %ecx
jmp 0x211a2
cltq
movsd (%r15,%rax,8), %xmm0
addsd 0xa6f7c(%rip), %xmm0    # 0xc8148
movsd %xmm0, (%r15,%rax,8)
addq %rbx, %r13
movl 0x10(%r12), %ebp
jmp 0x21151
movq 0x48(%rsp), %rdi
callq 0x13030
xorl %ecx, %ecx
movq 0x58(%rsp), %rbx
movq 0x10(%rsp), %rdx
cmpq %rcx, %r14
je 0x2120a
movsd (%r15,%rcx,8), %xmm0
movsd %xmm0, (%rax,%rcx,8)
incq %rcx
jmp 0x211f5
movsd 0xa78ce(%rip), %xmm0    # 0xc8ae0
movsd 0x8(%rax), %xmm1
mulsd %xmm0, %xmm1
movsd 0xa78c5(%rip), %xmm2    # 0xc8ae8
movsd (%rax), %xmm3
mulsd %xmm2, %xmm3
addsd %xmm1, %xmm3
movsd %xmm3, (%r15)
movslq 0x4(%rsp), %rcx
movslq %edx, %rdx
movsd 0x10(%rax,%rdx,8), %xmm1
mulsd %xmm0, %xmm1
mulsd (%rax,%rcx,8), %xmm2
addsd %xmm1, %xmm2
movsd %xmm2, (%r15,%rcx,8)
pushq $0x1
popq %rdx
movsd 0xa6ed8(%rip), %xmm1    # 0xc8138
cmpq %rcx, %rdx
jge 0x21295
movsd (%rax,%rdx,8), %xmm2
mulsd %xmm1, %xmm2
movsd -0x8(%rax,%rdx,8), %xmm3
mulsd %xmm0, %xmm3
addsd %xmm2, %xmm3
movsd 0x8(%rax,%rdx,8), %xmm2
mulsd %xmm0, %xmm2
addsd %xmm3, %xmm2
movsd %xmm2, (%r15,%rdx,8)
incq %rdx
jmp 0x21260
movq %rax, %rdi
callq 0x136a0
xorpd %xmm5, %xmm5
xorl %eax, %eax
xorpd %xmm6, %xmm6
movapd 0x20(%rsp), %xmm3
movsd 0x18(%rsp), %xmm4
cmpq %rax, %r14
je 0x212f6
movsd (%r15,%rax,8), %xmm0
cvtsi2sd %eax, %xmm1
mulsd %xmm4, %xmm1
divsd 0x30(%rsp), %xmm1
addsd %xmm3, %xmm1
movapd %xmm6, %xmm2
cmpltsd %xmm0, %xmm2
andpd %xmm2, %xmm1
andnpd %xmm5, %xmm2
orpd %xmm1, %xmm2
maxsd %xmm6, %xmm0
incq %rax
movapd %xmm2, %xmm5
movapd %xmm0, %xmm6
jmp 0x212b3
movapd %xmm6, 0x20(%rsp)
movapd %xmm5, 0x30(%rsp)
movq %r15, %rdi
callq 0x136a0
movq 0x50(%rsp), %rax
testq %rax, %rax
je 0x21328
cvtsi2sdl 0xc(%rsp), %xmm0
movapd 0x20(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, (%rax)
movaps 0x30(%rsp), %xmm3
xorps 0xa706c(%rip), %xmm3    # 0xc83a0
movsd 0xa6e0c(%rip), %xmm2    # 0xc8148
xorpd %xmm0, %xmm0
xorpd %xmm1, %xmm1
movq %rbx, %rdi
callq 0x76db2
movq %rbx, %rax
addq $0x78, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq