// File: /workspace/github_cpp_makefile_star2_O2/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:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x228, %rsp            # imm = 0x228
movq %rcx, %r14
movl 0x8(%rdx), %ecx
xorl %eax, %eax
testl %ecx, %ecx
je 0x21b80
movq %rdx, %r15
movl 0xc(%rdx), %edx
testl %edx, %edx
je 0x21b80
movq %rsi, %rbx
movl 0x8(%r14), %esi
testl %esi, %esi
je 0x21b80
movq %rdi, %r12
movl 0xc(%r14), %edi
testl %edi, %edi
je 0x21b80
cmpl $0x0, 0xc0(%r12)
je 0x21b80
cmpl $0x0, 0xc4(%r12)
je 0x21b80
cmpl %ecx, %esi
jne 0x21710
cmpl %edx, %edi
jne 0x21710
movupd 0x70(%r15), %xmm0
movupd 0x80(%r15), %xmm1
movupd 0x90(%r15), %xmm2
movupd 0xa0(%r15), %xmm3
xorpd %xmm4, %xmm4
cmpneqpd %xmm4, %xmm3
cmpneqpd %xmm4, %xmm2
packssdw %xmm3, %xmm2
cmpneqpd %xmm4, %xmm1
cmpneqpd %xmm4, %xmm0
packssdw %xmm1, %xmm0
packssdw %xmm2, %xmm0
packsswb %xmm0, %xmm0
pmovmskb %xmm0, %eax
testb %al, %al
jne 0x21710
leaq 0xb0(%r15), %rdi
callq 0x42df0
testl %eax, %eax
je 0x21710
movq %rbx, %rdi
movq %r12, %rsi
callq 0x5872a
jmp 0x21b7d
leaq 0x80(%rsp), %rdi
movq %r12, %rsi
callq 0x58808
movl 0x8(%r14), %esi
movl 0xc(%r14), %edx
leaq 0x150(%rsp), %rdi
callq 0x57e7c
leaq 0x150(%rsp), %rsi
movq %rbx, %rdi
callq 0x5872a
leaq 0x150(%rsp), %rdi
callq 0x589c0
movsd 0x868b6(%rip), %xmm0    # 0xa8010
movq %rbx, %rdi
callq 0x59dcc
movl 0x140(%rsp), %eax
cvtsi2sd %eax, %xmm1
movl 0x144(%rsp), %ecx
movsd 0x8689c(%rip), %xmm2    # 0xa8018
cvtsi2sdl 0xc0(%rbx), %xmm0
mulsd %xmm2, %xmm1
divsd %xmm0, %xmm1
cvtsi2sd %ecx, %xmm3
mulsd %xmm2, %xmm3
cvtsi2sdl 0xc4(%rbx), %xmm0
divsd %xmm0, %xmm3
movsd %xmm3, 0x20(%rsp)
mulsd %xmm2, %xmm3
movsd %xmm1, 0x38(%rsp)
mulsd %xmm2, %xmm1
movsd %xmm1, 0x28(%rsp)
leaq 0xb0(%r15), %rdx
movq %rdx, 0x30(%rsp)
leaq 0x40(%rsp), %r13
leaq 0x150(%rsp), %rbp
leaq 0x60(%rsp), %r12
cvtsi2sd %ecx, %xmm0
movapd %xmm3, (%rsp)
ucomisd %xmm3, %xmm0
movq 0x28(%rsp), %xmm0
jbe 0x21b70
cvtsi2sd %eax, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x21b59
movq %r13, %rdi
movaps %xmm0, 0x10(%rsp)
movapd (%rsp), %xmm1
callq 0x66640
movupd 0x80(%r15), %xmm2
movsd 0x90(%r15), %xmm6
movsd 0xa0(%r15), %xmm1
movapd %xmm6, %xmm0
unpcklpd %xmm1, %xmm0            # xmm0 = xmm0[0],xmm1[0]
movsd 0xa8(%r15), %xmm5
movapd %xmm2, %xmm3
xorpd %xmm4, %xmm4
cmpneqpd %xmm4, %xmm3
cmpneqpd %xmm4, %xmm0
shufps $0x88, %xmm0, %xmm3     # xmm3 = xmm3[0,2],xmm0[0,2]
movmskps %xmm3, %eax
testl %eax, %eax
jne 0x21871
ucomisd 0x867d6(%rip), %xmm5    # 0xa8038
jne 0x21871
jp 0x21871
movapd 0x40(%rsp), %xmm0
jmp 0x2199c
movupd 0x60(%r15), %xmm4
movupd 0x70(%r15), %xmm3
movapd 0x10(%rsp), %xmm0
subsd %xmm3, %xmm0
divsd %xmm4, %xmm0
movapd %xmm3, %xmm8
unpckhpd %xmm3, %xmm8            # xmm8 = xmm8[1],xmm3[1]
movapd (%rsp), %xmm7
subsd %xmm8, %xmm7
movapd %xmm4, %xmm8
unpckhpd %xmm4, %xmm8            # xmm8 = xmm8[1],xmm4[1]
divsd %xmm8, %xmm7
movapd %xmm7, %xmm9
mulsd %xmm7, %xmm9
movapd %xmm0, %xmm8
mulsd %xmm0, %xmm8
addsd %xmm9, %xmm8
movapd %xmm8, %xmm9
movapd %xmm8, %xmm10
movapd %xmm0, %xmm11
addsd %xmm0, %xmm11
mulsd %xmm0, %xmm11
addsd %xmm8, %xmm11
movapd %xmm7, %xmm12
addsd %xmm7, %xmm12
mulsd %xmm7, %xmm12
addsd %xmm8, %xmm12
mulsd %xmm8, %xmm8
mulsd %xmm2, %xmm9
addsd 0x8671c(%rip), %xmm9    # 0xa8028
unpckhpd %xmm2, %xmm2            # xmm2 = xmm2[1,1]
mulsd %xmm8, %xmm2
addsd %xmm9, %xmm2
movapd %xmm1, %xmm9
addsd %xmm1, %xmm9
mulsd %xmm8, %xmm10
mulsd %xmm6, %xmm10
movapd %xmm5, %xmm6
addsd %xmm5, %xmm6
addsd %xmm2, %xmm10
mulsd %xmm8, %xmm8
mulsd 0x98(%r15), %xmm8
addsd %xmm10, %xmm8
mulsd %xmm5, %xmm11
movapd %xmm8, %xmm2
mulsd %xmm0, %xmm2
addsd %xmm11, %xmm2
mulsd %xmm9, %xmm0
mulsd %xmm7, %xmm0
addsd %xmm2, %xmm0
mulsd %xmm1, %xmm12
mulsd %xmm7, %xmm8
addsd %xmm12, %xmm8
mulsd %xmm0, %xmm6
mulsd %xmm7, %xmm6
addsd %xmm8, %xmm6
unpcklpd %xmm6, %xmm0            # xmm0 = xmm0[0],xmm6[0]
mulpd %xmm4, %xmm0
addpd %xmm3, %xmm0
movapd %xmm0, 0x40(%rsp)
movapd %xmm0, %xmm1
addsd 0x86670(%rip), %xmm1    # 0xa8018
cvttsd2si %xmm1, %ecx
movl 0x140(%rsp), %eax
testl %ecx, %ecx
js 0x21b48
cmpl %ecx, %eax
jle 0x21b48
unpckhpd %xmm0, %xmm0            # xmm0 = xmm0[1,1]
addsd 0x86649(%rip), %xmm0    # 0xa8018
cvttsd2si %xmm0, %edx
testl %edx, %edx
js 0x21b48
cmpl %edx, 0x144(%rsp)
jle 0x21b48
movq 0x138(%rsp), %rsi
imull 0x148(%rsp), %edx
addl %ecx, %edx
movslq %edx, %rcx
movsd (%rsi,%rcx,8), %xmm1
xorpd %xmm0, %xmm0
movapd %xmm1, 0x50(%rsp)
ucomisd %xmm1, %xmm0
jae 0x21b48
movq %rbp, %rdi
callq 0x45d4a
movupd 0x60(%r15), %xmm0
movupd 0x70(%r15), %xmm1
movapd 0x10(%rsp), %xmm2
unpcklpd (%rsp), %xmm2           # xmm2 = xmm2[0],mem[0]
subpd %xmm1, %xmm2
movapd 0x50(%rsp), %xmm3
movapd %xmm3, %xmm1
unpcklpd %xmm3, %xmm1            # xmm1 = xmm1[0],xmm3[0]
mulpd %xmm2, %xmm1
divpd %xmm0, %xmm1
movapd %xmm1, 0x150(%rsp)
xorpd 0x865f0(%rip), %xmm3    # 0xa8050
movlpd %xmm3, 0x160(%rsp)
movq %r12, %rdi
movq 0x30(%rsp), %rsi
movq %rbp, %rdx
callq 0x447df
movq %rbp, %rdi
movq %r12, %rsi
callq 0x46036
movq %r12, %rdi
callq 0x45d4a
movsd 0x160(%rsp), %xmm0
movupd 0x60(%r14), %xmm2
mulpd 0x150(%rsp), %xmm2
movupd 0x70(%r14), %xmm1
movapd %xmm0, %xmm3
unpcklpd %xmm0, %xmm3            # xmm3 = xmm3[0],xmm0[0]
divpd %xmm3, %xmm2
subpd %xmm2, %xmm1
movapd %xmm1, 0x60(%rsp)
movsd %xmm0, 0x70(%rsp)
movapd %xmm1, %xmm2
addsd 0x86546(%rip), %xmm2    # 0xa8018
cvttsd2si %xmm2, %eax
testl %eax, %eax
js 0x21b41
cmpl %eax, 0xc0(%rbx)
jle 0x21b41
unpckhpd %xmm1, %xmm1            # xmm1 = xmm1[1,1]
addsd 0x8652a(%rip), %xmm1    # 0xa8018
cvttsd2si %xmm1, %edx
testl %edx, %edx
js 0x21b41
cmpl %edx, 0xc4(%rbx)
jle 0x21b41
xorpd 0x8654a(%rip), %xmm0    # 0xa8050
movq 0xb8(%rbx), %rcx
imull 0xc8(%rbx), %edx
addl %eax, %edx
movslq %edx, %rax
movsd (%rcx,%rax,8), %xmm1
ucomisd %xmm0, %xmm1
ja 0x21b3c
ucomisd 0x8650c(%rip), %xmm1    # 0xa8038
jne 0x21b30
jnp 0x21b3c
ucomisd 0x864d8(%rip), %xmm1    # 0xa8010
jne 0x21b41
jp 0x21b41
movsd %xmm0, (%rcx,%rax,8)
movl 0x140(%rsp), %eax
movapd 0x10(%rsp), %xmm0
addsd 0x38(%rsp), %xmm0
jmp 0x217f1
movapd (%rsp), %xmm3
addsd 0x20(%rsp), %xmm3
movl 0x144(%rsp), %ecx
jmp 0x217d8
leaq 0x80(%rsp), %rdi
callq 0x589c0
pushq $0x1
popq %rax
addq $0x228, %rsp            # imm = 0x228
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x21bae
movq %rax, %rbx
leaq 0x150(%rsp), %rdi
callq 0x589c0
jmp 0x21bb1
jmp 0x21bae
jmp 0x21bae
jmp 0x21bae
jmp 0x21bae
movq %rax, %rbx
leaq 0x80(%rsp), %rdi
callq 0x589c0
movq %rbx, %rdi
callq 0x11a80