// File: rachit31[P]CubeSatADCS/propagateANDupdate.cpp
// Function: update(double (&) [4], double (&) [3], double (&) [6][6], double const (&) [3], double const (&) [3], double)
void update(double (&q_BI)[4], double (&Bw_B)[3], double (&P)[6][6], const double (&v_B)[3], const double (&v_I)[3], const double sigmav)
{
  // Create DCM from estimate of q_BI
  double A_BI[3][3];
  q2A(q_BI,A_BI);

  // Project Inertial Vector to Body
  double vhat_B[3];
  matMult(A_BI,v_I,vhat_B);

  // Create Cross Body Matrix from vhat_B
  double vhat_BX[3][3];
  X(vhat_B,vhat_BX);

  // Transpose the Cross Product Matrix
  double vhat_BXT[3][3];
  trans3x3(vhat_BX,vhat_BXT);

  // Extract Upper Left Covariance Sub Matrix
  double P11[3][3];
  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      P11[i][j] = P[i][j];
    }
  }

  // P(1:3,1:3)*vhat_BX'
  double P11vhat_BXT[3][3];
  mult3x3(P11,vhat_BXT,P11vhat_BXT);

  // vhat_BX*P(1:3,1:3)*vhat_BX'
  double vhat_BXP11vhat_BXT[3][3];
  mult3x3(vhat_BX,P11vhat_BXT,vhat_BXP11vhat_BXT);

  // Form measurment covariance matrix
  double R[3][3] = { { 0, 0, 0},
                     { 0, 0, 0},
                     { 0, 0, 0} };
  R[0][0] = pow(sigmav,2);
  R[1][1] = pow(sigmav,2);
  R[2][2] = pow(sigmav,2);

  // H*P*H' + R
  double HPHT_R[3][3];
  add3x3(vhat_BXP11vhat_BXT,R,HPHT_R);

  // inv(H*P*H'+R)
  double invHPHT_R[3][3];
  inv3x3(HPHT_R,invHPHT_R);

  // H'*inv(H*P*H'+R)
  double vhat_BXTinvHPHT_R[3][3];
  mult3x3(vhat_BXT,invHPHT_R,vhat_BXTinvHPHT_R);

  // P(1:3,1:3)*H'*inv(H*P*H'+R)
  double K1[3][3];
  mult3x3(P11,vhat_BXTinvHPHT_R,K1);

  // Extract Bottom Left Covariance Sub Matrix
  double P21[3][3];
  for (int i = 3; i < 6; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      P21[i-3][j] = P[i][j];
    }
  }

  // P(4:6,1:3)*H'*inv(H*P*H'+R)
  double K2[3][3];
  mult3x3(P21,vhat_BXTinvHPHT_R,K2);

  // Calculate Residual
  double residual[3];
  residual[0] = v_B[0] - vhat_B[0];
  residual[1] = v_B[1] - vhat_B[1];
  residual[2] = v_B[2] - vhat_B[2];

  // Calculate Error Angles  K1*residual
  double dth[3];
  matMult(K1,residual,dth);

  // Calculate Error Bw K2*residual
  double dBw[3];
  matMult(K2,residual,dBw);

  // Update Quaternion
  double q[4];
  q[0] = q_BI[0] - (dth[0]*q_BI[1])/2.0 - (dth[1]*q_BI[2])/2.0 - (dth[2]*q_BI[3])/2.0;
  q[1] = q_BI[1] + (dth[0]*q_BI[0])/2.0 - (dth[1]*q_BI[3])/2.0 + (dth[2]*q_BI[2])/2.0;
  q[2] = q_BI[2] + (dth[1]*q_BI[0])/2.0 + (dth[0]*q_BI[3])/2.0 - (dth[2]*q_BI[1])/2.0;
  q[3] = q_BI[3] - (dth[0]*q_BI[2])/2.0 + (dth[1]*q_BI[1])/2.0 + (dth[2]*q_BI[0])/2.0;

  double qmag = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);

  q_BI[0] = q[0]/qmag;
  q_BI[1] = q[1]/qmag;
  q_BI[2] = q[2]/qmag;
  q_BI[3] = q[3]/qmag;

  // K1*vhat_BX
  double K1vhat_BX[3][3];
  mult3x3(K1,vhat_BX,K1vhat_BX);

  // K2*vhat_BX
  double K2vhat_BX[3][3];
  mult3x3(K2,vhat_BX,K2vhat_BX);

  double KH[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} };

  // For K*H
  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      KH[i][j] = K1vhat_BX[i][j];
    }
  }

  for (int i = 3; i < 6; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      KH[i][j] = K2vhat_BX[i-3][j];
    }
  }

  // -1.0*K*H
  double nKH[6][6];
  mult6x6Scalar(KH,-1.0,nKH);

  // Form eye(6)
  double I6[6][6] = { { 1, 0, 0, 0, 0, 0},
                      { 0, 1, 0, 0, 0, 0},
                      { 0, 0, 1, 0, 0, 0},
                      { 0, 0, 0, 1, 0, 0},
                      { 0, 0, 0, 0, 1, 0},
                      { 0, 0, 0, 0, 0, 1} };

  // I6 - K*H
  double I6nKH[6][6];
  add6x6(I6,nKH,I6nKH);

  // (I6 - K*H)*P
  double Ptemp[6][6];
  mult6x6(I6nKH,P,Ptemp);

  // Transpose P
  double PtempT[6][6];
  trans6x6(Ptemp,PtempT);

  // P + P'
  double PSymmetric[6][6];
  add6x6(Ptemp,PtempT,PSymmetric);

  // (P + P)/2
  mult6x6Scalar(PSymmetric,0.5,P);

  // Update Gyro Bias
  Bw_B[0] += dBw[0];
  Bw_B[1] += dBw[1];
  Bw_B[2] += dBw[2];
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0xd90, %rsp            # imm = 0xD90
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movq %rcx, -0x20(%rbp)
movq %r8, -0x28(%rbp)
movsd %xmm0, -0x30(%rbp)
movq -0x8(%rbp), %rdi
leaq -0x80(%rbp), %rsi
callq 0xb700
movq -0x28(%rbp), %rsi
leaq -0x80(%rbp), %rdi
leaq -0xa0(%rbp), %rdx
callq 0xbab0
leaq -0xa0(%rbp), %rdi
leaq -0xf0(%rbp), %rsi
callq 0xb1d0
leaq -0xf0(%rbp), %rdi
leaq -0x140(%rbp), %rsi
callq 0xbbb0
movl $0x0, -0x194(%rbp)
cmpl $0x3, -0x194(%rbp)
jge 0x9ef4
movl $0x0, -0x198(%rbp)
cmpl $0x3, -0x198(%rbp)
jge 0x9ee1
movq -0x18(%rbp), %rax
movslq -0x194(%rbp), %rcx
imulq $0x30, %rcx, %rcx
addq %rcx, %rax
movslq -0x198(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
movslq -0x194(%rbp), %rcx
leaq -0x190(%rbp), %rax
imulq $0x18, %rcx, %rcx
addq %rcx, %rax
movslq -0x198(%rbp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl -0x198(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x198(%rbp)
jmp 0x9e88
jmp 0x9ee3
movl -0x194(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x194(%rbp)
jmp 0x9e75
leaq -0x190(%rbp), %rdi
leaq -0x140(%rbp), %rsi
leaq -0x1e0(%rbp), %rdx
callq 0xb330
leaq -0xf0(%rbp), %rdi
leaq -0x1e0(%rbp), %rsi
leaq -0x230(%rbp), %rdx
callq 0xb330
leaq -0x280(%rbp), %rdi
xorl %esi, %esi
movl $0x48, %edx
callq 0x3140
movsd -0x30(%rbp), %xmm0
movsd 0x34c8(%rip), %xmm1     # 0xd410
callq 0x3410
movsd %xmm0, -0x280(%rbp)
movsd -0x30(%rbp), %xmm0
movsd 0x34ae(%rip), %xmm1     # 0xd410
callq 0x3410
movsd %xmm0, -0x260(%rbp)
movsd -0x30(%rbp), %xmm0
movsd 0x3494(%rip), %xmm1     # 0xd410
callq 0x3410
movsd %xmm0, -0x240(%rbp)
leaq -0x230(%rbp), %rdi
leaq -0x280(%rbp), %rsi
leaq -0x2d0(%rbp), %rdx
callq 0xb410
leaq -0x2d0(%rbp), %rdi
leaq -0x320(%rbp), %rsi
callq 0xbc20
leaq -0x140(%rbp), %rdi
leaq -0x320(%rbp), %rsi
leaq -0x370(%rbp), %rdx
callq 0xb330
leaq -0x190(%rbp), %rdi
leaq -0x370(%rbp), %rsi
leaq -0x3c0(%rbp), %rdx
callq 0xb330
movl $0x3, -0x414(%rbp)
cmpl $0x6, -0x414(%rbp)
jge 0xa07b
movl $0x0, -0x418(%rbp)
cmpl $0x3, -0x418(%rbp)
jge 0xa065
movq -0x18(%rbp), %rax
movslq -0x414(%rbp), %rcx
imulq $0x30, %rcx, %rcx
addq %rcx, %rax
movslq -0x418(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
movl -0x414(%rbp), %eax
subl $0x3, %eax
movslq %eax, %rcx
leaq -0x410(%rbp), %rax
imulq $0x18, %rcx, %rcx
addq %rcx, %rax
movslq -0x418(%rbp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl -0x418(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x418(%rbp)
jmp 0xa007
jmp 0xa067
movl -0x414(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x414(%rbp)
jmp 0x9ff4
leaq -0x410(%rbp), %rdi
leaq -0x370(%rbp), %rsi
leaq -0x460(%rbp), %rdx
callq 0xb330
movq -0x20(%rbp), %rax
movsd (%rax), %xmm0
subsd -0xa0(%rbp), %xmm0
movsd %xmm0, -0x480(%rbp)
movq -0x20(%rbp), %rax
movsd 0x8(%rax), %xmm0
subsd -0x98(%rbp), %xmm0
movsd %xmm0, -0x478(%rbp)
movq -0x20(%rbp), %rax
movsd 0x10(%rax), %xmm0
subsd -0x90(%rbp), %xmm0
movsd %xmm0, -0x470(%rbp)
leaq -0x3c0(%rbp), %rdi
leaq -0x480(%rbp), %rsi
leaq -0x4a0(%rbp), %rdx
callq 0xbab0
leaq -0x460(%rbp), %rdi
leaq -0x480(%rbp), %rsi
leaq -0x4c0(%rbp), %rdx
callq 0xbab0
movq -0x8(%rbp), %rax
movsd (%rax), %xmm0
movsd -0x4a0(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd 0x8(%rax), %xmm1
movsd 0x32dc(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd -0x498(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd 0x10(%rax), %xmm1
movsd 0x32bb(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd -0x490(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd 0x18(%rax), %xmm1
movsd 0x329a(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, -0x4e0(%rbp)
movq -0x8(%rbp), %rax
movsd 0x8(%rax), %xmm0
movsd -0x4a0(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd (%rax), %xmm1
movsd 0x3269(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd -0x498(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd 0x18(%rax), %xmm1
movsd 0x3248(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd -0x490(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd 0x10(%rax), %xmm1
movsd 0x3227(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, -0x4d8(%rbp)
movq -0x8(%rbp), %rax
movsd 0x10(%rax), %xmm0
movsd -0x498(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd (%rax), %xmm1
movsd 0x31f6(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd -0x4a0(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd 0x18(%rax), %xmm1
movsd 0x31d5(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd -0x490(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd 0x8(%rax), %xmm1
movsd 0x31b4(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, -0x4d0(%rbp)
movq -0x8(%rbp), %rax
movsd 0x18(%rax), %xmm0
movsd -0x4a0(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd 0x10(%rax), %xmm1
movsd 0x3182(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd -0x498(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd 0x8(%rax), %xmm1
movsd 0x3161(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd -0x490(%rbp), %xmm1
movq -0x8(%rbp), %rax
mulsd (%rax), %xmm1
movsd 0x3141(%rip), %xmm2     # 0xd410
divsd %xmm2, %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, -0x4c8(%rbp)
movsd -0x4e0(%rbp), %xmm0
movsd -0x4e0(%rbp), %xmm2
movsd -0x4d8(%rbp), %xmm1
mulsd -0x4d8(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd -0x4d0(%rbp), %xmm1
movsd -0x4d0(%rbp), %xmm2
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movsd -0x4c8(%rbp), %xmm0
movsd -0x4c8(%rbp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
callq 0x3200
movsd %xmm0, -0x4e8(%rbp)
movsd -0x4e0(%rbp), %xmm0
divsd -0x4e8(%rbp), %xmm0
movq -0x8(%rbp), %rax
movsd %xmm0, (%rax)
movsd -0x4d8(%rbp), %xmm0
divsd -0x4e8(%rbp), %xmm0
movq -0x8(%rbp), %rax
movsd %xmm0, 0x8(%rax)
movsd -0x4d0(%rbp), %xmm0
divsd -0x4e8(%rbp), %xmm0
movq -0x8(%rbp), %rax
movsd %xmm0, 0x10(%rax)
movsd -0x4c8(%rbp), %xmm0
divsd -0x4e8(%rbp), %xmm0
movq -0x8(%rbp), %rax
movsd %xmm0, 0x18(%rax)
leaq -0x3c0(%rbp), %rdi
leaq -0xf0(%rbp), %rsi
leaq -0x530(%rbp), %rdx
callq 0xb330
leaq -0x460(%rbp), %rdi
leaq -0xf0(%rbp), %rsi
leaq -0x580(%rbp), %rdx
callq 0xb330
leaq -0x6a0(%rbp), %rdi
xorl %esi, %esi
movl $0x120, %edx            # imm = 0x120
callq 0x3140
movl $0x0, -0x6a4(%rbp)
cmpl $0x3, -0x6a4(%rbp)
jge 0xa47d
movl $0x0, -0x6a8(%rbp)
cmpl $0x3, -0x6a8(%rbp)
jge 0xa467
movslq -0x6a4(%rbp), %rcx
leaq -0x530(%rbp), %rax
imulq $0x18, %rcx, %rcx
addq %rcx, %rax
movslq -0x6a8(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
movslq -0x6a4(%rbp), %rcx
leaq -0x6a0(%rbp), %rax
imulq $0x30, %rcx, %rcx
addq %rcx, %rax
movslq -0x6a8(%rbp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl -0x6a8(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x6a8(%rbp)
jmp 0xa40b
jmp 0xa469
movl -0x6a4(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x6a4(%rbp)
jmp 0xa3f8
movl $0x3, -0x6ac(%rbp)
cmpl $0x6, -0x6ac(%rbp)
jge 0xa515
movl $0x0, -0x6b0(%rbp)
cmpl $0x3, -0x6b0(%rbp)
jge 0xa4ff
movl -0x6ac(%rbp), %eax
subl $0x3, %eax
movslq %eax, %rcx
leaq -0x580(%rbp), %rax
imulq $0x18, %rcx, %rcx
addq %rcx, %rax
movslq -0x6b0(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
movslq -0x6ac(%rbp), %rcx
leaq -0x6a0(%rbp), %rax
imulq $0x30, %rcx, %rcx
addq %rcx, %rax
movslq -0x6b0(%rbp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl -0x6b0(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x6b0(%rbp)
jmp 0xa49e
jmp 0xa501
movl -0x6ac(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x6ac(%rbp)
jmp 0xa487
movsd 0x2f03(%rip), %xmm0     # 0xd420
movsd %xmm0, -0x7d8(%rbp)
leaq -0x6a0(%rbp), %rdi
leaq -0x7d8(%rbp), %rsi
leaq -0x7d0(%rbp), %rdx
callq 0xb680
leaq -0x900(%rbp), %rdi
xorl %esi, %esi
movl $0x120, %edx            # imm = 0x120
callq 0x3140
movsd 0x2cce(%rip), %xmm0     # 0xd228
movsd %xmm0, -0x900(%rbp)
movsd 0x2cbe(%rip), %xmm0     # 0xd228
movsd %xmm0, -0x8c8(%rbp)
movsd 0x2cae(%rip), %xmm0     # 0xd228
movsd %xmm0, -0x890(%rbp)
movsd 0x2c9e(%rip), %xmm0     # 0xd228
movsd %xmm0, -0x858(%rbp)
movsd 0x2c8e(%rip), %xmm0     # 0xd228
movsd %xmm0, -0x820(%rbp)
movsd 0x2c7e(%rip), %xmm0     # 0xd228
movsd %xmm0, -0x7e8(%rbp)
leaq -0x900(%rbp), %rdi
leaq -0x7d0(%rbp), %rsi
leaq -0xa20(%rbp), %rdx
callq 0xb5f0
movq -0x18(%rbp), %rsi
leaq -0xa20(%rbp), %rdi
leaq -0xb40(%rbp), %rdx
callq 0xb510
leaq -0xb40(%rbp), %rdi
leaq -0xc60(%rbp), %rsi
callq 0xb4a0
leaq -0xb40(%rbp), %rdi
leaq -0xc60(%rbp), %rsi
leaq -0xd80(%rbp), %rdx
callq 0xb5f0
movsd 0x2e00(%rip), %xmm0     # 0xd418
movsd %xmm0, -0xd88(%rbp)
movq -0x18(%rbp), %rdx
leaq -0xd80(%rbp), %rdi
leaq -0xd88(%rbp), %rsi
callq 0xb680
movsd -0x4c0(%rbp), %xmm0
movq -0x10(%rbp), %rax
addsd (%rax), %xmm0
movsd %xmm0, (%rax)
movsd -0x4b8(%rbp), %xmm0
movq -0x10(%rbp), %rax
addsd 0x8(%rax), %xmm0
movsd %xmm0, 0x8(%rax)
movsd -0x4b0(%rbp), %xmm0
movq -0x10(%rbp), %rax
addsd 0x10(%rax), %xmm0
movsd %xmm0, 0x10(%rax)
addq $0xd90, %rsp            # imm = 0xD90
popq %rbp
retq