// File: /workspace/github_cpp_makefile_star2_O1/tomfunkhouser[P]gaps/pkgs/R2Shapes/R2Align.cpp
// Function: gaps::R2AlignPoints(gaps::RNArray<gaps::R2Point*> const&, gaps::RNArray<gaps::R2Point*> const&, double const*, int, int, int)
R3Matrix
R2AlignPoints(const RNArray<R2Point *>& points1, const RNArray<R2Point *>& points2, 
  const RNScalar *weights, RNBoolean align_center, RNBoolean align_rotation, int align_scale)
{
  // Get count of points
  int count = points1.NEntries();
  if (points2.NEntries() < count) count = points2.NEntries();

  // Check number of points
  if (count < 1) align_center = 0;
  if (count < 2) align_scale = 0;
  if (count < 3) align_rotation = 0;

  // Compute centers (note: this should be weighted)
  R2Point center1(0.0, 0.0);
  R2Point center2(0.0, 0.0);
  if (align_center){
    center1 = R2Centroid(points1, weights);
    center2 = R2Centroid(points2, weights);
  }

  // Compute scales
  RNScalar s1 = 1;
  RNScalar s2 = 1;
  if (align_scale){
    s1 = R2AverageDistance(center1, points1, weights);
    s2 = R2AverageDistance(center2, points2, weights);
  }

  // Compute cross-covariance of two point sets
  R3Matrix rotation = R3identity_matrix;
  if (align_rotation) {
    // Compute covariance matrix
    RNScalar m[4] = { 0 };
    RNScalar total_weight = 0;
    for (int i = 0; i < count; i++){
      R2Point *point1 = points1[i];
      R2Point *point2 = points2[i];
      R2Vector p1 = (*point1 - center1) / s1;
      R2Vector p2 = (*point2 - center2) / s2;
      RNScalar weight = (weights) ? weights[i] : 1;
      total_weight += weight;
      for(int j = 0; j < 2; j++) {
        for(int k = 0; k < 2; k++) {
          m[k*2 + j] += weight * p1[j]*p2[k];
        }
      }
    }

    // Normalize covariance matrix
    if (total_weight == 0) return R3identity_matrix;
    for (int j = 0; j < 4; j++) m[j] /= total_weight;

    // Calculate SVD of covariance matrix
    RNScalar Um[4];
    RNScalar Wm[2];
    RNScalar Vmt[4];
    RNSvdDecompose(2, 2, m, Um, Wm, Vmt);

    // https://sakai.rutgers.edu/access/content/group/7bee3f05-9013-4fc2-8743-3c5078742791/material/svd_ls_rotation.pdf
    R3Matrix Ut(Um[0], Um[2], 0, Um[1], Um[3], 0, 0, 0, 1); 
    R3Matrix V(Vmt[0], Vmt[2], 0, Vmt[1], Vmt[3], 0, 0, 0, 1); 
    R3Matrix VUt = V * Ut;
    R3Matrix D = R3identity_matrix;
    D[1][1] = R3MatrixDet2(VUt[0][0], VUt[0][1], VUt[1][0], VUt[1][1]);
    rotation = V * D * Ut;
  }

  // Compute result
  R3Matrix result = R3identity_matrix;
  if (align_center) result.Translate(center1.Vector());
  if (align_scale) result.Scale(s1/s2);
  if (align_rotation) result.Transform(rotation);
  if (align_center) result.Translate(-(center2.Vector()));

  // Return resulting matrix that takes points2 to points1
  return result;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x2e8, %rsp            # imm = 0x2E8
movl %r9d, %ebp
movl %r8d, %ebx
movq %rcx, %r14
movq %rdi, 0x18(%rsp)
movl 0x320(%rsp), %r15d
movq %rsi, 0x30(%rsp)
movl 0xc(%rsi), %r13d
movq %rdx, 0x38(%rsp)
movl 0xc(%rdx), %eax
cmpl %r13d, %eax
cmovll %eax, %r13d
testl %r13d, %r13d
setle %r12b
leaq 0x50(%rsp), %rdi
xorpd %xmm0, %xmm0
xorpd %xmm1, %xmm1
callq 0x68e2e
leaq 0x40(%rsp), %rdi
xorpd %xmm0, %xmm0
xorpd %xmm1, %xmm1
callq 0x68e2e
testl %ebx, %ebx
sete %al
orb %r12b, %al
movb %al, 0x17(%rsp)
jne 0x6ef5d
leaq 0x98(%rsp), %rbx
movq %rbx, %rdi
movq 0x30(%rsp), %rsi
movq %r14, %rdx
callq 0x6e6b8
leaq 0x50(%rsp), %rdi
movq %rbx, %rsi
callq 0x68f66
leaq 0x98(%rsp), %rbx
movq %rbx, %rdi
movq 0x38(%rsp), %rsi
movq %r14, %rdx
callq 0x6e6b8
leaq 0x40(%rsp), %rdi
movq %rbx, %rsi
callq 0x68f66
cmpl $0x2, %r13d
setl %al
testl %r15d, %r15d
sete %r12b
orb %al, %r12b
je 0x6ef80
movsd 0x330c0(%rip), %xmm0    # 0xa2038
movsd %xmm0, 0x28(%rsp)
jmp 0x6efaa
leaq 0x50(%rsp), %rdi
movq 0x30(%rsp), %rsi
movq %r14, %rdx
callq 0x6ea2a
movsd %xmm0, 0x28(%rsp)
leaq 0x40(%rsp), %rdi
movq 0x38(%rsp), %rsi
movq %r14, %rdx
callq 0x6ea2a
movsd %xmm0, 0x20(%rsp)
movq 0x18(%rsp), %r15
cmpl $0x3, %r13d
setl %bl
leaq 0x3aa05(%rip), %rsi     # 0xa99c8
leaq 0x138(%rsp), %rdi
callq 0x66efa
testl %ebp, %ebp
sete %al
orb %bl, %al
movb %al, 0x16(%rsp)
jne 0x6f28e
movb %r12b, 0x15(%rsp)
xorpd %xmm0, %xmm0
leaq 0x70(%rsp), %rax
movapd %xmm0, 0x10(%rax)
movapd %xmm0, (%rax)
movl %r13d, %r15d
xorpd %xmm1, %xmm1
xorl %r12d, %r12d
leaq 0xf0(%rsp), %r13
leaq 0x1c8(%rsp), %rbp
movapd %xmm1, 0x60(%rsp)
movq 0x30(%rsp), %rax
movq (%rax), %rax
movq (%rax,%r12,8), %rsi
movq 0x38(%rsp), %rax
movq (%rax), %rax
movq (%rax,%r12,8), %rbx
movq %r13, %rdi
leaq 0x50(%rsp), %rdx
callq 0x6906b
leaq 0x98(%rsp), %rdi
movq %r13, %rsi
movsd 0x28(%rsp), %xmm0
callq 0x696f7
movq %rbp, %rdi
movq %rbx, %rsi
leaq 0x40(%rsp), %rdx
callq 0x6906b
movq %r13, %rdi
movq %rbp, %rsi
movsd 0x20(%rsp), %xmm0
callq 0x696f7
movsd 0x32fbc(%rip), %xmm0    # 0xa2038
testq %r14, %r14
je 0x6f087
movsd (%r14,%r12,8), %xmm0
leaq 0x70(%rsp), %rax
xorl %ecx, %ecx
movsd 0x98(%rsp,%rcx,8), %xmm1
mulsd %xmm0, %xmm1
xorl %edx, %edx
movsd 0xf0(%rsp,%rdx), %xmm2
mulsd %xmm1, %xmm2
addsd (%rax,%rdx,2), %xmm2
movsd %xmm2, (%rax,%rdx,2)
addq $0x8, %rdx
cmpq $0x8, %rdx
je 0x6f09d
leaq 0x1(%rcx), %rdx
addq $0x8, %rax
testq %rcx, %rcx
movq %rdx, %rcx
je 0x6f08e
movapd 0x60(%rsp), %xmm1
addsd %xmm0, %xmm1
incq %r12
cmpq %r15, %r12
jne 0x6f012
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm1
jne 0x6f0f4
jnp 0x6f259
movapd %xmm1, 0x60(%rsp)
movapd %xmm1, %xmm0
unpcklpd %xmm1, %xmm0            # xmm0 = xmm0[0],xmm1[0]
xorl %eax, %eax
movapd 0x70(%rsp,%rax,8), %xmm1
divpd %xmm0, %xmm1
movapd %xmm1, 0x70(%rsp,%rax,8)
addq $0x2, %rax
cmpq $0x4, %rax
jne 0x6f104
leaq 0x70(%rsp), %rdx
leaq 0x1a0(%rsp), %rbx
leaq 0xe0(%rsp), %r8
leaq 0x180(%rsp), %r14
movl $0x2, %edi
movl $0x2, %esi
movq %rbx, %rcx
movq %r14, %r9
callq 0x720bd
movsd 0x10(%rbx), %xmm1
movsd (%rbx), %xmm0
movsd 0x8(%rbx), %xmm3
movsd 0x18(%rbx), %xmm4
movabsq $0x3ff0000000000000, %r15 # imm = 0x3FF0000000000000
movq %r15, (%rsp)
leaq 0x98(%rsp), %rbx
xorpd %xmm2, %xmm2
xorps %xmm5, %xmm5
xorps %xmm6, %xmm6
xorps %xmm7, %xmm7
movq %rbx, %rdi
callq 0x66f54
movsd 0x10(%r14), %xmm1
movsd (%r14), %xmm0
movsd 0x8(%r14), %xmm3
movsd 0x18(%r14), %xmm4
movq %r15, (%rsp)
leaq 0xf0(%rsp), %r14
xorpd %xmm2, %xmm2
xorps %xmm5, %xmm5
xorps %xmm6, %xmm6
xorps %xmm7, %xmm7
movq %r14, %rdi
callq 0x66f54
leaq 0x1c8(%rsp), %r15
movq %r15, %rdi
movq %r14, %rsi
movq %rbx, %rdx
callq 0x67b81
leaq 0x3a7e5(%rip), %rsi     # 0xa99c8
leaq 0x2a0(%rsp), %r12
movq %r12, %rdi
callq 0x66efa
movsd (%r15), %xmm0
movsd 0x8(%r15), %xmm1
movsd 0x18(%r15), %xmm2
movsd 0x20(%r15), %xmm3
callq 0x67523
movsd %xmm0, 0x20(%r12)
leaq 0x210(%rsp), %r15
movq %r15, %rdi
movq %r14, %rsi
movq %r12, %rdx
callq 0x67b81
leaq 0x258(%rsp), %r14
movq %r14, %rdi
movq %r15, %rsi
movq %rbx, %rdx
callq 0x67b81
leaq 0x138(%rsp), %rdi
movq %r14, %rsi
callq 0x67944
movq 0x18(%rsp), %r15
jmp 0x6f273
movapd %xmm1, 0x60(%rsp)
leaq 0x3a762(%rip), %rsi     # 0xa99c8
movq 0x18(%rsp), %r15
movq %r15, %rdi
callq 0x66efa
xorpd %xmm0, %xmm0
movapd 0x60(%rsp), %xmm1
ucomisd %xmm0, %xmm1
movb 0x15(%rsp), %r12b
jne 0x6f28e
jnp 0x6f353
leaq 0x3a733(%rip), %rsi     # 0xa99c8
movq %r15, %rdi
callq 0x66efa
movb 0x17(%rsp), %bpl
testb %bpl, %bpl
jne 0x6f2ce
movsd 0x50(%rsp), %xmm0
movsd 0x58(%rsp), %xmm1
leaq 0x98(%rsp), %rbx
movq %rbx, %rdi
callq 0x6915a
movq %r15, %rdi
movq %rbx, %rsi
callq 0x67606
testb %r12b, %r12b
jne 0x6f2e7
movsd 0x28(%rsp), %xmm0
divsd 0x20(%rsp), %xmm0
movq %r15, %rdi
callq 0x676cc
cmpb $0x0, 0x16(%rsp)
jne 0x6f314
leaq 0x98(%rsp), %rbx
leaq 0x138(%rsp), %rdx
movq %rbx, %rdi
movq %r15, %rsi
callq 0x67b81
movq %r15, %rdi
movq %rbx, %rsi
callq 0x67944
testb %bpl, %bpl
jne 0x6f353
movsd 0x40(%rsp), %xmm0
movsd 0x48(%rsp), %xmm1
leaq 0xf0(%rsp), %rbx
movq %rbx, %rdi
callq 0x6915a
leaq 0x98(%rsp), %r14
movq %r14, %rdi
movq %rbx, %rsi
callq 0x69669
movq %r15, %rdi
movq %r14, %rsi
callq 0x67606
movq %r15, %rax
addq $0x2e8, %rsp            # imm = 0x2E8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq