// File: 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:
subq $0x3e8, %rsp            # imm = 0x3E8
movq %rdi, 0x40(%rsp)
movq %rdi, %rax
movq %rax, 0x48(%rsp)
movl 0x3f0(%rsp), %eax
movq %rdi, 0x3e0(%rsp)
movq %rsi, 0x3d8(%rsp)
movq %rdx, 0x3d0(%rsp)
movq %rcx, 0x3c8(%rsp)
movl %r8d, 0x3c4(%rsp)
movl %r9d, 0x3c0(%rsp)
movq 0x3d8(%rsp), %rdi
callq 0x13600
movl %eax, 0x3bc(%rsp)
movq 0x3d0(%rsp), %rdi
callq 0x13600
cmpl 0x3bc(%rsp), %eax
jge 0xcee69
movq 0x3d0(%rsp), %rdi
callq 0x13600
movl %eax, 0x3bc(%rsp)
cmpl $0x1, 0x3bc(%rsp)
jge 0xcee7e
movl $0x0, 0x3c4(%rsp)
cmpl $0x2, 0x3bc(%rsp)
jge 0xcee93
movl $0x0, 0x3f0(%rsp)
cmpl $0x3, 0x3bc(%rsp)
jge 0xceea8
movl $0x0, 0x3c0(%rsp)
leaq 0x3a8(%rsp), %rdi
xorps %xmm1, %xmm1
movaps %xmm1, %xmm0
callq 0xc3140
leaq 0x398(%rsp), %rdi
xorps %xmm1, %xmm1
movaps %xmm1, %xmm0
callq 0xc3140
cmpl $0x0, 0x3c4(%rsp)
je 0xcef3c
movq 0x3d8(%rsp), %rsi
movq 0x3c8(%rsp), %rdx
leaq 0x388(%rsp), %rdi
callq 0xcdf80
leaq 0x3a8(%rsp), %rdi
leaq 0x388(%rsp), %rsi
callq 0xc33e0
movq 0x3d0(%rsp), %rsi
movq 0x3c8(%rsp), %rdx
leaq 0x378(%rsp), %rdi
callq 0xcdf80
leaq 0x398(%rsp), %rdi
leaq 0x378(%rsp), %rsi
callq 0xc33e0
movsd 0x640c4(%rip), %xmm0    # 0x133008
movsd %xmm0, 0x370(%rsp)
movsd 0x640b3(%rip), %xmm0    # 0x133008
movsd %xmm0, 0x368(%rsp)
cmpl $0x0, 0x3f0(%rsp)
je 0xcefb4
movq 0x3d8(%rsp), %rsi
movq 0x3c8(%rsp), %rdx
leaq 0x3a8(%rsp), %rdi
callq 0xce730
movsd %xmm0, 0x370(%rsp)
movq 0x3d0(%rsp), %rsi
movq 0x3c8(%rsp), %rdx
leaq 0x398(%rsp), %rdi
callq 0xce730
movsd %xmm0, 0x368(%rsp)
leaq 0x320(%rsp), %rdi
leaq 0x4abb65(%rip), %rsi    # 0x57ab28
callq 0xbf730
cmpl $0x0, 0x3c0(%rsp)
je 0xcf477
leaq 0x300(%rsp), %rdi
xorl %esi, %esi
movl $0x20, %edx
callq 0xf280
xorps %xmm0, %xmm0
movsd %xmm0, 0x2f8(%rsp)
movl $0x0, 0x2f4(%rsp)
movl 0x2f4(%rsp), %eax
cmpl 0x3bc(%rsp), %eax
jge 0xcf21a
movq 0x3d8(%rsp), %rdi
movl 0x2f4(%rsp), %esi
callq 0xbd1e0
movq %rax, 0x2e8(%rsp)
movq 0x3d0(%rsp), %rdi
movl 0x2f4(%rsp), %esi
callq 0xbd1e0
movq %rax, 0x2e0(%rsp)
movq 0x2e8(%rsp), %rsi
leaq 0x2c0(%rsp), %rdi
leaq 0x3a8(%rsp), %rdx
callq 0xc3830
movsd 0x370(%rsp), %xmm0
leaq 0x2d0(%rsp), %rdi
leaq 0x2c0(%rsp), %rsi
callq 0xc49d0
movq 0x2e0(%rsp), %rsi
leaq 0x2a0(%rsp), %rdi
leaq 0x398(%rsp), %rdx
callq 0xc3830
movsd 0x368(%rsp), %xmm0
leaq 0x2b0(%rsp), %rdi
leaq 0x2a0(%rsp), %rsi
callq 0xc49d0
cmpq $0x0, 0x3c8(%rsp)
je 0xcf0eb
movq 0x3c8(%rsp), %rax
movslq 0x2f4(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
movsd %xmm0, 0x38(%rsp)
jmp 0xcf0fb
movsd 0x63f15(%rip), %xmm0    # 0x133008
movsd %xmm0, 0x38(%rsp)
jmp 0xcf0fb
movsd 0x38(%rsp), %xmm0
movsd %xmm0, 0x298(%rsp)
movsd 0x298(%rsp), %xmm0
addsd 0x2f8(%rsp), %xmm0
movsd %xmm0, 0x2f8(%rsp)
movl $0x0, 0x294(%rsp)
cmpl $0x2, 0x294(%rsp)
jge 0xcf202
movl $0x0, 0x290(%rsp)
cmpl $0x2, 0x290(%rsp)
jge 0xcf1ea
movsd 0x298(%rsp), %xmm0
movsd %xmm0, 0x28(%rsp)
movl 0x294(%rsp), %esi
leaq 0x2d0(%rsp), %rdi
callq 0xa5c10
movsd 0x28(%rsp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, 0x30(%rsp)
movl 0x290(%rsp), %esi
leaq 0x2b0(%rsp), %rdi
callq 0xa5c10
movsd 0x30(%rsp), %xmm0
movsd (%rax), %xmm2
movl 0x290(%rsp), %eax
shll %eax
addl 0x294(%rsp), %eax
cltq
movsd 0x300(%rsp,%rax,8), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x300(%rsp,%rax,8)
movl 0x290(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x290(%rsp)
jmp 0xcf149
jmp 0xcf1ec
movl 0x294(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x294(%rsp)
jmp 0xcf130
jmp 0xcf204
movl 0x2f4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x2f4(%rsp)
jmp 0xcf001
movsd 0x2f8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0xcf244
jp 0xcf244
movq 0x40(%rsp), %rdi
leaq 0x4ab8ee(%rip), %rsi    # 0x57ab28
callq 0xbf730
jmp 0xcf531
movl $0x0, 0x28c(%rsp)
cmpl $0x4, 0x28c(%rsp)
jge 0xcf293
movsd 0x2f8(%rsp), %xmm1
movslq 0x28c(%rsp), %rax
movsd 0x300(%rsp,%rax,8), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0x300(%rsp,%rax,8)
movl 0x28c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x28c(%rsp)
jmp 0xcf24f
leaq 0x300(%rsp), %rdx
leaq 0x260(%rsp), %rcx
leaq 0x250(%rsp), %r8
leaq 0x230(%rsp), %r9
movl $0x2, %esi
movl %esi, %edi
callq 0xd3dc0
movsd 0x260(%rsp), %xmm0
movsd 0x270(%rsp), %xmm1
movsd 0x268(%rsp), %xmm3
movsd 0x278(%rsp), %xmm4
leaq 0x1e8(%rsp), %rdi
xorps %xmm7, %xmm7
movsd 0x63d11(%rip), %xmm8    # 0x133008
movaps %xmm7, %xmm2
movaps %xmm7, %xmm5
movaps %xmm7, %xmm6
movsd %xmm8, (%rsp)
callq 0xbf7d0
movsd 0x230(%rsp), %xmm0
movsd 0x240(%rsp), %xmm1
movsd 0x238(%rsp), %xmm3
movsd 0x248(%rsp), %xmm4
leaq 0x1a0(%rsp), %rdi
xorps %xmm7, %xmm7
movsd 0x63cc5(%rip), %xmm8    # 0x133008
movaps %xmm7, %xmm2
movaps %xmm7, %xmm5
movaps %xmm7, %xmm6
movsd %xmm8, (%rsp)
callq 0xbf7d0
leaq 0x158(%rsp), %rdi
leaq 0x1a0(%rsp), %rsi
leaq 0x1e8(%rsp), %rdx
callq 0xc0ce0
leaq 0x110(%rsp), %rdi
leaq 0x4ab7a5(%rip), %rsi    # 0x57ab28
callq 0xbf730
leaq 0x158(%rsp), %rdi
xorl %esi, %esi
callq 0xcfb10
movsd (%rax), %xmm0
movsd %xmm0, 0x8(%rsp)
leaq 0x158(%rsp), %rdi
xorl %esi, %esi
callq 0xcfb10
movsd 0x8(%rax), %xmm0
movsd %xmm0, 0x10(%rsp)
leaq 0x158(%rsp), %rdi
movl $0x1, %esi
callq 0xcfb10
movsd (%rax), %xmm0
movsd %xmm0, 0x18(%rsp)
leaq 0x158(%rsp), %rdi
movl $0x1, %esi
callq 0xcfb10
movsd 0x8(%rsp), %xmm0
movsd 0x10(%rsp), %xmm1
movsd 0x18(%rsp), %xmm2
movsd 0x8(%rax), %xmm3
callq 0xc00f0
movsd %xmm0, 0x20(%rsp)
leaq 0x110(%rsp), %rdi
movl $0x1, %esi
callq 0xcfb10
movsd 0x20(%rsp), %xmm0
movsd %xmm0, 0x8(%rax)
leaq 0x80(%rsp), %rdi
leaq 0x1a0(%rsp), %rsi
leaq 0x110(%rsp), %rdx
callq 0xc0ce0
leaq 0xc8(%rsp), %rdi
leaq 0x80(%rsp), %rsi
leaq 0x1e8(%rsp), %rdx
callq 0xc0ce0
leaq 0x320(%rsp), %rdi
leaq 0xc8(%rsp), %rsi
callq 0xc06c0
movq 0x40(%rsp), %rdi
leaq 0x4ab6a5(%rip), %rsi    # 0x57ab28
callq 0xbf730
cmpl $0x0, 0x3c4(%rsp)
je 0xcf4b3
leaq 0x70(%rsp), %rdi
leaq 0x3a8(%rsp), %rsi
callq 0xa5b80
movq 0x40(%rsp), %rdi
leaq 0x70(%rsp), %rsi
callq 0xc0290
cmpl $0x0, 0x3f0(%rsp)
je 0xcf4d9
movq 0x40(%rsp), %rdi
movsd 0x370(%rsp), %xmm0
divsd 0x368(%rsp), %xmm0
callq 0xc03c0
cmpl $0x0, 0x3c0(%rsp)
je 0xcf4f5
movq 0x40(%rsp), %rdi
leaq 0x320(%rsp), %rsi
callq 0xcfb30
cmpl $0x0, 0x3c4(%rsp)
je 0xcf52f
leaq 0x50(%rsp), %rdi
leaq 0x398(%rsp), %rsi
callq 0xa5b80
leaq 0x60(%rsp), %rdi
leaq 0x50(%rsp), %rsi
callq 0xc4700
movq 0x40(%rsp), %rdi
leaq 0x60(%rsp), %rsi
callq 0xc0290
jmp 0xcf531
movq 0x48(%rsp), %rax
addq $0x3e8, %rsp            # imm = 0x3E8
retq
nop