// File: /workspace/github_cpp_makefile_star2_O2/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 $0x88, %rsp
movsd %xmm1, 0x28(%rsp)
movapd %xmm0, 0x30(%rsp)
movq %rdi, %rbx
movslq 0xcc(%rdi), %r14
leaq (,%r14,8), %rax
testq %r14, %r14
pushq $-0x1
popq %rdi
cmovnsq %rax, %rdi
callq 0x4030
movq %rax, 0x8(%rsp)
xorl %eax, %eax
testl %r14d, %r14d
movl $0x0, %ecx
cmovgl %r14d, %ecx
movq 0x8(%rsp), %rdx
cmpq %rax, %rcx
je 0x4e70
andq $0x0, (%rdx,%rax,8)
incq %rax
jmp 0x4e61
xorpd %xmm0, %xmm0
leaq 0x50(%rsp), %r15
movapd %xmm0, (%r15)
andq $0x0, 0x28(%r15)
movupd %xmm0, 0x18(%r15)
movl $0x1, 0x30(%r15)
movabsq $0x1000000008, %rax     # imm = 0x1000000008
movq %rax, 0x10(%r15)
xorl %r12d, %r12d
pushq $0x18
popq %r13
movslq %r14d, %rax
cmpq %rax, %r12
jge 0x4f4f
movq 0xb8(%rbx), %rax
movsd (%rax,%r12,8), %xmm0
movsd %xmm0, 0x18(%rsp)
ucomisd 0x4c13b(%rip), %xmm0    # 0x51008
jne 0x4ed1
jnp 0x4f40
movl %r12d, %eax
cltd
idivl 0xc8(%rbx)
cvtsi2sd %edx, %xmm0
cvtsi2sd %eax, %xmm1
movq %rbx, %rdi
movsd 0x28(%rsp), %xmm2
callq 0xe94c
movsd %xmm0, 0x10(%rsp)
ucomisd 0x4c109(%rip), %xmm0    # 0x51008
jne 0x4f03
jnp 0x4f40
movq %r13, %rdi
callq 0x4280
movsd 0x10(%rsp), %xmm1
subsd 0x18(%rsp), %xmm1
xorpd %xmm0, %xmm0
movapd %xmm0, (%rax)
movl %r12d, (%rax)
movsd %xmm1, 0x8(%rax)
andq $0x0, 0x10(%rax)
movq 0x8(%rsp), %rcx
movq %rax, (%rcx,%r12,8)
movq %r15, %rdi
movq %rax, %rsi
callq 0x77ae
incq %r12
movl 0xcc(%rbx), %r14d
jmp 0x4ea6
movapd 0x30(%rsp), %xmm0
xorpd 0x4c103(%rip), %xmm0    # 0x51060
movapd %xmm0, 0x30(%rsp)
cmpl $0x0, 0x58(%rsp)
je 0x507f
movq 0x50(%rsp), %rax
movq (%rax), %rax
movsd 0x8(%rax), %xmm0
ucomisd 0x30(%rsp), %xmm0
ja 0x507f
movslq (%rax), %rax
movq 0xb8(%rbx), %rcx
addsd (%rcx,%rax,8), %xmm0
movsd %xmm0, (%rcx,%rax,8)
cltd
idivl 0xc8(%rbx)
movq %rax, 0x40(%rsp)
leal -0x1(%rax), %r12d
decl %edx
movl %edx, 0x24(%rsp)
pushq $-0x1
popq %rax
movl %eax, %ecx
cmpl $0x2, %ecx
je 0x4f63
movq 0x40(%rsp), %rax
movq %rcx, 0x48(%rsp)
leal (%rcx,%rax), %r15d
cvtsi2sd %r15d, %xmm0
movsd %xmm0, 0x10(%rsp)
movl 0x24(%rsp), %r14d
pushq $0x3
popq %rbp
subl $0x1, %ebp
jb 0x5070
testl %r14d, %r14d
js 0x5068
testl %r15d, %r15d
sets %al
cmpl 0xc0(%rbx), %r14d
setge %cl
orb %al, %cl
jne 0x5068
cmpl 0xc4(%rbx), %r15d
jge 0x5068
movl 0xc8(%rbx), %eax
imull %r12d, %eax
addl %r14d, %eax
cltq
movq 0x8(%rsp), %rcx
movq (%rcx,%rax,8), %r13
testq %r13, %r13
je 0x5068
movq 0xb8(%rbx), %rcx
movsd (%rcx,%rax,8), %xmm0
movsd %xmm0, 0x18(%rsp)
cvtsi2sd %r14d, %xmm0
movq %rbx, %rdi
movsd 0x10(%rsp), %xmm1
movsd 0x28(%rsp), %xmm2
callq 0xe94c
subsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x8(%r13)
leaq 0x50(%rsp), %rdi
movq %r13, %rsi
callq 0x78b6
incl %r14d
jmp 0x4fdc
movq 0x48(%rsp), %rcx
incl %ecx
incl %r12d
jmp 0x4fb6
xorl %r14d, %r14d
pushq $0x18
popq %r15
movslq 0xcc(%rbx), %rax
cmpq %rax, %r14
jge 0x50ad
movq 0x8(%rsp), %rax
movq (%rax,%r14,8), %rdi
testq %rdi, %rdi
je 0x50a8
movq %r15, %rsi
callq 0x4290
incq %r14
jmp 0x5086
leaq 0x50(%rsp), %rdi
callq 0x7930
addq $0x88, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x50cd
jmp 0x50cd
movq %rax, %rbx
leaq 0x50(%rsp), %rdi
callq 0x7930
movq %rbx, %rdi
callq 0x44d0