// File: /workspace/github_cpp_makefile_star2_O1/rachit31[P]CubeSatADCS/readAccel.cpp
// Function: readAccel(void*)
void* readAccel(void *pArg)
 {
	 // Push Cleanup Handler to cleanup thread
	 pthread_cleanup_push(&Thread::cleanup,pArg);
	 
	 // Code goes here...
	
	 uint8_t buffer[6];
	 int16_t data;
	 
	 const double toAccel = 2.0/32767.0; // gs/count
	 /* const double toAccel = 2.0/(pow(2,15)-1); // gs/count */
	 
	 double a_B[3];
	 
	 SleepTimer myTimer(0.1);
	 
	 myTimer.initialize();
	 
	 while (true)
	{
		i2c.readBuffer(0x6b, 0x28, buffer, 6);
		
		data = (buffer[1] << 8) | buffer[0]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		a_B[0] = -1.0*toAccel*data; // converting to engineering units (g)
		
		data = (buffer[3] << 8) | buffer[2]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		a_B[1] = toAccel*data; // converting to engineering units (g)
		
		data = (buffer[5] << 8) | buffer[4]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		a_B[2] = toAccel*data; // converting to engineering units (g)
		
		accelMutex.lock();
		
		memcpy(accelBuffer, a_B, sizeof(a_B));
		
		accelMutex.unlock();
		
		// std::cout << a_B[0] << ", " << a_B[1] << ", " << a_B[2] << std::endl;
		
		myTimer.sleep();
		
		/*
		accelMutex.lock();
		
		memcpy(accelBuffer, a_B, sizeof(a_B));
		
		accelMutex.unlock();
		
		accelMutex.signal();
		*/
		
		/*
		a_B[0] = -1.0*toAccel*data - Accel_Bw_B[0]; // converting to engineering units (g)
		
		data = (buffer[3] << 8) | buffer[2]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		a_B[1] = toAccel*data - Accel_Bw_B[1]; // converting to engineering units (g)
		
		data = (buffer[5] << 8) | buffer[4]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		a_B[2] = toAccel*data - Accel_Bw_B[2]; // converting to engineering units (g)
		*/
	}
	
     // 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 $0x38, %rsp
movq %rdi, %rbx
leaq -0x60(%rbp), %rdi
movsd 0x3b38(%rip), %xmm0     # 0x8028
callq 0x3c70
leaq -0x60(%rbp), %rdi
callq 0x3cee
movq %rbx, -0x40(%rbp)
leaq 0x6f9f(%rip), %rdi      # 0xb4a8
movl $0x6b, %esi
movl $0x28, %edx
leaq -0x2e(%rbp), %rcx
movl $0x6, %r8d
callq 0x33be
movzbl -0x2d(%rbp), %r15d
movzbl -0x2e(%rbp), %eax
movl %eax, -0x34(%rbp)
movzbl -0x2b(%rbp), %r12d
movzbl -0x2c(%rbp), %r13d
movzbl -0x29(%rbp), %r14d
movzbl -0x2a(%rbp), %ebx
leaq 0x6da0(%rip), %rdi      # 0xb2e8
callq 0x3722
movzwl %r14w, %eax
shll $0x8, %eax
movzwl %bx, %ecx
orl %eax, %ecx
movswl %cx, %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
movsd 0x3acd(%rip), %xmm2     # 0x8038
mulsd %xmm2, %xmm0
movzwl %r12w, %eax
shll $0x8, %eax
movzwl %r13w, %ecx
orl %eax, %ecx
movswl %cx, %eax
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
mulsd %xmm2, %xmm1
movzwl %r15w, %eax
shll $0x8, %eax
movzwl -0x34(%rbp), %ecx
orl %eax, %ecx
movswl %cx, %eax
xorps %xmm2, %xmm2
cvtsi2sd %eax, %xmm2
mulsd 0x3a87(%rip), %xmm2     # 0x8030
leaq 0x6d20(%rip), %rax      # 0xb2d0
movsd %xmm2, (%rax)
movsd %xmm1, 0x8(%rax)
movsd %xmm0, 0x10(%rax)
leaq 0x6d23(%rip), %rdi      # 0xb2e8
callq 0x3850
leaq -0x60(%rbp), %rdi
callq 0x3d00
jmp 0x4502
movq %rax, %r14
movq -0x40(%rbp), %rbx
jmp 0x45e4
movq %rax, %r14
movq %rbx, %rdi
callq 0x5afa
movq %r14, %rdi
callq 0x23b0
movq %rax, %rdi
callq 0x36ce