// File: 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
subq $0x40, %rsp
leaq 0xe989(%rip), %rdi      # 0x124f8
movl $0x6b, %esi
movl $0x20, %edx
movl $0x40, %ecx
callq 0x4200
movsd 0x94ad(%rip), %xmm0     # 0xd038
movsd %xmm0, -0x8(%rbp)
leaq -0x30(%rbp), %rdi
movsd 0x9494(%rip), %xmm0     # 0xd030
callq 0x4fd0
movq 0xe430(%rip), %rdi      # 0x11fd8
leaq 0x9518(%rip), %rsi      # 0xd0c7
callq 0x3270
movq %rax, %rdi
movq 0xe402(%rip), %rsi      # 0x11fc0
callq 0x32b0
leaq -0x30(%rbp), %rdi
callq 0x5070
movl $0x0, -0x34(%rbp)
cmpl $0x14, -0x34(%rbp)
jge 0x3cbf
leaq -0xe(%rbp), %rcx
leaq 0xe910(%rip), %rdi      # 0x124f8
movl $0x6b, %esi
movl $0x28, %edx
movl $0x6, %r8d
callq 0x45b0
movzbl -0xd(%rbp), %eax
shll $0x8, %eax
movzbl -0xe(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x10(%rbp)
movswl -0x10(%rbp), %eax
cvtsi2sd %eax, %xmm0
movsd 0xe8a2(%rip), %xmm1     # 0x124c0
movsd 0x9422(%rip), %xmm2     # 0xd048
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xe88a(%rip)     # 0x124c0
movzbl -0xb(%rbp), %eax
shll $0x8, %eax
movzbl -0xc(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x10(%rbp)
movswl -0x10(%rbp), %eax
cvtsi2sd %eax, %xmm0
movsd 0xe871(%rip), %xmm1     # 0x124c8
movsd 0x93d9(%rip), %xmm2     # 0xd038
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xe859(%rip)     # 0x124c8
movzbl -0x9(%rbp), %eax
shll $0x8, %eax
movzbl -0xa(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x10(%rbp)
movswl -0x10(%rbp), %eax
cvtsi2sd %eax, %xmm0
movsd 0xe840(%rip), %xmm1     # 0x124d0
movsd 0x93a0(%rip), %xmm2     # 0xd038
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xe828(%rip)     # 0x124d0
leaq -0x30(%rbp), %rdi
callq 0x5090
movl -0x34(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x34(%rbp)
jmp 0x3bd3
movsd 0xe7f9(%rip), %xmm0     # 0x124c0
movsd 0x9371(%rip), %xmm1     # 0xd040
divsd %xmm1, %xmm0
movsd %xmm0, 0xe7e5(%rip)     # 0x124c0
movsd 0xe7e5(%rip), %xmm0     # 0x124c8
movsd 0x9355(%rip), %xmm1     # 0xd040
divsd %xmm1, %xmm0
movsd %xmm0, 0xe7d1(%rip)     # 0x124c8
movsd 0xe7d1(%rip), %xmm0     # 0x124d0
movsd 0x9339(%rip), %xmm1     # 0xd040
divsd %xmm1, %xmm0
movsd %xmm0, 0xe7bd(%rip)     # 0x124d0
movq 0xe2be(%rip), %rdi      # 0x11fd8
leaq 0x93cd(%rip), %rsi      # 0xd0ee
callq 0x3270
movq %rax, %rdi
movsd 0xe78f(%rip), %xmm0     # 0x124c0
callq 0x3480
movq %rax, %rdi
movq 0xe280(%rip), %rsi      # 0x11fc0
callq 0x32b0
movq 0xe28c(%rip), %rdi      # 0x11fd8
leaq 0x93a4(%rip), %rsi      # 0xd0f7
callq 0x3270
movq %rax, %rdi
movsd 0xe765(%rip), %xmm0     # 0x124c8
callq 0x3480
movq %rax, %rdi
movq 0xe24e(%rip), %rsi      # 0x11fc0
callq 0x32b0
movq 0xe25a(%rip), %rdi      # 0x11fd8
leaq 0x937b(%rip), %rsi      # 0xd100
callq 0x3270
movq %rax, %rdi
movsd 0xe73b(%rip), %xmm0     # 0x124d0
callq 0x3480
movq %rax, %rdi
movq 0xe21c(%rip), %rsi      # 0x11fc0
callq 0x32b0
movq 0xe228(%rip), %rdi      # 0x11fd8
leaq 0x9352(%rip), %rsi      # 0xd109
callq 0x3270
movq %rax, %rdi
movq 0xe1fa(%rip), %rsi      # 0x11fc0
callq 0x32b0
addq $0x40, %rsp
popq %rbp
retq
nopw %cs:(%rax,%rax)