// File: tomfunkhouser[P]gaps/apps/pfm2pfm/pfm2pfm.cpp
// Function: FlattenGrid(gaps::R2Grid*, double, double)
void 
FlattenGrid(R2Grid *grid, RNScalar delta, RNScalar sigma)
{
  // Allocate array of grid value wrappers
  GridValue **gvs = new GridValue * [ grid->NEntries() ];
  for (int i = 0; i < grid->NEntries(); i++) gvs[i] = NULL;

  // Build priority queue of grid values
  int x, y;
  GridValue tmp;
  RNHeap<GridValue *> heap(&tmp, &tmp.delta, &tmp.heap_ptr);
  for (int index = 0; index < grid->NEntries(); index++) {
    RNScalar value = grid->GridValue(index);
    if (value != R2_GRID_UNKNOWN_VALUE) {
      grid->IndexToIndices(index, x, y);
      RNScalar target_value = grid->GridValue(x, y, sigma);
      if (target_value != R2_GRID_UNKNOWN_VALUE) {
        RNScalar d = target_value - value;
        GridValue *gv = new GridValue();
        gv->index = index;
        gv->delta = d;
        gv->heap_ptr = NULL;
        gvs[index] = gv;
        heap.Push(gv);
      }
    }
  }

  // Iteratively adjust grid values from largest negative step to smallest
  while (!heap.IsEmpty()) {
    // Get grid entry with highest delta
    GridValue *gv = heap.Peek();
    if (gv->delta > -delta) break;
    RNScalar value = grid->GridValue(gv->index);
    assert(value != R2_GRID_UNKNOWN_VALUE);

    // Update grid entry
    grid->SetGridValue(gv->index, value + gv->delta);

    // Update heap deltas for entry and all neighbors
    int x, y, index;
    grid->IndexToIndices(gv->index, x, y);
    // printf("%d %d %.6f\n", x, y, gv->delta);
    for (int dy = -1; dy <= 1; dy++) {
      for (int dx = -1; dx <= 1; dx++) {
        int i = x + dx;
        if ((i < 0) || (i >= grid->XResolution())) continue;
        int j = y + dy;
        if ((j < 0) || (j >= grid->YResolution())) continue;
        grid->IndicesToIndex(i, j, index);
        if (!gvs[index]) continue;
        RNScalar current_value = grid->GridValue(i, j);
        RNScalar target_value = grid->GridValue(i, j, sigma);
        assert(current_value != R2_GRID_UNKNOWN_VALUE);
        assert(target_value != R2_GRID_UNKNOWN_VALUE);
        gvs[index]->delta = target_value - current_value;
        heap.Update(gvs[index]);
      }
    }
  }

  // Delete memory
  for (int i = 0; i < grid->NEntries(); i++) { 
    if (gvs[i]) delete gvs[i];
  }
}


void 
Poisson(R2Grid *grid, R2Grid *dx_grid, R2Grid *dy_grid)
{
  // Create system of equations
  int n = grid->NEntries();
  int nx = grid->XResolution();
  RNSystemOfEquations equations(n);
  
  // Add inertia equations
  RNScalar inertia_weight = 1.0E-3;
  for (int i = 0; i < n; i++) {
    RNScalar d = grid->GridValue(i);
    RNPolynomial *f = new RNPolynomial(1.0, i, 1.0);
    RNAlgebraic *e = new RNAlgebraic(RN_SUBTRACT_OPERATION, f, d);
    e->Multiply(inertia_weight);
    equations.InsertEquation(e);
  }

  // Add dx equations
  RNScalar dx_weight = 1.0;
  for (int iy = 1; iy < grid->YResolution()-1; iy++) {
    for (int ix = 1; ix < grid->XResolution()-1; ix++) {
      RNScalar dx = dx_grid->GridValue(ix, iy);
      RNPolynomial *e = new RNPolynomial();
      e->AddTerm(-0.25, (iy-1)*nx+(ix-1), 1.0);
      e->AddTerm(-0.50, (iy)*nx+(ix-1), 1.0);
      e->AddTerm(-0.25, (iy+1)*nx+(ix-1), 1.0);
      e->AddTerm( 0.25, (iy-1)*nx+(ix+1), 1.0);
      e->AddTerm( 0.50, (iy)*nx+(ix+1), 1.0);
      e->AddTerm( 0.25, (iy+1)*nx+(ix+1), 1.0);
      e->Subtract(dx);
      e->Multiply(dx_weight);
      equations.InsertEquation(e);
    }
  }
  
  // Add dy equations
  RNScalar dy_weight = 1.0;
  for (int iy = 1; iy < grid->YResolution()-1; iy++) {
    for (int ix = 1; ix < grid->XResolution()-1; ix++) {
      RNScalar dy = dy_grid->GridValue(ix, iy);
      RNPolynomial *e = new RNPolynomial();
      e->AddTerm(-0.25, (iy-1)*nx+(ix-1), 1.0);
      e->AddTerm(-0.50, (iy-1)*nx+(ix), 1.0);
      e->AddTerm(-0.25, (iy-1)*nx+(ix+1), 1.0);
      e->AddTerm( 0.25, (iy+1)*nx+(ix-1), 1.0);
      e->AddTerm( 0.50, (iy+1)*nx+(ix), 1.0);
      e->AddTerm( 0.25, (iy+1)*nx+(ix+1), 1.0);
      e->Subtract(dy);
      e->Multiply(dy_weight);
      equations.InsertEquation(e);
    }
  }
  
  // Initialize variables
  double *x = new double [ n ];
  for (int i = 0; i < n; i++) x[i] = grid->GridValue(i);

  // Solve system of equations
  if (equations.NEquations() >= n) {
    if (!equations.Minimize(x, RN_CSPARSE_SOLVER, 1E-3)) {
      RNFail("Unable to minimize system of equations\n");
      delete [] x;
      return;
    }
  }

  // Copy solution into grid
  for (int i = 0; i < n; i++) grid->SetGridValue(i, x[i]);

  // Delete variables
  delete [] x;
}


struct LineSegment {
  LineSegment(R2Point p1, R2Point p2, RNScalar score) : p1(p1), p2(p2), score(score) {};
  R2Point p1, p2;
  RNScalar score;
}

// Assembly:
subq $0x168, %rsp            # imm = 0x168
movq %rdi, 0x160(%rsp)
movsd %xmm0, 0x158(%rsp)
movsd %xmm1, 0x150(%rsp)
movq 0x160(%rsp), %rdi
callq 0xaf10
cltq
movl $0x8, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
callq 0x4030
movq %rax, 0x148(%rsp)
movl $0x0, 0x144(%rsp)
movl 0x144(%rsp), %eax
movl %eax, 0x68(%rsp)
movq 0x160(%rsp), %rdi
callq 0xaf10
movl %eax, %ecx
movl 0x68(%rsp), %eax
cmpl %ecx, %eax
jge 0x56d1
movq 0x148(%rsp), %rax
movslq 0x144(%rsp), %rcx
movq $0x0, (%rax,%rcx,8)
movl 0x144(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x144(%rsp)
jmp 0x5684
leaq 0x120(%rsp), %rdx
addq $0x8, %rdx
leaq 0x120(%rsp), %rcx
addq $0x10, %rcx
leaq 0xe8(%rsp), %rdi
leaq 0x120(%rsp), %rsi
movl $0x1, %r8d
callq 0xaf30
movl $0x0, 0xe4(%rsp)
movl 0xe4(%rsp), %eax
movl %eax, 0x60(%rsp)
movq 0x160(%rsp), %rdi
callq 0xaf10
movl %eax, 0x64(%rsp)
jmp 0x572d
movl 0x60(%rsp), %eax
movl 0x64(%rsp), %ecx
cmpl %ecx, %eax
jge 0x5900
movq 0x160(%rsp), %rdi
movl 0xe4(%rsp), %esi
callq 0xafd0
movsd %xmm0, 0x58(%rsp)
jmp 0x5759
movsd 0x58(%rsp), %xmm0
movsd %xmm0, 0xc8(%rsp)
movsd 0xc8(%rsp), %xmm0
movsd 0xa588f(%rip), %xmm1    # 0xab008
ucomisd %xmm1, %xmm0
jne 0x5786
jp 0x5786
jmp 0x58e8
movq 0x160(%rsp), %rdi
movl 0xe4(%rsp), %esi
leaq 0x140(%rsp), %rdx
leaq 0x13c(%rsp), %rcx
callq 0xaff0
jmp 0x57ac
movq 0x160(%rsp), %rdi
cvtsi2sdl 0x140(%rsp), %xmm0
cvtsi2sdl 0x13c(%rsp), %xmm1
movsd 0x150(%rsp), %xmm2
callq 0x19e70
movsd %xmm0, 0x50(%rsp)
jmp 0x57dc
movsd 0x50(%rsp), %xmm0
movsd %xmm0, 0xc0(%rsp)
movsd 0xc0(%rsp), %xmm0
movsd 0xa580c(%rip), %xmm1    # 0xab008
ucomisd %xmm1, %xmm0
jne 0x5809
jp 0x5809
jmp 0x58e6
movsd 0xc0(%rsp), %xmm0
movsd 0xc8(%rsp), %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0xb8(%rsp)
movl $0x18, %edi
callq 0x4280
movq %rax, 0x48(%rsp)
jmp 0x5839
movq 0x48(%rsp), %rax
xorps %xmm0, %xmm0
movaps %xmm0, (%rax)
movq $0x0, 0x10(%rax)
movq %rax, 0xb0(%rsp)
movl 0xe4(%rsp), %ecx
movq 0xb0(%rsp), %rax
movl %ecx, (%rax)
movsd 0xb8(%rsp), %xmm0
movq 0xb0(%rsp), %rax
movsd %xmm0, 0x8(%rax)
movq 0xb0(%rsp), %rax
movq $0x0, 0x10(%rax)
movq 0xb0(%rsp), %rdx
movq 0x148(%rsp), %rax
movslq 0xe4(%rsp), %rcx
movq %rdx, (%rax,%rcx,8)
movq 0xb0(%rsp), %rsi
leaq 0xe8(%rsp), %rdi
callq 0xb030
jmp 0x58be
jmp 0x58e6
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xd8(%rsp)
movl %eax, 0xd4(%rsp)
leaq 0xe8(%rsp), %rdi
callq 0xb410
jmp 0x5c92
jmp 0x58e8
jmp 0x58ea
movl 0xe4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xe4(%rsp)
jmp 0x570f
jmp 0x5902
leaq 0xe8(%rsp), %rdi
callq 0xb250
movl %eax, 0x44(%rsp)
jmp 0x5915
movl 0x44(%rsp), %eax
cmpl $0x0, %eax
setne %al
xorb $-0x1, %al
testb $0x1, %al
jne 0x592a
jmp 0x5c00
leaq 0xe8(%rsp), %rdi
callq 0xb270
movq %rax, 0x38(%rsp)
jmp 0x593e
movq 0x38(%rsp), %rax
movq %rax, 0xa8(%rsp)
movq 0xa8(%rsp), %rax
movsd 0x8(%rax), %xmm0
movsd 0x158(%rsp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x5983
jmp 0x5c00
movq 0x160(%rsp), %rdi
movq 0xa8(%rsp), %rax
movl (%rax), %esi
callq 0xafd0
movsd %xmm0, 0x30(%rsp)
jmp 0x59a2
movsd 0x30(%rsp), %xmm0
movsd %xmm0, 0xa0(%rsp)
movq 0x160(%rsp), %rdi
movq 0xa8(%rsp), %rax
movl (%rax), %esi
movsd 0xa0(%rsp), %xmm0
movsd 0x8(%rax), %xmm1
addsd %xmm1, %xmm0
callq 0xb2b0
jmp 0x59dc
movq 0x160(%rsp), %rdi
movq 0xa8(%rsp), %rax
movl (%rax), %esi
leaq 0x9c(%rsp), %rdx
leaq 0x98(%rsp), %rcx
callq 0xaff0
jmp 0x5a05
movl $0xffffffff, 0x90(%rsp) # imm = 0xFFFFFFFF
cmpl $0x1, 0x90(%rsp)
jg 0x5bfb
movl $0xffffffff, 0x8c(%rsp) # imm = 0xFFFFFFFF
cmpl $0x1, 0x8c(%rsp)
jg 0x5be3
movl 0x9c(%rsp), %eax
addl 0x8c(%rsp), %eax
movl %eax, 0x88(%rsp)
cmpl $0x0, 0x88(%rsp)
jl 0x5a80
movl 0x88(%rsp), %eax
movl %eax, 0x28(%rsp)
movq 0x160(%rsp), %rdi
callq 0xaed0
movl %eax, 0x2c(%rsp)
jmp 0x5a74
movl 0x28(%rsp), %eax
movl 0x2c(%rsp), %ecx
cmpl %ecx, %eax
jl 0x5a85
jmp 0x5bcd
movl 0x98(%rsp), %eax
addl 0x90(%rsp), %eax
movl %eax, 0x84(%rsp)
cmpl $0x0, 0x84(%rsp)
jl 0x5ace
movl 0x84(%rsp), %eax
movl %eax, 0x20(%rsp)
movq 0x160(%rsp), %rdi
callq 0xaef0
movl %eax, 0x24(%rsp)
jmp 0x5ac2
movl 0x20(%rsp), %eax
movl 0x24(%rsp), %ecx
cmpl %ecx, %eax
jl 0x5ad3
jmp 0x5bcd
movq 0x160(%rsp), %rdi
movl 0x88(%rsp), %esi
movl 0x84(%rsp), %edx
leaq 0x94(%rsp), %rcx
callq 0xb2e0
jmp 0x5af8
movq 0x148(%rsp), %rax
movslq 0x94(%rsp), %rcx
cmpq $0x0, (%rax,%rcx,8)
jne 0x5b14
jmp 0x5bcd
movq 0x160(%rsp), %rdi
movl 0x88(%rsp), %esi
movl 0x84(%rsp), %edx
callq 0xae90
movsd %xmm0, 0x18(%rsp)
jmp 0x5b37
movsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x78(%rsp)
movq 0x160(%rsp), %rdi
cvtsi2sdl 0x88(%rsp), %xmm0
cvtsi2sdl 0x84(%rsp), %xmm1
movsd 0x150(%rsp), %xmm2
callq 0x19e70
movsd %xmm0, 0x10(%rsp)
jmp 0x5b73
movsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x70(%rsp)
movsd 0x70(%rsp), %xmm0
movsd 0x78(%rsp), %xmm1
subsd %xmm1, %xmm0
movq 0x148(%rsp), %rax
movslq 0x94(%rsp), %rcx
movq (%rax,%rcx,8), %rax
movsd %xmm0, 0x8(%rax)
movq 0x148(%rsp), %rax
movslq 0x94(%rsp), %rcx
movq (%rax,%rcx,8), %rsi
leaq 0xe8(%rsp), %rdi
callq 0xb310
jmp 0x5bcb
jmp 0x5bcd
movl 0x8c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x8c(%rsp)
jmp 0x5a29
jmp 0x5be5
movl 0x90(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x90(%rsp)
jmp 0x5a10
jmp 0x5902
movl $0x0, 0x6c(%rsp)
movl 0x6c(%rsp), %eax
movl %eax, 0x8(%rsp)
movq 0x160(%rsp), %rdi
callq 0xaf10
movl %eax, 0xc(%rsp)
jmp 0x5c23
movl 0x8(%rsp), %eax
movl 0xc(%rsp), %ecx
cmpl %ecx, %eax
jge 0x5c7d
movq 0x148(%rsp), %rax
movslq 0x6c(%rsp), %rcx
cmpq $0x0, (%rax,%rcx,8)
je 0x5c6e
movq 0x148(%rsp), %rax
movslq 0x6c(%rsp), %rcx
movq (%rax,%rcx,8), %rax
movq %rax, (%rsp)
cmpq $0x0, %rax
je 0x5c6c
movq (%rsp), %rdi
movl $0x18, %esi
callq 0x4290
jmp 0x5c6e
jmp 0x5c70
movl 0x6c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x6c(%rsp)
jmp 0x5c08
leaq 0xe8(%rsp), %rdi
callq 0xb410
addq $0x168, %rsp            # imm = 0x168
retq
movq 0xd8(%rsp), %rdi
callq 0x44b0
nop