// File: /workspace/github_cpp_makefile_star2_O2/jamesgrimmett[P]orbital_determination/ex10-2_linear_least_squares.cc
// Function: main
int main() {

  int i, ii;

  // The state vector (alpha and beta)
  std::vector<double> state;
  // The inverse of the information matrix (A^T * A)^-1
  // i.e. the covariance matrix without weighting
  std::vector<std::vector<double> > inv;

  // The observed values of x  
  std::vector<double> xobs = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
  // The observed values of y (the observation matrix, b)
  std::vector<double> yobs = {1.0, 1.0, 2.0, 3.0, 3.0, 4.0, 7.0, 6.0};

  // The two components of the general solution (A^T * A, and A^T * b)
  std::vector<std::vector<double> > ata;
  std::vector<double> atb;

  // The expected values after propagation
  std::vector<double> yexp;
  // Residuals (observed - expected). 
  // Must be initialised to <= RMS for first loop 
  std::vector<double> res(xobs.size(), 0.0);
  // Absolute value of residuals. 
  std::vector<double> absres(xobs.size(), 0.0);
  // Root mean square of residuals.
  // Must be initialised  >= residuals for first loop
  double rms = 0.0;
  // Observational variance
  double obs_var;
  // Standard deviation in alpha, beta
  double stdev_a;
  double stdev_b;
 
  // Complete two iterations of the propagate and fit algorithm
  for ( i = 0; i < 2; i++ ) {
    std::cout << "\n********* Loop #" << i + 1 << " *********" <<std::endl;
    // Remove observed value with residuals outside 2*RMS
    // This will have no affect during the first iteration, if initialised 
    // RMS >= residuals.
    for ( ii = 0; ii < xobs.size(); ii++ ) {
      if (std::abs(res[ii]) > 2.0 * rms) {
        std::cout << "Dropping observation " << ii << ": (" << xobs[ii] << "," << yobs[ii] << ")" << std::endl;
        xobs.erase(xobs.begin() + ii);
        yobs.erase(yobs.begin() + ii);
        res.erase(res.begin() + ii);
        ii--;
      }
    }

    // Fill the matrices for the general solution
    tie(ata, atb) = fill_ata_atb(xobs, yobs);
    // Calculate the covariance matrix (A^T * A)*-1
    inv = inverse(ata);
    // Calculate the state vector
    state = dot_product(inv, atb);

    std::cout << "ATA matrix  " << ata[0][0] << "  " << ata[0][1] << std::endl;
    std::cout << "            " << ata[1][0] << "  " << ata[1][1] << std::endl;
    std::cout << "ATb matrix  " << atb[0] << "  " << atb[1] << std::endl; 
  
    // Propagate the x observations with the state to find the expected y values
    yexp = propagate(state, xobs);

    std::cout << "\nCalculated Y values" << std::endl;
    for ( ii = 0; ii < yexp.size(); ii++ ) {
      std::cout << yexp[ii] << " ";
    }
    std::cout << std::endl;

    // Calculate the residuals
    res = residuals(yobs, yexp);
    std::transform(res.begin(), res.end(), absres.begin(),
                         static_cast<float (*)(float)>(&std::abs));

    std::cout << "\nResidual values" << std::endl;
    for ( ii = 0; ii < res.size(); ii++ ) {
      std::cout << absres[ii] << " ";
    }
    std::cout << std::endl;

    // Calculate the RMS of the residuals
    rms = root_mean_sq(res);

    std::cout << "\nRMS: " << rms << std::endl;

    // Standard deviation in alpha and beta are taken from the covariance matrix
    stdev_a = std::sqrt(inv[0][0]);
    stdev_b = std::sqrt(inv[1][1]);

    // Without a weighting matrix, observational variance is estimated as:
    obs_var = std::sqrt(pow(rms, 2.0) * res.size() / (res.size() - 1.0)); 

    std::cout << "\nState:" << std::endl;
    std::cout << "alpha " << state[0] << " +/- " << obs_var * stdev_a << std::endl;
    std::cout << "beta  " << state[1] << " +/- " << obs_var * stdev_b << std::endl;
 
  }
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x268, %rsp            # imm = 0x268
xorps %xmm0, %xmm0
movaps %xmm0, 0xd0(%rsp)
andq $0x0, 0xe0(%rsp)
movaps %xmm0, 0xf0(%rsp)
andq $0x0, 0x100(%rsp)
movups 0x1dff(%rip), %xmm0     # 0x30b8
leaq 0x220(%rsp), %rsi
movaps %xmm0, 0x30(%rsi)
movups 0x1ddc(%rip), %xmm0     # 0x30a8
movaps %xmm0, 0x20(%rsi)
movups 0x1dc1(%rip), %xmm0     # 0x3098
movaps %xmm0, 0x10(%rsi)
movupd 0x1da5(%rip), %xmm0     # 0x3088
movapd %xmm0, (%rsi)
leaq 0x20(%rsp), %rdi
pushq $0x8
popq %rdx
leaq 0x48(%rsp), %rcx
callq 0x22d2
movups 0x1df8(%rip), %xmm0     # 0x30f8
leaq 0x220(%rsp), %rsi
movaps %xmm0, 0x30(%rsi)
movups 0x1dd5(%rip), %xmm0     # 0x30e8
movaps %xmm0, 0x20(%rsi)
movups 0x1dba(%rip), %xmm0     # 0x30d8
movaps %xmm0, 0x10(%rsi)
movupd 0x1d9e(%rip), %xmm0     # 0x30c8
movapd %xmm0, (%rsi)
leaq 0x48(%rsp), %rdi
pushq $0x8
popq %rdx
leaq 0x60(%rsp), %rcx
callq 0x22d2
xorpd %xmm0, %xmm0
movapd %xmm0, 0x60(%rsp)
andq $0x0, 0x70(%rsp)
movapd %xmm0, 0xb0(%rsp)
andq $0x0, 0xc0(%rsp)
movapd %xmm0, 0x90(%rsp)
andq $0x0, 0xa0(%rsp)
movq 0x28(%rsp), %rsi
subq 0x20(%rsp), %rsi
sarq $0x3, %rsi
leaq 0x220(%rsp), %rdx
andq $0x0, (%rdx)
leaq 0x8(%rsp), %rdi
leaq 0x118(%rsp), %rcx
callq 0x2308
movq 0x28(%rsp), %rsi
subq 0x20(%rsp), %rsi
sarq $0x3, %rsi
leaq 0x220(%rsp), %rdx
andq $0x0, (%rdx)
leaq 0x118(%rsp), %rdi
leaq 0x108(%rsp), %rcx
callq 0x2308
movq $0x0, 0x40(%rsp)
movq 0x4bf9(%rip), %r15      # 0x5fd8
leaq 0x208(%rsp), %r13
xorpd %xmm0, %xmm0
movsd %xmm0, (%rsp)
leaq 0x1d62(%rip), %rbp      # 0x3159
cmpl $0x2, 0x40(%rsp)
je 0x1b2e
movq %r15, %rdi
leaq 0x1cfc(%rip), %rsi      # 0x3108
callq 0x10b0
movq 0x40(%rsp), %rsi
incl %esi
movq %rax, %rdi
movq %rsi, 0x40(%rsp)
callq 0x1130
movq %rax, %rdi
leaq 0x1ceb(%rip), %rsi      # 0x311a
callq 0x10b0
movq %rax, %rdi
callq 0x1060
leaq 0x20(%rsp), %r14
movq %r13, %rbx
movsd (%rsp), %xmm0
addsd %xmm0, %xmm0
movsd %xmm0, (%rsp)
xorl %r12d, %r12d
movslq %r12d, %r13
movq 0x28(%rsp), %rax
subq 0x20(%rsp), %rax
sarq $0x3, %rax
cmpq %r13, %rax
jbe 0x15a0
movq 0x8(%rsp), %rax
movsd (%rax,%r13,8), %xmm0
andpd 0x1bbe(%rip), %xmm0     # 0x3040
ucomisd (%rsp), %xmm0
jbe 0x1598
movq %r15, %rdi
leaq 0x1c8e(%rip), %rsi      # 0x3125
callq 0x10b0
movq %rax, %rdi
movl %r12d, %esi
callq 0x1130
movq %rax, %rdi
leaq 0x1c8a(%rip), %rsi      # 0x313b
callq 0x10b0
movq 0x20(%rsp), %rcx
movsd (%rcx,%r13,8), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
leaq 0x1c6c(%rip), %rsi      # 0x313f
callq 0x10b0
movq 0x48(%rsp), %rcx
movsd (%rcx,%r13,8), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
leaq 0x1ceb(%rip), %rsi      # 0x31e0
callq 0x10b0
movq %rax, %rdi
callq 0x1060
movq 0x20(%rsp), %rax
movq 0x28(%rsp), %rdx
leaq (%rax,%r13,8), %rsi
addq $0x8, %rsi
cmpq %rdx, %rsi
je 0x152a
leaq (%rax,%r13,8), %rdi
subq %rsi, %rdx
callq 0x1110
movq 0x28(%rsp), %rdx
addq $-0x8, %rdx
movq %rdx, 0x28(%rsp)
movq 0x48(%rsp), %rax
movq 0x50(%rsp), %rdx
leaq (%rax,%r13,8), %rsi
addq $0x8, %rsi
cmpq %rdx, %rsi
je 0x155b
leaq (%rax,%r13,8), %rdi
subq %rsi, %rdx
callq 0x1110
movq 0x50(%rsp), %rdx
addq $-0x8, %rdx
movq %rdx, 0x50(%rsp)
movq 0x8(%rsp), %rax
movq 0x10(%rsp), %rdx
leaq (%rax,%r13,8), %rsi
addq $0x8, %rsi
cmpq %rdx, %rsi
je 0x158c
leaq (%rax,%r13,8), %rdi
subq %rsi, %rdx
callq 0x1110
movq 0x10(%rsp), %rdx
addq $-0x8, %rdx
movq %rdx, 0x10(%rsp)
decl %r12d
incl %r12d
jmp 0x1455
movq %rbx, %r13
movq %rbx, %rdi
movq %r14, %rsi
callq 0x235e
leaq 0x1f0(%rsp), %r14
movq %r14, %rdi
leaq 0x48(%rsp), %rsi
callq 0x235e
leaq 0x220(%rsp), %rbx
movq %rbx, %rdi
movq %r13, %rsi
movq %r14, %rdx
callq 0x1cf8
leaq 0xb0(%rsp), %rax
movq %rax, 0x108(%rsp)
leaq 0x60(%rsp), %r12
movq %r12, 0x110(%rsp)
leaq 0x108(%rsp), %rdi
movq %rbx, %rsi
callq 0x23ac
movq %rbx, %rdi
callq 0x23c2
movq %r14, %rdi
callq 0x25c2
movq %r13, %rdi
callq 0x25c2
leaq 0x1d8(%rsp), %r14
movq %r14, %rdi
movq %r12, %rsi
callq 0x23d8
movq %rbx, %rdi
movq %r14, %rsi
callq 0x1ec4
leaq 0xf0(%rsp), %r12
movq %r12, %rdi
movq %rbx, %rsi
callq 0x287e
movq %rbx, %rdi
callq 0x2432
movq %r14, %rdi
callq 0x2432
leaq 0x1c0(%rsp), %r14
movq %r14, %rdi
movq %r12, %rsi
callq 0x23d8
leaq 0x1a8(%rsp), %r12
movq %r12, %rdi
leaq 0xb0(%rsp), %rsi
callq 0x235e
movq %rbx, %rdi
movq %r14, %rsi
movq %r12, %rdx
callq 0x1fdc
leaq 0xd0(%rsp), %rdi
movq %rbx, %rsi
callq 0x28d0
movq %rbx, %rdi
callq 0x25c2
movq %r12, %rdi
callq 0x25c2
movq %r14, %rdi
callq 0x2432
movq %r15, %rdi
leaq 0x1a77(%rip), %rsi      # 0x3141
callq 0x10b0
leaq 0x1a82(%rip), %r14      # 0x3158
leaq 0x190(%rsp), %r12
movq 0x60(%rsp), %rcx
movq (%rcx), %rcx
movsd (%rcx), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
movq %r14, %rsi
callq 0x10b0
movq 0x60(%rsp), %rcx
movq (%rcx), %rcx
movsd 0x8(%rcx), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
callq 0x1060
movq %r15, %rdi
leaq 0x1a2a(%rip), %rsi      # 0x314e
callq 0x10b0
movq 0x60(%rsp), %rcx
movq 0x18(%rcx), %rcx
movsd (%rcx), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
movq %r14, %rsi
callq 0x10b0
movq 0x60(%rsp), %rcx
movq 0x18(%rcx), %rcx
movsd 0x8(%rcx), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
callq 0x1060
movq %r15, %rdi
leaq 0x19ea(%rip), %rsi      # 0x315b
callq 0x10b0
movq 0xb0(%rsp), %rcx
movsd (%rcx), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
movq %r14, %rsi
callq 0x10b0
movq 0xb0(%rsp), %rcx
movsd 0x8(%rcx), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
callq 0x1060
movq %r12, %rdi
leaq 0xd0(%rsp), %rsi
callq 0x235e
leaq 0x178(%rsp), %r14
movq %r14, %rdi
leaq 0x20(%rsp), %rsi
callq 0x235e
movq %rbx, %rdi
movq %r12, %rsi
movq %r14, %rdx
callq 0x2107
leaq 0x90(%rsp), %rdi
movq %rbx, %rsi
callq 0x28d0
movq %rbx, %rdi
callq 0x25c2
movq %r14, %rdi
callq 0x25c2
movq %r12, %rdi
callq 0x25c2
movq %r15, %rdi
leaq 0x1951(%rip), %rsi      # 0x3168
callq 0x10b0
movq %rax, %rdi
callq 0x1060
xorl %r12d, %r12d
movq 0x90(%rsp), %rax
movq 0x98(%rsp), %rcx
subq %rax, %rcx
sarq $0x3, %rcx
cmpq %r12, %rcx
jbe 0x1861
movsd (%rax,%r12,8), %xmm0
movq %r15, %rdi
callq 0x1100
movq %rax, %rdi
movq %rbp, %rsi
callq 0x10b0
incq %r12
jmp 0x1827
movq %r15, %rdi
callq 0x1060
leaq 0x148(%rsp), %rdi
leaq 0x48(%rsp), %rsi
callq 0x235e
leaq 0x160(%rsp), %r12
movq %r12, %rdi
leaq 0x90(%rsp), %rsi
callq 0x235e
movq %rbx, %rdi
leaq 0x148(%rsp), %r14
movq %r14, %rsi
movq %r12, %rdx
callq 0x2177
leaq 0x8(%rsp), %rdi
movq %rbx, %rsi
callq 0x28d0
movq %rbx, %rdi
callq 0x25c2
movq %r12, %rdi
callq 0x25c2
movq %r14, %rdi
callq 0x25c2
movq 0x8(%rsp), %rax
movq 0x10(%rsp), %rcx
movq 0x118(%rsp), %rdx
cmpq %rcx, %rax
je 0x1902
cvtsd2ss (%rax), %xmm0
andps 0x1720(%rip), %xmm0     # 0x3010
cvtss2sd %xmm0, %xmm0
movsd %xmm0, (%rdx)
addq $0x8, %rax
addq $0x8, %rdx
jmp 0x18e0
movq %r15, %rdi
leaq 0x1871(%rip), %rsi      # 0x317d
callq 0x10b0
movq %rax, %rdi
callq 0x1060
xorl %r12d, %r12d
movq 0x10(%rsp), %rax
subq 0x8(%rsp), %rax
sarq $0x3, %rax
cmpq %r12, %rax
jbe 0x1955
movq 0x118(%rsp), %rax
movsd (%rax,%r12,8), %xmm0
movq %r15, %rdi
callq 0x1100
movq %rax, %rdi
movq %rbp, %rsi
callq 0x10b0
incq %r12
jmp 0x191c
movq %r15, %rdi
callq 0x1060
leaq 0x130(%rsp), %rdi
leaq 0x8(%rsp), %rsi
callq 0x235e
leaq 0x130(%rsp), %rbx
movq %rbx, %rdi
callq 0x21d9
movsd %xmm0, (%rsp)
movq %rbx, %rdi
callq 0x25c2
movq %r15, %rdi
leaq 0x17f8(%rip), %rsi      # 0x318e
callq 0x10b0
movq %rax, %rdi
movsd (%rsp), %xmm0
callq 0x1100
movq %rax, %rdi
callq 0x1060
movq 0xf0(%rsp), %rax
movq (%rax), %rax
movsd (%rax), %xmm0
ucomisd 0x16b1(%rip), %xmm0     # 0x3078
jb 0x19cf
sqrtsd %xmm0, %xmm0
jmp 0x19d4
callq 0x1090
movsd %xmm0, 0x88(%rsp)
movq 0xf0(%rsp), %rax
movq 0x18(%rax), %rax
movsd 0x8(%rax), %xmm0
ucomisd 0x1682(%rip), %xmm0     # 0x3078
jb 0x19fe
sqrtsd %xmm0, %xmm0
jmp 0x1a03
callq 0x1090
movsd %xmm0, 0x80(%rsp)
movsd (%rsp), %xmm0
mulsd %xmm0, %xmm0
movq 0x10(%rsp), %rax
subq 0x8(%rsp), %rax
sarq $0x3, %rax
movq %rax, %xmm1
punpckldq 0x15f0(%rip), %xmm1 # xmm1 = xmm1[0],mem[0],xmm1[1],mem[1]
subpd 0x15f8(%rip), %xmm1     # 0x3030
movapd %xmm1, %xmm2
unpckhpd %xmm1, %xmm2            # xmm2 = xmm2[1],xmm1[1]
addsd %xmm1, %xmm2
mulsd %xmm2, %xmm0
addsd 0x1620(%rip), %xmm2     # 0x3070
divsd %xmm2, %xmm0
ucomisd 0x161c(%rip), %xmm0     # 0x3078
jb 0x1a64
sqrtsd %xmm0, %xmm0
jmp 0x1a69
callq 0x1090
movsd %xmm0, 0x38(%rsp)
movq %r15, %rdi
leaq 0x171c(%rip), %rsi      # 0x3195
callq 0x10b0
movq %rax, %rdi
callq 0x1060
movq %r15, %rdi
leaq 0x170d(%rip), %rsi      # 0x319d
callq 0x10b0
movq 0xd0(%rsp), %rcx
movsd (%rcx), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
leaq 0x16f1(%rip), %rsi      # 0x31a4
callq 0x10b0
movsd 0x88(%rsp), %xmm0
mulsd 0x38(%rsp), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
callq 0x1060
movq %r15, %rdi
leaq 0x16c9(%rip), %rsi      # 0x31aa
callq 0x10b0
movq 0xd0(%rsp), %rcx
movsd 0x8(%rcx), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
leaq 0x169f(%rip), %rsi      # 0x31a4
callq 0x10b0
movsd 0x80(%rsp), %xmm0
mulsd 0x38(%rsp), %xmm0
movq %rax, %rdi
callq 0x1100
movq %rax, %rdi
callq 0x1060
jmp 0x13f7
leaq 0x118(%rsp), %rdi
callq 0x25c2
leaq 0x8(%rsp), %rdi
callq 0x25c2
leaq 0x90(%rsp), %rdi
callq 0x25c2
leaq 0xb0(%rsp), %rdi
callq 0x25c2
leaq 0x60(%rsp), %rdi
callq 0x2432
leaq 0x48(%rsp), %rdi
callq 0x25c2
leaq 0x20(%rsp), %rdi
callq 0x25c2
leaq 0xf0(%rsp), %rdi
callq 0x2432
leaq 0xd0(%rsp), %rdi
callq 0x25c2
xorl %eax, %eax
addq $0x268, %rsp            # imm = 0x268
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
jmp 0x1c94
movq %rax, %rbx
jmp 0x1c9e
movq %rax, %rbx
jmp 0x1ccc
movq %rax, %rbx
jmp 0x1cd6
movq %rax, %rbx
leaq 0x130(%rsp), %rdi
jmp 0x1c77
jmp 0x1c84
movq %rax, %rbx
jmp 0x1bf5
movq %rax, %rbx
leaq 0x160(%rsp), %rdi
callq 0x25c2
leaq 0x148(%rsp), %rdi
jmp 0x1c77
jmp 0x1c84
movq %rax, %rbx
jmp 0x1c19
movq %rax, %rbx
leaq 0x178(%rsp), %rdi
callq 0x25c2
leaq 0x190(%rsp), %rdi
jmp 0x1c77
movq %rax, %rbx
leaq 0x1f0(%rsp), %rdi
callq 0x25c2
jmp 0x1c6f
jmp 0x1c84
movq %rax, %rbx
leaq 0x1d8(%rsp), %rdi
jmp 0x1c65
jmp 0x1c84
movq %rax, %rbx
jmp 0x1c5d
jmp 0x1c84
movq %rax, %rbx
leaq 0x1a8(%rsp), %rdi
callq 0x25c2
leaq 0x1c0(%rsp), %rdi
callq 0x2432
jmp 0x1c87
movq %rax, %rbx
leaq 0x208(%rsp), %rdi
callq 0x25c2
jmp 0x1c87
jmp 0x1c84
jmp 0x1c84
jmp 0x1c84
movq %rax, %rbx
leaq 0x118(%rsp), %rdi
callq 0x25c2
leaq 0x8(%rsp), %rdi
callq 0x25c2
leaq 0x90(%rsp), %rdi
callq 0x25c2
leaq 0xb0(%rsp), %rdi
callq 0x25c2
leaq 0x60(%rsp), %rdi
callq 0x2432
leaq 0x48(%rsp), %rdi
callq 0x25c2
leaq 0x20(%rsp), %rdi
callq 0x25c2
leaq 0xf0(%rsp), %rdi
callq 0x2432
leaq 0xd0(%rsp), %rdi
callq 0x25c2
movq %rbx, %rdi
callq 0x1140