// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/R3Shapes/R3Align.cpp
// Function: gaps::R3PrincipleAxes(gaps::R3Point const&, gaps::RNArray<gaps::R3Point*> const&, double const*, double*)
R3Triad 
R3PrincipleAxes(const R3Point& centroid, const RNArray<R3Point *>& points, const RNScalar *weights, RNScalar *variances)
{
  // Compute covariance matrix
  RNScalar m[9] = { 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];
    RNScalar z = points[i]->Z() - centroid[2];
    m[0] += weight * x*x;
    m[1] += weight * x*y;
    m[2] += weight * x*z;
    m[3] += weight * y*x;
    m[4] += weight * y*y;
    m[5] += weight * y*z;
    m[6] += weight * z*x;
    m[7] += weight * z*y;
    m[8] += weight * z*z;
    total_weight += weight;
  }

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

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

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

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

  // Compute orthonormal triad of axes
  axes[2] = axes[0] % axes[1];

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

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

  // Return triad of axes
  return R3Triad(axes[0], axes[1], axes[2]);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1b8, %rsp            # imm = 0x1B8
movq %rdx, %r14
movq %rdi, %rbx
xorpd %xmm1, %xmm1
movapd %xmm1, 0x60(%rsp)
movapd %xmm1, 0x50(%rsp)
andq $0x0, 0x70(%rsp)
movapd %xmm1, 0x40(%rsp)
movapd %xmm1, 0x30(%rsp)
movl 0xc(%rdx), %eax
movq (%rdx), %rdx
movupd (%rsi), %xmm2
movsd 0x10(%rsi), %xmm3
xorl %esi, %esi
testl %eax, %eax
cmovlel %esi, %eax
xorpd %xmm4, %xmm4
movsd 0x4d47e(%rip), %xmm5    # 0x93008
xorpd %xmm0, %xmm0
xorpd %xmm6, %xmm6
xorpd %xmm7, %xmm7
xorpd %xmm8, %xmm8
cmpq %rsi, %rax
je 0x45c68
movapd %xmm5, %xmm9
testq %rcx, %rcx
je 0x45bb4
movsd (%rcx,%rsi,8), %xmm9
movq (%rdx,%rsi,8), %rdi
movsd 0x10(%rdi), %xmm12
subsd %xmm3, %xmm12
movupd (%rdi), %xmm10
subpd %xmm2, %xmm10
movapd %xmm9, %xmm11
unpcklpd %xmm9, %xmm11           # xmm11 = xmm11[0],xmm9[0]
mulpd %xmm10, %xmm11
movapd %xmm11, %xmm13
unpcklpd %xmm11, %xmm13          # xmm13 = xmm13[0],xmm11[0]
mulpd %xmm10, %xmm13
addpd %xmm13, %xmm1
movapd %xmm10, %xmm13
shufpd $0x1, %xmm12, %xmm13    # xmm13 = xmm13[1],xmm12[0]
addsd %xmm9, %xmm0
mulsd %xmm12, %xmm9
movapd %xmm9, %xmm14
mulsd %xmm12, %xmm9
unpcklpd %xmm10, %xmm12          # xmm12 = xmm12[0],xmm10[0]
mulpd %xmm11, %xmm12
addpd %xmm12, %xmm6
movapd %xmm1, 0x30(%rsp)
movapd %xmm6, 0x40(%rsp)
unpckhpd %xmm11, %xmm11          # xmm11 = xmm11[1,1]
mulpd %xmm11, %xmm13
addpd %xmm13, %xmm7
movapd %xmm7, 0x50(%rsp)
unpcklpd %xmm14, %xmm14          # xmm14 = xmm14[0,0]
mulpd %xmm10, %xmm14
addpd %xmm14, %xmm8
movapd %xmm8, 0x60(%rsp)
addsd %xmm9, %xmm4
movsd %xmm4, 0x70(%rsp)
incq %rsi
jmp 0x45b9b
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x45c88
jp 0x45c88
leaq 0x9554d(%rip), %rsi     # 0xdb1c8
movq %rbx, %rdi
callq 0x28198
jmp 0x45efb
xorl %eax, %eax
cmpq $0x9, %rax
je 0x45ca5
movsd 0x30(%rsp,%rax,8), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x30(%rsp,%rax,8)
incq %rax
jmp 0x45c8a
movq %r8, 0x18(%rsp)
movq %rbx, 0x20(%rsp)
pushq $0x3
popq %rdi
leaq 0x30(%rsp), %rdx
leaq 0x170(%rsp), %rcx
leaq 0xf0(%rsp), %r8
leaq 0x120(%rsp), %r12
movl %edi, %esi
movq %r12, %r9
callq 0x678f6
xorl %ebx, %ebx
leaq (%rsp,%rbx), %rdi
addq $0x80, %rdi
callq 0x3830a
addq $0x18, %rbx
cmpq $0x48, %rbx
jne 0x45cdb
leaq 0x80(%rsp), %rax
xorl %ecx, %ecx
cmpq $0x3, %rcx
je 0x45d2a
xorl %edx, %edx
cmpq $0x3, %rdx
je 0x45d1d
movsd (%r12,%rdx,8), %xmm0
movsd %xmm0, (%rax,%rdx,8)
incq %rdx
jmp 0x45d07
incq %rcx
addq $0x18, %rax
addq $0x18, %r12
jmp 0x45cff
leaq 0x80(%rsp), %rdi
callq 0x38412
movsd %xmm0, 0x8(%rsp)
leaq 0x98(%rsp), %r15
movq %r15, %rdi
callq 0x38412
movsd %xmm0, 0x10(%rsp)
leaq 0xb0(%rsp), %r12
movq %r12, %rdi
callq 0x38412
movsd %xmm0, (%rsp)
movsd 0x8(%rsp), %xmm0
leaq 0x94e33(%rip), %rbx     # 0xdaba8
movsd (%rbx), %xmm2
ucomisd %xmm2, %xmm0
jbe 0x45d90
leaq 0x80(%rsp), %rdi
callq 0x38c0a
movsd (%rbx), %xmm2
movsd 0x10(%rsp), %xmm0
ucomisd %xmm2, %xmm0
jbe 0x45da8
movq %r15, %rdi
callq 0x38c0a
movsd (%rbx), %xmm2
movsd (%rsp), %xmm1
ucomisd %xmm2, %xmm1
jbe 0x45dbf
movapd %xmm1, %xmm0
movq %r12, %rdi
callq 0x38c0a
leaq 0xe0(%rsp), %rbx
andl $0x0, 0x8(%rbx)
andq $0x0, (%rbx)
leaq 0xd0(%rsp), %r13
andl $0x0, 0x8(%r13)
andq $0x0, (%r13)
xorl %ecx, %ecx
leaq 0x108(%rsp), %r12
movq %r14, 0x28(%rsp)
movslq 0xc(%r14), %rax
cmpq %rax, %rcx
jge 0x45e56
movq (%r14), %rax
movq %rcx, (%rsp)
movq (%rax,%rcx,8), %r14
leaq 0x80(%rsp), %rbp
xorl %r15d, %r15d
cmpq $0x3, %r15
je 0x45e48
movq %r12, %rdi
movq %r14, %rsi
callq 0x212f8
movq %rbp, %rdi
movq %r12, %rsi
callq 0x38586
ucomisd 0x4d3b5(%rip), %xmm0    # 0x931e8
movq %rbx, %rax
ja 0x45e3b
movq %r13, %rax
incl (%rax,%r15,4)
incq %r15
addq $0x18, %rbp
jmp 0x45e0f
movq (%rsp), %rcx
incq %rcx
movq 0x28(%rsp), %r14
jmp 0x45df0
leaq 0x80(%rsp), %r14
xorl %ebx, %ebx
leaq 0x98(%rsp), %r15
leaq 0xb0(%rsp), %r12
cmpq $0x3, %rbx
je 0x45e97
movl 0xe0(%rsp,%rbx,4), %eax
cmpl 0xd0(%rsp,%rbx,4), %eax
jge 0x45e8e
movq %r14, %rdi
callq 0x38662
incq %rbx
addq $0x18, %r14
jmp 0x45e70
leaq 0x108(%rsp), %r14
leaq 0x80(%rsp), %rsi
movq %r14, %rdi
movq %r15, %rdx
callq 0x389be
movq %r12, %rdi
movq %r14, %rsi
callq 0x3887c
movq 0x18(%rsp), %rax
testq %rax, %rax
je 0x45ee0
movaps 0xf0(%rsp), %xmm0
movups %xmm0, (%rax)
movsd 0x100(%rsp), %xmm0
movsd %xmm0, 0x10(%rax)
leaq 0x80(%rsp), %rsi
movq 0x20(%rsp), %rbx
movq %rbx, %rdi
movq %r15, %rdx
movq %r12, %rcx
callq 0x281ea
movq %rbx, %rax
addq $0x1b8, %rsp            # imm = 0x1B8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq