// File: /workspace/github_cpp_makefile_star2_O3/rachit31[P]CubeSatADCS/KalmanPi.cpp
// Function: initializeMag()
void initializeMag()
{
	 // TEMP_COMP Enabled, high Performance, 1.25 Hz Sampling
	 i2c.writeByte(0x1E, 0x20, 0b11100100);
	 
	 // Continuous COnversion Mode
	 i2c.writeByte(0x1E, 0x22, 0b00000000);
	 
	 const double toMag = 4.0/32767.0; // gauss/count
	 /* const double toMag = 4.0/(pow(2,15)-1); // gauss/count */
	 
	 uint8_t buffer[6];
	 int16_t data;
	 
	 SleepTimer myTimer(1.0/1.25);
	 
	 std::cout << "Hold still to initialize Magnetometer" << std::endl;
	
	 myTimer.initialize();
	
	 for (int i = 0; i < 10; i++)
	 {
		 i2c.readBuffer(0x1E, 0x28, buffer, 6);
		
		 data = (buffer[1] << 8) | buffer[0]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		 B_I[0] += 1.0*toMag*data; // converting to engineering units (gauss)
		
		 data = (buffer[3] << 8) | buffer[2]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		 B_I[1] += toMag*data; // converting to engineering units (gauss)
		
		 data = (buffer[5] << 8) | buffer[4]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		 B_I[2] += toMag*data; // converting to engineering units (gauss)
		
		 myTimer.sleep();
	 }
	
	 B_I[0] /= 10.0;
	 B_I[1] /= 10.0;
	 B_I[2] /= 10.0;
	
	 std::cout << "B_I[0]: " << B_I[0] << std::endl;
	 std::cout << "B_I[1]: " << B_I[1] << std::endl;
	 std::cout << "B_I[2]: " << B_I[2] << std::endl;
	 
	 std::cout << "Magnetometer initialized" << std::endl;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x28, %rsp
leaq 0x87f0(%rip), %r14      # 0xb4a8
movq %r14, %rdi
movl $0x1e, %esi
movl $0x20, %edx
movl $0xe4, %ecx
callq 0x30b8
movq %r14, %rdi
movl $0x1e, %esi
movl $0x22, %edx
xorl %ecx, %ecx
callq 0x30b8
leaq -0x50(%rbp), %r15
movsd 0x5359(%rip), %xmm0     # 0x8048
movq %r15, %rdi
callq 0x3c60
movq 0x82da(%rip), %rbx      # 0xafd8
leaq 0x5416(%rip), %rsi      # 0x811b
movl $0x25, %edx
movq %rbx, %rdi
callq 0x2280
movq (%rbx), %rax
movq -0x18(%rax), %rdi
addq %rbx, %rdi
movl $0xa, %r13d
movl $0xa, %esi
callq 0x2230
movsbl %al, %esi
movq %rbx, %rdi
callq 0x2040
movq %rax, %rdi
callq 0x2160
movq %r15, %rdi
callq 0x3cde
leaq -0x2e(%rbp), %r12
movq %r14, %rdi
movl $0x1e, %esi
movl $0x28, %edx
movq %r12, %rcx
movl $0x6, %r8d
callq 0x33ae
movswl -0x2e(%rbp), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
movsd 0x52d7(%rip), %xmm2     # 0x8050
mulsd %xmm2, %xmm0
addsd 0x870b(%rip), %xmm0     # 0xb490
movswl -0x2c(%rbp), %eax
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
movsd %xmm0, 0x86f8(%rip)     # 0xb490
mulsd %xmm2, %xmm1
addsd 0x86f4(%rip), %xmm1     # 0xb498
movsd %xmm1, 0x86ec(%rip)     # 0xb498
movswl -0x2a(%rbp), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd %xmm2, %xmm0
addsd 0x86dd(%rip), %xmm0     # 0xb4a0
movsd %xmm0, 0x86d5(%rip)     # 0xb4a0
movq %r15, %rdi
callq 0x3cf0
decl %r13d
jne 0x2d4b
movsd 0x86ac(%rip), %xmm0     # 0xb490
movsd 0x526c(%rip), %xmm1     # 0x8058
divsd %xmm1, %xmm0
movsd %xmm0, 0x8698(%rip)     # 0xb490
movsd 0x8698(%rip), %xmm0     # 0xb498
divsd %xmm1, %xmm0
movsd %xmm0, 0x868c(%rip)     # 0xb498
movsd 0x868c(%rip), %xmm0     # 0xb4a0
divsd %xmm1, %xmm0
movsd %xmm0, 0x8680(%rip)     # 0xb4a0
leaq 0x531a(%rip), %rsi      # 0x8141
movl $0x8, %edx
movq %rbx, %rdi
callq 0x2280
movsd 0x8654(%rip), %xmm0     # 0xb490
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 0x52d5(%rip), %rsi      # 0x814a
movl $0x8, %edx
movq %rbx, %rdi
callq 0x2280
movsd 0x860e(%rip), %xmm0     # 0xb498
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 0x5290(%rip), %rsi      # 0x8153
movl $0x8, %edx
movq %rbx, %rdi
callq 0x2280
movsd 0x85c8(%rip), %xmm0     # 0xb4a0
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 0x524b(%rip), %rsi      # 0x815c
movl $0x18, %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