// File: /workspace/github_cpp_makefile_star2_O1/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 0x87e0(%rip), %r14      # 0xb4a8
movq %r14, %rdi
movl $0x1e, %esi
movl $0x20, %edx
movl $0xe4, %ecx
callq 0x30c8
movq %r14, %rdi
movl $0x1e, %esi
movl $0x22, %edx
xorl %ecx, %ecx
callq 0x30c8
leaq -0x50(%rbp), %r15
movsd 0x5349(%rip), %xmm0     # 0x8048
movq %r15, %rdi
callq 0x3c70
movq 0x82ca(%rip), %rbx      # 0xafd8
leaq 0x5406(%rip), %rsi      # 0x811b
movl $0x25, %edx
movq %rbx, %rdi
callq 0x2290
movq (%rbx), %rax
movq -0x18(%rax), %rdi
addq %rbx, %rdi
movl $0xa, %r13d
movl $0xa, %esi
callq 0x2240
movsbl %al, %esi
movq %rbx, %rdi
callq 0x2040
movq %rax, %rdi
callq 0x2160
movq %r15, %rdi
callq 0x3cee
leaq -0x2e(%rbp), %r12
movq %r14, %rdi
movl $0x1e, %esi
movl $0x28, %edx
movq %r12, %rcx
movl $0x6, %r8d
callq 0x33be
movswl -0x2e(%rbp), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
movsd 0x52c7(%rip), %xmm2     # 0x8050
mulsd %xmm2, %xmm0
addsd 0x86fb(%rip), %xmm0     # 0xb490
movswl -0x2c(%rbp), %eax
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
movsd %xmm0, 0x86e8(%rip)     # 0xb490
mulsd %xmm2, %xmm1
addsd 0x86e4(%rip), %xmm1     # 0xb498
movsd %xmm1, 0x86dc(%rip)     # 0xb498
movswl -0x2a(%rbp), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd %xmm2, %xmm0
addsd 0x86cd(%rip), %xmm0     # 0xb4a0
movsd %xmm0, 0x86c5(%rip)     # 0xb4a0
movq %r15, %rdi
callq 0x3d00
decl %r13d
jne 0x2d5b
movsd 0x869c(%rip), %xmm0     # 0xb490
movsd 0x525c(%rip), %xmm1     # 0x8058
divsd %xmm1, %xmm0
movsd %xmm0, 0x8688(%rip)     # 0xb490
movsd 0x8688(%rip), %xmm0     # 0xb498
divsd %xmm1, %xmm0
movsd %xmm0, 0x867c(%rip)     # 0xb498
movsd 0x867c(%rip), %xmm0     # 0xb4a0
divsd %xmm1, %xmm0
movsd %xmm0, 0x8670(%rip)     # 0xb4a0
leaq 0x530a(%rip), %rsi      # 0x8141
movl $0x8, %edx
movq %rbx, %rdi
callq 0x2290
movsd 0x8644(%rip), %xmm0     # 0xb490
movq %rbx, %rdi
callq 0x2340
movq %rax, %r14
movq (%rax), %rax
movq -0x18(%rax), %rdi
addq %r14, %rdi
movl $0xa, %esi
callq 0x2240
movsbl %al, %esi
movq %r14, %rdi
callq 0x2040
movq %rax, %rdi
callq 0x2160
leaq 0x52c5(%rip), %rsi      # 0x814a
movl $0x8, %edx
movq %rbx, %rdi
callq 0x2290
movsd 0x85fe(%rip), %xmm0     # 0xb498
movq %rbx, %rdi
callq 0x2340
movq %rax, %r14
movq (%rax), %rax
movq -0x18(%rax), %rdi
addq %r14, %rdi
movl $0xa, %esi
callq 0x2240
movsbl %al, %esi
movq %r14, %rdi
callq 0x2040
movq %rax, %rdi
callq 0x2160
leaq 0x5280(%rip), %rsi      # 0x8153
movl $0x8, %edx
movq %rbx, %rdi
callq 0x2290
movsd 0x85b8(%rip), %xmm0     # 0xb4a0
movq %rbx, %rdi
callq 0x2340
movq %rax, %r14
movq (%rax), %rax
movq -0x18(%rax), %rdi
addq %r14, %rdi
movl $0xa, %esi
callq 0x2240
movsbl %al, %esi
movq %r14, %rdi
callq 0x2040
movq %rax, %rdi
callq 0x2160
leaq 0x523b(%rip), %rsi      # 0x815c
movl $0x18, %edx
movq %rbx, %rdi
callq 0x2290
movq (%rbx), %rax
movq -0x18(%rax), %rdi
addq %rbx, %rdi
movl $0xa, %esi
callq 0x2240
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