// File: 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:
subq $0x298, %rsp            # imm = 0x298
movq %rdi, 0x290(%rsp)
movl %esi, 0x28c(%rsp)
movq %rdx, 0x280(%rsp)
movq %rcx, 0x278(%rsp)
movq 0x290(%rsp), %rsi
leaq 0x1a8(%rsp), %rdi
callq 0xf2470
movq 0x290(%rsp), %rsi
leaq 0xd8(%rsp), %rdi
callq 0xf2470
jmp 0x310c2
leaq 0x1a8(%rsp), %rdi
movsd 0x189fbe(%rip), %xmm0   # 0x1bb090
callq 0xf4df0
jmp 0x310d9
leaq 0xd8(%rsp), %rdi
movsd 0x189f57(%rip), %xmm0   # 0x1bb040
callq 0xf4df0
jmp 0x310f0
leaq 0xd8(%rsp), %rdi
callq 0x360d0
movq %rax, 0x30(%rsp)
jmp 0x31104
movq 0x30(%rsp), %rax
movq %rax, 0xc0(%rsp)
movl $0xffffffff, 0xb0(%rsp) # imm = 0xFFFFFFFF
leaq 0x78(%rsp), %rdi
xorl %esi, %esi
movl $0xffffffff, %edx       # imm = 0xFFFFFFFF
movl $0x1, %ecx
callq 0x360f0
jmp 0x31134
movl 0x28c(%rsp), %esi
leaq 0xd8(%rsp), %rdi
xorps %xmm0, %xmm0
callq 0x36150
jmp 0x3114d
movl 0x28c(%rsp), %esi
cvtsi2sd %esi, %xmm0
leaq 0x1a8(%rsp), %rdi
callq 0x36150
jmp 0x31167
movq 0xc0(%rsp), %rax
movslq 0x28c(%rsp), %rcx
leaq (%rax,%rcx,8), %rsi
leaq 0x78(%rsp), %rdi
callq 0x36180
jmp 0x31187
jmp 0x31189
leaq 0x78(%rsp), %rdi
callq 0x363a0
movl %eax, 0x2c(%rsp)
jmp 0x31199
movl 0x2c(%rsp), %eax
cmpl $0x0, %eax
setne %al
xorb $-0x1, %al
testb $0x1, %al
jne 0x311ae
jmp 0x31497
leaq 0x78(%rsp), %rdi
callq 0x363c0
movq %rax, 0x20(%rsp)
jmp 0x311bf
movq 0x20(%rsp), %rax
movq %rax, 0x70(%rsp)
movq 0x70(%rsp), %rax
movq 0xc0(%rsp), %rcx
subq %rcx, %rax
shrq $0x3, %rax
movl %eax, 0x6c(%rsp)
movl 0x6c(%rsp), %eax
movl %eax, 0xb0(%rsp)
movq 0x290(%rsp), %rdi
movl 0x6c(%rsp), %esi
leaq 0xb8(%rsp), %rdx
leaq 0xb4(%rsp), %rcx
callq 0x36090
jmp 0x3120f
movl $0xffffffff, 0x68(%rsp) # imm = 0xFFFFFFFF
cmpl $0x1, 0x68(%rsp)
jg 0x31492
movl 0xb8(%rsp), %eax
addl 0x68(%rsp), %eax
movl %eax, 0x64(%rsp)
cmpl $0x0, 0x64(%rsp)
jl 0x3125a
movl 0x64(%rsp), %eax
movl %eax, 0x1c(%rsp)
movq 0x290(%rsp), %rdi
callq 0x33710
movl %eax, %ecx
movl 0x1c(%rsp), %eax
subl $0x1, %ecx
cmpl %ecx, %eax
jle 0x312b4
jmp 0x31482
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xd0(%rsp)
movl %eax, 0xcc(%rsp)
jmp 0x31624
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xd0(%rsp)
movl %eax, 0xcc(%rsp)
jmp 0x31617
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xd0(%rsp)
movl %eax, 0xcc(%rsp)
leaq 0x78(%rsp), %rdi
callq 0x36620
jmp 0x31617
movl $0xffffffff, 0x60(%rsp) # imm = 0xFFFFFFFF
cmpl $0x1, 0x60(%rsp)
jg 0x31480
movl 0xb4(%rsp), %eax
addl 0x60(%rsp), %eax
movl %eax, 0x5c(%rsp)
cmpl $0x0, 0x5c(%rsp)
jl 0x312ff
movl 0x5c(%rsp), %eax
movl %eax, 0x18(%rsp)
movq 0x290(%rsp), %rdi
callq 0x33730
movl %eax, %ecx
movl 0x18(%rsp), %eax
subl $0x1, %ecx
cmpl %ecx, %eax
jle 0x31304
jmp 0x31470
movq 0x290(%rsp), %rdi
movl 0x64(%rsp), %esi
movl 0x5c(%rsp), %edx
leaq 0xbc(%rsp), %rcx
callq 0x364d0
jmp 0x31323
movl 0xbc(%rsp), %eax
cmpl 0x6c(%rsp), %eax
jne 0x31335
jmp 0x31470
movq 0x290(%rsp), %rdi
movl 0xbc(%rsp), %esi
callq 0x33540
movsd 0x189d3f(%rip), %xmm1   # 0x1bb090
ucomisd %xmm1, %xmm0
jne 0x3135e
jp 0x3135e
jmp 0x3146e
movl 0x6c(%rsp), %esi
leaq 0xd8(%rsp), %rdi
callq 0x33540
movsd %xmm0, 0x8(%rsp)
movl 0x68(%rsp), %edi
imull %edi, %edi
movl 0x60(%rsp), %eax
imull %eax, %eax
addl %eax, %edi
callq 0x36500
movsd %xmm0, 0x10(%rsp)
jmp 0x31392
movsd 0x10(%rsp), %xmm1
movsd 0x8(%rsp), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x50(%rsp)
movl 0xbc(%rsp), %esi
leaq 0xd8(%rsp), %rdi
callq 0x33540
movsd %xmm0, 0x48(%rsp)
movsd 0x50(%rsp), %xmm1
movsd 0x48(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x3146c
movl 0xbc(%rsp), %esi
movsd 0x50(%rsp), %xmm0
leaq 0xd8(%rsp), %rdi
callq 0x36150
jmp 0x313f4
movl 0xbc(%rsp), %esi
cvtsi2sdl 0x6c(%rsp), %xmm0
leaq 0x1a8(%rsp), %rdi
callq 0x36150
jmp 0x31410
movsd 0x48(%rsp), %xmm0
movsd 0x189c22(%rip), %xmm1   # 0x1bb040
ucomisd %xmm1, %xmm0
jne 0x31448
jp 0x31448
movq 0xc0(%rsp), %rax
movslq 0xbc(%rsp), %rcx
leaq (%rax,%rcx,8), %rsi
leaq 0x78(%rsp), %rdi
callq 0x36180
jmp 0x31446
jmp 0x3146a
movq 0xc0(%rsp), %rax
movslq 0xbc(%rsp), %rcx
leaq (%rax,%rcx,8), %rsi
leaq 0x78(%rsp), %rdi
callq 0x36520
jmp 0x31468
jmp 0x3146a
jmp 0x3146c
jmp 0x3146e
jmp 0x31470
movl 0x60(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x60(%rsp)
jmp 0x312bc
jmp 0x31482
movl 0x68(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x68(%rsp)
jmp 0x31217
jmp 0x31189
cmpq $0x0, 0x280(%rsp)
je 0x315de
cmpq $0x0, 0x278(%rsp)
je 0x315de
movl $0x0, 0x44(%rsp)
movl 0xb0(%rsp), %edx
movq 0x280(%rsp), %rax
movl 0x44(%rsp), %ecx
movl %ecx, %esi
addl $0x1, %esi
movl %esi, 0x44(%rsp)
movslq %ecx, %rcx
movl %edx, (%rax,%rcx,4)
movq 0x280(%rsp), %rax
movl 0x44(%rsp), %ecx
subl $0x1, %ecx
movslq %ecx, %rcx
movl (%rax,%rcx,4), %eax
cmpl 0x28c(%rsp), %eax
je 0x31540
movq 0x280(%rsp), %rax
movl 0x44(%rsp), %ecx
subl $0x1, %ecx
movslq %ecx, %rcx
movl (%rax,%rcx,4), %esi
leaq 0x1a8(%rsp), %rdi
callq 0x33540
cvttsd2si %xmm0, %edx
movq 0x280(%rsp), %rax
movslq 0x44(%rsp), %rcx
movl %edx, (%rax,%rcx,4)
movl 0x44(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x44(%rsp)
jmp 0x314df
movl $0x0, 0x40(%rsp)
movl 0x40(%rsp), %eax
movl %eax, 0x4(%rsp)
movl 0x44(%rsp), %eax
movl $0x2, %ecx
cltd
idivl %ecx
movl %eax, %ecx
movl 0x4(%rsp), %eax
cmpl %ecx, %eax
jge 0x315d0
movq 0x280(%rsp), %rax
movslq 0x40(%rsp), %rcx
movl (%rax,%rcx,4), %eax
movl %eax, 0x3c(%rsp)
movq 0x280(%rsp), %rax
movl 0x44(%rsp), %ecx
subl $0x1, %ecx
subl 0x40(%rsp), %ecx
movslq %ecx, %rcx
movl (%rax,%rcx,4), %edx
movq 0x280(%rsp), %rax
movslq 0x40(%rsp), %rcx
movl %edx, (%rax,%rcx,4)
movl 0x3c(%rsp), %edx
movq 0x280(%rsp), %rax
movl 0x44(%rsp), %ecx
subl $0x1, %ecx
subl 0x40(%rsp), %ecx
movslq %ecx, %rcx
movl %edx, (%rax,%rcx,4)
movl 0x40(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x40(%rsp)
jmp 0x31548
movl 0x44(%rsp), %ecx
movq 0x278(%rsp), %rax
movl %ecx, (%rax)
movl 0xb0(%rsp), %eax
movl %eax, (%rsp)
leaq 0x78(%rsp), %rdi
callq 0x36620
leaq 0xd8(%rsp), %rdi
callq 0xf2760
leaq 0x1a8(%rsp), %rdi
callq 0xf2760
movl (%rsp), %eax
addq $0x298, %rsp            # imm = 0x298
retq
leaq 0xd8(%rsp), %rdi
callq 0xf2760
leaq 0x1a8(%rsp), %rdi
callq 0xf2760
movq 0xd0(%rsp), %rdi
callq 0x24250
nop