// File: /workspace/github_cpp_makefile_star2_O3/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:
movsd (%rdi), %xmm6
movsd 0x8(%rdi), %xmm7
movapd %xmm7, %xmm4
mulsd %xmm7, %xmm4
movapd %xmm6, %xmm3
mulsd %xmm6, %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 0x674f
jp 0x674f
retq
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %rbx
subq $0xd58, %rsp            # imm = 0xD58
movq %rsi, %rbx
movsd %xmm1, -0x20(%rbp)
movsd %xmm2, -0x28(%rbp)
xorl %r14d, %r14d
leaq -0x150(%rbp), %r15
movq %r14, (%r15)
movapd 0x1cbf(%rip), %xmm1     # 0x8440
movapd %xmm5, %xmm3
xorpd %xmm1, %xmm3
movlpd %xmm3, 0x8(%r15)
movsd %xmm7, 0x10(%r15)
movsd %xmm5, 0x18(%r15)
movq %r14, 0x20(%r15)
movapd %xmm6, %xmm2
unpcklpd %xmm7, %xmm2            # xmm2 = xmm2[0],xmm7[0]
xorpd %xmm1, %xmm2
movupd %xmm2, 0x28(%r15)
movsd %xmm6, 0x38(%r15)
movq %r14, 0x40(%r15)
movapd %xmm4, %xmm1
movapd %xmm0, -0x50(%rbp)
mulsd %xmm0, %xmm1
movapd %xmm1, -0x40(%rbp)
movapd %xmm1, %xmm0
movsd %xmm4, -0x58(%rbp)
callq 0x2090
movapd %xmm0, -0xb0(%rbp)
movapd -0x40(%rbp), %xmm0
callq 0x23d0
movapd -0xb0(%rbp), %xmm4
movsd -0x58(%rbp), %xmm3
xorpd %xmm1, %xmm1
movapd %xmm1, -0xa0(%rbp)
movapd %xmm1, -0x80(%rbp)
movapd %xmm1, -0x70(%rbp)
movapd %xmm1, -0x90(%rbp)
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, -0xa0(%rbp)
movq %rax, -0x80(%rbp)
movq %rax, -0x60(%rbp)
movq %r14, -0xc0(%rbp)
movapd %xmm1, -0xd0(%rbp)
movapd %xmm1, -0xe0(%rbp)
movapd %xmm1, -0xf0(%rbp)
movapd %xmm1, -0x100(%rbp)
xorpd 0x1be0(%rip), %xmm4     # 0x8440
movapd %xmm4, %xmm1
divsd %xmm3, %xmm1
leaq -0x590(%rbp), %rax
xorl %ecx, %ecx
movsd (%r15,%rcx,8), %xmm2
mulsd %xmm1, %xmm2
movsd %xmm2, (%rax,%rcx,8)
incq %rcx
cmpq $0x3, %rcx
jne 0x6871
incq %r14
addq $0x18, %rax
addq $0x18, %r15
cmpq $0x3, %r14
jne 0x686f
movsd 0x1b76(%rip), %xmm1     # 0x8418
subsd %xmm0, %xmm1
movapd %xmm3, %xmm0
mulsd %xmm3, %xmm0
divsd %xmm0, %xmm1
leaq -0x240(%rbp), %rax
leaq -0x150(%rbp), %rcx
xorl %edx, %edx
movapd -0x50(%rbp), %xmm2
xorl %esi, %esi
movsd (%rcx,%rsi,8), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, (%rax,%rsi,8)
incq %rsi
cmpq $0x3, %rsi
jne 0x68c9
incq %rdx
addq $0x18, %rax
addq $0x18, %rcx
cmpq $0x3, %rdx
jne 0x68c7
leaq -0x150(%rbp), %rax
xorl %ecx, %ecx
leaq -0x240(%rbp), %rdx
leaq (%rcx,%rcx,2), %rsi
leaq -0x540(,%rsi,8), %rsi
addq %rbp, %rsi
movq %rdx, %rdi
xorl %r8d, %r8d
leaq (%rsi,%r8,8), %r9
xorpd %xmm0, %xmm0
movq %rdi, %r10
xorl %r11d, %r11d
movsd (%rax,%r11,8), %xmm1
mulsd (%r10), %xmm1
addsd %xmm1, %xmm0
incq %r11
addq $0x18, %r10
cmpq $0x3, %r11
jne 0x6924
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x3, %r8
jne 0x6916
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x6901
leaq -0x4f0(%rbp), %rax
leaq -0x590(%rbp), %rcx
leaq -0xa0(%rbp), %rdx
xorl %esi, %esi
xorl %edi, %edi
movsd (%rdx,%rdi,8), %xmm0
addsd (%rcx,%rdi,8), %xmm0
movsd %xmm0, (%rax,%rdi,8)
incq %rdi
cmpq $0x3, %rdi
jne 0x6978
incq %rsi
addq $0x18, %rax
addq $0x18, %rcx
addq $0x18, %rdx
cmpq $0x3, %rsi
jne 0x6976
leaq -0x1f0(%rbp), %rax
leaq -0x540(%rbp), %rcx
leaq -0x4f0(%rbp), %rdx
xorl %esi, %esi
xorl %edi, %edi
movsd (%rdx,%rdi,8), %xmm0
addsd (%rcx,%rdi,8), %xmm0
movsd %xmm0, (%rax,%rdi,8)
incq %rdi
cmpq $0x3, %rdi
jne 0x69be
incq %rsi
addq $0x18, %rax
addq $0x18, %rcx
addq $0x18, %rdx
cmpq $0x3, %rsi
jne 0x69bc
movapd 0x1a4d(%rip), %xmm0     # 0x8440
xorpd %xmm2, %xmm0
leaq -0x4a0(%rbp), %rax
leaq -0xa0(%rbp), %rcx
xorl %edx, %edx
movapd -0x40(%rbp), %xmm2
xorl %esi, %esi
movsd (%rcx,%rsi,8), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, (%rax,%rsi,8)
incq %rsi
cmpq $0x3, %rsi
jne 0x6a0e
incq %rdx
addq $0x18, %rax
addq $0x18, %rcx
cmpq $0x3, %rdx
jne 0x6a0c
addsd %xmm4, %xmm2
xorpd 0x19fe(%rip), %xmm2     # 0x8440
movapd %xmm2, -0x40(%rbp)
movsd 0x19d1(%rip), %xmm1     # 0x8420
movapd %xmm3, %xmm0
callq 0x2390
movapd -0x40(%rbp), %xmm1
divsd %xmm0, %xmm1
leaq -0x450(%rbp), %rax
leaq -0x150(%rbp), %rcx
xorl %edx, %edx
xorl %esi, %esi
movsd (%rcx,%rsi,8), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, (%rax,%rsi,8)
incq %rsi
cmpq $0x3, %rsi
jne 0x6a73
incq %rdx
addq $0x18, %rax
addq $0x18, %rcx
cmpq $0x3, %rdx
jne 0x6a71
leaq -0x150(%rbp), %rax
xorl %ecx, %ecx
leaq -0x450(%rbp), %rdx
leaq (%rcx,%rcx,2), %rsi
leaq -0x400(,%rsi,8), %rsi
addq %rbp, %rsi
movq %rdx, %rdi
xorl %r8d, %r8d
leaq (%rsi,%r8,8), %r9
xorpd %xmm0, %xmm0
movq %rdi, %r10
xorl %r11d, %r11d
movsd (%rax,%r11,8), %xmm1
mulsd (%r10), %xmm1
addsd %xmm1, %xmm0
incq %r11
addq $0x18, %r10
cmpq $0x3, %r11
jne 0x6ace
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x3, %r8
jne 0x6ac0
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x6aab
leaq -0x3b0(%rbp), %rax
leaq -0x4a0(%rbp), %rcx
leaq -0x240(%rbp), %rdx
xorl %esi, %esi
xorl %edi, %edi
movsd (%rdx,%rdi,8), %xmm0
addsd (%rcx,%rdi,8), %xmm0
movsd %xmm0, (%rax,%rdi,8)
incq %rdi
cmpq $0x3, %rdi
jne 0x6b22
incq %rsi
addq $0x18, %rax
addq $0x18, %rcx
addq $0x18, %rdx
cmpq $0x3, %rsi
jne 0x6b20
leaq -0x1a0(%rbp), %rax
leaq -0x400(%rbp), %rcx
leaq -0x3b0(%rbp), %rdx
xorl %esi, %esi
xorl %edi, %edi
movsd (%rdx,%rdi,8), %xmm0
addsd (%rcx,%rdi,8), %xmm0
movsd %xmm0, (%rax,%rdi,8)
incq %rdi
cmpq $0x3, %rdi
jne 0x6b68
incq %rsi
addq $0x18, %rax
addq $0x18, %rcx
addq $0x18, %rdx
cmpq $0x3, %rsi
jne 0x6b66
xorl %eax, %eax
movq -0x1e0(%rbp,%rax), %rcx
movq %rcx, -0x350(%rbp,%rax,2)
movupd -0x1f0(%rbp,%rax), %xmm0
movupd %xmm0, -0x360(%rbp,%rax,2)
addq $0x18, %rax
cmpq $0x48, %rax
jne 0x6b97
movl $0xc, %eax
movq -0x19c(%rbp,%rax), %rcx
movq %rcx, -0x350(%rbp,%rax,2)
movupd -0x1ac(%rbp,%rax), %xmm0
movupd %xmm0, -0x360(%rbp,%rax,2)
addq $0x18, %rax
cmpq $0x54, %rax
jne 0x6bc8
xorl %eax, %eax
movq -0xf0(%rbp,%rax), %rcx
movq %rcx, -0x2c0(%rbp,%rax,2)
movupd -0x100(%rbp,%rax), %xmm0
movupd %xmm0, -0x2d0(%rbp,%rax,2)
addq $0x18, %rax
cmpq $0x48, %rax
jne 0x6bf6
xorl %eax, %eax
movq -0x90(%rbp,%rax), %rcx
movq %rcx, -0x2a8(%rbp,%rax,2)
movupd -0xa0(%rbp,%rax), %xmm0
movupd %xmm0, -0x2b8(%rbp,%rax,2)
addq $0x18, %rax
cmpq $0x48, %rax
jne 0x6c24
leaq -0xc50(%rbp), %rax
leaq -0x360(%rbp), %rcx
xorl %edx, %edx
movq %rcx, %rsi
xorl %edi, %edi
movsd (%rsi), %xmm0
movsd %xmm0, (%rax,%rdi,8)
incq %rdi
addq $0x30, %rsi
cmpq $0x6, %rdi
jne 0x6c65
incq %rdx
addq $0x30, %rax
addq $0x8, %rcx
cmpq $0x6, %rdx
jne 0x6c60
leaq -0xd70(%rbp), %r14
xorl %r15d, %r15d
movl $0x120, %edx            # imm = 0x120
movq %r14, %rdi
xorl %esi, %esi
callq 0x2130
movsd -0x20(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x20(%rbp)
movsd -0x28(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x28(%rbp)
movsd 0x175f(%rip), %xmm1     # 0x8428
mulsd %xmm0, %xmm1
movsd %xmm1, -0x40(%rbp)
movsd 0x1746(%rip), %xmm1     # 0x8420
movapd -0x50(%rbp), %xmm0
callq 0x2390
mulsd -0x40(%rbp), %xmm0
movsd -0x20(%rbp), %xmm2
movapd -0x50(%rbp), %xmm3
mulsd %xmm3, %xmm2
addsd %xmm0, %xmm2
movsd 0x172d(%rip), %xmm0     # 0x8430
movsd -0x28(%rbp), %xmm1
mulsd %xmm1, %xmm0
mulsd %xmm3, %xmm1
mulsd %xmm3, %xmm3
mulsd %xmm0, %xmm3
movsd %xmm2, (%r14)
movsd %xmm2, 0x38(%r14)
movsd %xmm2, 0x70(%r14)
movsd %xmm3, 0x18(%r14)
movsd %xmm3, 0x50(%r14)
movsd %xmm3, 0x88(%r14)
movsd %xmm3, 0x90(%r14)
movsd %xmm3, 0xc8(%r14)
movsd %xmm3, 0x100(%r14)
movsd %xmm1, 0xa8(%r14)
movsd %xmm1, 0xe0(%r14)
movsd %xmm1, 0x118(%r14)
leaq -0xc50(%rbp), %rax
movq %rbx, %rcx
leaq (%r15,%r15,2), %rdx
shlq $0x4, %rdx
addq %rbp, %rdx
addq $-0xb30, %rdx           # imm = 0xF4D0
movq %rax, %rsi
xorl %edi, %edi
leaq (%rdx,%rdi,8), %r8
xorpd %xmm0, %xmm0
movq %rsi, %r9
xorl %r10d, %r10d
movsd (%rcx,%r10,8), %xmm1
mulsd (%r9), %xmm1
addsd %xmm1, %xmm0
incq %r10
addq $0x30, %r9
cmpq $0x6, %r10
jne 0x6da3
movsd %xmm0, (%r8)
incq %rdi
addq $0x8, %rsi
cmpq $0x6, %rdi
jne 0x6d95
incq %r15
addq $0x30, %rcx
cmpq $0x6, %r15
jne 0x6d7e
leaq -0x360(%rbp), %rax
xorl %ecx, %ecx
leaq -0xb30(%rbp), %rdx
leaq (%rcx,%rcx,2), %rsi
shlq $0x4, %rsi
addq %rbp, %rsi
addq $-0xa10, %rsi           # imm = 0xF5F0
movq %rdx, %rdi
xorl %r8d, %r8d
leaq (%rsi,%r8,8), %r9
xorpd %xmm0, %xmm0
movq %rdi, %r10
xorl %r11d, %r11d
movsd (%rax,%r11,8), %xmm1
mulsd (%r10), %xmm1
addsd %xmm1, %xmm0
incq %r11
addq $0x30, %r10
cmpq $0x6, %r11
jne 0x6e14
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x6, %r8
jne 0x6e06
incq %rcx
addq $0x30, %rax
cmpq $0x6, %rcx
jne 0x6dee
leaq -0x6b0(%rbp), %rax
leaq -0xa10(%rbp), %rcx
xorl %edx, %edx
xorl %esi, %esi
movsd (%rcx,%rsi,8), %xmm0
addsd (%r14,%rsi,8), %xmm0
movsd %xmm0, (%rax,%rsi,8)
incq %rsi
cmpq $0x6, %rsi
jne 0x6e61
incq %rdx
addq $0x30, %rax
addq $0x30, %r14
addq $0x30, %rcx
cmpq $0x6, %rdx
jne 0x6e5f
leaq -0x8f0(%rbp), %rax
leaq -0x6b0(%rbp), %rcx
xorl %edx, %edx
movq %rcx, %rsi
xorl %edi, %edi
movsd (%rsi), %xmm0
movsd %xmm0, (%rax,%rdi,8)
incq %rdi
addq $0x30, %rsi
cmpq $0x6, %rdi
jne 0x6ea4
incq %rdx
addq $0x30, %rax
addq $0x8, %rcx
cmpq $0x6, %rdx
jne 0x6e9f
leaq -0x7d0(%rbp), %rax
leaq -0x8f0(%rbp), %rcx
leaq -0x6b0(%rbp), %rdx
xorl %esi, %esi
xorl %edi, %edi
movsd (%rdx,%rdi,8), %xmm0
addsd (%rcx,%rdi,8), %xmm0
movsd %xmm0, (%rax,%rdi,8)
incq %rdi
cmpq $0x6, %rdi
jne 0x6ee4
incq %rsi
addq $0x30, %rax
addq $0x30, %rcx
addq $0x30, %rdx
cmpq $0x6, %rsi
jne 0x6ee2
leaq -0x7d0(%rbp), %rax
xorl %ecx, %ecx
movsd 0x14ee(%rip), %xmm0     # 0x8410
xorl %edx, %edx
movsd (%rax,%rdx,8), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, (%rbx,%rdx,8)
incq %rdx
cmpq $0x6, %rdx
jne 0x6f24
incq %rcx
addq $0x30, %rbx
addq $0x30, %rax
cmpq $0x6, %rcx
jne 0x6f22
addq $0xd58, %rsp            # imm = 0xD58
popq %rbx
popq %r14
popq %r15
popq %rbp
retq