// File: 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
subq $0x80, %rsp
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rdx
leaq 0x41f6(%rip), %rsi      # 0x9a20
leaq -0x20(%rbp), %rdi
callq 0x5980
movabsq $0x400921fb54442d18, %rax # imm = 0x400921FB54442D18
movq %rax, -0x28(%rbp)
movabsq $0x3f31742de2e0734b, %rax # imm = 0x3F31742DE2E0734B
movq %rax, -0x30(%rbp)
leaq -0x70(%rbp), %rdi
movsd 0x77ad(%rip), %xmm0     # 0xd008
callq 0x4fd0
jmp 0x5862
leaq -0x70(%rbp), %rdi
callq 0x5070
jmp 0x586d
jmp 0x586f
jmp 0x5871
leaq 0xcc80(%rip), %rdi      # 0x124f8
movl $0x6b, %esi
movl $0x18, %edx
leaq -0x36(%rbp), %rcx
movl $0x6, %r8d
callq 0x45b0
jmp 0x5893
movzbl -0x35(%rbp), %eax
shll $0x8, %eax
movzbl -0x36(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x38(%rbp)
movswl -0x38(%rbp), %eax
cvtsi2sd %eax, %xmm0
movsd 0x7774(%rip), %xmm1     # 0xd028
mulsd %xmm1, %xmm0
movsd %xmm0, -0x50(%rbp)
movzbl -0x33(%rbp), %eax
shll $0x8, %eax
movzbl -0x34(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x38(%rbp)
movswl -0x38(%rbp), %eax
cvtsi2sd %eax, %xmm0
movsd 0x7732(%rip), %xmm1     # 0xd010
mulsd %xmm1, %xmm0
movsd %xmm0, -0x48(%rbp)
movzbl -0x31(%rbp), %eax
shll $0x8, %eax
movzbl -0x32(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x38(%rbp)
movswl -0x38(%rbp), %eax
cvtsi2sd %eax, %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0x40(%rbp)
leaq 0xc9a8(%rip), %rdi      # 0x122b8
callq 0x49e0
jmp 0x5917
movq -0x40(%rbp), %rcx
leaq 0xc97e(%rip), %rax      # 0x122a0
movq %rcx, 0x10(%rax)
movaps -0x50(%rbp), %xmm0
movaps %xmm0, (%rax)
leaq 0xc984(%rip), %rdi      # 0x122b8
callq 0x4b30
jmp 0x593b
leaq 0xc976(%rip), %rdi      # 0x122b8
callq 0x4e90
jmp 0x5949
leaq -0x70(%rbp), %rdi
callq 0x5090
jmp 0x5954
jmp 0x586f
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x78(%rbp)
movl %eax, -0x7c(%rbp)
leaq -0x20(%rbp), %rdi
callq 0x59b0
movq -0x78(%rbp), %rdi
callq 0x3420
nopw (%rax,%rax)