// File: 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:
subq $0xd8, %rsp
movq %rdi, 0x20(%rsp)
movq %rdi, %rax
movq %rax, 0x28(%rsp)
movq %rdi, 0xd0(%rsp)
movq %rsi, 0xc8(%rsp)
movsd %xmm0, 0xc0(%rsp)
movq %rdx, 0xb8(%rsp)
movq 0xc8(%rsp), %rdi
callq 0x18700
cmpl $0x0, %eax
jne 0x32d67
cmpq $0x0, 0xb8(%rsp)
je 0x32d51
movq 0xb8(%rsp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movq 0x20(%rsp), %rdi
leaq 0x5caf83(%rip), %rsi    # 0x5fdce0
callq 0xf5990
jmp 0x332e2
movq 0xc8(%rsp), %rdi
callq 0x435d0
movq %rax, 0xb0(%rsp)
movq 0xb0(%rsp), %rdi
callq 0x435e0
movsd %xmm0, 0xa8(%rsp)
movq 0xb0(%rsp), %rdi
callq 0x44170
movsd %xmm0, 0xa0(%rsp)
movsd 0xa0(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x32dfd
jp 0x32dfd
movq 0xb0(%rsp), %rdi
callq 0x435e0
movq 0x20(%rsp), %rdi
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm3
xorps %xmm1, %xmm1
movsd 0x171378(%rip), %xmm2   # 0x1a4168
movaps %xmm1, %xmm0
callq 0xf59d0
jmp 0x332e2
movsd 0x171373(%rip), %xmm0   # 0x1a4178
mulsd 0xa0(%rsp), %xmm0
divsd 0xc0(%rsp), %xmm0
cvttsd2si %xmm0, %eax
addl $0x4, %eax
movl %eax, 0x9c(%rsp)
movslq 0x9c(%rsp), %rax
movl $0x8, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
callq 0x13030
movq %rax, 0x90(%rsp)
movl $0x0, 0x8c(%rsp)
movl 0x8c(%rsp), %eax
cmpl 0x9c(%rsp), %eax
jge 0x32e9c
movq 0x90(%rsp), %rax
movslq 0x8c(%rsp), %rcx
xorps %xmm0, %xmm0
movsd %xmm0, (%rax,%rcx,8)
movl 0x8c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x8c(%rsp)
jmp 0x32e61
movq 0xc8(%rsp), %rdi
callq 0x18700
imull $0xa, %eax, %eax
cltd
idivl 0x9c(%rsp)
addl $0x1, %eax
movl %eax, 0x88(%rsp)
movl $0x0, 0x84(%rsp)
movl 0x84(%rsp), %eax
movl %eax, 0x1c(%rsp)
movq 0xc8(%rsp), %rdi
callq 0x18700
movl %eax, %ecx
movl 0x1c(%rsp), %eax
cmpl %ecx, %eax
jge 0x32fe6
movq 0xc8(%rsp), %rdi
movl 0x84(%rsp), %esi
callq 0x18710
movq %rax, 0x78(%rsp)
movq 0x78(%rsp), %rsi
leaq 0x60(%rsp), %rdi
callq 0x18730
cvtsi2sdl 0x9c(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
leaq 0x60(%rsp), %rdi
callq 0x187a0
movaps %xmm0, %xmm1
movsd 0x10(%rsp), %xmm0
subsd 0xa8(%rsp), %xmm1
mulsd %xmm1, %xmm0
divsd 0xa0(%rsp), %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x5c(%rsp)
movl 0x5c(%rsp), %eax
cmpl 0x9c(%rsp), %eax
jl 0x32f72
movl 0x9c(%rsp), %eax
subl $0x1, %eax
movl %eax, 0x5c(%rsp)
movl 0x5c(%rsp), %eax
movl %eax, 0x58(%rsp)
cmpl $0x0, 0x58(%rsp)
jl 0x32fad
movq 0x90(%rsp), %rax
movslq 0x58(%rsp), %rcx
movsd 0x171b42(%rip), %xmm0   # 0x1a4ad8
addsd (%rax,%rcx,8), %xmm0
movsd %xmm0, (%rax,%rcx,8)
movl 0x58(%rsp), %eax
addl $-0x1, %eax
movl %eax, 0x58(%rsp)
jmp 0x32f7a
movq 0x90(%rsp), %rax
movslq 0x5c(%rsp), %rcx
movsd 0x1711a6(%rip), %xmm0   # 0x1a4168
addsd (%rax,%rcx,8), %xmm0
movsd %xmm0, (%rax,%rcx,8)
movl 0x88(%rsp), %eax
addl 0x84(%rsp), %eax
movl %eax, 0x84(%rsp)
jmp 0x32ec9
movslq 0x9c(%rsp), %rax
movl $0x8, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
callq 0x13030
movq %rax, 0x50(%rsp)
movl $0x0, 0x4c(%rsp)
movl 0x4c(%rsp), %eax
cmpl 0x9c(%rsp), %eax
jge 0x33057
movq 0x90(%rsp), %rax
movslq 0x4c(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
movq 0x50(%rsp), %rax
movslq 0x4c(%rsp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl 0x4c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x4c(%rsp)
jmp 0x3301c
movq 0x50(%rsp), %rax
movsd (%rax), %xmm0
movq 0x50(%rsp), %rax
movsd 0x171a63(%rip), %xmm1   # 0x1a4ad0
mulsd 0x8(%rax), %xmm1
movsd 0x171a4e(%rip), %xmm2   # 0x1a4ac8
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movq 0x90(%rsp), %rax
movsd %xmm0, (%rax)
movq 0x50(%rsp), %rax
movl 0x9c(%rsp), %ecx
subl $0x1, %ecx
movslq %ecx, %rcx
movsd (%rax,%rcx,8), %xmm0
movq 0x50(%rsp), %rax
movl 0x9c(%rsp), %ecx
subl $0x2, %ecx
movslq %ecx, %rcx
movsd 0x171a11(%rip), %xmm1   # 0x1a4ad0
mulsd (%rax,%rcx,8), %xmm1
movsd 0x1719fc(%rip), %xmm2   # 0x1a4ac8
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movq 0x90(%rsp), %rax
movl 0x9c(%rsp), %ecx
subl $0x1, %ecx
movslq %ecx, %rcx
movsd %xmm0, (%rax,%rcx,8)
movl $0x1, 0x48(%rsp)
movl 0x48(%rsp), %eax
movl 0x9c(%rsp), %ecx
subl $0x1, %ecx
cmpl %ecx, %eax
jge 0x331bb
movq 0x50(%rsp), %rax
movslq 0x48(%rsp), %rcx
movsd 0x17103a(%rip), %xmm0   # 0x1a4158
mulsd (%rax,%rcx,8), %xmm0
movq 0x90(%rsp), %rax
movslq 0x48(%rsp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movq 0x50(%rsp), %rax
movl 0x48(%rsp), %ecx
subl $0x1, %ecx
movslq %ecx, %rcx
movsd (%rax,%rcx,8), %xmm0
movq 0x90(%rsp), %rax
movslq 0x48(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm1
movsd 0x17196d(%rip), %xmm2   # 0x1a4ad0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, (%rax,%rcx,8)
movq 0x50(%rsp), %rax
movl 0x48(%rsp), %ecx
addl $0x1, %ecx
movslq %ecx, %rcx
movsd (%rax,%rcx,8), %xmm0
movq 0x90(%rsp), %rax
movslq 0x48(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm1
movsd 0x171932(%rip), %xmm2   # 0x1a4ad0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, (%rax,%rcx,8)
movl 0x48(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x48(%rsp)
jmp 0x330f6
movq 0x50(%rsp), %rax
movq %rax, 0x8(%rsp)
cmpq $0x0, %rax
je 0x331d5
movq 0x8(%rsp), %rdi
callq 0x13750
xorps %xmm0, %xmm0
movsd %xmm0, 0x40(%rsp)
xorps %xmm0, %xmm0
movsd %xmm0, 0x38(%rsp)
movl $0x0, 0x34(%rsp)
movl 0x34(%rsp), %eax
cmpl 0x9c(%rsp), %eax
jge 0x3326c
movq 0x90(%rsp), %rax
movslq 0x34(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
ucomisd 0x38(%rsp), %xmm0
jbe 0x3325d
movq 0x90(%rsp), %rax
movslq 0x34(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
movsd %xmm0, 0x38(%rsp)
movsd 0xa0(%rsp), %xmm0
cvtsi2sdl 0x34(%rsp), %xmm1
mulsd %xmm1, %xmm0
cvtsi2sdl 0x9c(%rsp), %xmm1
divsd %xmm1, %xmm0
addsd 0xa8(%rsp), %xmm0
movsd %xmm0, 0x40(%rsp)
jmp 0x3325f
movl 0x34(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x34(%rsp)
jmp 0x331ef
movq 0x90(%rsp), %rax
movq %rax, (%rsp)
cmpq $0x0, %rax
je 0x33287
movq (%rsp), %rdi
callq 0x13750
cmpq $0x0, 0xb8(%rsp)
je 0x332ad
cvtsi2sdl 0x88(%rsp), %xmm0
mulsd 0x38(%rsp), %xmm0
movq 0xb8(%rsp), %rax
movsd %xmm0, (%rax)
movq 0x20(%rsp), %rdi
movsd 0x40(%rsp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm3
xorps %xmm1, %xmm1
movsd 0x170e8e(%rip), %xmm2   # 0x1a4168
movaps %xmm1, %xmm0
callq 0xf59d0
movq 0x28(%rsp), %rax
addq $0xd8, %rsp
retq
nop