// File: /workspace/github_cpp_makefile_star2_O3/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:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x128, %rsp            # imm = 0x128
movq %rcx, 0x58(%rsp)
movq %rsi, %r15
movq %rdi, %r13
leaq 0x40(%rsp), %rdi
testq %rdx, %rdx
je 0x59f1e
movq %rdx, %rsi
callq 0x680b0
jmp 0x59f26
movq %r15, %rsi
callq 0x59df6
xorpd %xmm0, %xmm0
movapd %xmm0, 0x70(%rsp)
movapd %xmm0, 0x60(%rsp)
movl 0xc4(%r15), %ecx
testl %ecx, %ecx
jle 0x5a03c
movq 0xb8(%r15), %rbx
movl 0xc0(%r15), %eax
xorl %ebp, %ebp
xorpd %xmm3, %xmm3
leaq 0x80(%rsp), %r14
xorpd %xmm1, %xmm1
testl %eax, %eax
jle 0x5a026
xorps %xmm2, %xmm2
cvtsi2sd %ebp, %xmm2
movsd %xmm2, (%rsp)
xorl %r12d, %r12d
movapd %xmm1, 0x10(%rsp)
movapd %xmm0, 0x30(%rsp)
movapd %xmm3, 0x20(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %r12d, %xmm0
movq %r14, %rdi
movsd (%rsp), %xmm1
callq 0x680a6
movapd 0x20(%rsp), %xmm3
movsd (%rbx), %xmm0
movapd 0x80(%rsp), %xmm1
subpd 0x40(%rsp), %xmm1
addq $0x8, %rbx
addsd %xmm0, %xmm3
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
mulpd %xmm1, %xmm0
movapd %xmm0, %xmm2
unpcklpd %xmm0, %xmm2            # xmm2 = xmm2[0],xmm0[0]
mulpd %xmm1, %xmm2
unpckhpd %xmm1, %xmm1            # xmm1 = xmm1[1,1]
mulpd %xmm0, %xmm1
movapd 0x30(%rsp), %xmm0
movapd 0x10(%rsp), %xmm4
addpd %xmm1, %xmm4
movapd %xmm4, 0x10(%rsp)
movapd 0x10(%rsp), %xmm1
addpd %xmm2, %xmm0
movapd %xmm0, 0x60(%rsp)
movapd %xmm1, 0x70(%rsp)
incl %r12d
movl 0xc0(%r15), %eax
cmpl %eax, %r12d
jl 0x59f7c
movl 0xc4(%r15), %ecx
incl %ebp
cmpl %ecx, %ebp
jl 0x59f65
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm3
jne 0x5a050
jp 0x5a050
leaq 0x48df6d(%rip), %rsi    # 0x4e7fb0
movq %r13, %rdi
callq 0x65f9e
jmp 0x5a267
unpcklpd %xmm3, %xmm3            # xmm3 = xmm3[0,0]
xorl %eax, %eax
movapd 0x60(%rsp,%rax,8), %xmm0
divpd %xmm3, %xmm0
movapd %xmm0, 0x60(%rsp,%rax,8)
addq $0x2, %rax
cmpq $0x4, %rax
jne 0x5a056
leaq 0x60(%rsp), %rdx
leaq 0x100(%rsp), %rcx
leaq 0xb0(%rsp), %r8
leaq 0xc0(%rsp), %r9
movl $0x2, %edi
movl $0x2, %esi
callq 0x710b6
xorl %ebx, %ebx
leaq (%rsp,%rbx), %rdi
addq $0x80, %rdi
callq 0x683d0
addq $0x10, %rbx
cmpq $0x30, %rbx
jne 0x5a09e
movapd 0xc0(%rsp), %xmm0
movapd 0xd0(%rsp), %xmm1
movapd %xmm0, 0x80(%rsp)
movapd %xmm1, 0x90(%rsp)
movl 0xc4(%r15), %ecx
testl %ecx, %ecx
jle 0x5a212
movq %r13, 0x50(%rsp)
movq 0xb8(%r15), %rbx
movl 0xc0(%r15), %eax
xorl %edx, %edx
leaq 0xf0(%rsp), %r12
leaq 0xe0(%rsp), %r13
leaq 0x80(%rsp), %r14
xorpd %xmm0, %xmm0
movsd %xmm0, (%rsp)
xorpd %xmm1, %xmm1
testl %eax, %eax
jle 0x5a1eb
movl %edx, 0xc(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %edx, %xmm0
movsd %xmm0, 0x30(%rsp)
xorl %ebp, %ebp
movapd %xmm1, 0x10(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %ebp, %xmm0
movq %r12, %rdi
movsd 0x30(%rsp), %xmm1
callq 0x680a6
movsd (%rbx), %xmm0
movapd %xmm0, 0x20(%rsp)
movq %r13, %rdi
movq %r12, %rsi
leaq 0x40(%rsp), %rdx
callq 0x682e3
movq %r14, %rdi
movq %r13, %rsi
callq 0x68540
ucomisd 0x48060(%rip), %xmm0    # 0xa21e8
ja 0x5a1a6
movsd (%rsp), %xmm1
movapd 0x20(%rsp), %xmm3
addsd %xmm3, %xmm1
movsd %xmm1, (%rsp)
movapd 0x10(%rsp), %xmm2
jmp 0x5a1b2
movapd 0x10(%rsp), %xmm2
movapd 0x20(%rsp), %xmm3
addq $0x8, %rbx
addsd %xmm2, %xmm3
xorpd %xmm1, %xmm1
cmpltsd %xmm0, %xmm1
andpd %xmm1, %xmm3
andnpd %xmm2, %xmm1
orpd %xmm3, %xmm1
incl %ebp
movl 0xc0(%r15), %eax
cmpl %eax, %ebp
jl 0x5a140
movl 0xc4(%r15), %ecx
movl 0xc(%rsp), %edx
incl %edx
cmpl %ecx, %edx
jl 0x5a125
movsd (%rsp), %xmm0
ucomisd %xmm1, %xmm0
movq 0x50(%rsp), %r13
jbe 0x5a212
leaq 0x80(%rsp), %rdi
callq 0x685fe
leaq 0x90(%rsp), %r14
leaq 0x80(%rsp), %rsi
movq %r14, %rdi
callq 0x68720
leaq 0x4fda7(%rip), %rax     # 0xa9fd8
movsd (%rax), %xmm0
movq %r14, %rdi
callq 0x6861e
movq 0x58(%rsp), %rax
testq %rax, %rax
je 0x5a254
movapd 0xb0(%rsp), %xmm0
movupd %xmm0, (%rax)
leaq 0x80(%rsp), %rsi
movq %r13, %rdi
movq %r14, %rdx
callq 0x65fe2
movq %r13, %rax
addq $0x128, %rsp            # imm = 0x128
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq