// File: rachit31[P]CubeSatADCS/propagateANDupdate.cpp
// Function: covarianceProp(double const (&) [3], double, double, double, double (&) [6][6])
void covarianceProp(const double (&wbi_B)[3], const double dtGyro, const double sigmaw, const double sigmaBw, double (&P)[6][6])
{
  // norm(wbi_b)
  double wmag = sqrt(wbi_B[0]*wbi_B[0]+wbi_B[1]*wbi_B[1]+wbi_B[2]*wbi_B[2]);
  if (wmag == 0)
  {
    return;
  }

  // wX = X(wbi_B)
  double wX[3][3];
  X(wbi_B, wX);

  const double s = sin(wmag*dtGyro);
  const double c = cos(wmag*dtGyro);

  // I3 = eye(3);
  const double I3[3][3] = { { 1, 0, 0},
                            { 0, 1, 0},
                            { 0, 0, 1} };

  // O3 = zeros(3,3);
  const double O3[3][3] = { { 0, 0, 0},
                            { 0, 0, 0},
                            { 0, 0, 0} };

  // - wX*sin(wmag*dt)/wmag
  double wXs_wmag[3][3];
  mult3x3Scalar(wX, -1.0*s/wmag, wXs_wmag);

  // wX*(1-cos(wmag*dt))/wmag^2
  double wXc_wmag2[3][3];
  mult3x3Scalar(wX,(1.0-c)/pow(wmag,2), wXc_wmag2);

  // wX*wX*(1-cos(wmag*dt))/wmag^2
  double wXwXc_wmag2[3][3];
  mult3x3(wX, wXc_wmag2, wXwXc_wmag2);

  // I3 - wX*sin(wmag*dt)/wmag
  double sum1[3][3];
  add3x3(I3, wXs_wmag, sum1);

  // I3 - wX*sin(wmag*dt)/wmag + wX*wX*(1-cos(wmag*dt))/wmag^2
  double Phi11[3][3];
  add3x3(sum1, wXwXc_wmag2, Phi11);

  // - I3*dt
  double I3dtGyros[3][3];
  mult3x3Scalar(I3,-1.0*dtGyro,I3dtGyros);

  // - wX*(wmag*dt-sin(wmag*dt))/wmag^3
  double wXs_wmag3[3][3];
  mult3x3Scalar(wX,-1.0*(wmag*dtGyro-s)/pow(wmag,3), wXs_wmag3);

  // - wX*wX*(wmag*dt-sin(wmag*dt))/wmag^3
  double wXwXs_wmag3[3][3];
  mult3x3(wX,wXs_wmag3,wXwXs_wmag3);

  //  wX*(1-cos(wmag*dt))/wmag^2 - I3*dt
  double sum2[3][3];
  add3x3(wXc_wmag2, I3dtGyros, sum2);

  // wX*(1-cos(wmag*dt))/wmag^2 - I3*dt - wX*wX*(wmag*dt-sin(wmag*dt))/wmag^3
  double Phi12[3][3];
  add3x3(sum2,wXwXs_wmag3,Phi12);

  // Form State Transition Matrix
  double Phi[6][6];

  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      Phi[i][j] = Phi11[i][j];
    }
  }

  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      Phi[i][j+3] = Phi12[i][j];
    }
  }

  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      Phi[i+3][j] = O3[i][j];
    }
  }

  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      Phi[i+3][j+3] = I3[i][j];
    }
  }

  // Transpose State Transition Matrix
  double PhiT[6][6];
  trans6x6(Phi,PhiT);

  // Form Q Matrix
  double Q[6][6] = { { 0, 0, 0, 0, 0, 0},
                     { 0, 0, 0, 0, 0, 0},
                     { 0, 0, 0, 0, 0, 0},
                     { 0, 0, 0, 0, 0, 0},
                     { 0, 0, 0, 0, 0, 0},
                     { 0, 0, 0, 0, 0, 0} };

  const double Q11 = pow(sigmaw,2)*dtGyro + 1.0/3.0*pow(sigmaBw,2)*pow(dtGyro,3);
  const double Q12 = -1.0/2.0*pow(sigmaBw,2)*pow(dtGyro,2);
  const double Q21 = Q12;
  const double Q22 = pow(sigmaBw,2)*dtGyro;

  Q[0][0] = Q11;
  Q[1][1] = Q11;
  Q[2][2] = Q11;
  Q[0][3] = Q12;
  Q[1][4] = Q12;
  Q[2][5] = Q12;
  Q[3][0] = Q21;
  Q[4][1] = Q21;
  Q[5][2] = Q21;
  Q[3][3] = Q22;
  Q[4][4] = Q22;
  Q[5][5] = Q22;

  // P*Phi'
  double PPhiT[6][6];
  mult6x6(P,PhiT,PPhiT);

  // Phi*P*Phi'
  double PhiPPhiT[6][6];
  mult6x6(Phi,PPhiT,PhiPPhiT);

  // Phi*P*Phi' + Q
  double Pnew[6][6];
  add6x6(PhiPPhiT,Q,Pnew);

  // Make Sure Covariance Matrix is Symmetric
  double PnewT[6][6];
  trans6x6(Pnew,PnewT);

  double PSymmetric[6][6];
  add6x6(Pnew,PnewT,PSymmetric);

  mult6x6Scalar(PSymmetric,0.5,P);
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0xdd0, %rsp            # imm = 0xDD0
movq %rdi, -0x8(%rbp)
movsd %xmm0, -0x10(%rbp)
movsd %xmm1, -0x18(%rbp)
movsd %xmm2, -0x20(%rbp)
movq %rsi, -0x28(%rbp)
movq -0x8(%rbp), %rax
movsd (%rax), %xmm1
movq -0x8(%rbp), %rax
movsd (%rax), %xmm2
movq -0x8(%rbp), %rax
movsd 0x8(%rax), %xmm0
movq -0x8(%rbp), %rax
mulsd 0x8(%rax), %xmm0
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movq -0x8(%rbp), %rax
movsd 0x10(%rax), %xmm0
movq -0x8(%rbp), %rax
movsd 0x10(%rax), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
callq 0x3200
movsd %xmm0, -0x30(%rbp)
movsd -0x30(%rbp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0xa705
jp 0xa705
jmp 0xae5f
movq -0x8(%rbp), %rdi
leaq -0x80(%rbp), %rsi
callq 0xb1d0
movsd -0x30(%rbp), %xmm0
mulsd -0x10(%rbp), %xmm0
callq 0x30a0
movsd %xmm0, -0x88(%rbp)
movsd -0x30(%rbp), %xmm0
mulsd -0x10(%rbp), %xmm0
callq 0x3460
movsd %xmm0, -0x90(%rbp)
leaq -0xe0(%rbp), %rdi
xorl %esi, %esi
movl $0x48, %edx
callq 0x3140
movsd 0x2acd(%rip), %xmm0     # 0xd228
movsd %xmm0, -0xe0(%rbp)
movsd 0x2abd(%rip), %xmm0     # 0xd228
movsd %xmm0, -0xc0(%rbp)
movsd 0x2aad(%rip), %xmm0     # 0xd228
movsd %xmm0, -0xa0(%rbp)
leaq -0x130(%rbp), %rdi
xorl %esi, %esi
movl $0x48, %edx
callq 0x3140
movsd 0x2c82(%rip), %xmm0     # 0xd420
mulsd -0x88(%rbp), %xmm0
divsd -0x30(%rbp), %xmm0
movsd %xmm0, -0x188(%rbp)
leaq -0x80(%rbp), %rdi
leaq -0x188(%rbp), %rsi
leaq -0x180(%rbp), %rdx
callq 0xb2b0
movsd 0x2a56(%rip), %xmm0     # 0xd228
subsd -0x90(%rbp), %xmm0
movsd %xmm0, -0xda8(%rbp)
movsd -0x30(%rbp), %xmm0
movsd 0x2c21(%rip), %xmm1     # 0xd410
callq 0x3410
movaps %xmm0, %xmm1
movsd -0xda8(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x1d8(%rbp)
leaq -0x80(%rbp), %rdi
leaq -0x1d8(%rbp), %rsi
leaq -0x1d0(%rbp), %rdx
callq 0xb2b0
leaq -0x80(%rbp), %rdi
leaq -0x1d0(%rbp), %rsi
leaq -0x220(%rbp), %rdx
callq 0xb330
leaq -0xe0(%rbp), %rdi
leaq -0x180(%rbp), %rsi
leaq -0x270(%rbp), %rdx
callq 0xb410
leaq -0x270(%rbp), %rdi
leaq -0x220(%rbp), %rsi
leaq -0x2c0(%rbp), %rdx
callq 0xb410
movsd 0x2bab(%rip), %xmm0     # 0xd420
mulsd -0x10(%rbp), %xmm0
movsd %xmm0, -0x318(%rbp)
leaq -0xe0(%rbp), %rdi
leaq -0x318(%rbp), %rsi
leaq -0x310(%rbp), %rdx
callq 0xb2b0
movsd -0x30(%rbp), %xmm1
movsd -0x10(%rbp), %xmm2
movsd -0x88(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movsd 0x2b4b(%rip), %xmm0     # 0xd420
mulsd %xmm1, %xmm0
movsd %xmm0, -0xda0(%rbp)
movsd -0x30(%rbp), %xmm0
movsd 0x2b3a(%rip), %xmm1     # 0xd428
callq 0x3410
movaps %xmm0, %xmm1
movsd -0xda0(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x368(%rbp)
leaq -0x80(%rbp), %rdi
leaq -0x368(%rbp), %rsi
leaq -0x360(%rbp), %rdx
callq 0xb2b0
leaq -0x80(%rbp), %rdi
leaq -0x360(%rbp), %rsi
leaq -0x3b0(%rbp), %rdx
callq 0xb330
leaq -0x1d0(%rbp), %rdi
leaq -0x310(%rbp), %rsi
leaq -0x400(%rbp), %rdx
callq 0xb410
leaq -0x400(%rbp), %rdi
leaq -0x3b0(%rbp), %rsi
leaq -0x450(%rbp), %rdx
callq 0xb410
movl $0x0, -0x574(%rbp)
cmpl $0x3, -0x574(%rbp)
jge 0xa9fb
movl $0x0, -0x578(%rbp)
cmpl $0x3, -0x578(%rbp)
jge 0xa9e5
movslq -0x574(%rbp), %rcx
leaq -0x2c0(%rbp), %rax
imulq $0x18, %rcx, %rcx
addq %rcx, %rax
movslq -0x578(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
movslq -0x574(%rbp), %rcx
leaq -0x570(%rbp), %rax
imulq $0x30, %rcx, %rcx
addq %rcx, %rax
movslq -0x578(%rbp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl -0x578(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x578(%rbp)
jmp 0xa989
jmp 0xa9e7
movl -0x574(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x574(%rbp)
jmp 0xa976
movl $0x0, -0x57c(%rbp)
cmpl $0x3, -0x57c(%rbp)
jge 0xaa93
movl $0x0, -0x580(%rbp)
cmpl $0x3, -0x580(%rbp)
jge 0xaa7d
movslq -0x57c(%rbp), %rcx
leaq -0x450(%rbp), %rax
imulq $0x18, %rcx, %rcx
addq %rcx, %rax
movslq -0x580(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
movslq -0x57c(%rbp), %rcx
leaq -0x570(%rbp), %rax
imulq $0x30, %rcx, %rcx
addq %rcx, %rax
movl -0x580(%rbp), %ecx
addl $0x3, %ecx
movslq %ecx, %rcx
movsd %xmm0, (%rax,%rcx,8)
movl -0x580(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x580(%rbp)
jmp 0xaa1c
jmp 0xaa7f
movl -0x57c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x57c(%rbp)
jmp 0xaa05
movl $0x0, -0x584(%rbp)
cmpl $0x3, -0x584(%rbp)
jge 0xab2b
movl $0x0, -0x588(%rbp)
cmpl $0x3, -0x588(%rbp)
jge 0xab15
movslq -0x584(%rbp), %rcx
leaq -0x130(%rbp), %rax
imulq $0x18, %rcx, %rcx
addq %rcx, %rax
movslq -0x588(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
movl -0x584(%rbp), %eax
addl $0x3, %eax
movslq %eax, %rcx
leaq -0x570(%rbp), %rax
imulq $0x30, %rcx, %rcx
addq %rcx, %rax
movslq -0x588(%rbp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl -0x588(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x588(%rbp)
jmp 0xaab4
jmp 0xab17
movl -0x584(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x584(%rbp)
jmp 0xaa9d
movl $0x0, -0x58c(%rbp)
cmpl $0x3, -0x58c(%rbp)
jge 0xabc8
movl $0x0, -0x590(%rbp)
cmpl $0x3, -0x590(%rbp)
jge 0xabb2
movslq -0x58c(%rbp), %rcx
leaq -0xe0(%rbp), %rax
imulq $0x18, %rcx, %rcx
addq %rcx, %rax
movslq -0x590(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
movl -0x58c(%rbp), %eax
addl $0x3, %eax
movslq %eax, %rcx
leaq -0x570(%rbp), %rax
imulq $0x30, %rcx, %rcx
addq %rcx, %rax
movl -0x590(%rbp), %ecx
addl $0x3, %ecx
movslq %ecx, %rcx
movsd %xmm0, (%rax,%rcx,8)
movl -0x590(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x590(%rbp)
jmp 0xab4c
jmp 0xabb4
movl -0x58c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x58c(%rbp)
jmp 0xab35
leaq -0x570(%rbp), %rdi
leaq -0x6b0(%rbp), %rsi
callq 0xb4a0
leaq -0x7d0(%rbp), %rdi
xorl %esi, %esi
movl $0x120, %edx            # imm = 0x120
callq 0x3140
movsd -0x18(%rbp), %xmm0
movsd 0x2815(%rip), %xmm1     # 0xd410
callq 0x3410
movsd %xmm0, -0xdb8(%rbp)
movsd -0x10(%rbp), %xmm0
movsd %xmm0, -0xdc0(%rbp)
movsd -0x20(%rbp), %xmm0
movsd 0x27ee(%rip), %xmm1     # 0xd410
callq 0x3410
movaps %xmm0, %xmm1
movsd 0x2806(%rip), %xmm0     # 0xd438
mulsd %xmm1, %xmm0
movsd %xmm0, -0xdc8(%rbp)
movsd -0x10(%rbp), %xmm0
movsd 0x27dd(%rip), %xmm1     # 0xd428
callq 0x3410
movsd -0xdc8(%rbp), %xmm1
movsd -0xdc0(%rbp), %xmm2
movaps %xmm0, %xmm3
movsd -0xdb8(%rbp), %xmm0
mulsd %xmm3, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x7d8(%rbp)
movsd -0x20(%rbp), %xmm0
movsd 0x2784(%rip), %xmm1     # 0xd410
callq 0x3410
movaps %xmm0, %xmm1
movsd 0x2794(%rip), %xmm0     # 0xd430
mulsd %xmm1, %xmm0
movsd %xmm0, -0xdb0(%rbp)
movsd -0x10(%rbp), %xmm0
movsd 0x275b(%rip), %xmm1     # 0xd410
callq 0x3410
movaps %xmm0, %xmm1
movsd -0xdb0(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0x7e0(%rbp)
movsd -0x7e0(%rbp), %xmm0
movsd %xmm0, -0x7e8(%rbp)
movsd -0x20(%rbp), %xmm0
movsd 0x2722(%rip), %xmm1     # 0xd410
callq 0x3410
mulsd -0x10(%rbp), %xmm0
movsd %xmm0, -0x7f0(%rbp)
movsd -0x7d8(%rbp), %xmm0
movsd %xmm0, -0x7d0(%rbp)
movsd -0x7d8(%rbp), %xmm0
movsd %xmm0, -0x798(%rbp)
movsd -0x7d8(%rbp), %xmm0
movsd %xmm0, -0x760(%rbp)
movsd -0x7e0(%rbp), %xmm0
movsd %xmm0, -0x7b8(%rbp)
movsd -0x7e0(%rbp), %xmm0
movsd %xmm0, -0x780(%rbp)
movsd -0x7e0(%rbp), %xmm0
movsd %xmm0, -0x748(%rbp)
movsd -0x7e8(%rbp), %xmm0
movsd %xmm0, -0x740(%rbp)
movsd -0x7e8(%rbp), %xmm0
movsd %xmm0, -0x708(%rbp)
movsd -0x7e8(%rbp), %xmm0
movsd %xmm0, -0x6d0(%rbp)
movsd -0x7f0(%rbp), %xmm0
movsd %xmm0, -0x728(%rbp)
movsd -0x7f0(%rbp), %xmm0
movsd %xmm0, -0x6f0(%rbp)
movsd -0x7f0(%rbp), %xmm0
movsd %xmm0, -0x6b8(%rbp)
movq -0x28(%rbp), %rdi
leaq -0x6b0(%rbp), %rsi
leaq -0x910(%rbp), %rdx
callq 0xb510
leaq -0x570(%rbp), %rdi
leaq -0x910(%rbp), %rsi
leaq -0xa30(%rbp), %rdx
callq 0xb510
leaq -0xa30(%rbp), %rdi
leaq -0x7d0(%rbp), %rsi
leaq -0xb50(%rbp), %rdx
callq 0xb5f0
leaq -0xb50(%rbp), %rdi
leaq -0xc70(%rbp), %rsi
callq 0xb4a0
leaq -0xb50(%rbp), %rdi
leaq -0xc70(%rbp), %rsi
leaq -0xd90(%rbp), %rdx
callq 0xb5f0
movsd 0x25d8(%rip), %xmm0     # 0xd418
movsd %xmm0, -0xd98(%rbp)
movq -0x28(%rbp), %rdx
leaq -0xd90(%rbp), %rdi
leaq -0xd98(%rbp), %rsi
callq 0xb680
addq $0xdd0, %rsp            # imm = 0xDD0
popq %rbp
retq
nopl (%rax,%rax)