// File: 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
subq $0x240, %rsp            # imm = 0x240
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rdx
leaq -0x20(%rbp), %rdi
leaq -0x3e(%rip), %rsi       # 0x9a20
callq 0x5980
movsd 0x35ad(%rip), %xmm0     # 0xd018
movsd %xmm0, -0x28(%rbp)
movsd 0x35b8(%rip), %xmm0     # 0xd030
movsd %xmm0, -0x30(%rbp)
movsd 0x3983(%rip), %xmm0     # 0xd408
movsd %xmm0, -0x38(%rbp)
movsd 0x396e(%rip), %xmm0     # 0xd400
movsd %xmm0, -0x40(%rbp)
movsd 0x3959(%rip), %xmm0     # 0xd3f8
movsd %xmm0, -0x48(%rbp)
movsd 0x355c(%rip), %xmm0     # 0xd008
movsd %xmm0, -0x50(%rbp)
movq 0x3988(%rip), %rax      # 0xd440
movq %rax, -0x90(%rbp)
movq 0x3982(%rip), %rax      # 0xd448
movq %rax, -0x88(%rbp)
movq 0x397c(%rip), %rax      # 0xd450
movq %rax, -0x80(%rbp)
movq 0x3979(%rip), %rax      # 0xd458
movq %rax, -0x78(%rbp)
leaq -0x1b0(%rbp), %rdi
xorl %esi, %esi
movl $0x120, %edx            # imm = 0x120
callq 0x3140
movsd 0x3562(%rip), %xmm0     # 0xd060
movsd %xmm0, -0x1b0(%rbp)
movsd 0x3552(%rip), %xmm0     # 0xd060
movsd %xmm0, -0x178(%rbp)
movsd 0x3542(%rip), %xmm0     # 0xd060
movsd %xmm0, -0x140(%rbp)
movsd 0x38c2(%rip), %xmm0     # 0xd3f0
movsd %xmm0, -0x108(%rbp)
movsd 0x38b2(%rip), %xmm0     # 0xd3f0
movsd %xmm0, -0xd0(%rbp)
movsd 0x38a2(%rip), %xmm0     # 0xd3f0
movsd %xmm0, -0x98(%rbp)
leaq -0x1d0(%rbp), %rdi
xorl %esi, %esi
movl $0x18, %edx
callq 0x3140
leaq -0x1f0(%rbp), %rdi
xorl %esi, %esi
movl $0x18, %edx
callq 0x3140
jmp 0x9b7e
leaq 0x8733(%rip), %rdi      # 0x122b8
callq 0x4c80
jmp 0x9b8c
leaq 0x870d(%rip), %rax      # 0x122a0
movq 0x10(%rax), %rcx
movq %rcx, -0x60(%rbp)
movaps (%rax), %xmm0
movaps %xmm0, -0x70(%rbp)
leaq 0x870f(%rip), %rdi      # 0x122b8
callq 0x4b30
jmp 0x9bb0
leaq 0x8781(%rip), %rdi      # 0x12338
callq 0x49e0
jmp 0x9bbe
leaq 0x875b(%rip), %rax      # 0x12320
movq 0x10(%rax), %rcx
movq %rcx, -0x220(%rbp)
movaps (%rax), %xmm0
movaps %xmm0, -0x230(%rbp)
leaq 0x8757(%rip), %rdi      # 0x12338
callq 0x4b30
jmp 0x9be8
leaq 0x87c9(%rip), %rdi      # 0x123b8
callq 0x49e0
jmp 0x9bf6
leaq 0x87a3(%rip), %rax      # 0x123a0
movq 0x10(%rax), %rcx
movq %rcx, -0x200(%rbp)
movaps (%rax), %xmm0
movaps %xmm0, -0x210(%rbp)
leaq 0x879f(%rip), %rdi      # 0x123b8
callq 0x4b30
jmp 0x9c20
movsd -0x230(%rbp), %xmm0
ucomisd -0x1f0(%rbp), %xmm0
jne 0x9c36
jp 0x9c36
jmp 0x9cb4
leaq 0x8863(%rip), %rsi      # 0x124a0
leaq 0x887c(%rip), %r8       # 0x124c0
leaq -0x90(%rbp), %rdi
leaq -0x1b0(%rbp), %rdx
leaq -0x230(%rbp), %rcx
movsd 0x37a7(%rip), %xmm0     # 0xd408
callq 0x9e00
jmp 0x9c68
movq -0x230(%rbp), %rax
movq %rax, -0x1f0(%rbp)
movq -0x228(%rbp), %rax
movq %rax, -0x1e8(%rbp)
movq -0x220(%rbp), %rax
movq %rax, -0x1e0(%rbp)
jmp 0x9cb4
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x238(%rbp)
movl %eax, -0x23c(%rbp)
leaq -0x20(%rbp), %rdi
callq 0x59b0
jmp 0x9df1
movsd -0x210(%rbp), %xmm0
ucomisd -0x1d0(%rbp), %xmm0
jne 0x9cca
jp 0x9cca
jmp 0x9d26
leaq 0x87cf(%rip), %rsi      # 0x124a0
leaq 0x8808(%rip), %r8       # 0x124e0
leaq -0x90(%rbp), %rdi
leaq -0x1b0(%rbp), %rdx
leaq -0x210(%rbp), %rcx
movsd 0x333b(%rip), %xmm0     # 0xd030
callq 0x9e00
jmp 0x9cfc
movq -0x210(%rbp), %rax
movq %rax, -0x1d0(%rbp)
movq -0x208(%rbp), %rax
movq %rax, -0x1c8(%rbp)
movq -0x200(%rbp), %rax
movq %rax, -0x1c0(%rbp)
leaq 0x8773(%rip), %rax      # 0x124a0
movsd (%rax), %xmm1
movsd -0x70(%rbp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, -0x70(%rbp)
movsd 0x8(%rax), %xmm1
movsd -0x68(%rbp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, -0x68(%rbp)
movsd 0x10(%rax), %xmm1
movsd -0x60(%rbp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, -0x60(%rbp)
leaq -0x70(%rbp), %rdi
movsd 0x3297(%rip), %xmm0     # 0xd008
movsd 0x3687(%rip), %xmm1     # 0xd400
movsd 0x3677(%rip), %xmm2     # 0xd3f8
leaq -0x1b0(%rbp), %rsi
callq 0xa680
jmp 0x9d8f
leaq -0x70(%rbp), %rdi
movsd 0x326d(%rip), %xmm0     # 0xd008
leaq -0x90(%rbp), %rsi
callq 0xae70
jmp 0x9da9
leaq 0x8690(%rip), %rdi      # 0x12440
callq 0x49e0
jmp 0x9db7
movaps -0x90(%rbp), %xmm0
movaps -0x80(%rbp), %xmm1
leaq 0x8657(%rip), %rax      # 0x12420
movaps %xmm1, 0x10(%rax)
movaps %xmm0, (%rax)
leaq 0x8669(%rip), %rdi      # 0x12440
callq 0x4b30
jmp 0x9dde
leaq 0x865b(%rip), %rdi      # 0x12440
callq 0x4e90
jmp 0x9dec
jmp 0x9b7c
movq -0x238(%rbp), %rdi
callq 0x3420
nopl (%rax)