// File: tomfunkhouser[P]gaps/pkgs/R2Shapes/R2Align.cpp
// Function: gaps::R2PrincipleAxes(gaps::R2Point const&, gaps::RNArray<gaps::R2Point*> const&, double const*, double*)
R2Diad 
R2PrincipleAxes(const R2Point& centroid, const RNArray<R2Point *>& points, const RNScalar *weights, RNScalar *variances)
{
  // Compute covariance matrix
  RNScalar m[4] = { 0 };
  RNScalar total_weight = 0;
  for (int i = 0; i < points.NEntries(); i++) {
    RNScalar weight = (weights) ? weights[i] : 1;
    RNScalar x = points[i]->X() - centroid[0];
    RNScalar y = points[i]->Y() - centroid[1];
    m[0] += weight * x*x;
    m[1] += weight * x*y;
    m[2] += weight * y*x;
    m[3] += weight * y*y;
    total_weight += weight;
  }

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

  // Calculate SVD of second order moments
  RNScalar U[4];
  RNScalar W[2];
  RNScalar Vt[4];
  RNSvdDecompose(2, 2, m, U, W, Vt);  // m == U . DiagonalMatrix(W) . Vt

  // Principle axes are in Vt
  R2Vector axes[2];
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 2; j++) {
      axes[i][j] = Vt[2*i+j];
    }
  }
  
  // Normalize all axis vectors (probably not necessary)
  RNLength length0 = axes[0].Length();
  RNLength length1 = axes[1].Length();
  if (RNIsPositive(length0)) axes[0] /= length0;
  if (RNIsPositive(length1)) axes[1] /= length1;

  // Flip axes so that "heavier" on positive side
  int positive_count[2] = { 0, 0 };
  int negative_count[2] = { 0, 0 };
  for (int i = 0; i < points.NEntries(); i++) {
    R2Point *point = points[i];
    for (int j = 0; j < 2; j++) {
      RNScalar dot = axes[j].Dot(point->Vector());
      if (dot > 0.0) positive_count[j]++;
      else negative_count[j]++;
    }
  }
  for (int j =0; j < 2; j++) {
    if (positive_count[j] < negative_count[j]) {
      axes[j].Flip();
    }
  }

  // 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 diad of axes
  return R2Diad(axes[0], axes[1]);
}

// Assembly:
subq $0x1a8, %rsp            # imm = 0x1A8
movq %rdi, 0x50(%rsp)
movq %rdi, %rax
movq %rax, 0x58(%rsp)
movq %rdi, 0x1a0(%rsp)
movq %rsi, 0x198(%rsp)
movq %rdx, 0x190(%rsp)
movq %rcx, 0x188(%rsp)
movq %r8, 0x180(%rsp)
leaq 0x160(%rsp), %rdi
xorl %esi, %esi
movl $0x20, %edx
callq 0xf280
xorps %xmm0, %xmm0
movsd %xmm0, 0x158(%rsp)
movl $0x0, 0x154(%rsp)
movl 0x154(%rsp), %eax
movl %eax, 0x4c(%rsp)
movq 0x190(%rsp), %rdi
callq 0x13600
movl %eax, %ecx
movl 0x4c(%rsp), %eax
cmpl %ecx, %eax
jge 0xce30a
cmpq $0x0, 0x188(%rsp)
je 0xce155
movq 0x188(%rsp), %rax
movslq 0x154(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
movsd %xmm0, 0x40(%rsp)
jmp 0xce165
movsd 0x64eab(%rip), %xmm0    # 0x133008
movsd %xmm0, 0x40(%rsp)
jmp 0xce165
movsd 0x40(%rsp), %xmm0
movsd %xmm0, 0x148(%rsp)
movq 0x190(%rsp), %rdi
movl 0x154(%rsp), %esi
callq 0xbd1e0
movq %rax, %rdi
callq 0x38c50
movsd %xmm0, 0x30(%rsp)
movq 0x198(%rsp), %rdi
xorl %esi, %esi
callq 0xa5bf0
movaps %xmm0, %xmm1
movsd 0x30(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x140(%rsp)
movq 0x190(%rsp), %rdi
movl 0x154(%rsp), %esi
callq 0xbd1e0
movq %rax, %rdi
callq 0x38c60
movsd %xmm0, 0x38(%rsp)
movq 0x198(%rsp), %rdi
movl $0x1, %esi
callq 0xa5bf0
movaps %xmm0, %xmm1
movsd 0x38(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x138(%rsp)
movsd 0x148(%rsp), %xmm0
mulsd 0x140(%rsp), %xmm0
movsd 0x140(%rsp), %xmm2
movsd 0x160(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x160(%rsp)
movsd 0x148(%rsp), %xmm0
mulsd 0x140(%rsp), %xmm0
movsd 0x138(%rsp), %xmm2
movsd 0x168(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x168(%rsp)
movsd 0x148(%rsp), %xmm0
mulsd 0x138(%rsp), %xmm0
movsd 0x140(%rsp), %xmm2
movsd 0x170(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x170(%rsp)
movsd 0x148(%rsp), %xmm0
mulsd 0x138(%rsp), %xmm0
movsd 0x138(%rsp), %xmm2
movsd 0x178(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x178(%rsp)
movsd 0x148(%rsp), %xmm0
addsd 0x158(%rsp), %xmm0
movsd %xmm0, 0x158(%rsp)
movl 0x154(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x154(%rsp)
jmp 0xce107
movsd 0x158(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0xce334
jp 0xce334
movq 0x50(%rsp), %rdi
leaq 0x4ac796(%rip), %rsi    # 0x57aac0
callq 0xbf370
jmp 0xce719
movl $0x0, 0x134(%rsp)
cmpl $0x4, 0x134(%rsp)
jge 0xce383
movsd 0x158(%rsp), %xmm1
movslq 0x134(%rsp), %rax
movsd 0x160(%rsp,%rax,8), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0x160(%rsp,%rax,8)
movl 0x134(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x134(%rsp)
jmp 0xce33f
leaq 0x160(%rsp), %rdx
leaq 0x110(%rsp), %rcx
leaq 0x100(%rsp), %r8
leaq 0xe0(%rsp), %r9
movl $0x2, %esi
movl %esi, %edi
callq 0xd3dc0
leaq 0xc0(%rsp), %rax
movq %rax, %rcx
addq $0x20, %rcx
movq %rcx, 0x20(%rsp)
movq %rax, 0x28(%rsp)
movq 0x28(%rsp), %rdi
movq %rdi, 0x18(%rsp)
callq 0xc3c20
movq 0x18(%rsp), %rax
movq 0x20(%rsp), %rcx
addq $0x10, %rax
cmpq %rcx, %rax
movq %rax, 0x28(%rsp)
jne 0xce3c8
movl $0x0, 0xbc(%rsp)
cmpl $0x2, 0xbc(%rsp)
jge 0xce496
movl $0x0, 0xb8(%rsp)
cmpl $0x2, 0xb8(%rsp)
jge 0xce47e
movl 0xbc(%rsp), %eax
shll %eax
addl 0xb8(%rsp), %eax
cltq
movsd 0xe0(%rsp,%rax,8), %xmm0
movsd %xmm0, 0x10(%rsp)
movslq 0xbc(%rsp), %rax
leaq 0xc0(%rsp), %rdi
shlq $0x4, %rax
addq %rax, %rdi
movl 0xb8(%rsp), %esi
callq 0xa5c10
movsd 0x10(%rsp), %xmm0
movsd %xmm0, (%rax)
movl 0xb8(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xb8(%rsp)
jmp 0xce413
jmp 0xce480
movl 0xbc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xbc(%rsp)
jmp 0xce3fa
leaq 0xc0(%rsp), %rdi
callq 0xc3d60
movsd %xmm0, 0xb0(%rsp)
leaq 0xc0(%rsp), %rdi
addq $0x10, %rdi
callq 0xc3d60
movsd %xmm0, 0xa8(%rsp)
movsd 0xb0(%rsp), %xmm0
leaq 0xaa00a(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm1
callq 0x36d50
cmpl $0x0, %eax
je 0xce4fa
movsd 0xb0(%rsp), %xmm0
leaq 0xc0(%rsp), %rdi
callq 0xc4650
movsd 0xa8(%rsp), %xmm0
leaq 0xa9fd6(%rip), %rax     # 0x1784e0
movsd (%rax), %xmm1
callq 0x36d50
cmpl $0x0, %eax
je 0xce532
movsd 0xa8(%rsp), %xmm0
leaq 0xc0(%rsp), %rdi
addq $0x10, %rdi
callq 0xc4650
leaq 0xa0(%rsp), %rdi
xorl %esi, %esi
movl $0x8, %edx
callq 0xf280
leaq 0x98(%rsp), %rdi
xorl %esi, %esi
movl $0x8, %edx
callq 0xf280
movl $0x0, 0x94(%rsp)
movl 0x94(%rsp), %eax
movl %eax, 0xc(%rsp)
movq 0x190(%rsp), %rdi
callq 0x13600
movl %eax, %ecx
movl 0xc(%rsp), %eax
cmpl %ecx, %eax
jge 0xce674
movq 0x190(%rsp), %rdi
movl 0x94(%rsp), %esi
callq 0xbd1e0
movq %rax, 0x88(%rsp)
movl $0x0, 0x84(%rsp)
cmpl $0x2, 0x84(%rsp)
jge 0xce65c
movslq 0x84(%rsp), %rcx
leaq 0xc0(%rsp), %rax
shlq $0x4, %rcx
addq %rcx, %rax
movq %rax, (%rsp)
movq 0x88(%rsp), %rsi
leaq 0x68(%rsp), %rdi
callq 0xa5b80
movq (%rsp), %rdi
leaq 0x68(%rsp), %rsi
callq 0xc3f40
movsd %xmm0, 0x78(%rsp)
movsd 0x78(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0xce62b
movslq 0x84(%rsp), %rax
movl 0xa0(%rsp,%rax,4), %ecx
addl $0x1, %ecx
movl %ecx, 0xa0(%rsp,%rax,4)
jmp 0xce644
movslq 0x84(%rsp), %rax
movl 0x98(%rsp,%rax,4), %ecx
addl $0x1, %ecx
movl %ecx, 0x98(%rsp,%rax,4)
jmp 0xce646
movl 0x84(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x84(%rsp)
jmp 0xce5b2
jmp 0xce65e
movl 0x94(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x94(%rsp)
jmp 0xce565
movl $0x0, 0x64(%rsp)
cmpl $0x2, 0x64(%rsp)
jge 0xce6c5
movslq 0x64(%rsp), %rax
movl 0xa0(%rsp,%rax,4), %eax
movslq 0x64(%rsp), %rcx
cmpl 0x98(%rsp,%rcx,4), %eax
jge 0xce6b6
movslq 0x64(%rsp), %rax
leaq 0xc0(%rsp), %rdi
shlq $0x4, %rax
addq %rax, %rdi
callq 0xc40f0
jmp 0xce6b8
movl 0x64(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x64(%rsp)
jmp 0xce67c
cmpq $0x0, 0x180(%rsp)
je 0xce6fb
movsd 0x100(%rsp), %xmm0
movq 0x180(%rsp), %rax
movsd %xmm0, (%rax)
movsd 0x108(%rsp), %xmm0
movq 0x180(%rsp), %rax
movsd %xmm0, 0x8(%rax)
movq 0x50(%rsp), %rdi
leaq 0xc0(%rsp), %rsi
leaq 0xc0(%rsp), %rdx
addq $0x10, %rdx
callq 0xbf3f0
movq 0x58(%rsp), %rax
addq $0x1a8, %rsp            # imm = 0x1A8
retq
nopw %cs:(%rax,%rax)