// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/RGBD/RGBDUtil.cpp
// Function: gaps::RGBDResampleDepthImage(gaps::R2Grid&, gaps::R3Matrix&, int, int)
int RGBDResampleDepthImage(R2Grid& image, R3Matrix& intrinsics_matrix, int xres, int yres)
{
  // Just checking
  if ((xres == 0) || (yres == 0)) return 0;
  if ((image.XResolution() == 0) || (image.YResolution() == 0)) return 0;
  if ((image.XResolution() == xres) && (image.YResolution() == yres)) return 1;

  // Compute scale factors
  double xscale = (double) image.XResolution() / (double) xres;
  double yscale = (double) image.YResolution() / (double) yres;

  // Copy original image and then initialize to zeroes
  R2Grid copy_image(image);
  image = R2Grid(xres, yres);
  image.Clear(R2_GRID_UNKNOWN_VALUE);
  
  // Resample images
  for (int iy = 0; iy < yres; iy++) {
    double cy = yscale * iy;
    int min_y = cy - 0.5*yscale + 0.5;
    if (min_y >= copy_image.YResolution()) continue;
    int max_y = cy + 0.5*yscale + 0.5;
    if (max_y < 0) continue;
    if (min_y < 0) min_y = 0;
    if (max_y >= copy_image.YResolution()) max_y = copy_image.YResolution()-1;
    if (max_y < min_y) max_y = min_y;
    
    for (int ix = 0; ix < xres; ix++) {
      double cx = xscale * ix;
      int min_x = cx - 0.5*xscale + 0.5;
      if (min_x >= copy_image.XResolution()) continue;
      int max_x = cx + 0.5*xscale + 0.5;
      if (max_x < 0) continue;
      if (min_x < 0) min_x = 0;
      if (max_x >= copy_image.XResolution()) max_x = copy_image.XResolution()-1;
      if (max_x < min_x) max_x = min_x;

      // Find minimum depth in neighborhood
      RNScalar min_depth = FLT_MAX;
      for (int j = min_y; j <= max_y; j++) {
        for (int i = min_x; i <= max_x; i++) {
          RNScalar depth = copy_image.GridValue(i, j);
          if ((depth <= 0) || (depth == R2_GRID_UNKNOWN_VALUE)) continue;
          if (depth < min_depth) min_depth = depth;
        }
      }

      // Set depth
      if (min_depth < FLT_MAX) {
        image.SetGridValue(ix, iy, min_depth);
      }
    }
  }

  // Update depth intrinsics matrix
  intrinsics_matrix[0][0] /= xscale;
  intrinsics_matrix[0][2] /= xscale;
  intrinsics_matrix[1][1] /= yscale;
  intrinsics_matrix[1][2] /= yscale;

  // Return success
  return 1;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1c8, %rsp            # imm = 0x1C8
movl %ecx, %ebp
movl %edx, %r15d
testl %edx, %edx
sete %dl
testl %ecx, %ecx
sete %cl
xorl %eax, %eax
orb %dl, %cl
jne 0x21ef0
movq %rdi, %r12
movl 0xc0(%rdi), %ebx
testl %ebx, %ebx
je 0x21ef0
movl 0xc4(%r12), %r14d
testl %r14d, %r14d
je 0x21ef0
movl %ebx, %edx
xorl %r15d, %edx
movl %r14d, %ecx
xorl %ebp, %ecx
pushq $0x1
popq %rax
orl %edx, %ecx
je 0x21ef0
movq %rsi, 0x18(%rsp)
leaq 0x28(%rsp), %rdi
movq %r12, %rsi
callq 0x58808
leaq 0xf8(%rsp), %rdi
movl %r15d, %esi
movl %ebp, %edx
callq 0x57e7c
leaq 0xf8(%rsp), %rsi
movq %r12, %rdi
callq 0x5872a
leaq 0xf8(%rsp), %rdi
callq 0x589c0
movsd 0x863a0(%rip), %xmm0    # 0xa8010
movq %r12, 0x20(%rsp)
movq %r12, %rdi
callq 0x59dcc
cvtsi2sd %ebx, %xmm1
cvtsi2sd %r15d, %xmm0
divsd %xmm0, %xmm1
cvtsi2sd %r14d, %xmm0
cvtsi2sd %ebp, %xmm2
divsd %xmm2, %xmm0
movl 0xec(%rsp), %eax
movq %rax, 0x10(%rsp)
decl %eax
movl %eax, 0x8(%rsp)
xorl %eax, %eax
testl %r15d, %r15d
cmovlel %eax, %r15d
testl %ebp, %ebp
cmovlel %eax, %ebp
xorpd %xmm2, %xmm2
movsd 0x8730d(%rip), %xmm3    # 0xa8fd0
mulsd %xmm0, %xmm3
movsd 0x86349(%rip), %xmm4    # 0xa8018
movapd %xmm0, %xmm5
mulsd %xmm4, %xmm5
movsd 0x872f1(%rip), %xmm6    # 0xa8fd0
mulsd %xmm1, %xmm6
movapd %xmm1, %xmm7
mulsd %xmm4, %xmm7
movsd 0x8632c(%rip), %xmm8    # 0xa8020
movsd 0x8632b(%rip), %xmm9    # 0xa8028
xorpd %xmm10, %xmm10
xorl %esi, %esi
movl %ebp, 0xc(%rsp)
cmpl %ebp, %esi
je 0x21eb2
movapd %xmm0, %xmm11
mulsd %xmm10, %xmm11
movapd %xmm3, %xmm12
addsd %xmm11, %xmm12
addsd %xmm4, %xmm12
cvttsd2si %xmm12, %ebp
cmpl %ebp, 0x10(%rsp)
jle 0x21ea2
addsd %xmm5, %xmm11
addsd %xmm4, %xmm11
cvttsd2si %xmm11, %r8d
testl %r8d, %r8d
js 0x21ea2
testl %ebp, %ebp
movl $0x0, %eax
cmovlel %eax, %ebp
cmpl %r8d, 0x10(%rsp)
cmovlel 0x8(%rsp), %r8d
cmpl %ebp, %r8d
cmovlel %ebp, %r8d
movl 0xe8(%rsp), %r9d
leal -0x1(%r9), %r10d
movq 0x20(%rsp), %rcx
movslq 0xc8(%rcx), %rax
movslq %esi, %r11
imulq %rax, %r11
shlq $0x3, %r11
addq 0xb8(%rcx), %r11
leaq (,%rbp,8), %rax
xorl %ecx, %ecx
cmpq %r15, %rcx
je 0x21ea2
cvtsi2sd %ecx, %xmm11
mulsd %xmm1, %xmm11
movapd %xmm6, %xmm12
addsd %xmm11, %xmm12
addsd %xmm4, %xmm12
cvttsd2si %xmm12, %edx
cmpl %edx, %r9d
jle 0x21e9a
addsd %xmm7, %xmm11
addsd %xmm4, %xmm11
cvttsd2si %xmm11, %ebx
testl %ebx, %ebx
js 0x21e9a
testl %edx, %edx
movl $0x0, %edi
cmovlel %edi, %edx
cmpl %ebx, %r9d
cmovlel %r10d, %ebx
cmpl %edx, %ebx
cmovlel %edx, %ebx
movslq 0xf0(%rsp), %r12
movq %rax, %r13
imulq %r12, %r13
addq 0xe0(%rsp), %r13
shlq $0x3, %r12
movq %rbp, %rdi
movapd %xmm8, %xmm11
cmpq %r8, %rdi
ja 0x21e8d
movq %rdx, %r14
cmpq %rbx, %r14
ja 0x21e85
movsd (%r13,%r14,8), %xmm12
movapd %xmm12, %xmm13
minsd %xmm11, %xmm13
movapd %xmm12, %xmm14
cmpeqsd 0x861c3(%rip), %xmm14   # 0xa8010
movapd %xmm14, %xmm15
andnpd %xmm13, %xmm15
andpd %xmm11, %xmm14
orpd %xmm15, %xmm14
cmplesd %xmm2, %xmm12
movapd %xmm12, %xmm13
andnpd %xmm14, %xmm13
andpd %xmm11, %xmm12
orpd %xmm13, %xmm12
incq %r14
movapd %xmm12, %xmm11
jmp 0x21e28
incq %rdi
addq %r12, %r13
jmp 0x21e20
ucomisd %xmm11, %xmm8
jbe 0x21e9a
movsd %xmm11, (%r11,%rcx,8)
incq %rcx
jmp 0x21da0
addsd %xmm9, %xmm10
incl %esi
movl 0xc(%rsp), %ebp
jmp 0x21d08
movq 0x18(%rsp), %rax
movsd (%rax), %xmm2
movsd 0x10(%rax), %xmm3
divsd %xmm1, %xmm2
movsd %xmm2, (%rax)
divsd %xmm1, %xmm3
movsd %xmm3, 0x10(%rax)
movupd 0x20(%rax), %xmm1
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
divpd %xmm0, %xmm1
movupd %xmm1, 0x20(%rax)
leaq 0x28(%rsp), %rdi
callq 0x589c0
pushq $0x1
popq %rax
addq $0x1c8, %rsp            # imm = 0x1C8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x21f16
movq %rax, %rbx
leaq 0xf8(%rsp), %rdi
callq 0x589c0
jmp 0x21f19
movq %rax, %rbx
leaq 0x28(%rsp), %rdi
callq 0x589c0
movq %rbx, %rdi
callq 0x11a80