// File: /workspace/github_cpp_makefile_star2_O3/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 0x8a94(%rip), %r14      # 0xb4a8
movq %r14, %rdi
movl $0x6b, %esi
movl $0x20, %edx
movl $0x40, %ecx
callq 0x30b8
leaq -0x50(%rbp), %r15
movsd 0x55f1(%rip), %xmm0     # 0x8028
movq %r15, %rdi
callq 0x3c60
movq 0x8592(%rip), %rbx      # 0xafd8
leaq 0x5672(%rip), %rsi      # 0x80bf
movl $0x26, %edx
movq %rbx, %rdi
callq 0x2280
movq (%rbx), %rax
movq -0x18(%rax), %rdi
addq %rbx, %rdi
movl $0xa, %esi
callq 0x2230
movsbl %al, %esi
movq %rbx, %rdi
callq 0x2040
movq %rax, %rdi
callq 0x2160
movq %r15, %rdi
callq 0x3cde
movl $0x14, %r13d
leaq -0x2e(%rbp), %r12
movq %r14, %rdi
movl $0x6b, %esi
movl $0x28, %edx
movq %r12, %rcx
movl $0x6, %r8d
callq 0x33ae
movswl -0x2e(%rbp), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd 0x556f(%rip), %xmm0     # 0x8030
addsd 0x89a7(%rip), %xmm0     # 0xb470
movswl -0x2c(%rbp), %eax
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
movsd %xmm0, 0x8994(%rip)     # 0xb470
movsd 0x5554(%rip), %xmm2     # 0x8038
mulsd %xmm2, %xmm1
addsd 0x8988(%rip), %xmm1     # 0xb478
movsd %xmm1, 0x8980(%rip)     # 0xb478
movswl -0x2a(%rbp), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd %xmm2, %xmm0
addsd 0x8971(%rip), %xmm0     # 0xb480
movsd %xmm0, 0x8969(%rip)     # 0xb480
movq %r15, %rdi
callq 0x3cf0
decl %r13d
jne 0x2a93
movsd 0x8940(%rip), %xmm0     # 0xb470
movsd 0x5508(%rip), %xmm1     # 0x8040
divsd %xmm1, %xmm0
movsd %xmm0, 0x892c(%rip)     # 0xb470
movsd 0x892c(%rip), %xmm0     # 0xb478
divsd %xmm1, %xmm0
movsd %xmm0, 0x8920(%rip)     # 0xb478
movsd 0x8920(%rip), %xmm0     # 0xb480
divsd %xmm1, %xmm0
movsd %xmm0, 0x8914(%rip)     # 0xb480
leaq 0x5573(%rip), %rsi      # 0x80e6
movl $0x8, %edx
movq %rbx, %rdi
callq 0x2280
movsd 0x88e8(%rip), %xmm0     # 0xb470
movq %rbx, %rdi
callq 0x2330
movq %rax, %r14
movq (%rax), %rax
movq -0x18(%rax), %rdi
addq %r14, %rdi
movl $0xa, %esi
callq 0x2230
movsbl %al, %esi
movq %r14, %rdi
callq 0x2040
movq %rax, %rdi
callq 0x2160
leaq 0x552e(%rip), %rsi      # 0x80ef
movl $0x8, %edx
movq %rbx, %rdi
callq 0x2280
movsd 0x88a2(%rip), %xmm0     # 0xb478
movq %rbx, %rdi
callq 0x2330
movq %rax, %r14
movq (%rax), %rax
movq -0x18(%rax), %rdi
addq %r14, %rdi
movl $0xa, %esi
callq 0x2230
movsbl %al, %esi
movq %r14, %rdi
callq 0x2040
movq %rax, %rdi
callq 0x2160
leaq 0x54e9(%rip), %rsi      # 0x80f8
movl $0x8, %edx
movq %rbx, %rdi
callq 0x2280
movsd 0x885c(%rip), %xmm0     # 0xb480
movq %rbx, %rdi
callq 0x2330
movq %rax, %r14
movq (%rax), %rax
movq -0x18(%rax), %rdi
addq %r14, %rdi
movl $0xa, %esi
callq 0x2230
movsbl %al, %esi
movq %r14, %rdi
callq 0x2040
movq %rax, %rdi
callq 0x2160
leaq 0x54a4(%rip), %rsi      # 0x8101
movl $0x19, %edx
movq %rbx, %rdi
callq 0x2280
movq (%rbx), %rax
movq -0x18(%rax), %rdi
addq %rbx, %rdi
movl $0xa, %esi
callq 0x2230
movsbl %al, %esi
movq %rbx, %rdi
callq 0x2040
movq %rax, %rdi
callq 0x2160
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq