// File: /workspace/github_cpp_makefile_star2_O2/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:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xe8, %rsp
movq %r8, %r14
movq %rdx, %r15
movq %rdi, %rbx
xorpd %xmm1, %xmm1
movapd %xmm1, 0x70(%rsp)
movapd %xmm1, 0x60(%rsp)
movl 0xc(%rdx), %eax
movupd (%rsi), %xmm2
xorl %edx, %edx
testl %eax, %eax
cmovlel %edx, %eax
movq (%r15), %rsi
xorpd %xmm0, %xmm0
movsd 0x2ea02(%rip), %xmm3    # 0x93008
xorpd %xmm4, %xmm4
cmpq %rdx, %rax
je 0x6466a
movapd %xmm3, %xmm5
testq %rcx, %rcx
je 0x6461d
movsd (%rcx,%rdx,8), %xmm5
movq (%rsi,%rdx,8), %rdi
movupd (%rdi), %xmm6
subpd %xmm2, %xmm6
movapd %xmm5, %xmm7
mulsd %xmm6, %xmm7
unpcklpd %xmm7, %xmm7            # xmm7 = xmm7[0,0]
mulpd %xmm6, %xmm7
addpd %xmm7, %xmm1
movapd %xmm1, 0x60(%rsp)
movapd %xmm6, %xmm7
unpckhpd %xmm6, %xmm7            # xmm7 = xmm7[1],xmm6[1]
mulsd %xmm5, %xmm7
unpcklpd %xmm7, %xmm7            # xmm7 = xmm7[0,0]
mulpd %xmm6, %xmm7
addpd %xmm7, %xmm4
movapd %xmm4, 0x70(%rsp)
addsd %xmm5, %xmm0
incq %rdx
jmp 0x6460a
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x6468a
jp 0x6468a
leaq 0x478943(%rip), %rsi    # 0x4dcfc0
movq %rbx, %rdi
callq 0x5d660
jmp 0x6486d
xorl %eax, %eax
cmpq $0x4, %rax
je 0x646a7
movsd 0x60(%rsp,%rax,8), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x60(%rsp,%rax,8)
incq %rax
jmp 0x6468c
movq %rbx, 0x40(%rsp)
pushq $0x2
popq %rdi
leaq 0x60(%rsp), %rdx
leaq 0xc0(%rsp), %rcx
leaq 0x80(%rsp), %r8
leaq 0xa0(%rsp), %r12
movl %edi, %esi
movq %r12, %r9
callq 0x678f6
xorl %ebx, %ebx
leaq (%rsp,%rbx), %rdi
addq $0x10, %rdi
callq 0x5f8b2
addq $0x10, %rbx
cmpq $0x20, %rbx
jne 0x646d8
leaq 0x10(%rsp), %rax
xorl %ecx, %ecx
cmpq $0x2, %rcx
je 0x64721
xorl %edx, %edx
cmpq $0x2, %rdx
je 0x64714
movsd (%r12,%rdx,8), %xmm0
movsd %xmm0, (%rax,%rdx,8)
incq %rdx
jmp 0x646fe
incq %rcx
addq $0x10, %r12
addq $0x10, %rax
jmp 0x646f6
leaq 0x10(%rsp), %rdi
callq 0x5f958
movsd %xmm0, 0x8(%rsp)
leaq 0x20(%rsp), %r12
movq %r12, %rdi
callq 0x5f958
movsd 0x8(%rsp), %xmm2
movapd %xmm0, %xmm1
leaq 0x76459(%rip), %rbx     # 0xdaba8
movsd (%rbx), %xmm0
ucomisd %xmm0, %xmm2
jbe 0x64777
leaq 0x10(%rsp), %rdi
movapd %xmm2, %xmm0
movsd %xmm1, 0x8(%rsp)
callq 0x5fd1c
movsd 0x8(%rsp), %xmm1
movsd (%rbx), %xmm0
movq %r14, 0x38(%rsp)
ucomisd %xmm0, %xmm1
jbe 0x6478e
movq %r12, %rdi
movapd %xmm1, %xmm0
callq 0x5fd1c
leaq 0x58(%rsp), %rbx
andq $0x0, (%rbx)
leaq 0x50(%rsp), %r14
andq $0x0, (%r14)
xorl %ecx, %ecx
leaq 0x90(%rsp), %rbp
movq %r15, 0x48(%rsp)
movslq 0xc(%r15), %rax
cmpq %rax, %rcx
jge 0x64814
movq (%r15), %rax
movq %rcx, 0x8(%rsp)
movq (%rax,%rcx,8), %r12
leaq 0x10(%rsp), %r13
xorl %r15d, %r15d
cmpq $0x2, %r15
je 0x64805
movq %rbp, %rdi
movq %r12, %rsi
callq 0x51538
movq %r13, %rdi
movq %rbp, %rsi
callq 0x5fa20
ucomisd 0x2e9f8(%rip), %xmm0    # 0x931e8
movq %rbx, %rax
ja 0x647f8
movq %r14, %rax
incl (%rax,%r15,4)
incq %r15
addq $0x10, %r13
jmp 0x647cc
movq 0x8(%rsp), %rcx
incq %rcx
movq 0x48(%rsp), %r15
jmp 0x647af
leaq 0x10(%rsp), %r14
xorl %ebx, %ebx
leaq 0x20(%rsp), %r15
cmpq $0x2, %rbx
je 0x64841
movl 0x58(%rsp,%rbx,4), %eax
cmpl 0x50(%rsp,%rbx,4), %eax
jge 0x64838
movq %r14, %rdi
callq 0x5fada
incq %rbx
addq $0x10, %r14
jmp 0x64820
movq 0x38(%rsp), %rax
testq %rax, %rax
je 0x64858
movapd 0x80(%rsp), %xmm0
movupd %xmm0, (%rax)
leaq 0x10(%rsp), %rsi
movq 0x40(%rsp), %rbx
movq %rbx, %rdi
movq %r15, %rdx
callq 0x5d6a4
movq %rbx, %rax
addq $0xe8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq