// File: /workspace/github_cpp_makefile_star2_O2/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 $0x1c8, %rsp            # imm = 0x1C8
leaq -0x27(%rip), %rax       # 0x4f18
movq %rax, -0xa8(%rbp)
movq %rdi, -0xa0(%rbp)
movl $0x1, -0x98(%rbp)
movaps 0x2502(%rip), %xmm0     # 0x7460
leaq -0xd0(%rbp), %rax
movaps %xmm0, 0x10(%rax)
movaps 0x24e0(%rip), %xmm0     # 0x7450
movaps %xmm0, (%rax)
leaq -0x1f0(%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 0x527c(%rip), %r14      # 0xa248
leaq 0x52f5(%rip), %r12      # 0xa2c8
leaq 0x536e(%rip), %rbx      # 0xa348
leaq 0x544f(%rip), %r15      # 0xa430
movsd %xmm0, -0x48(%rbp)
movq %r14, %rdi
callq 0x33a0
leaq 0x523b(%rip), %rcx      # 0xa230
movq 0x10(%rcx), %rax
movq %rax, -0x30(%rbp)
movaps (%rcx), %xmm0
movaps %xmm0, -0x40(%rbp)
movq %r14, %rdi
callq 0x32e4
movq %r12, %rdi
callq 0x3228
leaq 0x5295(%rip), %rcx      # 0xa2b0
movq 0x10(%rcx), %rax
movq %rax, -0x60(%rbp)
movaps (%rcx), %xmm0
movaps %xmm0, -0x70(%rbp)
movq %r12, %rdi
callq 0x32e4
movq %rbx, %rdi
callq 0x3228
leaq 0x52ef(%rip), %rcx      # 0xa330
movq 0x10(%rcx), %rax
movq %rax, -0x80(%rbp)
movaps (%rcx), %xmm0
movaps %xmm0, -0x90(%rbp)
movq %rbx, %rdi
callq 0x32e4
movq %r14, %r13
movsd -0x70(%rbp), %xmm0
ucomisd -0x50(%rbp), %xmm0
jne 0x506c
jnp 0x509a
leaq -0xd0(%rbp), %rdi
movq %r15, %rsi
leaq -0x1f0(%rbp), %rdx
leaq -0x70(%rbp), %rcx
leaq 0x53c8(%rip), %r8       # 0xa450
movsd %xmm0, -0x50(%rbp)
movsd 0x2363(%rip), %xmm0     # 0x73f8
callq 0x51bf
movsd -0x90(%rbp), %xmm0
ucomisd -0x48(%rbp), %xmm0
leaq -0xd0(%rbp), %rbx
leaq -0x1f0(%rbp), %rsi
jne 0x50b9
jnp 0x50ed
movq %rbx, %rdi
movq %r15, %rsi
leaq -0x1f0(%rbp), %rdx
leaq -0x90(%rbp), %rcx
leaq 0x539c(%rip), %r8       # 0xa470
movsd %xmm0, -0x48(%rbp)
movsd 0x1f47(%rip), %xmm0     # 0x7028
callq 0x51bf
leaq -0x1f0(%rbp), %rsi
movsd -0x40(%rbp), %xmm0
movsd -0x38(%rbp), %xmm1
subsd (%r15), %xmm0
movsd %xmm0, -0x40(%rbp)
subsd 0x8(%r15), %xmm1
movsd %xmm1, -0x38(%rbp)
movsd -0x30(%rbp), %xmm0
subsd 0x10(%r15), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %r14
leaq -0x40(%rbp), %r15
movq %r15, %rdi
movsd 0x1eda(%rip), %xmm0     # 0x7008
movsd 0x22ca(%rip), %xmm1     # 0x7400
movsd 0x22ca(%rip), %xmm2     # 0x7408
callq 0x5879
movq %r15, %rdi
movq %r14, %r15
movsd 0x1eb7(%rip), %xmm0     # 0x7008
movq %rbx, %rsi
callq 0x5d7c
leaq 0x5270(%rip), %rdi      # 0xa3d0
callq 0x3228
leaq 0x51dc(%rip), %rbx      # 0xa348
movq %r13, %r14
movaps -0xd0(%rbp), %xmm0
movaps -0xc0(%rbp), %xmm1
leaq 0x522c(%rip), %rax      # 0xa3b0
movaps %xmm1, 0x10(%rax)
movaps %xmm0, (%rax)
leaq 0x523e(%rip), %rdi      # 0xa3d0
callq 0x32e4
leaq 0x5232(%rip), %rdi      # 0xa3d0
callq 0x34b8
jmp 0x4fe6
movq %rax, %rbx
leaq -0xa8(%rbp), %rdi
callq 0x3b3e
movq %rbx, %rdi
callq 0x2350