// File: /workspace/github_cpp_makefile_star2_O3/rachit31[P]CubeSatADCS/propagateANDupdate.cpp
// Function: propagateANDupdate(void*)
void* propagateANDupdate(void *pArg)
 {
 	 // Push Cleanup Handler to cleanup thread
	 pthread_cleanup_push(&Thread::cleanup,pArg);
	 
	 // Code goes here...
	 
	 const double pi = 3.141592653589793;
	 
	 const double sigmaB = 0.1; // variance on the Magnetometer Data
	 const double sigmaa = 0.15; // variance on the Accelerometer Data
	 const double sigmaw = 0.1*pi/180; // variance on the Gyro Data (in radians/second)
	 const double sigmaBw = 0.01*pi/180; // variance on the Gyro Bias (in radians/second)
	 
	 const double dtGyro = 1.0/119.0;
	 
	 /*double Bw_B[3];
     double a_I[3];
     double B_I[3];
     const double dtGyro;
	 */
	 
     double wbi_B[3];
	 double q_BI[4] = {1, 0, 0, 0};
	 /*
	 double P[6][6] = { {9.8696, 0, 0, 0, 0, 0},
		                {0, 9.8696, 0, 0, 0, 0},
					    {0, 0, 9.8696, 0, 0, 0},
						{0, 0, 0, 0.0076, 0, 0},
						{0, 0, 0, 0, 0.0076, 0},
						{0, 0, 0, 0, 0, 0.0076} };
	*/

     double P[6][6] = { {10, 0, 0, 0, 0, 0},
		                {0, 10, 0, 0, 0, 0},
					    {0, 0, 10, 0, 0, 0},
						{0, 0, 0, 0.008, 0, 0},
						{0, 0, 0, 0, 0.008, 0},
						{0, 0, 0, 0, 0, 0.008} };	
						  
     double Bold_B[3] = {0, 0, 0};
	 double aold_B[3] = {0, 0, 0};
	 
	 double B_B[3];
	 double a_B[3];
	
	 while (true)
	 {
	 	 gyroMutex.wait(); // wait for Gyro data
	     std::memcpy(wbi_B, gyroBuffer, sizeof(wbi_B)); // copy Gyro data
	     gyroMutex.unlock();
		 
		 accelMutex.lock(); // wait for Accelerometer data
		 std::memcpy(a_B, accelBuffer, sizeof(a_B)); // copy Accelerometer data
	     accelMutex.unlock();
		 
		 magMutex.lock(); // wait for Magnetometer data
		 std::memcpy(B_B, magBuffer, sizeof(B_B)); // copy Magnetometer data
	     magMutex.unlock();
		 
		 if(a_B[0] != aold_B[0])
		 {
			 update(q_BI, Bw_B, P, a_B, a_I, sigmaa);
			 std::memcpy(aold_B, a_B, sizeof(a_B));
		 }
	 
	     if(B_B[0] != Bold_B[0])
		 {
			 update(q_BI, Bw_B, P, B_B, B_I, sigmaB);
			 std::memcpy(Bold_B, B_B, sizeof(B_B));
		 }
		 
		 wbi_B[0] -= Bw_B[0];
		 wbi_B[1] -= Bw_B[1];
		 wbi_B[2] -= Bw_B[2];
		 
		 covarianceProp(wbi_B, dtGyro, sigmaw, sigmaBw, P);
		 
	 	 quatProp(wbi_B, dtGyro, q_BI);
	 
		 sendMutex.lock();
	 	 std::memcpy(sendBuffer, q_BI, sizeof(sendBuffer));
	 	 sendMutex.unlock();
	 	 sendMutex.signal();
	 }
	 
	 // Run clean up handler to cleanup thread (TLPI pg 676)
     pthread_cleanup_pop(1);
 }

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1a8, %rsp            # imm = 0x1A8
movq %rdi, -0x58(%rbp)
movaps 0x2a41(%rip), %xmm0     # 0x8460
leaq -0xb0(%rbp), %rax
movaps %xmm0, 0x10(%rax)
movaps 0x2a1f(%rip), %xmm0     # 0x8450
movaps %xmm0, (%rax)
leaq -0x1d0(%rbp), %rbx
movl $0x120, %edx            # imm = 0x120
movq %rbx, %rdi
xorl %esi, %esi
callq 0x2130
movabsq $0x4024000000000000, %rax # imm = 0x4024000000000000
movq %rax, (%rbx)
movq %rax, 0x38(%rbx)
movq %rax, 0x70(%rbx)
movabsq $0x3f80624dd2f1a9fc, %rax # imm = 0x3F80624DD2F1A9FC
movq %rax, 0xa8(%rbx)
movq %rax, 0xe0(%rbx)
movq %rax, 0x118(%rbx)
xorps %xmm0, %xmm0
movsd %xmm0, -0x50(%rbp)
leaq 0x57db(%rip), %r15      # 0xb268
leaq 0x5854(%rip), %r13      # 0xb2e8
leaq 0x58cd(%rip), %r14      # 0xb368
leaq 0x59ae(%rip), %r12      # 0xb450
movsd %xmm0, -0x48(%rbp)
movq %r15, %rdi
callq 0x396e
leaq 0x579a(%rip), %rcx      # 0xb250
movq 0x10(%rcx), %rax
movq %rax, -0x30(%rbp)
movaps (%rcx), %xmm0
movaps %xmm0, -0x40(%rbp)
movq %r15, %rdi
callq 0x3840
movq %r13, %rdi
callq 0x3712
leaq 0x57f4(%rip), %rcx      # 0xb2d0
movq 0x10(%rcx), %rax
movq %rax, -0x60(%rbp)
movaps (%rcx), %xmm0
movaps %xmm0, -0x70(%rbp)
movq %r13, %rdi
callq 0x3840
movq %r14, %rdi
callq 0x3712
leaq 0x584e(%rip), %rcx      # 0xb350
movq 0x10(%rcx), %rax
movq %rax, -0x80(%rbp)
movaps (%rcx), %xmm0
movaps %xmm0, -0x90(%rbp)
movq %r14, %rdi
callq 0x3840
movq %r15, %rbx
movsd -0x70(%rbp), %xmm0
ucomisd -0x50(%rbp), %xmm0
jne 0x5b2d
jnp 0x5b5b
leaq -0xb0(%rbp), %rdi
movq %r12, %rsi
leaq -0x1d0(%rbp), %rdx
leaq -0x70(%rbp), %rcx
leaq 0x5927(%rip), %r8       # 0xb470
movsd %xmm0, -0x50(%rbp)
movsd 0x28a2(%rip), %xmm0     # 0x83f8
callq 0x5c88
movsd -0x90(%rbp), %xmm0
ucomisd -0x48(%rbp), %xmm0
leaq -0xb0(%rbp), %r14
leaq -0x1d0(%rbp), %rsi
jne 0x5b7a
jnp 0x5bae
movq %r14, %rdi
movq %r12, %rsi
leaq -0x1d0(%rbp), %rdx
leaq -0x90(%rbp), %rcx
leaq 0x58fb(%rip), %r8       # 0xb490
movsd %xmm0, -0x48(%rbp)
movsd 0x2486(%rip), %xmm0     # 0x8028
callq 0x5c88
leaq -0x1d0(%rbp), %rsi
movsd -0x40(%rbp), %xmm0
movsd -0x38(%rbp), %xmm1
subsd (%r12), %xmm0
movsd %xmm0, -0x40(%rbp)
subsd 0x8(%r12), %xmm1
movsd %xmm1, -0x38(%rbp)
movsd -0x30(%rbp), %xmm0
subsd 0x10(%r12), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r12, %r15
leaq -0x40(%rbp), %r12
movq %r12, %rdi
movsd 0x2416(%rip), %xmm0     # 0x8008
movsd 0x2806(%rip), %xmm1     # 0x8400
movsd 0x2806(%rip), %xmm2     # 0x8408
callq 0x6710
movq %r12, %rdi
movq %r15, %r12
movsd 0x23f3(%rip), %xmm0     # 0x8008
movq %r14, %rsi
callq 0x6f5a
leaq 0x57cc(%rip), %rdi      # 0xb3f0
callq 0x3712
leaq 0x5738(%rip), %r14      # 0xb368
movq %rbx, %r15
movaps -0xb0(%rbp), %xmm0
movaps -0xa0(%rbp), %xmm1
leaq 0x5788(%rip), %rax      # 0xb3d0
movaps %xmm1, 0x10(%rax)
movaps %xmm0, (%rax)
leaq 0x579a(%rip), %rdi      # 0xb3f0
callq 0x3840
leaq 0x578e(%rip), %rdi      # 0xb3f0
callq 0x3b30
jmp 0x5aa7
movq %rax, %rbx
movq -0x58(%rbp), %rdi
callq 0x59f6
movq %rbx, %rdi
callq 0x23a0
movq %rax, %rdi
callq 0x36be