// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/apps/scn2cam/scn2cam.cpp
// Function: FindIndexOfFurthestPointAlongPath(gaps::R2Grid const&, int, int*, int*)
static int
FindIndexOfFurthestPointAlongPath(const R2Grid& grid, int start_index,
  int *path = NULL, int *path_size = NULL)
{
  // Initialize Dijksra bookkeeping
  R2Grid parent_grid(grid), distance_grid(grid);
  parent_grid.Clear(R2_GRID_UNKNOWN_VALUE);
  distance_grid.Clear(FLT_MAX);
  const RNScalar *grid_values = distance_grid.GridValues();
  int neighbor_index, ix, iy;
  int end_index = -1;
  
  // Compute shortest path to all nonzero points
  RNHeap<const RNScalar *> heap(0);
  distance_grid.SetGridValue(start_index, 0);
  parent_grid.SetGridValue(start_index, start_index);
  heap.Push(grid_values + start_index);
  while (!heap.IsEmpty()) {
    const RNScalar *grid_entry = heap.Pop();
    int grid_index = grid_entry - grid_values;
    end_index = grid_index;
    grid.IndexToIndices(grid_index, ix, iy);
    for (int dx = -1; dx <= 1; dx++) {
      int nx = ix + dx;
      if ((nx < 0) || (nx > grid.XResolution()-1)) continue;
      for (int dy = -1; dy <= 1; dy++) {
        int ny = iy + dy;
        if ((ny < 0) || (ny > grid.YResolution()-1)) continue;
        grid.IndicesToIndex(nx, ny, neighbor_index);
        if (neighbor_index == grid_index) continue;
        if (grid.GridValue(neighbor_index) != R2_GRID_UNKNOWN_VALUE) {
          RNScalar d = distance_grid.GridValue(grid_index) + sqrt(dx*dx + dy*dy);
          RNScalar old_d = distance_grid.GridValue(neighbor_index);
          if (d < old_d) {
            distance_grid.SetGridValue(neighbor_index, d);
            parent_grid.SetGridValue(neighbor_index, grid_index);
            if (old_d == FLT_MAX) heap.Push(grid_values + neighbor_index);
            else heap.Update(grid_values + neighbor_index);
          }
        }
      }
    }
  }

  // Fill in path
  if (path && path_size) {
    // Construct path backwards
    int count = 0;
    path[count++] = end_index;
    while (path[count-1] != start_index) {
      path[count] = (int) parent_grid.GridValue(path[count-1]);
      count++;
    }

    // Reverse order of path
    for (int i = 0; i < count/2; i++) {
      int swap = path[i];
      path[i] = path[count-1-i];
      path[count-1-i] = swap;
    }

    // Fill in path size
    *path_size = count;
  }

  // Return last visited index
  return end_index;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x218, %rsp            # imm = 0x218
movq %rcx, 0x18(%rsp)
movq %rdx, %rbx
movl %esi, 0xc(%rsp)
movq %rdi, %r12
leaq 0x78(%rsp), %rdi
movq %r12, %rsi
callq 0x7d494
leaq 0x148(%rsp), %rdi
movq %r12, %rsi
callq 0x7d494
leaq 0x78(%rsp), %rdi
movsd 0xaf061(%rip), %xmm0    # 0xda088
callq 0x7ea58
leaq 0x148(%rsp), %rdi
movsd 0xaf07c(%rip), %xmm0    # 0xda0b8
callq 0x7ea58
movq 0x200(%rsp), %rdx
xorpd %xmm0, %xmm0
leaq 0x40(%rsp), %rdi
movapd %xmm0, (%rdi)
movabsq $-0x100000000, %rax     # imm = 0xFFFFFFFF00000000
movq %rax, 0x10(%rdi)
movupd %xmm0, 0x18(%rdi)
andq $0x0, 0x28(%rdi)
movl $0x1, 0x30(%rdi)
movslq 0xc(%rsp), %rax
andq $0x0, (%rdx,%rax,8)
cvtsi2sd %eax, %xmm0
movq 0x130(%rsp), %rcx
movsd %xmm0, (%rcx,%rax,8)
movl %eax, %eax
movq %rdx, 0x20(%rsp)
leaq (%rdx,%rax,8), %rsi
callq 0x2c522
pushq $-0x1
popq %rax
movl %eax, %r13d
movq %rbx, 0x28(%rsp)
cmpl $0x0, 0x48(%rsp)
je 0x2b20c
leaq 0x40(%rsp), %rdi
callq 0x2c62a
movq %rax, %rcx
subq 0x20(%rsp), %rcx
shrq $0x3, %rcx
movl %ecx, %eax
cltd
idivl 0xc8(%r12)
movl %eax, %ebx
movl %edx, 0x14(%rsp)
movq %rcx, %r13
movslq %ecx, %rcx
movq %rcx, 0x38(%rsp)
cvtsi2sd %ecx, %xmm0
movsd %xmm0, 0x30(%rsp)
pushq $-0x1
popq %r14
cmpl $0x2, %r14d
je 0x2b202
movl %r14d, 0x10(%rsp)
addl 0x14(%rsp), %r14d
js 0x2b1f5
cmpl 0xc0(%r12), %r14d
jge 0x2b1f5
movl 0x10(%rsp), %ebp
imull %ebp, %ebp
pushq $-0x1
popq %r15
cmpl $0x2, %r15d
je 0x2b1f5
movl %ebx, %eax
addl %r15d, %eax
js 0x2b1ed
cmpl 0xc4(%r12), %eax
jge 0x2b1ed
imull 0xc8(%r12), %eax
addl %r14d, %eax
cmpl %r13d, %eax
je 0x2b1ed
movq 0xb8(%r12), %rcx
cltq
movsd (%rcx,%rax,8), %xmm0
ucomisd 0xaef10(%rip), %xmm0    # 0xda088
jne 0x2b17c
jnp 0x2b1ed
movl %r15d, %ecx
imull %r15d, %ecx
addl %ebp, %ecx
cvtsi2sd %ecx, %xmm0
movq 0x200(%rsp), %rcx
sqrtsd %xmm0, %xmm0
movq 0x38(%rsp), %rdx
addsd (%rcx,%rdx,8), %xmm0
movsd (%rcx,%rax,8), %xmm1
ucomisd %xmm0, %xmm1
jbe 0x2b1ed
movsd %xmm0, (%rcx,%rax,8)
movq 0x130(%rsp), %rcx
movsd 0x30(%rsp), %xmm0
movsd %xmm0, (%rcx,%rax,8)
ucomisd 0xaeeee(%rip), %xmm1    # 0xda0b8
movq 0x20(%rsp), %rcx
leaq (%rcx,%rax,8), %rsi
jne 0x2b1e3
jp 0x2b1e3
leaq 0x40(%rsp), %rdi
callq 0x2c522
jmp 0x2b1ed
leaq 0x40(%rsp), %rdi
callq 0x2c6ca
incl %r15d
jmp 0x2b129
movl 0x10(%rsp), %r14d
incl %r14d
jmp 0x2b0f6
movq 0x28(%rsp), %rbx
jmp 0x2b0ab
testq %rbx, %rbx
sete %al
cmpq $0x0, 0x18(%rsp)
sete %cl
orb %al, %cl
jne 0x2b27d
movl %r13d, (%rbx)
pushq $0x1
popq %rax
xorl %ecx, %ecx
movq 0x130(%rsp), %rdx
movl %r13d, %esi
cmpl 0xc(%rsp), %esi
je 0x2b24a
movslq %esi, %rsi
cvttsd2si (%rdx,%rsi,8), %esi
movl %esi, (%rbx,%rax,4)
incq %rax
incl %ecx
jmp 0x2b232
movq %rax, %rdx
shrq %rdx
negq %rdx
movslq %ecx, %rcx
leaq (%rbx,%rcx,4), %rcx
xorl %esi, %esi
cmpq %rsi, %rdx
je 0x2b276
movl (%rbx), %edi
movl (%rcx,%rsi,4), %r8d
movl %r8d, (%rbx)
movl %edi, (%rcx,%rsi,4)
decq %rsi
addq $0x4, %rbx
jmp 0x2b25c
movq 0x18(%rsp), %rcx
movl %eax, (%rcx)
leaq 0x40(%rsp), %rdi
callq 0x2c744
leaq 0x148(%rsp), %rdi
callq 0x7d64c
leaq 0x78(%rsp), %rdi
callq 0x7d64c
movl %r13d, %eax
addq $0x218, %rsp            # imm = 0x218
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2b2c1
movq %rax, %rbx
jmp 0x2b2db
movq %rax, %rbx
jmp 0x2b2ce
jmp 0x2b2c1
movq %rax, %rbx
leaq 0x40(%rsp), %rdi
callq 0x2c744
leaq 0x148(%rsp), %rdi
callq 0x7d64c
leaq 0x78(%rsp), %rdi
callq 0x7d64c
movq %rbx, %rdi
callq 0x230b0
nop