// File: tomfunkhouser[P]gaps/pkgs/R2Shapes/R2Grid.cpp
// Function: gaps::R2Grid::GridPrincipleAxes(gaps::R2Point const*, double*) const
R2Diad R2Grid::
GridPrincipleAxes(const R2Point *grid_centroid, RNScalar *variances) const
{
  // Get centroid
  R2Point center = (grid_centroid) ? *grid_centroid : GridCentroid();

  // Compute covariance matrix
  RNScalar m[4] = { 0, 0, 0, 0 };
  RNScalar total_value = 0;
  RNScalar *grid_valuesp = grid_values;
  for (int j = 0; j < grid_resolution[1]; j++) {
    for (int i = 0; i < grid_resolution[0]; i++) {
      R2Point position(i, j);
      RNScalar value = *(grid_valuesp++);
      RNScalar x = position[0] - center[0];
      RNScalar y = position[1] - center[1];
      m[0] += value * x*x;
      m[1] += value * x*y;
      m[2] += value * x*y;
      m[3] += value * y*y;
      total_value += value;
    }
  }

  // Normalize covariance matrix
  if (total_value == 0) return R2xy_diad;
  for (int i = 0; i < 4; i++) m[i] /= total_value;

  // Compute eigenvalues and eigenvectors
  RNScalar U[4];
  RNScalar W[2];
  RNScalar Vt[4];
  RNSvdDecompose(2, 2, m, U, W, Vt);  // m == U . DiagonalMatrix(W) . Vt

  // Copy principle axes into more convenient form
  // W has eigenvalues (greatest to smallest) and Vt has eigenvectors (normalized)
  R2Vector axes[3];
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 2; j++) {
      axes[i][j] = Vt[2*i+j];
    }
  }

  // Find heavier side of first axis
  grid_valuesp = grid_values;
  RNScalar positive_count = 0;
  RNScalar negative_count = 0;
  for (int j = 0; j < grid_resolution[1]; j++) {
    for (int i = 0; i < grid_resolution[0]; i++) {
      R2Point position(i, j);
      RNScalar value = *(grid_valuesp++);
      R2Vector vector = position - center;
      RNScalar dot = axes[0].Dot(vector);
      if (dot > 0.0) positive_count += value;
      else negative_count += value;
    }
  }

  // Set second axis to form orthonormal triad with first one
  if (positive_count < negative_count) axes[0].Flip();
  axes[1] = axes[0]; axes[1].Rotate(RN_PI_OVER_TWO);

  // Just checking
  assert(RNIsEqual(axes[0].Length(), 1.0, RN_BIG_EPSILON));
  assert(RNIsEqual(axes[1].Length(), 1.0, RN_BIG_EPSILON));
  assert(RNIsZero(axes[0].Dot(axes[1]), RN_BIG_EPSILON));

  // Return variances (eigenvalues)
  if (variances) {
    variances[0] = W[0];
    variances[1] = W[1];
  }

  // Return triad
  return R2Diad(axes[0], axes[1]);
}

// Assembly:
subq $0x1c8, %rsp            # imm = 0x1C8
movq %rdi, 0x38(%rsp)
movq %rdi, %rax
movq %rax, 0x40(%rsp)
movq %rdi, 0x1c0(%rsp)
movq %rsi, 0x1b8(%rsp)
movq %rdx, 0x1b0(%rsp)
movq %rcx, 0x1a8(%rsp)
movq 0x1b8(%rsp), %rax
movq %rax, 0x48(%rsp)
cmpq $0x0, 0x1b0(%rsp)
je 0xa7de3
movq 0x1b0(%rsp), %rsi
leaq 0x198(%rsp), %rdi
callq 0xc3170
jmp 0xa7df5
movq 0x48(%rsp), %rsi
leaq 0x198(%rsp), %rdi
callq 0xa7c50
leaq 0x170(%rsp), %rdi
xorl %esi, %esi
movl $0x20, %edx
callq 0xf280
movq 0x48(%rsp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, 0x168(%rsp)
movq 0xb8(%rax), %rax
movq %rax, 0x160(%rsp)
movl $0x0, 0x15c(%rsp)
movq 0x48(%rsp), %rcx
movl 0x15c(%rsp), %eax
cmpl 0xc4(%rcx), %eax
jge 0xa804b
movl $0x0, 0x158(%rsp)
movq 0x48(%rsp), %rcx
movl 0x158(%rsp), %eax
cmpl 0xc0(%rcx), %eax
jge 0xa8033
cvtsi2sdl 0x158(%rsp), %xmm0
cvtsi2sdl 0x15c(%rsp), %xmm1
leaq 0x148(%rsp), %rdi
callq 0xc3140
movq 0x160(%rsp), %rax
movq %rax, %rcx
addq $0x8, %rcx
movq %rcx, 0x160(%rsp)
movsd (%rax), %xmm0
movsd %xmm0, 0x140(%rsp)
leaq 0x148(%rsp), %rdi
xorl %esi, %esi
callq 0xa5b60
movsd (%rax), %xmm0
movsd %xmm0, 0x28(%rsp)
leaq 0x198(%rsp), %rdi
xorl %esi, %esi
callq 0xa5b60
movsd 0x28(%rsp), %xmm0
subsd (%rax), %xmm0
movsd %xmm0, 0x138(%rsp)
leaq 0x148(%rsp), %rdi
movl $0x1, %esi
callq 0xa5b60
movsd (%rax), %xmm0
movsd %xmm0, 0x30(%rsp)
leaq 0x198(%rsp), %rdi
movl $0x1, %esi
callq 0xa5b60
movsd 0x30(%rsp), %xmm0
subsd (%rax), %xmm0
movsd %xmm0, 0x130(%rsp)
movsd 0x140(%rsp), %xmm0
mulsd 0x138(%rsp), %xmm0
movsd 0x138(%rsp), %xmm2
movsd 0x170(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x170(%rsp)
movsd 0x140(%rsp), %xmm0
mulsd 0x138(%rsp), %xmm0
movsd 0x130(%rsp), %xmm2
movsd 0x178(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x178(%rsp)
movsd 0x140(%rsp), %xmm0
mulsd 0x138(%rsp), %xmm0
movsd 0x130(%rsp), %xmm2
movsd 0x180(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x180(%rsp)
movsd 0x140(%rsp), %xmm0
mulsd 0x130(%rsp), %xmm0
movsd 0x130(%rsp), %xmm2
movsd 0x188(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x188(%rsp)
movsd 0x140(%rsp), %xmm0
addsd 0x168(%rsp), %xmm0
movsd %xmm0, 0x168(%rsp)
movl 0x158(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x158(%rsp)
jmp 0xa7e57
jmp 0xa8035
movl 0x15c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x15c(%rsp)
jmp 0xa7e34
movsd 0x168(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0xa8075
jp 0xa8075
movq 0x38(%rsp), %rdi
leaq 0x4d2a55(%rip), %rsi    # 0x57aac0
callq 0xbf370
jmp 0xa83d4
movl $0x0, 0x12c(%rsp)
cmpl $0x4, 0x12c(%rsp)
jge 0xa80c4
movsd 0x168(%rsp), %xmm1
movslq 0x12c(%rsp), %rax
movsd 0x170(%rsp,%rax,8), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0x170(%rsp,%rax,8)
movl 0x12c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x12c(%rsp)
jmp 0xa8080
leaq 0x170(%rsp), %rdx
leaq 0x100(%rsp), %rcx
leaq 0xf0(%rsp), %r8
leaq 0xd0(%rsp), %r9
movl $0x2, %esi
movl %esi, %edi
callq 0xd3dc0
leaq 0xa0(%rsp), %rax
movq %rax, %rcx
addq $0x30, %rcx
movq %rcx, 0x18(%rsp)
movq %rax, 0x20(%rsp)
movq 0x20(%rsp), %rdi
movq %rdi, 0x10(%rsp)
callq 0xc3c20
movq 0x10(%rsp), %rax
movq 0x18(%rsp), %rcx
addq $0x10, %rax
cmpq %rcx, %rax
movq %rax, 0x20(%rsp)
jne 0xa8109
movl $0x0, 0x9c(%rsp)
cmpl $0x2, 0x9c(%rsp)
jge 0xa81d7
movl $0x0, 0x98(%rsp)
cmpl $0x2, 0x98(%rsp)
jge 0xa81bf
movl 0x9c(%rsp), %eax
shll %eax
addl 0x98(%rsp), %eax
cltq
movsd 0xd0(%rsp,%rax,8), %xmm0
movsd %xmm0, 0x8(%rsp)
movslq 0x9c(%rsp), %rax
leaq 0xa0(%rsp), %rdi
shlq $0x4, %rax
addq %rax, %rdi
movl 0x98(%rsp), %esi
callq 0xa5c10
movsd 0x8(%rsp), %xmm0
movsd %xmm0, (%rax)
movl 0x98(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x98(%rsp)
jmp 0xa8154
jmp 0xa81c1
movl 0x9c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x9c(%rsp)
jmp 0xa813b
movq 0x48(%rsp), %rax
movq 0xb8(%rax), %rax
movq %rax, 0x160(%rsp)
xorps %xmm0, %xmm0
movsd %xmm0, 0x90(%rsp)
xorps %xmm0, %xmm0
movsd %xmm0, 0x88(%rsp)
movl $0x0, 0x84(%rsp)
movq 0x48(%rsp), %rcx
movl 0x84(%rsp), %eax
cmpl 0xc4(%rcx), %eax
jge 0xa8326
movl $0x0, 0x80(%rsp)
movq 0x48(%rsp), %rcx
movl 0x80(%rsp), %eax
cmpl 0xc0(%rcx), %eax
jge 0xa830e
cvtsi2sdl 0x80(%rsp), %xmm0
cvtsi2sdl 0x84(%rsp), %xmm1
leaq 0x70(%rsp), %rdi
callq 0xc3140
movq 0x160(%rsp), %rax
movq %rax, %rcx
addq $0x8, %rcx
movq %rcx, 0x160(%rsp)
movsd (%rax), %xmm0
movsd %xmm0, 0x68(%rsp)
leaq 0x58(%rsp), %rdi
leaq 0x70(%rsp), %rsi
leaq 0x198(%rsp), %rdx
callq 0xc3830
leaq 0xa0(%rsp), %rdi
leaq 0x58(%rsp), %rsi
callq 0xc3f40
movsd %xmm0, 0x50(%rsp)
movsd 0x50(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0xa82de
movsd 0x68(%rsp), %xmm0
addsd 0x90(%rsp), %xmm0
movsd %xmm0, 0x90(%rsp)
jmp 0xa82f6
movsd 0x68(%rsp), %xmm0
addsd 0x88(%rsp), %xmm0
movsd %xmm0, 0x88(%rsp)
jmp 0xa82f8
movl 0x80(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x80(%rsp)
jmp 0xa8231
jmp 0xa8310
movl 0x84(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x84(%rsp)
jmp 0xa820e
movsd 0x90(%rsp), %xmm1
movsd 0x88(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0xa834b
leaq 0xa0(%rsp), %rdi
callq 0xc40f0
leaq 0xa0(%rsp), %rsi
leaq 0xa0(%rsp), %rdi
addq $0x10, %rdi
callq 0xc4350
leaq 0xa0(%rsp), %rdi
addq $0x10, %rdi
leaq 0x8e571(%rip), %rax     # 0x1368e8
movsd (%rax), %xmm0
callq 0xc4170
cmpq $0x0, 0x1a8(%rsp)
je 0xa83b6
movsd 0xf0(%rsp), %xmm0
movq 0x1a8(%rsp), %rax
movsd %xmm0, (%rax)
movsd 0xf8(%rsp), %xmm0
movq 0x1a8(%rsp), %rax
movsd %xmm0, 0x8(%rax)
movq 0x38(%rsp), %rdi
leaq 0xa0(%rsp), %rsi
leaq 0xa0(%rsp), %rdx
addq $0x10, %rdx
callq 0xbf3f0
movq 0x40(%rsp), %rax
addq $0x1c8, %rsp            # imm = 0x1C8
retq
nopw %cs:(%rax,%rax)