// File: tomfunkhouser[P]gaps/pkgs/GSV/GSVScanline.cpp
// Function: gaps::GSVScanline::EstimatedGroundZ() const
RNCoord GSVScanline::
EstimatedGroundZ(void) const
{
  // Get convenient variables
  GSVLaser *laser = Laser();
  const double vlp16_laser_height = 2.0;
  const int sick_scan_ground_index = 25;
  const int sick_radius = 1;

  // Check device
  if ((laser->DeviceType() == GSV_SICK_LASER) &&
      (NPoints() > 0) &&
      (NPoints() > sick_scan_ground_index+sick_radius) &&
      points) {

    // Get three points around scan_ground_index
    R3Point p0 = PointPosition(NPoints() - sick_scan_ground_index - sick_radius);
    R3Point p1 = PointPosition(NPoints() - sick_scan_ground_index);
    R3Point p2 = PointPosition(NPoints() - sick_scan_ground_index + sick_radius);

    // Find median Z coordinate
    RNCoord z;
    if (p0.Z() < p1.Z()) {
      if (p1.Z() < p2.Z()) {
        z = p1.Z();
      }
      else {
        if (p0.Z() < p2.Z()) z = p2.Z();
        else z = p0.Z();
      }
    }
    else {
      if (p0.Z() < p2.Z()) {
        z = p0.Z();
      }
      else {
        if (p1.Z() < p2.Z()) z = p2.Z();
        else z = p1.Z();
      }
    }
  
    // Return estimate of ground Z coordinate
    return z;
  }
  else {
    const GSVPose& pose = Pose();
    const R3Point& viewpoint = pose.Viewpoint();
    return viewpoint.Z() - vlp16_laser_height;
  }

  // Should not get here
  return RN_UNKNOWN;
}

// Assembly:
subq $0xb8, %rsp
movq %rdi, 0xa8(%rsp)
movq 0xa8(%rsp), %rdi
movq %rdi, 0x28(%rsp)
callq 0x56b40
movq %rax, 0xa0(%rsp)
movsd 0x20efdf(%rip), %xmm0   # 0x2660b0
movsd %xmm0, 0x98(%rsp)
movl $0x19, 0x94(%rsp)
movl $0x1, 0x90(%rsp)
movq 0xa0(%rsp), %rdi
callq 0x44ac0
cmpl $0x2, %eax
jne 0x572e0
movq 0x28(%rsp), %rdi
callq 0x3a120
cmpl $0x0, %eax
jle 0x572e0
movq 0x28(%rsp), %rdi
callq 0x3a120
cmpl $0x1a, %eax
jle 0x572e0
movq 0x28(%rsp), %rax
cmpq $0x0, 0x28(%rax)
je 0x572e0
movq 0x28(%rsp), %rdi
callq 0x3a120
movq 0x28(%rsp), %rsi
movl %eax, %edx
subl $0x19, %edx
subl $0x1, %edx
leaq 0x78(%rsp), %rdi
callq 0x59950
movq 0x28(%rsp), %rdi
callq 0x3a120
movq 0x28(%rsp), %rsi
movl %eax, %edx
subl $0x19, %edx
leaq 0x60(%rsp), %rdi
callq 0x59950
movq 0x28(%rsp), %rdi
callq 0x3a120
movq 0x28(%rsp), %rsi
movl %eax, %edx
subl $0x19, %edx
addl $0x1, %edx
leaq 0x48(%rsp), %rdi
callq 0x59950
leaq 0x78(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x20(%rsp)
leaq 0x60(%rsp), %rdi
callq 0x38c20
movsd 0x20(%rsp), %xmm1
ucomisd %xmm1, %xmm0
jbe 0x5724d
leaq 0x60(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x18(%rsp)
leaq 0x48(%rsp), %rdi
callq 0x38c20
movsd 0x18(%rsp), %xmm1
ucomisd %xmm1, %xmm0
jbe 0x571fe
leaq 0x60(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x40(%rsp)
jmp 0x57248
leaq 0x78(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x10(%rsp)
leaq 0x48(%rsp), %rdi
callq 0x38c20
movsd 0x10(%rsp), %xmm1
ucomisd %xmm1, %xmm0
jbe 0x57236
leaq 0x48(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x40(%rsp)
jmp 0x57246
leaq 0x78(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x40(%rsp)
jmp 0x57248
jmp 0x572cf
leaq 0x78(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x8(%rsp)
leaq 0x48(%rsp), %rdi
callq 0x38c20
movsd 0x8(%rsp), %xmm1
ucomisd %xmm1, %xmm0
jbe 0x57285
leaq 0x78(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x40(%rsp)
jmp 0x572cd
leaq 0x60(%rsp), %rdi
callq 0x38c20
movsd %xmm0, (%rsp)
leaq 0x48(%rsp), %rdi
callq 0x38c20
movsd (%rsp), %xmm1
ucomisd %xmm1, %xmm0
jbe 0x572bb
leaq 0x48(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x40(%rsp)
jmp 0x572cb
leaq 0x60(%rsp), %rdi
callq 0x38c20
movsd %xmm0, 0x40(%rsp)
jmp 0x572cd
jmp 0x572cf
movsd 0x40(%rsp), %xmm0
movsd %xmm0, 0xb0(%rsp)
jmp 0x5731d
movq 0x28(%rsp), %rdi
callq 0x44c60
movq %rax, 0x38(%rsp)
movq 0x38(%rsp), %rdi
callq 0x3a330
movq %rax, 0x30(%rsp)
movq 0x30(%rsp), %rdi
callq 0x38c20
movsd 0x20eda0(%rip), %xmm1   # 0x2660b0
subsd %xmm1, %xmm0
movsd %xmm0, 0xb0(%rsp)
movsd 0xb0(%rsp), %xmm0
addq $0xb8, %rsp
retq
nop