// File: /workspace/github_cpp_makefile_star2_O2/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:
movapd %xmm0, %xmm6
movsd (%rdi), %xmm0
movsd 0x8(%rdi), %xmm7
movapd %xmm7, %xmm4
mulsd %xmm7, %xmm4
movapd %xmm0, %xmm3
mulsd %xmm0, %xmm3
addsd %xmm4, %xmm3
movsd 0x10(%rdi), %xmm5
movapd %xmm5, %xmm4
mulsd %xmm5, %xmm4
addsd %xmm3, %xmm4
sqrtsd %xmm4, %xmm4
xorpd %xmm3, %xmm3
ucomisd %xmm3, %xmm4
jne 0x58bc
jp 0x58bc
retq
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xd68, %rsp            # imm = 0xD68
movsd %xmm1, -0x38(%rbp)
movsd %xmm2, -0x40(%rbp)
movq %rsi, -0x70(%rbp)
leaq -0x5b0(%rbp), %r15
andq $0x0, (%r15)
movapd 0x1b4f(%rip), %xmm1     # 0x7440
movapd %xmm5, %xmm3
xorpd %xmm1, %xmm3
movlpd %xmm3, 0x8(%r15)
movsd %xmm7, 0x10(%r15)
movsd %xmm5, 0x18(%r15)
andq $0x0, 0x20(%r15)
movapd %xmm0, %xmm2
unpcklpd %xmm7, %xmm2            # xmm2 = xmm2[0],xmm7[0]
xorpd %xmm1, %xmm2
movupd %xmm2, 0x28(%r15)
movsd %xmm0, 0x38(%r15)
andq $0x0, 0x40(%r15)
movapd %xmm4, %xmm0
mulsd %xmm6, %xmm0
movapd %xmm0, -0x50(%rbp)
movapd %xmm6, -0x60(%rbp)
movsd %xmm4, -0x30(%rbp)
callq 0x2080
movapd %xmm0, -0x80(%rbp)
movaps -0x50(%rbp), %xmm0
callq 0x2380
movsd %xmm0, -0x68(%rbp)
xorps %xmm0, %xmm0
leaq -0x560(%rbp), %r14
movaps %xmm0, (%r14)
movaps %xmm0, 0x20(%r14)
movaps %xmm0, 0x30(%r14)
movaps %xmm0, 0x10(%r14)
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, (%r14)
movq %rax, 0x20(%r14)
movq %rax, 0x40(%r14)
movaps %xmm0, -0xa0(%rbp)
movaps %xmm0, -0xb0(%rbp)
movaps %xmm0, -0xc0(%rbp)
movaps %xmm0, -0xd0(%rbp)
andq $0x0, -0x90(%rbp)
movapd -0x80(%rbp), %xmm0
xorpd 0x1a81(%rip), %xmm0     # 0x7440
divsd -0x30(%rbp), %xmm0
leaq -0x1f0(%rbp), %r12
movsd %xmm0, (%r12)
leaq -0x330(%rbp), %rdx
movq %r15, %rdi
movq %r12, %rsi
callq 0x5f96
movsd 0x1a2d(%rip), %xmm0     # 0x7418
subsd -0x68(%rbp), %xmm0
movsd -0x30(%rbp), %xmm1
mulsd %xmm1, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, (%r12)
leaq -0x1f0(%rbp), %rsi
leaq -0x2e0(%rbp), %r12
movq %r15, %rdi
movq %r12, %rdx
callq 0x5f96
leaq -0x510(%rbp), %rbx
movq %r15, %rdi
movq %r12, %rsi
movq %rbx, %rdx
callq 0x5fcc
leaq -0x4c0(%rbp), %r13
movq %r14, %rdi
leaq -0x330(%rbp), %rsi
movq %r13, %rdx
callq 0x6034
leaq -0x470(%rbp), %r12
movq %r13, %rdi
movq %rbx, %rsi
movq %r12, %rdx
callq 0x6034
movaps -0x60(%rbp), %xmm0
xorps 0x19d6(%rip), %xmm0     # 0x7440
leaq -0x1f0(%rbp), %rax
movlps %xmm0, (%rax)
leaq -0x1f0(%rbp), %rbx
leaq -0x290(%rbp), %rdx
movq %r14, %rdi
movq %rbx, %rsi
callq 0x5f96
movapd -0x50(%rbp), %xmm0
subsd -0x80(%rbp), %xmm0
xorpd 0x19a1(%rip), %xmm0     # 0x7440
movapd %xmm0, -0x50(%rbp)
movsd 0x1974(%rip), %xmm1     # 0x7420
movsd -0x30(%rbp), %xmm0
callq 0x2340
movapd -0x50(%rbp), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, (%rbx)
leaq -0x420(%rbp), %r13
movq %r15, %rdi
movq %rbx, %rsi
movq %r13, %rdx
callq 0x5f96
leaq -0x240(%rbp), %rdx
movq %r15, %rdi
movq %r13, %rsi
callq 0x5fcc
leaq -0x3d0(%rbp), %r13
leaq -0x2e0(%rbp), %rdi
leaq -0x290(%rbp), %rsi
movq %r13, %rdx
callq 0x6034
leaq -0x380(%rbp), %r15
movq %r13, %rdi
leaq -0x240(%rbp), %rsi
movq %r15, %rdx
callq 0x6034
xorl %eax, %eax
cmpq $0x3, %rax
je 0x5b4d
xorl %ecx, %ecx
cmpq $0x3, %rcx
je 0x5b40
movsd (%r12,%rcx,8), %xmm0
movsd %xmm0, (%rbx,%rcx,8)
incq %rcx
jmp 0x5b2a
incq %rax
addq $0x18, %r12
addq $0x30, %rbx
jmp 0x5b22
leaq -0x1d8(%rbp), %rax
xorl %ecx, %ecx
cmpq $0x3, %rcx
je 0x5b81
xorl %edx, %edx
cmpq $0x3, %rdx
je 0x5b74
movsd (%r15,%rdx,8), %xmm0
movsd %xmm0, (%rax,%rdx,8)
incq %rdx
jmp 0x5b5e
incq %rcx
addq $0x18, %r15
addq $0x30, %rax
jmp 0x5b56
leaq -0xd0(%rbp), %rax
leaq -0x160(%rbp), %rcx
xorl %edx, %edx
cmpq $0x3, %rdx
je 0x5bbb
xorl %esi, %esi
cmpq $0x3, %rsi
je 0x5bae
movsd (%rax,%rsi,8), %xmm0
movsd %xmm0, (%rcx,%rsi,8)
incq %rsi
jmp 0x5b99
incq %rdx
addq $0x18, %rax
addq $0x30, %rcx
jmp 0x5b91
leaq -0x148(%rbp), %rax
xorl %ecx, %ecx
cmpq $0x3, %rcx
je 0x5bef
xorl %edx, %edx
cmpq $0x3, %rdx
je 0x5be2
movsd (%r14,%rdx,8), %xmm0
movsd %xmm0, (%rax,%rdx,8)
incq %rdx
jmp 0x5bcc
incq %rcx
addq $0x18, %r14
addq $0x30, %rax
jmp 0x5bc4
leaq -0x1f0(%rbp), %r14
leaq -0xd90(%rbp), %r15
movq %r14, %rdi
movq %r15, %rsi
callq 0x606f
leaq -0xc70(%rbp), %rbx
movl $0x120, %edx            # imm = 0x120
movq %rbx, %rdi
xorl %esi, %esi
callq 0x2130
movsd -0x38(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x38(%rbp)
movsd -0x40(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x40(%rbp)
movsd 0x17e6(%rip), %xmm1     # 0x7428
mulsd %xmm0, %xmm1
movsd %xmm1, -0x50(%rbp)
movsd 0x17cd(%rip), %xmm1     # 0x7420
movapd -0x60(%rbp), %xmm0
callq 0x2340
mulsd -0x50(%rbp), %xmm0
movsd -0x38(%rbp), %xmm2
movapd -0x60(%rbp), %xmm3
mulsd %xmm3, %xmm2
addsd %xmm0, %xmm2
movsd 0x17b4(%rip), %xmm0     # 0x7430
movsd -0x40(%rbp), %xmm1
mulsd %xmm1, %xmm0
mulsd %xmm3, %xmm1
mulsd %xmm3, %xmm3
mulsd %xmm0, %xmm3
movsd %xmm2, (%rbx)
movsd %xmm2, 0x38(%rbx)
movsd %xmm2, 0x70(%rbx)
movsd %xmm3, 0x18(%rbx)
movsd %xmm3, 0x50(%rbx)
movsd %xmm3, 0x88(%rbx)
movsd %xmm3, 0x90(%rbx)
movsd %xmm3, 0xc8(%rbx)
movsd %xmm3, 0x100(%rbx)
movsd %xmm1, 0xa8(%rbx)
movsd %xmm1, 0xe0(%rbx)
movsd %xmm1, 0x118(%rbx)
leaq -0xb50(%rbp), %r12
movq -0x70(%rbp), %r13
movq %r13, %rdi
movq %r15, %rsi
movq %r12, %rdx
callq 0x60a7
leaq -0xa30(%rbp), %r15
movq %r14, %rdi
movq %r12, %rsi
movq %r15, %rdx
callq 0x60a7
leaq -0x910(%rbp), %r14
movq %r15, %rdi
movq %rbx, %rsi
movq %r14, %rdx
callq 0x610f
leaq -0x7f0(%rbp), %rbx
movq %r14, %rdi
movq %rbx, %rsi
callq 0x606f
leaq -0x6d0(%rbp), %r15
movq %r14, %rdi
movq %rbx, %rsi
movq %r15, %rdx
callq 0x610f
movabsq $0x3fe0000000000000, %rax # imm = 0x3FE0000000000000
leaq -0x88(%rbp), %rsi
movq %rax, (%rsi)
movq %r15, %rdi
movq %r13, %rdx
callq 0x614a
addq $0xd68, %rsp            # imm = 0xD68
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq