// File: /workspace/github_cpp_makefile_star2_O2/rachit31[P]CubeSatADCS/KalmanPi.cpp
// Function: initializeAccel()
void initializeAccel()
{	 
	 // 10 Hz Sampling, 2 gs
	 i2c.writeByte(0x6b, 0x20, 0b001000000);
	 
	 const double toAccel = 2.0/32767.0; // gs/count
	 /* const double toAccel = 2.0/(pow(2,15)-1); // gs/count */
	 
	 uint8_t buffer[6];
	 int16_t data;
	 
	 SleepTimer myTimer(0.1);
	 
	 std::cout << "Hold still to initialize Accelerometer" << std::endl;
	
	 myTimer.initialize();
	
	 for (int i = 0; i < 20; i++)
	 {
		 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_I[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_I[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_I[2] += toAccel*data; // converting to engineering units (g)
		
		 myTimer.sleep();
	 }
	
	 a_I[0] /= 20.0;
	 a_I[1] /= 20.0;
	 a_I[2] /= 20.0;
	
	 std::cout << "a_I[0]: " << a_I[0] << std::endl;
	 std::cout << "a_I[1]: " << a_I[1] << std::endl;
	 std::cout << "a_I[2]: " << a_I[2] << std::endl;
	 
	 std::cout << "Accelerometer initialized" << std::endl;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x28, %rsp
leaq 0x7b86(%rip), %rdi      # 0xa488
pushq $0x6b
popq %rsi
pushq $0x20
popq %rdx
pushq $0x40
popq %rcx
callq 0x2dae
leaq -0x50(%rbp), %r15
movsd 0x470c(%rip), %xmm0     # 0x7028
movq %r15, %rdi
callq 0x3574
movq 0x76ad(%rip), %rdi      # 0x9fd8
leaq 0x478d(%rip), %rsi      # 0x70bf
callq 0x2220
movq %rax, %rdi
callq 0x2100
movq %r15, %rdi
callq 0x35f2
pushq $0x14
popq %r14
pushq $0x28
popq %r12
leaq -0x2e(%rbp), %r13
pushq $0x6
popq %rbx
subl $0x1, %r14d
jb 0x29e5
leaq 0x7b21(%rip), %rdi      # 0xa488
pushq $0x6b
popq %rsi
movl %r12d, %edx
movq %r13, %rcx
movl %ebx, %r8d
callq 0x2fd2
movswl -0x2e(%rbp), %eax
cvtsi2sd %eax, %xmm0
mulsd 0x46a8(%rip), %xmm0     # 0x7030
addsd 0x7ac0(%rip), %xmm0     # 0xa450
movswl -0x2c(%rbp), %eax
cvtsi2sd %eax, %xmm1
movsd %xmm0, 0x7ab0(%rip)     # 0xa450
movsd 0x4690(%rip), %xmm2     # 0x7038
mulsd %xmm2, %xmm1
addsd 0x7aa4(%rip), %xmm1     # 0xa458
movsd %xmm1, 0x7a9c(%rip)     # 0xa458
movswl -0x2a(%rbp), %eax
cvtsi2sd %eax, %xmm0
mulsd %xmm2, %xmm0
addsd 0x7a90(%rip), %xmm0     # 0xa460
movsd %xmm0, 0x7a88(%rip)     # 0xa460
movq %r15, %rdi
callq 0x3604
jmp 0x2956
movsd 0x7a63(%rip), %xmm0     # 0xa450
movsd 0x464b(%rip), %xmm1     # 0x7040
divsd %xmm1, %xmm0
movsd %xmm0, 0x7a4f(%rip)     # 0xa450
movsd 0x7a4f(%rip), %xmm0     # 0xa458
divsd %xmm1, %xmm0
movsd %xmm0, 0x7a43(%rip)     # 0xa458
movsd 0x7a43(%rip), %xmm0     # 0xa460
divsd %xmm1, %xmm0
movsd %xmm0, 0x7a37(%rip)     # 0xa460
movq 0x75a8(%rip), %rbx      # 0x9fd8
leaq 0x46af(%rip), %rsi      # 0x70e6
movq %rbx, %rdi
callq 0x2220
movsd 0x7a09(%rip), %xmm0     # 0xa450
movq %rax, %rdi
callq 0x22f0
movq %rax, %rdi
callq 0x2100
leaq 0x4691(%rip), %rsi      # 0x70ef
movq %rbx, %rdi
callq 0x2220
movsd 0x79ea(%rip), %xmm0     # 0xa458
movq %rax, %rdi
callq 0x22f0
movq %rax, %rdi
callq 0x2100
leaq 0x4673(%rip), %rsi      # 0x70f8
movq %rbx, %rdi
callq 0x2220
movsd 0x79cb(%rip), %xmm0     # 0xa460
movq %rax, %rdi
callq 0x22f0
movq %rax, %rdi
callq 0x2100
leaq 0x4655(%rip), %rsi      # 0x7101
movq %rbx, %rdi
callq 0x2220
movq %rax, %rdi
callq 0x2100
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq