// File: tomfunkhouser[P]gaps/pkgs/RGBD/RGBDUtil.cpp
// Function: gaps::RGBDCreateUndistortedDepthImage(gaps::R2Grid const&, gaps::R2Grid&, gaps::RGBDCamera const&, gaps::RGBDCamera&)
int RGBDCreateUndistortedDepthImage(
   const R2Grid& input_distorted_depth_image,
   R2Grid& output_undistorted_depth_image,
   const RGBDCamera& distorted_camera, RGBDCamera& undistorted_camera)
{
  // Check output camera
  assert((undistorted_camera.k1_depth == 0) && (undistorted_camera.k2_depth == 0) &&
         (undistorted_camera.k3_depth == 0) && (undistorted_camera.k4_depth == 0) &&
         (undistorted_camera.p1_depth == 0) && (undistorted_camera.p2_depth == 0) &&
         (undistorted_camera.depthToColorExtrinsics.IsIdentity()));

  // Check inputs
  if ((distorted_camera.depthWidth == 0) || (distorted_camera.depthHeight == 0)) return 0;
  if ((undistorted_camera.depthWidth == 0) || (undistorted_camera.depthHeight == 0)) return 0;
  if ((input_distorted_depth_image.XResolution() == 0) || (input_distorted_depth_image.YResolution() == 0)) return 0;
  
  // Check if don't need to undistort or warp
  if ((undistorted_camera.depthWidth == distorted_camera.depthWidth) &&
      (undistorted_camera.depthHeight == distorted_camera.depthHeight) && 
      (distorted_camera.mx_depth == 0) && (distorted_camera.my_depth == 0) &&
      (distorted_camera.k1_depth == 0) && (distorted_camera.k2_depth == 0) &&
      (distorted_camera.k3_depth == 0) && (distorted_camera.k4_depth == 0) &&
      (distorted_camera.p1_depth == 0) && (distorted_camera.p2_depth == 0) &&
      (distorted_camera.depthToColorExtrinsics.IsIdentity())) {
    output_undistorted_depth_image = input_distorted_depth_image;
    return 1;
  }

  // Save copy of input image (so can pass same image both in and out)
  R2Grid source_depth_image(input_distorted_depth_image);

  // Initialize the output image
  output_undistorted_depth_image = R2Grid(undistorted_camera.depthWidth, undistorted_camera.depthHeight);
  output_undistorted_depth_image.Clear(R2_GRID_UNKNOWN_VALUE);

#ifdef MASK_HOLES
  // Create image with no holes 
  source_depth_image.Substitute(0, R2_GRID_UNKNOWN_VALUE);
  source_depth_image.FillHoles();
#endif
  
  // Compute output image via a combination of reverse and forward mapping
  double step_factor = 0.5; // make <1 to avoid pin-holes in output image (or use InterpolateDepthImage, which is faster but not as good)
  double x_step = step_factor * source_depth_image.XResolution() / (double) output_undistorted_depth_image.XResolution();
  double y_step = step_factor * source_depth_image.YResolution() / (double) output_undistorted_depth_image.YResolution();
  for (double undistorted_input_y = 0.5*y_step; undistorted_input_y < source_depth_image.YResolution(); undistorted_input_y += y_step) {
    for (double undistorted_input_x = 0.5*x_step; undistorted_input_x < source_depth_image.XResolution(); undistorted_input_x += x_step) {
      // Compute coordinates in distorted input image
      // Note that this ignores p1 and p2
      R2Point distorted_input_position(undistorted_input_x, undistorted_input_y);
      if ((distorted_camera.k1_depth != 0.0) || (distorted_camera.k2_depth != 0.0) ||
          (distorted_camera.k3_depth != 0.0) || (distorted_camera.k3_depth != 0.0) ||
          (distorted_camera.p1_depth != 0.0) || (distorted_camera.p2_depth != 0.0)) {
        double nx = (undistorted_input_x - distorted_camera.mx_depth) / distorted_camera.fx_depth;
        double ny = (undistorted_input_y - distorted_camera.my_depth) / distorted_camera.fy_depth;
        double rr = nx*nx + ny*ny; double rrrr = rr*rr;
        double s = 1.0 + rr*distorted_camera.k1_depth + rrrr*distorted_camera.k2_depth + rrrr*rr*distorted_camera.k3_depth + rrrr*rrrr*distorted_camera.k4_depth;
        nx = s*nx + distorted_camera.p2_depth*(rr + 2*nx*nx) + 2*distorted_camera.p1_depth*nx*ny;
        ny = s*ny + distorted_camera.p1_depth*(rr + 2*ny*ny) + 2*distorted_camera.p2_depth*nx*ny;
        double x = nx*distorted_camera.fx_depth + distorted_camera.mx_depth;
        double y = ny*distorted_camera.fy_depth + distorted_camera.my_depth;
        distorted_input_position.Reset(x, y);
      }

      // Get closest distorted input pixel
      int distorted_input_ix = (int) (distorted_input_position.X() + 0.5);
      int distorted_input_iy = (int) (distorted_input_position.Y() + 0.5);
      if (distorted_input_ix < 0) continue;
      if (distorted_input_ix >= source_depth_image.XResolution()) continue;
      if (distorted_input_iy < 0) continue;
      if (distorted_input_iy >= source_depth_image.YResolution()) continue;

#ifdef MASK_HOLES
      // Get depth, even if there was a hole there originally
      RNBoolean depth_was_missing = FALSE;
      RNScalar depth = source_depth_image.GridValue(distorted_input_ix, distorted_input_iy);
      if (depth <= 0) {
        depth = interpolated_depth_image.GridValue(distorted_input_ix, distorted_input_iy);
        if (depth <= 0) continue;
        depth_was_missing = TRUE;
      }
#else
      // Get/check depth
      RNScalar depth = source_depth_image.GridValue(distorted_input_ix, distorted_input_iy);
      if (depth <= 0) continue;
#endif
      
      // Find 3D position in depth camera coordinates
      R3Point world_position;
      world_position[0] = (undistorted_input_x - distorted_camera.mx_depth) * depth / distorted_camera.fx_depth;
      world_position[1] = (undistorted_input_y - distorted_camera.my_depth) * depth / distorted_camera.fy_depth;
      world_position[2] = -depth;

      // Transform into output (color) camera coordinates
      world_position = distorted_camera.depthToColorExtrinsics * world_position;

      // Project into output pixel coordinates
      R3Point undistorted_output_position;
      undistorted_output_position[0] = undistorted_camera.mx_depth + world_position[0] * undistorted_camera.fx_depth / -world_position[2];
      undistorted_output_position[1] = undistorted_camera.my_depth + world_position[1] * undistorted_camera.fy_depth / -world_position[2];
      undistorted_output_position[2] = world_position[2];

      // Snap to closest pixel in output image
      int undistorted_output_ix = (int) (undistorted_output_position[0] + 0.5);
      if (undistorted_output_ix < 0) continue;
      if (undistorted_output_ix >= output_undistorted_depth_image.XResolution()) continue;
      int undistorted_output_iy = (int) (undistorted_output_position[1] + 0.5);
      if (undistorted_output_iy < 0) continue;
      if (undistorted_output_iy >= output_undistorted_depth_image.YResolution()) continue;

#ifdef MASK_HOLES
      // Assign depth of zero in pixels where a hole is mapped
      if (depth_was_missing) undistorted_output_position[2] = 0.0;
#endif
      
      // Store only closest grid value at every ix,iy in output image
      RNScalar new_depth = -undistorted_output_position[2];
      RNScalar old_depth = output_undistorted_depth_image.GridValue(undistorted_output_ix, undistorted_output_iy);
      if ((old_depth == 0.0) || (old_depth == R2_GRID_UNKNOWN_VALUE) || (new_depth < old_depth)) {
        output_undistorted_depth_image.SetGridValue(undistorted_output_ix, undistorted_output_iy, new_depth);
      }

#if 0
      // Check if center goes to center
      if (distorted_camera.depthToColorExtrinsics.IsIdentity() &&
          undistorted_camera.depthToColorExtrinsics.IsIdentity() &&
          RNIsEqual(undistorted_output_position.X(), undistorted_camera.mx_depth, 0.5) &&
          RNIsEqual(undistorted_output_position.Y(), undistorted_camera.my_depth, 0.5)) {
        if (RNIsNotEqual(distorted_input_position.X(), distorted_camera.mx_depth, 0.5) ||
            RNIsNotEqual(distorted_input_position.Y(), distorted_camera.my_depth, 0.5)) {
          printf("%g %g | %g %g\n",
                 distorted_input_position.X(), distorted_input_position.Y(),
                 undistorted_output_position.X(), undistorted_output_position.Y());
                 
        }
      }
#endif
    }
  }

  // Return success
  return 1;
}

// Assembly:
subq $0x418, %rsp            # imm = 0x418
movq %rdi, 0x408(%rsp)
movq %rsi, 0x400(%rsp)
movq %rdx, 0x3f8(%rsp)
movq %rcx, 0x3f0(%rsp)
movq 0x3f8(%rsp), %rax
cmpl $0x0, 0x8(%rax)
je 0x33b93
movq 0x3f8(%rsp), %rax
cmpl $0x0, 0xc(%rax)
jne 0x33ba3
movl $0x0, 0x414(%rsp)
jmp 0x349e5
movq 0x3f0(%rsp), %rax
cmpl $0x0, 0x8(%rax)
je 0x33bbf
movq 0x3f0(%rsp), %rax
cmpl $0x0, 0xc(%rax)
jne 0x33bcf
movl $0x0, 0x414(%rsp)
jmp 0x349e5
movq 0x408(%rsp), %rdi
callq 0x27940
cmpl $0x0, %eax
je 0x33bf3
movq 0x408(%rsp), %rdi
callq 0x27960
cmpl $0x0, %eax
jne 0x33c03
movl $0x0, 0x414(%rsp)
jmp 0x349e5
movq 0x3f0(%rsp), %rax
movl 0x8(%rax), %eax
movq 0x3f8(%rsp), %rcx
cmpl 0x8(%rcx), %eax
jne 0x33d73
movq 0x3f0(%rsp), %rax
movl 0xc(%rax), %eax
movq 0x3f8(%rsp), %rcx
cmpl 0xc(%rcx), %eax
jne 0x33d73
movq 0x3f8(%rsp), %rax
movsd 0x70(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x33d73
jp 0x33d73
movq 0x3f8(%rsp), %rax
movsd 0x78(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x33d73
jp 0x33d73
movq 0x3f8(%rsp), %rax
movsd 0x80(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x33d73
jp 0x33d73
movq 0x3f8(%rsp), %rax
movsd 0x88(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x33d73
jp 0x33d73
movq 0x3f8(%rsp), %rax
movsd 0x90(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x33d73
jp 0x33d73
movq 0x3f8(%rsp), %rax
movsd 0x98(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x33d73
jp 0x33d73
movq 0x3f8(%rsp), %rax
movsd 0xa0(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x33d73
jp 0x33d73
movq 0x3f8(%rsp), %rax
movsd 0xa8(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x33d73
jp 0x33d73
movq 0x3f8(%rsp), %rdi
addq $0xb0, %rdi
callq 0x845c0
cmpl $0x0, %eax
je 0x33d73
movq 0x408(%rsp), %rsi
movq 0x400(%rsp), %rdi
callq 0xb2f20
movl $0x1, 0x414(%rsp)
jmp 0x349e5
movq 0x408(%rsp), %rsi
leaq 0x320(%rsp), %rdi
callq 0xb30a0
movq 0x3f0(%rsp), %rax
movl 0x8(%rax), %esi
movl 0xc(%rax), %edx
leaq 0x250(%rsp), %rdi
callq 0xb1ee0
jmp 0x33da5
movq 0x400(%rsp), %rdi
leaq 0x250(%rsp), %rsi
callq 0xb2f20
jmp 0x33dbc
leaq 0x250(%rsp), %rdi
callq 0xb3390
movq 0x400(%rsp), %rdi
movsd 0x132247(%rip), %xmm0   # 0x166020
callq 0xb5a20
jmp 0x33de0
movabsq $0x3fe0000000000000, %rax # imm = 0x3FE0000000000000
movq %rax, 0x238(%rsp)
movsd 0x238(%rsp), %xmm0
movsd %xmm0, 0x150(%rsp)
leaq 0x320(%rsp), %rdi
callq 0x27940
movl %eax, 0x15c(%rsp)
jmp 0x33e1a
movsd 0x150(%rsp), %xmm0
movl 0x15c(%rsp), %eax
cvtsi2sd %eax, %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x140(%rsp)
movq 0x400(%rsp), %rdi
callq 0x27940
movl %eax, 0x14c(%rsp)
jmp 0x33e51
movsd 0x140(%rsp), %xmm0
movl 0x14c(%rsp), %eax
cvtsi2sd %eax, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, 0x230(%rsp)
movsd 0x238(%rsp), %xmm0
movsd %xmm0, 0x130(%rsp)
leaq 0x320(%rsp), %rdi
callq 0x27960
movl %eax, 0x13c(%rsp)
jmp 0x33e9a
movsd 0x130(%rsp), %xmm0
movl 0x13c(%rsp), %eax
cvtsi2sd %eax, %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x120(%rsp)
movq 0x400(%rsp), %rdi
callq 0x27960
movl %eax, 0x12c(%rsp)
jmp 0x33ed1
movsd 0x120(%rsp), %xmm0
movl 0x12c(%rsp), %eax
cvtsi2sd %eax, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, 0x228(%rsp)
movsd 0x13212e(%rip), %xmm0   # 0x166028
mulsd 0x228(%rsp), %xmm0
movsd %xmm0, 0x220(%rsp)
movsd 0x220(%rsp), %xmm0
movsd %xmm0, 0x110(%rsp)
leaq 0x320(%rsp), %rdi
callq 0x27960
movl %eax, 0x11c(%rsp)
jmp 0x33f34
movsd 0x110(%rsp), %xmm1
movl 0x11c(%rsp), %eax
cvtsi2sd %eax, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x349bc
movsd 0x1320ce(%rip), %xmm0   # 0x166028
mulsd 0x230(%rsp), %xmm0
movsd %xmm0, 0x218(%rsp)
movsd 0x218(%rsp), %xmm0
movsd %xmm0, 0x100(%rsp)
leaq 0x320(%rsp), %rdi
callq 0x27940
movl %eax, 0x10c(%rsp)
jmp 0x33f94
movsd 0x100(%rsp), %xmm1
movl 0x10c(%rsp), %eax
cvtsi2sd %eax, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x3499a
movsd 0x218(%rsp), %xmm0
movsd 0x220(%rsp), %xmm1
leaq 0x208(%rsp), %rdi
callq 0xcfd80
jmp 0x33fd3
movq 0x3f8(%rsp), %rax
movsd 0x80(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x34082
jp 0x34082
movq 0x3f8(%rsp), %rax
movsd 0x88(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x34082
jp 0x34082
movq 0x3f8(%rsp), %rax
movsd 0x90(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x34082
jp 0x34082
movq 0x3f8(%rsp), %rax
movsd 0x90(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x34082
jp 0x34082
movq 0x3f8(%rsp), %rax
movsd 0xa0(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x34082
jp 0x34082
movq 0x3f8(%rsp), %rax
movsd 0xa8(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x34082
jp 0x34082
jmp 0x3432d
movsd 0x218(%rsp), %xmm0
movq 0x3f8(%rsp), %rax
movsd 0x60(%rax), %xmm1
movsd 0x70(%rax), %xmm2
subsd %xmm2, %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0x200(%rsp)
movsd 0x220(%rsp), %xmm0
movq 0x3f8(%rsp), %rax
movsd 0x68(%rax), %xmm1
movsd 0x78(%rax), %xmm2
subsd %xmm2, %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0x1f8(%rsp)
movsd 0x200(%rsp), %xmm0
movsd 0x1f8(%rsp), %xmm1
mulsd %xmm1, %xmm1
mulsd %xmm0, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x1f0(%rsp)
movsd 0x1f0(%rsp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x1e8(%rsp)
movsd 0x1f0(%rsp), %xmm3
movq 0x3f8(%rsp), %rax
movsd 0x80(%rax), %xmm0
movsd 0x88(%rax), %xmm4
movaps %xmm3, %xmm1
mulsd %xmm0, %xmm1
movsd 0x131ee9(%rip), %xmm0   # 0x166030
addsd %xmm0, %xmm1
movsd 0x1e8(%rsp), %xmm0
movaps %xmm0, %xmm2
mulsd %xmm4, %xmm2
addsd %xmm1, %xmm2
movaps %xmm0, %xmm1
mulsd %xmm3, %xmm1
movsd 0x90(%rax), %xmm3
mulsd %xmm3, %xmm1
addsd %xmm2, %xmm1
mulsd %xmm0, %xmm0
movsd 0x98(%rax), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x1e0(%rsp)
movsd 0x1e0(%rsp), %xmm1
movsd 0x200(%rsp), %xmm2
movq 0x3f8(%rsp), %rax
movsd 0xa0(%rax), %xmm0
movsd 0xa8(%rax), %xmm3
movsd 0x1f0(%rsp), %xmm5
movaps %xmm2, %xmm4
addsd %xmm4, %xmm4
mulsd %xmm2, %xmm4
addsd %xmm5, %xmm4
mulsd %xmm4, %xmm3
mulsd %xmm2, %xmm1
addsd %xmm3, %xmm1
addsd %xmm0, %xmm0
mulsd %xmm2, %xmm0
movsd 0x1f8(%rsp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x200(%rsp)
movsd 0x1e0(%rsp), %xmm1
movsd 0x1f8(%rsp), %xmm2
movq 0x3f8(%rsp), %rax
movsd 0x1f0(%rsp), %xmm0
movaps %xmm2, %xmm4
addsd %xmm4, %xmm4
mulsd %xmm2, %xmm4
addsd %xmm0, %xmm4
movsd 0xa0(%rax), %xmm3
movsd 0xa8(%rax), %xmm0
mulsd %xmm4, %xmm3
mulsd %xmm2, %xmm1
addsd %xmm3, %xmm1
addsd %xmm0, %xmm0
movsd 0x200(%rsp), %xmm3
mulsd %xmm3, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x1f8(%rsp)
movsd 0x200(%rsp), %xmm0
movq 0x3f8(%rsp), %rax
movsd 0x60(%rax), %xmm2
movsd 0x70(%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x1d8(%rsp)
movsd 0x1f8(%rsp), %xmm0
movq 0x3f8(%rsp), %rax
movsd 0x68(%rax), %xmm2
movsd 0x78(%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x1d0(%rsp)
movsd 0x1d8(%rsp), %xmm0
movsd 0x1d0(%rsp), %xmm1
leaq 0x208(%rsp), %rdi
callq 0x35a30
jmp 0x342ec
jmp 0x3432d
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x248(%rsp)
movl %eax, 0x244(%rsp)
jmp 0x349d6
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x248(%rsp)
movl %eax, 0x244(%rsp)
leaq 0x250(%rsp), %rdi
callq 0xb3390
jmp 0x349d6
leaq 0x208(%rsp), %rdi
callq 0x1af20
movsd %xmm0, 0xf8(%rsp)
jmp 0x34345
movsd 0xf8(%rsp), %xmm0
movsd 0x131cd2(%rip), %xmm1   # 0x166028
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x1cc(%rsp)
leaq 0x208(%rsp), %rdi
callq 0x1af70
movsd %xmm0, 0xf0(%rsp)
jmp 0x3437d
movsd 0xf0(%rsp), %xmm0
movsd 0x131c9a(%rip), %xmm1   # 0x166028
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x1c8(%rsp)
cmpl $0x0, 0x1cc(%rsp)
jge 0x343ac
jmp 0x3497a
movl 0x1cc(%rsp), %eax
movl %eax, 0xe8(%rsp)
leaq 0x320(%rsp), %rdi
callq 0x27940
movl %eax, 0xec(%rsp)
jmp 0x343d0
movl 0xe8(%rsp), %eax
movl 0xec(%rsp), %ecx
cmpl %ecx, %eax
jl 0x343e7
jmp 0x3497a
cmpl $0x0, 0x1c8(%rsp)
jge 0x343f6
jmp 0x3497a
movl 0x1c8(%rsp), %eax
movl %eax, 0xe0(%rsp)
leaq 0x320(%rsp), %rdi
callq 0x27960
movl %eax, 0xe4(%rsp)
jmp 0x3441a
movl 0xe0(%rsp), %eax
movl 0xe4(%rsp), %ecx
cmpl %ecx, %eax
jl 0x34431
jmp 0x3497a
movl 0x1cc(%rsp), %esi
movl 0x1c8(%rsp), %edx
leaq 0x320(%rsp), %rdi
callq 0x1a060
movsd %xmm0, 0xd8(%rsp)
jmp 0x34457
movsd 0xd8(%rsp), %xmm0
movsd %xmm0, 0x1c0(%rsp)
xorps %xmm0, %xmm0
ucomisd 0x1c0(%rsp), %xmm0
jb 0x3447c
jmp 0x3497a
leaq 0x1a8(%rsp), %rdi
callq 0x8a990
jmp 0x3448b
movsd 0x218(%rsp), %xmm0
movq 0x3f8(%rsp), %rax
movsd 0x60(%rax), %xmm1
movsd 0x70(%rax), %xmm2
subsd %xmm2, %xmm0
movsd 0x1c0(%rsp), %xmm2
mulsd %xmm2, %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0xc8(%rsp)
leaq 0x1a8(%rsp), %rdi
xorl %esi, %esi
callq 0x19c00
movq %rax, 0xd0(%rsp)
jmp 0x344dd
movq 0xd0(%rsp), %rax
movsd 0xc8(%rsp), %xmm0
movsd %xmm0, (%rax)
movsd 0x220(%rsp), %xmm0
movq 0x3f8(%rsp), %rax
movsd 0x68(%rax), %xmm1
movsd 0x78(%rax), %xmm2
subsd %xmm2, %xmm0
movsd 0x1c0(%rsp), %xmm2
mulsd %xmm2, %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0xb8(%rsp)
leaq 0x1a8(%rsp), %rdi
movl $0x1, %esi
callq 0x19c00
movq %rax, 0xc0(%rsp)
jmp 0x34547
movq 0xc0(%rsp), %rax
movsd 0xb8(%rsp), %xmm0
movsd %xmm0, (%rax)
movaps 0x13200d(%rip), %xmm1   # 0x166570
movsd 0x1c0(%rsp), %xmm0
pxor %xmm1, %xmm0
movsd %xmm0, 0xa8(%rsp)
leaq 0x1a8(%rsp), %rdi
movl $0x2, %esi
callq 0x19c00
movq %rax, 0xb0(%rsp)
jmp 0x34595
movq 0xb0(%rsp), %rax
movsd 0xa8(%rsp), %xmm0
movsd %xmm0, (%rax)
movq 0x3f8(%rsp), %rsi
addq $0xb0, %rsi
leaq 0x190(%rsp), %rdi
leaq 0x1a8(%rsp), %rdx
callq 0x87960
jmp 0x345d0
leaq 0x1a8(%rsp), %rdi
leaq 0x190(%rsp), %rsi
callq 0x8b030
jmp 0x345e7
leaq 0x178(%rsp), %rdi
callq 0x8a990
jmp 0x345f6
movq 0x3f0(%rsp), %rax
movsd 0x70(%rax), %xmm0
movsd %xmm0, 0x98(%rsp)
leaq 0x1a8(%rsp), %rdi
xorl %esi, %esi
callq 0x19c00
movq %rax, 0xa0(%rsp)
jmp 0x34625
movq 0xa0(%rsp), %rax
movsd (%rax), %xmm0
movq 0x3f0(%rsp), %rax
movsd 0x60(%rax), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x88(%rsp)
leaq 0x1a8(%rsp), %rdi
movl $0x2, %esi
callq 0x19c00
movq %rax, 0x90(%rsp)
jmp 0x34667
movsd 0x98(%rsp), %xmm0
movsd 0x88(%rsp), %xmm1
movq 0x90(%rsp), %rax
movsd (%rax), %xmm2
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x78(%rsp)
leaq 0x178(%rsp), %rdi
xorl %esi, %esi
callq 0x19c00
movq %rax, 0x80(%rsp)
jmp 0x346ac
movq 0x80(%rsp), %rax
movsd 0x78(%rsp), %xmm0
movsd %xmm0, (%rax)
movq 0x3f0(%rsp), %rax
movsd 0x78(%rax), %xmm0
movsd %xmm0, 0x68(%rsp)
leaq 0x1a8(%rsp), %rdi
movl $0x1, %esi
callq 0x19c00
movq %rax, 0x70(%rsp)
jmp 0x346ea
movq 0x70(%rsp), %rax
movsd (%rax), %xmm0
movq 0x3f0(%rsp), %rax
movsd 0x68(%rax), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x58(%rsp)
leaq 0x1a8(%rsp), %rdi
movl $0x2, %esi
callq 0x19c00
movq %rax, 0x60(%rsp)
jmp 0x34723
movsd 0x68(%rsp), %xmm0
movsd 0x58(%rsp), %xmm1
movq 0x60(%rsp), %rax
movsd (%rax), %xmm2
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x48(%rsp)
leaq 0x178(%rsp), %rdi
movl $0x1, %esi
callq 0x19c00
movq %rax, 0x50(%rsp)
jmp 0x3475f
movq 0x50(%rsp), %rax
movsd 0x48(%rsp), %xmm0
movsd %xmm0, (%rax)
leaq 0x1a8(%rsp), %rdi
movl $0x2, %esi
callq 0x19c00
movq %rax, 0x40(%rsp)
jmp 0x34787
movq 0x40(%rsp), %rax
movsd (%rax), %xmm0
movsd %xmm0, 0x30(%rsp)
leaq 0x178(%rsp), %rdi
movl $0x2, %esi
callq 0x19c00
movq %rax, 0x38(%rsp)
jmp 0x347af
movq 0x38(%rsp), %rax
movsd 0x30(%rsp), %xmm0
movsd %xmm0, (%rax)
leaq 0x178(%rsp), %rdi
xorl %esi, %esi
callq 0x19c00
movq %rax, 0x28(%rsp)
jmp 0x347d4
movq 0x28(%rsp), %rax
movsd 0x131847(%rip), %xmm0   # 0x166028
addsd (%rax), %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x174(%rsp)
cmpl $0x0, 0x174(%rsp)
jge 0x347ff
jmp 0x3497a
movl 0x174(%rsp), %eax
movl %eax, 0x20(%rsp)
movq 0x400(%rsp), %rdi
callq 0x27940
movl %eax, 0x24(%rsp)
jmp 0x3481d
movl 0x20(%rsp), %eax
movl 0x24(%rsp), %ecx
cmpl %ecx, %eax
jl 0x3482e
jmp 0x3497a
leaq 0x178(%rsp), %rdi
movl $0x1, %esi
callq 0x19c00
movq %rax, 0x18(%rsp)
jmp 0x34847
movq 0x18(%rsp), %rax
movsd 0x1317d4(%rip), %xmm0   # 0x166028
addsd (%rax), %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x170(%rsp)
cmpl $0x0, 0x170(%rsp)
jge 0x34872
jmp 0x3497a
movl 0x170(%rsp), %eax
movl %eax, 0x10(%rsp)
movq 0x400(%rsp), %rdi
callq 0x27960
movl %eax, 0x14(%rsp)
jmp 0x34890
movl 0x10(%rsp), %eax
movl 0x14(%rsp), %ecx
cmpl %ecx, %eax
jl 0x348a1
jmp 0x3497a
leaq 0x178(%rsp), %rdi
movl $0x2, %esi
callq 0x19c00
movq %rax, 0x8(%rsp)
jmp 0x348ba
movq 0x8(%rsp), %rax
movsd (%rax), %xmm0
movaps 0x131ca6(%rip), %xmm1   # 0x166570
pxor %xmm1, %xmm0
movlpd %xmm0, 0x168(%rsp)
movq 0x400(%rsp), %rdi
movl 0x174(%rsp), %esi
movl 0x170(%rsp), %edx
callq 0x1a060
movsd %xmm0, (%rsp)
jmp 0x348f9
movsd (%rsp), %xmm0
movsd %xmm0, 0x160(%rsp)
movsd 0x160(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x3491d
jp 0x3491d
jmp 0x34950
movsd 0x160(%rsp), %xmm0
movsd 0x1316f2(%rip), %xmm1   # 0x166020
ucomisd %xmm1, %xmm0
jne 0x34938
jp 0x34938
jmp 0x34950
movsd 0x168(%rsp), %xmm1
movsd 0x160(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x34978
movq 0x400(%rsp), %rdi
movl 0x174(%rsp), %esi
movl 0x170(%rsp), %edx
movsd 0x168(%rsp), %xmm0
callq 0x1a1b0
jmp 0x34976
jmp 0x34978
jmp 0x3497a
movsd 0x230(%rsp), %xmm0
addsd 0x218(%rsp), %xmm0
movsd %xmm0, 0x218(%rsp)
jmp 0x33f6c
jmp 0x3499c
movsd 0x228(%rsp), %xmm0
addsd 0x220(%rsp), %xmm0
movsd %xmm0, 0x220(%rsp)
jmp 0x33f0c
movl $0x1, 0x414(%rsp)
leaq 0x320(%rsp), %rdi
callq 0xb3390
jmp 0x349e5
leaq 0x320(%rsp), %rdi
callq 0xb3390
jmp 0x349f4
movl 0x414(%rsp), %eax
addq $0x418, %rsp            # imm = 0x418
retq
movq 0x248(%rsp), %rdi
callq 0x12c20
nopw %cs:(%rax,%rax)