// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/RGBD/RGBDUtil.cpp
// Function: gaps::RGBDCreateUndistortedColorImage(gaps::R2Grid const&, gaps::R2Grid&, gaps::RGBDCamera const&, gaps::RGBDCamera const&)
int RGBDCreateUndistortedColorImage(
  const R2Grid& input_distorted_color_image,
  R2Grid& output_undistorted_color_image,
  const RGBDCamera& distorted_camera, const RGBDCamera& undistorted_camera)
{
  // Check output camera
  assert((undistorted_camera.k1_color == 0) && (undistorted_camera.k2_color == 0) &&
         (undistorted_camera.k3_color == 0) && (undistorted_camera.k4_color == 0) &&
         (undistorted_camera.p1_color == 0) && (undistorted_camera.p2_color == 0));

  // Get convenient variables
  if ((distorted_camera.colorWidth == 0) || (distorted_camera.colorHeight == 0)) return 0;
  if ((undistorted_camera.colorWidth == 0) || (undistorted_camera.colorHeight == 0)) return 0;
  double sx = (double) distorted_camera.colorWidth / (double) undistorted_camera.colorWidth;
  double sy = (double) distorted_camera.colorHeight / (double) undistorted_camera.colorHeight;

  // Check if don't need to scale or undistort
  if ((RNIsEqual(sx, 1.0) && RNIsEqual(sy, 1.0)) &&
      (distorted_camera.mx_color == undistorted_camera.mx_color) && (distorted_camera.my_color == undistorted_camera.my_color) &&
      (distorted_camera.k1_color == undistorted_camera.k1_color) && (distorted_camera.k2_color == undistorted_camera.k2_color) &&
      (distorted_camera.k3_color == undistorted_camera.k3_color) && (distorted_camera.k4_color == undistorted_camera.k4_color) &&
      (distorted_camera.p1_color == undistorted_camera.p1_color) && (distorted_camera.p2_color == undistorted_camera.p2_color)) {
    output_undistorted_color_image = input_distorted_color_image;
    return 1;
  }

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

  // Initialize the output image
  output_undistorted_color_image = R2Grid(undistorted_camera.colorWidth, undistorted_camera.colorHeight);
  output_undistorted_color_image.Clear(R2_GRID_UNKNOWN_VALUE);
  
  // Compute output image via reverse mapping
  for (int output_iy = 0; output_iy < output_undistorted_color_image.YResolution(); output_iy++) {
    for (int output_ix = 0; output_ix < output_undistorted_color_image.XResolution(); output_ix++) {
      // Compute coordinates in undistorted output image
      R2Point undistorted_output_position(output_ix + 0.5, output_iy + 0.5);

      // Compute coordinates in undistorted input image
      // Note that this ignores p1 and p2
      R2Point undistorted_input_position(undistorted_output_position);
      RNScalar x = sx * (undistorted_output_position.X() - undistorted_camera.mx_color) + distorted_camera.mx_color;
      RNScalar y = sy * (undistorted_output_position.Y() - undistorted_camera.my_color) + distorted_camera.my_color;
      undistorted_input_position.Reset(x, y);
      
      // Compute coordinates in distorted input image
      R2Point distorted_input_position(undistorted_input_position);
      if ((distorted_camera.k1_color != 0.0) || (distorted_camera.k2_color != 0.0) ||
          (distorted_camera.k3_color != 0.0) || (distorted_camera.k4_color != 0.0) ||
          (distorted_camera.p1_color != 0.0) || (distorted_camera.k2_color != 0.0)) {
        double nx = (undistorted_input_position.X() - distorted_camera.mx_color) / distorted_camera.fx_color;
        double ny = (undistorted_input_position.Y() - distorted_camera.my_color) / distorted_camera.fy_color;
        double rr = nx*nx + ny*ny; double rrrr = rr*rr;
        double s = 1.0 + rr*distorted_camera.k1_color + rrrr*distorted_camera.k2_color + rrrr*rr*distorted_camera.k3_color + rrrr*rrrr*distorted_camera.k4_color;
        nx = s*nx + distorted_camera.p2_color*(rr + 2*nx*nx) + 2*distorted_camera.p1_color*nx*ny;
        ny = s*ny + distorted_camera.p1_color*(rr + 2*ny*ny) + 2*distorted_camera.p2_color*nx*ny;
        double x = nx*distorted_camera.fx_color + distorted_camera.mx_color;
        double y = ny*distorted_camera.fy_color + distorted_camera.my_color;
        distorted_input_position.Reset(x, y);
      }

      // Check if outside input image
      if (distorted_input_position.X() < 0) continue;
      if (distorted_input_position.X() > source_color_image.XResolution()-1) continue;
      if (distorted_input_position.Y() < 0) continue;
      if (distorted_input_position.Y() > source_color_image.YResolution()-1) continue;

#if 0
      // Check if center goes to center
      if (RNIsEqual(undistorted_output_position.X(), distorted_camera.mx_color, 0.5) &&
          RNIsEqual(undistorted_output_position.Y(), distorted_camera.my_color, 0.5) &&
          !R2Contains(undistorted_output_position, distorted_input_position)) RNAbort("HERE");
#endif
      
      // Copy value from input image to output image (bilinear interpolation)
      RNScalar value = source_color_image.GridValue(distorted_input_position);
      output_undistorted_color_image.SetGridValue(output_ix, output_iy, value);
    }
  }

  // Return success
  return 1;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1f8, %rsp            # imm = 0x1F8
movq %rcx, %r14
movl (%rdx), %ecx
xorl %eax, %eax
testl %ecx, %ecx
je 0x215e9
movq %rdx, %r15
movq %rsi, %rbx
movq %rdi, %rsi
movl 0x4(%rdx), %edi
testl %edi, %edi
je 0x215e9
movl (%r14), %edx
testl %edx, %edx
je 0x215e9
movl 0x4(%r14), %r8d
testl %r8d, %r8d
je 0x215e9
movd %edi, %xmm0
movd %ecx, %xmm1
punpckldq %xmm0, %xmm1    # xmm1 = xmm1[0],xmm0[0],xmm1[1],xmm0[1]
cvtdq2pd %xmm1, %xmm3
movd %r8d, %xmm0
movd %edx, %xmm1
punpckldq %xmm0, %xmm1    # xmm1 = xmm1[0],xmm0[0],xmm1[1],xmm0[1]
cvtdq2pd %xmm1, %xmm0
divpd %xmm0, %xmm3
leaq 0xd6ada(%rip), %rax     # 0xf7cf8
movsd (%rax), %xmm0
movsd 0x86dde(%rip), %xmm2    # 0xa8008
movapd %xmm3, 0x40(%rsp)
addsd %xmm3, %xmm2
movapd 0x86e14(%rip), %xmm1    # 0xa8050
xorpd %xmm0, %xmm1
ucomisd %xmm1, %xmm2
jb 0x21307
ucomisd %xmm2, %xmm0
jb 0x21307
movapd 0x40(%rsp), %xmm2
unpckhpd %xmm2, %xmm2            # xmm2 = xmm2[1,1]
addsd 0x86da2(%rip), %xmm2    # 0xa8008
ucomisd %xmm1, %xmm2
jb 0x21307
ucomisd %xmm2, %xmm0
jb 0x21307
movsd 0x20(%r15), %xmm0
ucomisd 0x20(%r14), %xmm0
jne 0x21307
jp 0x21307
movsd 0x28(%r15), %xmm0
ucomisd 0x28(%r14), %xmm0
jne 0x21307
jp 0x21307
movsd 0x30(%r15), %xmm0
ucomisd 0x30(%r14), %xmm0
jne 0x21307
jp 0x21307
movsd 0x38(%r15), %xmm0
ucomisd 0x38(%r14), %xmm0
jne 0x21307
jp 0x21307
movsd 0x40(%r15), %xmm0
ucomisd 0x40(%r14), %xmm0
jne 0x21307
jp 0x21307
movsd 0x48(%r15), %xmm0
ucomisd 0x48(%r14), %xmm0
jne 0x21307
jp 0x21307
movsd 0x50(%r15), %xmm0
ucomisd 0x50(%r14), %xmm0
jne 0x21307
jp 0x21307
movsd 0x58(%r15), %xmm0
ucomisd 0x58(%r14), %xmm0
jne 0x21307
jp 0x21307
movq %rbx, %rdi
callq 0x5872a
jmp 0x215e6
leaq 0x128(%rsp), %rdi
callq 0x58808
movl (%r14), %esi
movl 0x4(%r14), %edx
leaq 0x50(%rsp), %rdi
callq 0x57e7c
leaq 0x50(%rsp), %rsi
movq %rbx, %rdi
callq 0x5872a
leaq 0x50(%rsp), %rdi
callq 0x589c0
movsd 0x86ccc(%rip), %xmm0    # 0xa8010
movq %rbx, %rdi
callq 0x59dcc
xorl %eax, %eax
leaq 0x50(%rsp), %r12
leaq 0x10(%rsp), %r13
cmpl 0xc4(%rbx), %eax
jge 0x215d9
cvtsi2sd %eax, %xmm0
addsd 0x86ca8(%rip), %xmm0    # 0xa8018
movsd %xmm0, 0x38(%rsp)
movq %rax, 0x28(%rsp)
cltq
movq %rax, 0x30(%rsp)
xorl %ebp, %ebp
cmpl 0xc0(%rbx), %ebp
jge 0x215cd
cvtsi2sd %ebp, %xmm0
addsd 0x86c7c(%rip), %xmm0    # 0xa8018
movq %r12, %rdi
movsd 0x38(%rsp), %xmm1
callq 0x66640
movq %r13, %rdi
movq %r12, %rsi
callq 0x6664a
movapd 0x50(%rsp), %xmm0
movupd 0x20(%r14), %xmm1
subpd %xmm1, %xmm0
movupd 0x20(%r15), %xmm1
mulpd 0x40(%rsp), %xmm0
addpd %xmm1, %xmm0
movapd %xmm0, 0x10(%rsp)
movq %rsp, %rdi
movq %r13, %rsi
callq 0x6664a
movupd 0x30(%r15), %xmm3
movupd 0x40(%r15), %xmm1
movsd 0x50(%r15), %xmm2
movapd %xmm1, %xmm0
xorpd %xmm5, %xmm5
cmpneqpd %xmm5, %xmm0
movapd %xmm3, %xmm4
cmpneqpd %xmm5, %xmm4
shufps $0x88, %xmm0, %xmm4     # xmm4 = xmm4[0,2],xmm0[0,2]
movmskps %xmm4, %eax
testl %eax, %eax
jne 0x2142f
ucomisd 0x86c17(%rip), %xmm2    # 0xa8038
jne 0x2142f
jp 0x2142f
movsd (%rsp), %xmm0
jmp 0x21559
movsd 0x10(%rsp), %xmm0
movsd 0x18(%rsp), %xmm6
movsd 0x58(%r15), %xmm7
movupd 0x10(%r15), %xmm5
movupd 0x20(%r15), %xmm4
subsd %xmm4, %xmm0
divsd %xmm5, %xmm0
movapd %xmm4, %xmm8
unpckhpd %xmm4, %xmm8            # xmm8 = xmm8[1],xmm4[1]
subsd %xmm8, %xmm6
movapd %xmm5, %xmm8
unpckhpd %xmm5, %xmm8            # xmm8 = xmm8[1],xmm5[1]
divsd %xmm8, %xmm6
movapd %xmm6, %xmm8
mulsd %xmm6, %xmm8
movapd %xmm0, %xmm9
mulsd %xmm0, %xmm9
addsd %xmm8, %xmm9
movapd %xmm9, %xmm10
movapd %xmm9, %xmm8
movapd %xmm0, %xmm11
addsd %xmm0, %xmm11
mulsd %xmm0, %xmm11
addsd %xmm9, %xmm11
movapd %xmm6, %xmm12
addsd %xmm6, %xmm12
mulsd %xmm6, %xmm12
addsd %xmm9, %xmm12
mulsd %xmm9, %xmm9
mulsd %xmm3, %xmm10
addsd 0x86b57(%rip), %xmm10   # 0xa8028
unpckhpd %xmm3, %xmm3            # xmm3 = xmm3[1,1]
mulsd %xmm9, %xmm3
addsd %xmm10, %xmm3
movapd %xmm2, %xmm10
addsd %xmm2, %xmm10
mulsd %xmm7, %xmm11
addsd %xmm7, %xmm7
mulsd %xmm9, %xmm8
mulsd %xmm1, %xmm8
addsd %xmm3, %xmm8
mulsd %xmm9, %xmm9
unpckhpd %xmm1, %xmm1            # xmm1 = xmm1[1,1]
mulsd %xmm9, %xmm1
addsd %xmm8, %xmm1
movapd %xmm1, %xmm3
mulsd %xmm0, %xmm3
addsd %xmm11, %xmm3
mulsd %xmm10, %xmm0
mulsd %xmm6, %xmm0
addsd %xmm3, %xmm0
mulsd %xmm2, %xmm12
mulsd %xmm6, %xmm1
addsd %xmm12, %xmm1
mulsd %xmm0, %xmm7
mulsd %xmm6, %xmm7
addsd %xmm1, %xmm7
unpcklpd %xmm7, %xmm0            # xmm0 = xmm0[0],xmm7[0]
mulpd %xmm5, %xmm0
addpd %xmm4, %xmm0
movapd %xmm0, (%rsp)
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
ja 0x215c5
movl 0x1e8(%rsp), %eax
decl %eax
cvtsi2sd %eax, %xmm1
ucomisd %xmm1, %xmm0
ja 0x215c5
movsd 0x8(%rsp), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
ja 0x215c5
movl 0x1ec(%rsp), %eax
decl %eax
cvtsi2sd %eax, %xmm1
ucomisd %xmm1, %xmm0
ja 0x215c5
leaq 0x128(%rsp), %rdi
movq %rsp, %rsi
callq 0x15678
movq 0xb8(%rbx), %rax
movslq 0xc8(%rbx), %rcx
imulq 0x30(%rsp), %rcx
addq %rbp, %rcx
movsd %xmm0, (%rax,%rcx,8)
incq %rbp
jmp 0x21384
movq 0x28(%rsp), %rax
incl %eax
jmp 0x21358
leaq 0x128(%rsp), %rdi
callq 0x589c0
pushq $0x1
popq %rax
addq $0x1f8, %rsp            # imm = 0x1F8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x21614
movq %rax, %rbx
leaq 0x50(%rsp), %rdi
callq 0x589c0
jmp 0x21617
jmp 0x21614
jmp 0x21614
jmp 0x21614
jmp 0x21614
movq %rax, %rbx
leaq 0x128(%rsp), %rdi
callq 0x589c0
movq %rbx, %rdi
callq 0x11a80