// File: /workspace/github_cpp_makefile_star2_O1/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
pushq %r15
pushq %r14
pushq %r12
pushq %rbx
subq $0xd50, %rsp            # imm = 0xD50
movq %rsi, %rbx
movaps %xmm0, -0x50(%rbp)
movq %rdi, %r14
movsd (%rdi), %xmm3
movsd 0x8(%rdi), %xmm0
mulsd %xmm0, %xmm0
mulsd %xmm3, %xmm3
addsd %xmm0, %xmm3
movsd 0x10(%rdi), %xmm0
mulsd %xmm0, %xmm0
addsd %xmm3, %xmm0
xorpd %xmm4, %xmm4
ucomisd %xmm4, %xmm0
movsd %xmm2, -0x30(%rbp)
movsd %xmm1, -0x28(%rbp)
jb 0x6923
xorps %xmm3, %xmm3
sqrtsd %xmm0, %xmm3
jmp 0x6930
callq 0x21f0
xorpd %xmm4, %xmm4
movapd %xmm0, %xmm3
ucomisd %xmm4, %xmm3
jne 0x693c
jnp 0x7145
xorl %r15d, %r15d
leaq -0x150(%rbp), %r12
movq %r15, (%r12)
movsd 0x10(%r14), %xmm0
movapd 0x1af8(%rip), %xmm4     # 0x8450
movapd %xmm0, %xmm1
xorpd %xmm4, %xmm1
movlpd %xmm1, 0x8(%r12)
movsd 0x8(%r14), %xmm1
movsd %xmm1, 0x10(%r12)
movsd %xmm0, 0x18(%r12)
movq %r15, 0x20(%r12)
movsd (%r14), %xmm0
movapd %xmm0, %xmm2
unpcklpd %xmm1, %xmm2            # xmm2 = xmm2[0],xmm1[0]
xorpd %xmm4, %xmm2
movupd %xmm2, 0x28(%r12)
movsd %xmm0, 0x38(%r12)
movq %r15, 0x40(%r12)
movapd %xmm3, %xmm0
mulsd -0x50(%rbp), %xmm0
movapd %xmm0, -0x40(%rbp)
movsd %xmm3, -0x58(%rbp)
callq 0x2090
movapd %xmm0, -0xb0(%rbp)
movapd -0x40(%rbp), %xmm0
callq 0x23e0
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 %r15, -0xc0(%rbp)
movapd %xmm1, -0xd0(%rbp)
movapd %xmm1, -0xe0(%rbp)
movapd %xmm1, -0xf0(%rbp)
movapd %xmm1, -0x100(%rbp)
xorpd 0x1a0f(%rip), %xmm4     # 0x8450
movapd %xmm4, %xmm1
divsd %xmm3, %xmm1
leaq -0x590(%rbp), %rax
xorl %ecx, %ecx
movsd (%r12,%rcx,8), %xmm2
mulsd %xmm1, %xmm2
movsd %xmm2, (%rax,%rcx,8)
incq %rcx
cmpq $0x3, %rcx
jne 0x6a52
incq %r15
addq $0x18, %rax
addq $0x18, %r12
cmpq $0x3, %r15
jne 0x6a50
movsd 0x1995(%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 -0x40(%rbp), %xmm2
xorl %esi, %esi
movsd (%rcx,%rsi,8), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, (%rax,%rsi,8)
incq %rsi
cmpq $0x3, %rsi
jne 0x6aaa
incq %rdx
addq $0x18, %rax
addq $0x18, %rcx
cmpq $0x3, %rdx
jne 0x6aa8
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
movq $0x0, (%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 0x6b0c
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x3, %r8
jne 0x6af7
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x6ae2
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 0x6b60
incq %rsi
addq $0x18, %rax
addq $0x18, %rcx
addq $0x18, %rdx
cmpq $0x3, %rsi
jne 0x6b5e
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 0x6ba6
incq %rsi
addq $0x18, %rax
addq $0x18, %rcx
addq $0x18, %rdx
cmpq $0x3, %rsi
jne 0x6ba4
movapd 0x1875(%rip), %xmm0     # 0x8450
xorpd -0x50(%rbp), %xmm0
leaq -0x4a0(%rbp), %rax
leaq -0xa0(%rbp), %rcx
xorl %edx, %edx
xorl %esi, %esi
movsd (%rcx,%rsi,8), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, (%rax,%rsi,8)
incq %rsi
cmpq $0x3, %rsi
jne 0x6bf2
incq %rdx
addq $0x18, %rax
addq $0x18, %rcx
cmpq $0x3, %rdx
jne 0x6bf0
addsd %xmm4, %xmm2
xorpd 0x182a(%rip), %xmm2     # 0x8450
movapd %xmm2, -0x40(%rbp)
movsd 0x17ed(%rip), %xmm1     # 0x8420
movapd %xmm3, %xmm0
callq 0x23a0
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 0x6c57
incq %rdx
addq $0x18, %rax
addq $0x18, %rcx
cmpq $0x3, %rdx
jne 0x6c55
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
movq $0x0, (%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 0x6cb9
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x3, %r8
jne 0x6ca4
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x6c8f
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 0x6d0d
incq %rsi
addq $0x18, %rax
addq $0x18, %rcx
addq $0x18, %rdx
cmpq $0x3, %rsi
jne 0x6d0b
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 0x6d53
incq %rsi
addq $0x18, %rax
addq $0x18, %rcx
addq $0x18, %rdx
cmpq $0x3, %rsi
jne 0x6d51
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 0x6d82
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 0x6db3
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 0x6de1
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 0x6e0f
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 0x6e50
incq %rdx
addq $0x30, %rax
addq $0x8, %rcx
cmpq $0x6, %rdx
jne 0x6e4b
leaq -0xd70(%rbp), %r14
xorl %r15d, %r15d
movl $0x120, %edx            # imm = 0x120
movq %r14, %rdi
xorl %esi, %esi
callq 0x2130
movsd -0x28(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x28(%rbp)
movsd -0x30(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x30(%rbp)
movsd 0x1574(%rip), %xmm1     # 0x8428
mulsd %xmm0, %xmm1
movsd %xmm1, -0x40(%rbp)
movsd 0x155b(%rip), %xmm1     # 0x8420
movapd -0x50(%rbp), %xmm0
callq 0x23a0
mulsd -0x40(%rbp), %xmm0
movsd -0x28(%rbp), %xmm2
movapd -0x50(%rbp), %xmm3
mulsd %xmm3, %xmm2
addsd %xmm0, %xmm2
movsd 0x1542(%rip), %xmm0     # 0x8430
movsd -0x30(%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
movq $0x0, (%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 0x6f95
movsd %xmm0, (%r8)
incq %rdi
addq $0x8, %rsi
cmpq $0x6, %rdi
jne 0x6f80
incq %r15
addq $0x30, %rcx
cmpq $0x6, %r15
jne 0x6f69
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
movq $0x0, (%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 0x700d
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x6, %r8
jne 0x6ff8
incq %rcx
addq $0x30, %rax
cmpq $0x6, %rcx
jne 0x6fe0
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 0x705a
incq %rdx
addq $0x30, %rax
addq $0x30, %r14
addq $0x30, %rcx
cmpq $0x6, %rdx
jne 0x7058
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 0x709d
incq %rdx
addq $0x30, %rax
addq $0x8, %rcx
cmpq $0x6, %rdx
jne 0x7098
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 0x70dd
incq %rsi
addq $0x30, %rax
addq $0x30, %rcx
addq $0x30, %rdx
cmpq $0x6, %rsi
jne 0x70db
leaq -0x7d0(%rbp), %rax
xorl %ecx, %ecx
movsd 0x12f5(%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 0x711d
incq %rcx
addq $0x30, %rbx
addq $0x30, %rax
cmpq $0x6, %rcx
jne 0x711b
addq $0xd50, %rsp            # imm = 0xD50
popq %rbx
popq %r12
popq %r14
popq %r15
popq %rbp
retq