// File: /workspace/github_cpp_makefile_star2_O2/rachit31[P]CubeSatADCS/readGyros.cpp
// Function: readGyros(void*)
void* readGyros(void *pArg)
 {
	 // Push Cleanup Handler to cleanup thread
	 pthread_cleanup_push(&Thread::cleanup,pArg);
	 
	 // Code goes here...
	
	 const double pi = 3.141592653589793;
	 const double gyroToW = 500.0/32767.0*pi/180.0; // radians/second/count
	 /* const double gyroToW = 500/(pow(2,15)-1)*pi/180.0; // radians/second/count */
	 
	 uint8_t buffer[6];
	 int16_t data;
	 
	 double wbi_B[3];
	 
	 SleepTimer myTimer(1.0/119.0);
	 
	 myTimer.initialize();
	 
	 while (true)
	{
		i2c.readBuffer(0x6b, 0x18, buffer, 6);
		
		data = (buffer[1] << 8) | buffer[0]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		wbi_B[0] = -1.0*gyroToW*data; // converting to engineering units (radians/second)
		
		data = (buffer[3] << 8) | buffer[2]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		wbi_B[1] = gyroToW*data; // converting to engineering units (radians/second)
		
		data = (buffer[5] << 8) | buffer[4]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		wbi_B[2] = gyroToW*data; // converting to engineering units (radians/second)
		
		gyroMutex.lock();
		
		memcpy(gyroBuffer, wbi_B, sizeof(wbi_B));
		
		gyroMutex.unlock();
		
		gyroMutex.signal();
		
		myTimer.sleep();
		
		// std::cout << wbi_B[0] << ", " << wbi_B[1] << ", " << wbi_B[2] << std::endl;
		
		/*
		wbi_B[0] = -1.0*gyroToW*data - Bw_B[0]; // converting to engineering units (radians/second)
		
		data = (buffer[3] << 8) | buffer[2]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		wbi_B[1] = gyroToW*data - Bw_B[1]; // converting to engineering units (radians/second)
		
		data = (buffer[5] << 8) | buffer[4]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		wbi_B[2] = gyroToW*data - Bw_B[2]; // converting to engineering units (radians/second)
		*/
	}
	
     // 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 $0x48, %rsp
leaq 0x14b8(%rip), %rax      # 0x4f18
movq %rax, -0x48(%rbp)
movq %rdi, -0x40(%rbp)
movl $0x1, -0x38(%rbp)
leaq -0x68(%rbp), %rdi
movsd 0x358d(%rip), %xmm0     # 0x7008
callq 0x3574
leaq -0x68(%rbp), %rdi
callq 0x35f2
leaq 0x67b8(%rip), %rbx      # 0xa248
leaq 0x6799(%rip), %r15      # 0xa230
leaq 0x69ea(%rip), %rdi      # 0xa488
pushq $0x6b
popq %rsi
pushq $0x18
popq %rdx
leaq -0x2e(%rbp), %rcx
pushq $0x6
popq %r8
callq 0x2fd2
movzwl -0x2e(%rbp), %r12d
movzwl -0x2c(%rbp), %r13d
movzwl -0x2a(%rbp), %r14d
movq %rbx, %rdi
callq 0x3228
movswl %r14w, %eax
cvtsi2sd %eax, %xmm0
movsd 0x3540(%rip), %xmm2     # 0x7018
mulsd %xmm2, %xmm0
movswl %r13w, %eax
cvtsi2sd %eax, %xmm1
mulsd %xmm2, %xmm1
movswl %r12w, %eax
cvtsi2sd %eax, %xmm2
mulsd 0x3518(%rip), %xmm2     # 0x7010
movsd %xmm2, (%r15)
movsd %xmm1, 0x8(%r15)
movsd %xmm0, 0x10(%r15)
movq %rbx, %rdi
callq 0x32e4
movq %rbx, %rdi
callq 0x34b8
leaq -0x68(%rbp), %rdi
callq 0x3604
jmp 0x3a97
jmp 0x3b29
movq %rax, %rbx
leaq -0x48(%rbp), %rdi
callq 0x3b3e
movq %rbx, %rdi
callq 0x2350
nop