// File: /workspace/github_cpp_makefile_star2_O1/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
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xd28, %rsp            # imm = 0xD28
movsd %xmm0, -0x30(%rbp)
movq %r8, %rbx
movq %rcx, %r12
movq %rdx, %r14
movq %rsi, -0xc8(%rbp)
movq %rdi, %r15
leaq -0x690(%rbp), %r13
movq %r13, %rsi
callq 0x75a3
movsd (%rbx), %xmm0
movsd 0x8(%rbx), %xmm1
movsd 0x8(%r13), %xmm2
mulsd %xmm1, %xmm2
movsd (%r13), %xmm3
mulsd %xmm0, %xmm3
addsd %xmm2, %xmm3
movsd 0x10(%rbx), %xmm4
movsd 0x10(%r13), %xmm5
mulsd %xmm4, %xmm5
addsd %xmm3, %xmm5
movsd 0x20(%r13), %xmm2
mulsd %xmm1, %xmm2
movsd 0x18(%r13), %xmm3
mulsd %xmm0, %xmm3
addsd %xmm2, %xmm3
movsd 0x28(%r13), %xmm2
mulsd %xmm4, %xmm2
mulsd 0x38(%r13), %xmm1
addsd %xmm3, %xmm2
mulsd 0x30(%r13), %xmm0
addsd %xmm1, %xmm0
mulsd 0x40(%r13), %xmm4
addsd %xmm0, %xmm4
xorl %eax, %eax
leaq -0x200(%rbp), %rcx
movq %rax, (%rcx)
movapd 0x2601(%rip), %xmm0     # 0x8450
movapd %xmm4, %xmm1
xorpd %xmm0, %xmm1
movlpd %xmm1, 0x8(%rcx)
movsd %xmm2, 0x10(%rcx)
movapd %xmm4, -0x60(%rbp)
movsd %xmm4, 0x18(%rcx)
movq %rax, 0x20(%rcx)
movapd %xmm5, %xmm1
movapd %xmm2, -0x40(%rbp)
unpcklpd %xmm2, %xmm1            # xmm1 = xmm1[0],xmm2[0]
xorpd %xmm0, %xmm1
movupd %xmm1, 0x28(%rcx)
movapd %xmm5, -0x50(%rbp)
movsd %xmm5, 0x38(%rcx)
movq %rax, 0x40(%rcx)
leaq -0x390(%rbp), %rdx
movq %rcx, %rsi
xorl %edi, %edi
movsd (%rsi), %xmm0
movsd %xmm0, (%rdx,%rdi,8)
incq %rdi
addq $0x18, %rsi
cmpq $0x3, %rdi
jne 0x5e9f
incq %rax
addq $0x18, %rdx
addq $0x8, %rcx
cmpq $0x3, %rax
jne 0x5e9a
xorl %eax, %eax
movq 0x10(%r14,%rax,2), %rcx
movq %rcx, -0x1a0(%rbp,%rax)
movupd (%r14,%rax,2), %xmm0
movupd %xmm0, -0x1b0(%rbp,%rax)
addq $0x18, %rax
cmpq $0x48, %rax
jne 0x5ec8
leaq -0x1b0(%rbp), %rax
xorl %ecx, %ecx
leaq -0x390(%rbp), %rdx
leaq (%rcx,%rcx,2), %rsi
leaq -0x480(,%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 0x5f28
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x3, %r8
jne 0x5f13
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x5efe
leaq -0x200(%rbp), %rax
xorl %ecx, %ecx
leaq -0x480(%rbp), %rdx
leaq (%rcx,%rcx,2), %rsi
leaq -0x430(,%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 0x5f9d
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x3, %r8
jne 0x5f88
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x5f73
xorpd %xmm0, %xmm0
leaq -0x640(%rbp), %rax
movapd %xmm0, (%rax)
movapd %xmm0, 0x20(%rax)
movapd %xmm0, 0x30(%rax)
movapd %xmm0, 0x10(%rax)
movsd -0x30(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, (%rax)
movsd %xmm0, 0x20(%rax)
movsd %xmm0, 0x40(%rax)
leaq -0x3e0(%rbp), %rcx
leaq -0x430(%rbp), %rdx
xorl %esi, %esi
xorl %edi, %edi
movsd (%rdx,%rdi,8), %xmm0
addsd (%rax,%rdi,8), %xmm0
movsd %xmm0, (%rcx,%rdi,8)
incq %rdi
cmpq $0x3, %rdi
jne 0x601f
incq %rsi
addq $0x18, %rcx
addq $0x18, %rax
addq $0x18, %rdx
cmpq $0x3, %rsi
jne 0x601d
leaq -0x3e0(%rbp), %rdi
leaq -0x5f0(%rbp), %r13
movq %r13, %rsi
callq 0x77d6
leaq -0x390(%rbp), %rax
xorl %ecx, %ecx
leaq (%rcx,%rcx,2), %rdx
leaq -0x340(,%rdx,8), %rdx
addq %rbp, %rdx
movq %r13, %rsi
xorl %edi, %edi
leaq (%rdx,%rdi,8), %r8
movq $0x0, (%r8)
xorpd %xmm0, %xmm0
movq %rsi, %r9
xorl %r10d, %r10d
movsd (%rax,%r10,8), %xmm1
mulsd (%r9), %xmm1
addsd %xmm1, %xmm0
incq %r10
addq $0x18, %r9
cmpq $0x3, %r10
jne 0x6094
movsd %xmm0, (%r8)
incq %rdi
addq $0x8, %rsi
cmpq $0x3, %rdi
jne 0x607f
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x606b
leaq -0x1b0(%rbp), %rax
xorl %ecx, %ecx
leaq -0x340(%rbp), %rdx
leaq (%rcx,%rcx,2), %rsi
leaq -0x160(,%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 0x6109
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x3, %r8
jne 0x60f4
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x60df
leaq 0x90(%r14), %rax
xorl %ecx, %ecx
movq 0x10(%rax,%rcx,2), %rdx
movq %rdx, -0x2e0(%rbp,%rcx)
movupd (%rax,%rcx,2), %xmm0
movupd %xmm0, -0x2f0(%rbp,%rcx)
addq $0x18, %rcx
cmpq $0x48, %rcx
jne 0x614d
leaq -0x2f0(%rbp), %rax
xorl %ecx, %ecx
leaq -0x340(%rbp), %rdx
leaq (%rcx,%rcx,2), %rsi
leaq -0x110(,%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 0x61ac
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x3, %r8
jne 0x6197
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x6182
movsd (%r12), %xmm4
movsd 0x8(%r12), %xmm3
subsd -0x50(%rbp), %xmm4
subsd -0x40(%rbp), %xmm3
movsd 0x10(%r12), %xmm5
subsd -0x60(%rbp), %xmm5
movsd -0x158(%rbp), %xmm0
mulsd %xmm3, %xmm0
movsd -0x160(%rbp), %xmm1
mulsd %xmm4, %xmm1
addsd %xmm0, %xmm1
movsd -0x150(%rbp), %xmm0
mulsd %xmm5, %xmm0
addsd %xmm1, %xmm0
movsd -0x140(%rbp), %xmm1
mulsd %xmm3, %xmm1
movsd -0x148(%rbp), %xmm2
mulsd %xmm4, %xmm2
addsd %xmm1, %xmm2
movsd -0x138(%rbp), %xmm1
mulsd %xmm5, %xmm1
addsd %xmm2, %xmm1
movsd -0x128(%rbp), %xmm2
movsd %xmm3, -0x40(%rbp)
mulsd %xmm3, %xmm2
movsd -0x130(%rbp), %xmm3
movsd %xmm4, -0x60(%rbp)
mulsd %xmm4, %xmm3
addsd %xmm2, %xmm3
movsd -0x120(%rbp), %xmm10
movsd %xmm5, -0x50(%rbp)
mulsd %xmm5, %xmm10
addsd %xmm3, %xmm10
movsd (%r15), %xmm3
movsd 0x8(%r15), %xmm2
movapd %xmm0, %xmm4
mulsd %xmm2, %xmm4
movsd 0x2154(%rip), %xmm9     # 0x8410
mulsd %xmm9, %xmm4
movapd %xmm10, %xmm6
movapd %xmm0, %xmm5
mulsd %xmm3, %xmm5
movapd %xmm10, %xmm11
movapd %xmm1, %xmm7
mulsd %xmm3, %xmm7
movapd %xmm10, %xmm8
mulsd %xmm3, %xmm10
movapd %xmm3, %xmm12
subsd %xmm4, %xmm12
movsd 0x10(%r15), %xmm3
movapd %xmm1, %xmm4
mulsd %xmm3, %xmm4
mulsd %xmm9, %xmm4
subsd %xmm4, %xmm12
movsd 0x18(%r15), %xmm4
mulsd %xmm4, %xmm6
mulsd %xmm9, %xmm6
subsd %xmm6, %xmm12
mulsd %xmm9, %xmm5
addsd %xmm2, %xmm5
movapd %xmm1, %xmm6
mulsd %xmm4, %xmm6
mulsd %xmm9, %xmm6
subsd %xmm6, %xmm5
movsd -0x110(%rbp), %xmm6
movsd %xmm6, -0x30(%rbp)
mulsd %xmm3, %xmm11
mulsd %xmm9, %xmm11
addsd %xmm5, %xmm11
mulsd %xmm9, %xmm7
addsd %xmm3, %xmm7
movapd %xmm0, %xmm6
mulsd %xmm4, %xmm6
mulsd %xmm9, %xmm6
addsd %xmm7, %xmm6
movsd -0x108(%rbp), %xmm5
movsd %xmm5, -0xb0(%rbp)
mulsd %xmm2, %xmm8
mulsd %xmm9, %xmm8
subsd %xmm8, %xmm6
movsd -0x100(%rbp), %xmm5
movsd %xmm5, -0xc0(%rbp)
mulsd %xmm3, %xmm0
movsd -0xf8(%rbp), %xmm3
movsd %xmm3, -0xb8(%rbp)
mulsd %xmm9, %xmm0
subsd %xmm0, %xmm4
movsd -0xf0(%rbp), %xmm0
movsd %xmm0, -0xa8(%rbp)
mulsd %xmm2, %xmm1
mulsd %xmm9, %xmm1
addsd %xmm4, %xmm1
mulsd %xmm9, %xmm10
addsd %xmm1, %xmm10
movapd %xmm11, %xmm0
mulsd %xmm11, %xmm0
movapd %xmm12, %xmm1
mulsd %xmm12, %xmm1
addsd %xmm0, %xmm1
movapd %xmm6, %xmm2
mulsd %xmm6, %xmm2
addsd %xmm1, %xmm2
movapd %xmm10, %xmm0
mulsd %xmm10, %xmm0
addsd %xmm2, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
movsd -0xe8(%rbp), %xmm1
movsd %xmm1, -0x98(%rbp)
movsd -0xe0(%rbp), %xmm1
movsd %xmm1, -0x90(%rbp)
movsd -0xd8(%rbp), %xmm1
movsd %xmm1, -0x88(%rbp)
movsd -0xd0(%rbp), %xmm1
movsd %xmm1, -0xa0(%rbp)
jb 0x6460
sqrtsd %xmm0, %xmm0
jmp 0x6493
movsd %xmm10, -0x80(%rbp)
movsd %xmm11, -0x78(%rbp)
movsd %xmm12, -0x70(%rbp)
movsd %xmm6, -0x68(%rbp)
callq 0x21f0
movsd -0x68(%rbp), %xmm6
movsd -0x70(%rbp), %xmm12
movsd -0x78(%rbp), %xmm11
movsd -0x80(%rbp), %xmm10
divsd %xmm0, %xmm12
movsd %xmm12, (%r15)
divsd %xmm0, %xmm11
movsd %xmm11, 0x8(%r15)
divsd %xmm0, %xmm6
movsd %xmm6, 0x10(%r15)
divsd %xmm0, %xmm10
movsd %xmm10, 0x18(%r15)
leaq -0x160(%rbp), %rax
xorl %ecx, %ecx
leaq -0x200(%rbp), %rdx
leaq (%rcx,%rcx,2), %rsi
leaq -0x2a0(,%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 0x64f7
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x3, %r8
jne 0x64e2
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x64cd
leaq -0x110(%rbp), %rax
xorl %ecx, %ecx
leaq -0x200(%rbp), %rdx
leaq (%rcx,%rcx,2), %rsi
leaq -0x250(,%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 0x656c
movsd %xmm0, (%r9)
incq %r8
addq $0x8, %rdi
cmpq $0x3, %r8
jne 0x6557
incq %rcx
addq $0x18, %rax
cmpq $0x3, %rcx
jne 0x6542
leaq -0x5a0(%rbp), %r15
xorl %ebx, %ebx
movl $0x120, %edx            # imm = 0x120
movq %r15, %rdi
xorl %esi, %esi
callq 0x2130
movq -0x290(%rbp,%rbx), %rax
movq %rax, -0x590(%rbp,%rbx,2)
movupd -0x2a0(%rbp,%rbx), %xmm0
movupd %xmm0, -0x5a0(%rbp,%rbx,2)
addq $0x18, %rbx
cmpq $0x48, %rbx
jne 0x65bf
xorl %eax, %eax
movq -0x240(%rbp,%rax), %rcx
movq %rcx, -0x500(%rbp,%rax,2)
movupd -0x250(%rbp,%rax), %xmm0
movupd %xmm0, -0x510(%rbp,%rax,2)
addq $0x18, %rax
cmpq $0x48, %rax
jne 0x65ed
leaq -0xc30(%rbp), %rax
xorl %ecx, %ecx
movapd 0x1e26(%rip), %xmm0     # 0x8450
xorl %edx, %edx
movsd (%r15,%rdx,8), %xmm1
xorpd %xmm0, %xmm1
movlpd %xmm1, (%rax,%rdx,8)
incq %rdx
cmpq $0x6, %rdx
jne 0x662c
incq %rcx
addq $0x30, %rax
addq $0x30, %r15
cmpq $0x6, %rcx
jne 0x662a
leaq -0xd50(%rbp), %r15
xorl %ebx, %ebx
movl $0x120, %edx            # imm = 0x120
movq %r15, %rdi
xorl %esi, %esi
callq 0x2130
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, (%r15)
movq %rax, 0x38(%r15)
movq %rax, 0x70(%r15)
movq %rax, 0xa8(%r15)
movq %rax, 0xe0(%r15)
movq %rax, 0x118(%r15)
leaq -0xb10(%rbp), %rax
leaq -0xc30(%rbp), %rcx
movsd 0x1d63(%rip), %xmm2     # 0x8410
xorl %edx, %edx
movsd (%r15,%rdx,8), %xmm0
addsd (%rcx,%rdx,8), %xmm0
movsd %xmm0, (%rax,%rdx,8)
incq %rdx
cmpq $0x6, %rdx
jne 0x66af
incq %rbx
addq $0x30, %rax
addq $0x30, %rcx
addq $0x30, %r15
cmpq $0x6, %rbx
jne 0x66ad
leaq -0xb10(%rbp), %rax
xorl %ecx, %ecx
leaq (%rcx,%rcx,2), %rdx
shlq $0x4, %rdx
addq %rbp, %rdx
addq $-0x7b0, %rdx           # imm = 0xF850
movq %r14, %rsi
xorl %edi, %edi
leaq (%rdx,%rdi,8), %r8
movq $0x0, (%r8)
xorpd %xmm0, %xmm0
movq %rsi, %r9
xorl %r10d, %r10d
movsd (%rax,%r10,8), %xmm1
mulsd (%r9), %xmm1
addsd %xmm1, %xmm0
incq %r10
addq $0x30, %r9
cmpq $0x6, %r10
jne 0x6712
movsd %xmm0, (%r8)
incq %rdi
addq $0x8, %rsi
cmpq $0x6, %rdi
jne 0x66fd
incq %rcx
addq $0x30, %rax
cmpq $0x6, %rcx
jne 0x66e6
leaq -0x9f0(%rbp), %rax
leaq -0x7b0(%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 0x6762
incq %rdx
addq $0x30, %rax
addq $0x8, %rcx
cmpq $0x6, %rdx
jne 0x675d
leaq -0x8d0(%rbp), %rax
leaq -0x9f0(%rbp), %rcx
leaq -0x7b0(%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 0x67a2
incq %rsi
addq $0x30, %rax
addq $0x30, %rcx
addq $0x30, %rdx
cmpq $0x6, %rsi
jne 0x67a0
leaq -0x8d0(%rbp), %rax
xorl %ecx, %ecx
xorl %edx, %edx
movsd (%rax,%rdx,8), %xmm0
mulsd %xmm2, %xmm0
movsd %xmm0, (%r14,%rdx,8)
incq %rdx
cmpq $0x6, %rdx
jne 0x67da
incq %rcx
addq $0x30, %r14
addq $0x30, %rax
cmpq $0x6, %rcx
jne 0x67d8
movsd -0x40(%rbp), %xmm2
movsd -0x88(%rbp), %xmm1
mulsd %xmm2, %xmm1
movsd -0x60(%rbp), %xmm0
movsd -0x90(%rbp), %xmm3
mulsd %xmm0, %xmm3
addsd %xmm1, %xmm3
movsd -0x50(%rbp), %xmm1
movsd -0xa0(%rbp), %xmm5
mulsd %xmm1, %xmm5
addsd %xmm3, %xmm5
movsd -0xa8(%rbp), %xmm4
mulsd %xmm2, %xmm4
movsd -0xb8(%rbp), %xmm3
mulsd %xmm0, %xmm3
addsd %xmm4, %xmm3
movsd -0x98(%rbp), %xmm6
mulsd %xmm1, %xmm6
movsd -0xb0(%rbp), %xmm4
mulsd %xmm2, %xmm4
movsd -0x30(%rbp), %xmm2
mulsd %xmm0, %xmm2
addsd %xmm4, %xmm2
movsd -0xc0(%rbp), %xmm0
mulsd %xmm1, %xmm0
addsd %xmm2, %xmm0
movq -0xc8(%rbp), %rax
addsd (%rax), %xmm0
addsd %xmm3, %xmm6
movsd %xmm0, (%rax)
addsd 0x8(%rax), %xmm6
movsd %xmm6, 0x8(%rax)
addsd 0x10(%rax), %xmm5
movsd %xmm5, 0x10(%rax)
addq $0xd28, %rsp            # imm = 0xD28
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq