// File: /workspace/github_cpp_makefile_star2_O1/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:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x98, %rsp
movsd %xmm1, 0x20(%rsp)
movapd %xmm0, 0x30(%rsp)
movq %rdi, %rbx
movslq 0xcc(%rdi), %r14
leaq (,%r14,8), %r15
testq %r14, %r14
movq $-0x1, %rdi
cmovnsq %r15, %rdi
callq 0x4030
movq %rax, %r13
testq %r14, %r14
jle 0x500c
movq %r13, %rdi
xorl %esi, %esi
movq %r15, %rdx
callq 0x4160
movabsq $0x1000000000, %rax     # imm = 0x1000000000
movq %rax, 0x70(%rsp)
xorpd %xmm0, %xmm0
movapd %xmm0, 0x60(%rsp)
movupd %xmm0, 0x78(%rsp)
movq $0x0, 0x88(%rsp)
movl $0x1, 0x90(%rsp)
movl $0x8, 0x70(%rsp)
cmpl $0x0, 0xcc(%rbx)
jle 0x5107
xorl %r15d, %r15d
leaq 0x60(%rsp), %r12
movq 0xb8(%rbx), %rax
movsd (%rax,%r15,8), %xmm0
movsd %xmm0, 0x10(%rsp)
ucomisd 0x53f8e(%rip), %xmm0    # 0x59008
jne 0x507e
jnp 0x50f4
movl %r15d, %eax
cltd
idivl 0xc8(%rbx)
xorps %xmm0, %xmm0
cvtsi2sd %edx, %xmm0
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
movq %rbx, %rdi
movsd 0x20(%rsp), %xmm2
callq 0xf7cc
movsd %xmm0, 0x8(%rsp)
ucomisd 0x53f56(%rip), %xmm0    # 0x59008
jne 0x50b6
jnp 0x50f4
movl $0x18, %edi
callq 0x4270
movsd 0x8(%rsp), %xmm1
subsd 0x10(%rsp), %xmm1
xorpd %xmm0, %xmm0
movapd %xmm0, (%rax)
movq $0x0, 0x10(%rax)
movl %r15d, (%rax)
movsd %xmm1, 0x8(%rax)
movq %rax, (%r13,%r15,8)
movq %r12, %rdi
movq %rax, %rsi
callq 0x7c94
incq %r15
movslq 0xcc(%rbx), %rax
cmpq %rax, %r15
jl 0x505f
movq %r13, 0x28(%rsp)
cmpl $0x0, 0x68(%rsp)
je 0x5269
movapd 0x30(%rsp), %xmm0
xorpd 0x53f3b(%rip), %xmm0    # 0x59060
movapd %xmm0, 0x30(%rsp)
movq 0x60(%rsp), %rax
movq (%rax), %rax
movsd 0x8(%rax), %xmm0
ucomisd 0x30(%rsp), %xmm0
ja 0x5269
movslq (%rax), %rax
movq 0xb8(%rbx), %rcx
movapd %xmm0, %xmm1
movsd (%rcx,%rax,8), %xmm0
movsd %xmm1, 0x40(%rsp)
addsd %xmm1, %xmm0
movsd %xmm0, (%rcx,%rax,8)
cltd
idivl 0xc8(%rbx)
movq %rax, 0x50(%rsp)
leal -0x1(%rax), %ecx
decl %edx
movslq %edx, %rax
movq %rax, 0x48(%rsp)
movl $0xffffffff, %r12d      # imm = 0xFFFFFFFF
movq 0x50(%rsp), %rax
leal (%r12,%rax), %r15d
xorps %xmm0, %xmm0
cvtsi2sd %r15d, %xmm0
movsd %xmm0, 0x58(%rsp)
movl $0x3, %r14d
movl %ecx, 0x1c(%rsp)
movslq %ecx, %rax
movq %rax, 0x8(%rsp)
movq 0x48(%rsp), %rbp
testl %ebp, %ebp
js 0x5231
cmpl 0xc0(%rbx), %ebp
jge 0x5231
testl %r15d, %r15d
js 0x5231
cmpl 0xc4(%rbx), %r15d
jge 0x5231
movslq 0xc8(%rbx), %rax
imulq 0x8(%rsp), %rax
addq %rbp, %rax
movq 0x28(%rsp), %rcx
movq (%rcx,%rax,8), %r13
testq %r13, %r13
je 0x5231
movq 0xb8(%rbx), %rcx
movsd (%rcx,%rax,8), %xmm0
movsd %xmm0, 0x10(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %ebp, %xmm0
movq %rbx, %rdi
movsd 0x58(%rsp), %xmm1
movsd 0x20(%rsp), %xmm2
callq 0xf7cc
subsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x8(%r13)
leaq 0x60(%rsp), %rdi
movq %r13, %rsi
callq 0x7d90
incq %rbp
decl %r14d
jne 0x51b3
incl %r12d
movl 0x1c(%rsp), %ecx
incl %ecx
cmpl $0x2, %r12d
jne 0x5185
movsd 0x40(%rsp), %xmm0
ucomisd 0x30(%rsp), %xmm0
ja 0x5269
cmpl $0x0, 0x68(%rsp)
jne 0x512b
cmpl $0x0, 0xcc(%rbx)
movq 0x28(%rsp), %r15
jle 0x529c
xorl %r14d, %r14d
movq (%r15,%r14,8), %rdi
testq %rdi, %rdi
je 0x528d
movl $0x18, %esi
callq 0x4280
incq %r14
movslq 0xcc(%rbx), %rax
cmpq %rax, %r14
jl 0x527a
movq 0x60(%rsp), %rdi
testq %rdi, %rdi
je 0x52ab
callq 0x4350
addq $0x98, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x52c1
jmp 0x52c1
movq %rax, %rbx
movq 0x60(%rsp), %rdi
testq %rdi, %rdi
je 0x52d3
callq 0x4350
movq %rbx, %rdi
callq 0x44b0