// File: 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:
subq $0x2e8, %rsp            # imm = 0x2E8
movq %rdi, 0x2d8(%rsp)
movq %rsi, 0x2d0(%rsp)
movq %rdx, 0x2c8(%rsp)
movq %rcx, 0x2c0(%rsp)
movq 0x2c8(%rsp), %rax
cmpl $0x0, (%rax)
je 0x33232
movq 0x2c8(%rsp), %rax
cmpl $0x0, 0x4(%rax)
jne 0x33242
movl $0x0, 0x2e4(%rsp)
jmp 0x33b28
movq 0x2c0(%rsp), %rax
cmpl $0x0, (%rax)
je 0x3325d
movq 0x2c0(%rsp), %rax
cmpl $0x0, 0x4(%rax)
jne 0x3326d
movl $0x0, 0x2e4(%rsp)
jmp 0x33b28
movq 0x2c8(%rsp), %rax
cvtsi2sdl (%rax), %xmm0
movq 0x2c0(%rsp), %rax
cvtsi2sdl (%rax), %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, 0x2b8(%rsp)
movq 0x2c8(%rsp), %rax
cvtsi2sdl 0x4(%rax), %xmm0
movq 0x2c0(%rsp), %rax
cvtsi2sdl 0x4(%rax), %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, 0x2b0(%rsp)
movsd 0x2b8(%rsp), %xmm0
leaq 0x17ebf7(%rip), %rax    # 0x1b1ec0
movsd (%rax), %xmm2
movsd 0x132d5b(%rip), %xmm1   # 0x166030
callq 0x359f0
cmpl $0x0, %eax
je 0x33446
movsd 0x2b0(%rsp), %xmm0
leaq 0x17ebcd(%rip), %rax    # 0x1b1ec0
movsd (%rax), %xmm2
movsd 0x132d31(%rip), %xmm1   # 0x166030
callq 0x359f0
cmpl $0x0, %eax
je 0x33446
movq 0x2c8(%rsp), %rax
movsd 0x20(%rax), %xmm0
movq 0x2c0(%rsp), %rax
ucomisd 0x20(%rax), %xmm0
jne 0x33446
jp 0x33446
movq 0x2c8(%rsp), %rax
movsd 0x28(%rax), %xmm0
movq 0x2c0(%rsp), %rax
ucomisd 0x28(%rax), %xmm0
jne 0x33446
jp 0x33446
movq 0x2c8(%rsp), %rax
movsd 0x30(%rax), %xmm0
movq 0x2c0(%rsp), %rax
ucomisd 0x30(%rax), %xmm0
jne 0x33446
jp 0x33446
movq 0x2c8(%rsp), %rax
movsd 0x38(%rax), %xmm0
movq 0x2c0(%rsp), %rax
ucomisd 0x38(%rax), %xmm0
jne 0x33446
jp 0x33446
movq 0x2c8(%rsp), %rax
movsd 0x40(%rax), %xmm0
movq 0x2c0(%rsp), %rax
ucomisd 0x40(%rax), %xmm0
jne 0x33446
jp 0x33446
movq 0x2c8(%rsp), %rax
movsd 0x48(%rax), %xmm0
movq 0x2c0(%rsp), %rax
ucomisd 0x48(%rax), %xmm0
jne 0x33446
jp 0x33446
movq 0x2c8(%rsp), %rax
movsd 0x50(%rax), %xmm0
movq 0x2c0(%rsp), %rax
ucomisd 0x50(%rax), %xmm0
jne 0x33446
jp 0x33446
movq 0x2c8(%rsp), %rax
movsd 0x58(%rax), %xmm0
movq 0x2c0(%rsp), %rax
ucomisd 0x58(%rax), %xmm0
jne 0x33446
jp 0x33446
movq 0x2d8(%rsp), %rsi
movq 0x2d0(%rsp), %rdi
callq 0xb2f20
movl $0x1, 0x2e4(%rsp)
jmp 0x33b28
movq 0x2d8(%rsp), %rsi
leaq 0x1e0(%rsp), %rdi
callq 0xb30a0
movq 0x2c0(%rsp), %rax
movl (%rax), %esi
movl 0x4(%rax), %edx
leaq 0x110(%rsp), %rdi
callq 0xb1ee0
jmp 0x33477
movq 0x2d0(%rsp), %rdi
leaq 0x110(%rsp), %rsi
callq 0xb2f20
jmp 0x3348e
leaq 0x110(%rsp), %rdi
callq 0xb3390
movq 0x2d0(%rsp), %rdi
movsd 0x132b75(%rip), %xmm0   # 0x166020
callq 0xb5a20
jmp 0x334b2
movl $0x0, 0x100(%rsp)
movl 0x100(%rsp), %eax
movl %eax, 0x70(%rsp)
movq 0x2d0(%rsp), %rdi
callq 0x27960
movl %eax, 0x74(%rsp)
jmp 0x334db
movl 0x70(%rsp), %eax
movl 0x74(%rsp), %ecx
cmpl %ecx, %eax
jge 0x33aff
movl $0x0, 0xfc(%rsp)
movl 0xfc(%rsp), %eax
movl %eax, 0x68(%rsp)
movq 0x2d0(%rsp), %rdi
callq 0x27940
movl %eax, 0x6c(%rsp)
jmp 0x33514
movl 0x68(%rsp), %eax
movl 0x6c(%rsp), %ecx
cmpl %ecx, %eax
jge 0x33ae7
cvtsi2sdl 0xfc(%rsp), %xmm0
movsd 0x132af3(%rip), %xmm2   # 0x166028
addsd %xmm2, %xmm0
cvtsi2sdl 0x100(%rsp), %xmm1
addsd %xmm2, %xmm1
leaq 0xe8(%rsp), %rdi
callq 0xcfd80
jmp 0x33555
leaq 0xd8(%rsp), %rdi
leaq 0xe8(%rsp), %rsi
callq 0xcfdb0
jmp 0x3356c
movsd 0x2b8(%rsp), %xmm0
movsd %xmm0, 0x58(%rsp)
leaq 0xe8(%rsp), %rdi
callq 0x1af20
movsd %xmm0, 0x60(%rsp)
jmp 0x33590
movsd 0x58(%rsp), %xmm0
movsd 0x60(%rsp), %xmm2
movq 0x2c0(%rsp), %rax
movsd 0x20(%rax), %xmm1
subsd %xmm1, %xmm2
movq 0x2c8(%rsp), %rax
movsd 0x20(%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xd0(%rsp)
movsd 0x2b0(%rsp), %xmm0
movsd %xmm0, 0x48(%rsp)
leaq 0xe8(%rsp), %rdi
callq 0x1af70
movsd %xmm0, 0x50(%rsp)
jmp 0x335ef
movsd 0x48(%rsp), %xmm0
movsd 0x50(%rsp), %xmm2
movq 0x2c0(%rsp), %rax
movsd 0x28(%rax), %xmm1
subsd %xmm1, %xmm2
movq 0x2c8(%rsp), %rax
movsd 0x28(%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xc8(%rsp)
movsd 0xd0(%rsp), %xmm0
movsd 0xc8(%rsp), %xmm1
leaq 0xd8(%rsp), %rdi
callq 0x35a30
jmp 0x3364b
leaq 0xb8(%rsp), %rdi
leaq 0xd8(%rsp), %rsi
callq 0xcfdb0
jmp 0x33662
movq 0x2c8(%rsp), %rax
movsd 0x30(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x336f7
jp 0x336f7
movq 0x2c8(%rsp), %rax
movsd 0x38(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x336f7
jp 0x336f7
movq 0x2c8(%rsp), %rax
movsd 0x40(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x336f7
jp 0x336f7
movq 0x2c8(%rsp), %rax
movsd 0x48(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x336f7
jp 0x336f7
movq 0x2c8(%rsp), %rax
movsd 0x50(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x336f7
jp 0x336f7
movq 0x2c8(%rsp), %rax
movsd 0x38(%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x336f7
jp 0x336f7
jmp 0x339ae
leaq 0xd8(%rsp), %rdi
callq 0x1af20
movsd %xmm0, 0x40(%rsp)
jmp 0x3370c
movsd 0x40(%rsp), %xmm0
movq 0x2c8(%rsp), %rax
movsd 0x10(%rax), %xmm1
movsd 0x20(%rax), %xmm2
subsd %xmm2, %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0xb0(%rsp)
leaq 0xd8(%rsp), %rdi
callq 0x1af70
movsd %xmm0, 0x38(%rsp)
jmp 0x3374a
movsd 0x38(%rsp), %xmm0
movq 0x2c8(%rsp), %rax
movsd 0x18(%rax), %xmm1
movsd 0x28(%rax), %xmm2
subsd %xmm2, %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0xa8(%rsp)
movsd 0xb0(%rsp), %xmm0
movsd 0xa8(%rsp), %xmm1
mulsd %xmm1, %xmm1
mulsd %xmm0, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xa0(%rsp)
movsd 0xa0(%rsp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x98(%rsp)
movsd 0xa0(%rsp), %xmm3
movq 0x2c8(%rsp), %rax
movsd 0x30(%rax), %xmm0
movsd 0x38(%rax), %xmm4
movaps %xmm3, %xmm1
mulsd %xmm0, %xmm1
movsd 0x132856(%rip), %xmm0   # 0x166030
addsd %xmm0, %xmm1
movsd 0x98(%rsp), %xmm0
movaps %xmm0, %xmm2
mulsd %xmm4, %xmm2
addsd %xmm1, %xmm2
movaps %xmm0, %xmm1
mulsd %xmm3, %xmm1
movsd 0x40(%rax), %xmm3
mulsd %xmm3, %xmm1
addsd %xmm2, %xmm1
mulsd %xmm0, %xmm0
movsd 0x48(%rax), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x90(%rsp)
movsd 0x90(%rsp), %xmm1
movsd 0xb0(%rsp), %xmm2
movq 0x2c8(%rsp), %rax
movsd 0x50(%rax), %xmm0
movsd 0x58(%rax), %xmm3
movsd 0xa0(%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 0xa8(%rsp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xb0(%rsp)
movsd 0x90(%rsp), %xmm1
movsd 0xa8(%rsp), %xmm2
movq 0x2c8(%rsp), %rax
movsd 0xa0(%rsp), %xmm0
movaps %xmm2, %xmm4
addsd %xmm4, %xmm4
mulsd %xmm2, %xmm4
addsd %xmm0, %xmm4
movsd 0x50(%rax), %xmm3
movsd 0x58(%rax), %xmm0
mulsd %xmm4, %xmm3
mulsd %xmm2, %xmm1
addsd %xmm3, %xmm1
addsd %xmm0, %xmm0
movsd 0xb0(%rsp), %xmm3
mulsd %xmm3, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xa8(%rsp)
movsd 0xb0(%rsp), %xmm0
movq 0x2c8(%rsp), %rax
movsd 0x10(%rax), %xmm2
movsd 0x20(%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x88(%rsp)
movsd 0xa8(%rsp), %xmm0
movq 0x2c8(%rsp), %rax
movsd 0x18(%rax), %xmm2
movsd 0x28(%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x80(%rsp)
movsd 0x88(%rsp), %xmm0
movsd 0x80(%rsp), %xmm1
leaq 0xb8(%rsp), %rdi
callq 0x35a30
jmp 0x3396d
jmp 0x339ae
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x108(%rsp)
movl %eax, 0x104(%rsp)
jmp 0x33b19
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x108(%rsp)
movl %eax, 0x104(%rsp)
leaq 0x110(%rsp), %rdi
callq 0xb3390
jmp 0x33b19
leaq 0xb8(%rsp), %rdi
callq 0x1af20
movsd %xmm0, 0x30(%rsp)
jmp 0x339c3
movsd 0x30(%rsp), %xmm1
xorps %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x339d7
jmp 0x33ad1
leaq 0xb8(%rsp), %rdi
callq 0x1af20
movsd %xmm0, 0x28(%rsp)
jmp 0x339ec
leaq 0x1e0(%rsp), %rdi
callq 0x27940
movl %eax, 0x24(%rsp)
jmp 0x339ff
movsd 0x28(%rsp), %xmm0
movl 0x24(%rsp), %eax
subl $0x1, %eax
cvtsi2sd %eax, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x33a1b
jmp 0x33ad1
leaq 0xb8(%rsp), %rdi
callq 0x1af70
movsd %xmm0, 0x18(%rsp)
jmp 0x33a30
movsd 0x18(%rsp), %xmm1
xorps %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x33a44
jmp 0x33ad1
leaq 0xb8(%rsp), %rdi
callq 0x1af70
movsd %xmm0, 0x10(%rsp)
jmp 0x33a59
leaq 0x1e0(%rsp), %rdi
callq 0x27960
movl %eax, 0xc(%rsp)
jmp 0x33a6c
movsd 0x10(%rsp), %xmm0
movl 0xc(%rsp), %eax
subl $0x1, %eax
cvtsi2sd %eax, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x33a85
jmp 0x33ad1
leaq 0x1e0(%rsp), %rdi
leaq 0xb8(%rsp), %rsi
callq 0x1a2c0
movsd %xmm0, (%rsp)
jmp 0x33aa1
movsd (%rsp), %xmm0
movsd %xmm0, 0x78(%rsp)
movq 0x2d0(%rsp), %rdi
movl 0xfc(%rsp), %esi
movl 0x100(%rsp), %edx
movsd 0x78(%rsp), %xmm0
callq 0x1a1b0
jmp 0x33acf
jmp 0x33ad1
movl 0xfc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xfc(%rsp)
jmp 0x334f6
jmp 0x33ae9
movl 0x100(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x100(%rsp)
jmp 0x334bd
movl $0x1, 0x2e4(%rsp)
leaq 0x1e0(%rsp), %rdi
callq 0xb3390
jmp 0x33b28
leaq 0x1e0(%rsp), %rdi
callq 0xb3390
jmp 0x33b37
movl 0x2e4(%rsp), %eax
addq $0x2e8, %rsp            # imm = 0x2E8
retq
movq 0x108(%rsp), %rdi
callq 0x12c20
nopw %cs:(%rax,%rax)