// File: /workspace/github_cpp_makefile_star2_O2/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 0x79a5(%rip), %rbx      # 0xa488
pushq $0x1e
popq %r14
pushq $0x20
popq %rdx
movq %rbx, %rdi
movl %r14d, %esi
movl $0xe4, %ecx
callq 0x2dae
pushq $0x22
popq %rdx
movq %rbx, %rdi
movl %r14d, %esi
xorl %ecx, %ecx
callq 0x2dae
leaq -0x50(%rbp), %r15
movsd 0x4532(%rip), %xmm0     # 0x7048
movq %r15, %rdi
callq 0x3574
movq 0x74b3(%rip), %rdi      # 0x9fd8
leaq 0x45ef(%rip), %rsi      # 0x711b
callq 0x2220
movq %rax, %rdi
callq 0x2100
movq %r15, %rdi
callq 0x35f2
pushq $0xa
popq %r14
pushq $0x28
popq %r12
leaq -0x2e(%rbp), %r13
pushq $0x6
popq %rbx
subl $0x1, %r14d
jb 0x2bdb
leaq 0x7927(%rip), %rdi      # 0xa488
pushq $0x1e
popq %rsi
movl %r12d, %edx
movq %r13, %rcx
movl %ebx, %r8d
callq 0x2fd2
movswl -0x2e(%rbp), %eax
cvtsi2sd %eax, %xmm0
movsd 0x44ce(%rip), %xmm2     # 0x7050
mulsd %xmm2, %xmm0
addsd 0x78e2(%rip), %xmm0     # 0xa470
movswl -0x2c(%rbp), %eax
cvtsi2sd %eax, %xmm1
movsd %xmm0, 0x78d2(%rip)     # 0xa470
mulsd %xmm2, %xmm1
addsd 0x78ce(%rip), %xmm1     # 0xa478
movsd %xmm1, 0x78c6(%rip)     # 0xa478
movswl -0x2a(%rbp), %eax
cvtsi2sd %eax, %xmm0
mulsd %xmm2, %xmm0
addsd 0x78ba(%rip), %xmm0     # 0xa480
movsd %xmm0, 0x78b2(%rip)     # 0xa480
movq %r15, %rdi
callq 0x3604
jmp 0x2b50
movsd 0x788d(%rip), %xmm0     # 0xa470
movsd 0x446d(%rip), %xmm1     # 0x7058
divsd %xmm1, %xmm0
movsd %xmm0, 0x7879(%rip)     # 0xa470
movsd 0x7879(%rip), %xmm0     # 0xa478
divsd %xmm1, %xmm0
movsd %xmm0, 0x786d(%rip)     # 0xa478
movsd 0x786d(%rip), %xmm0     # 0xa480
divsd %xmm1, %xmm0
movsd %xmm0, 0x7861(%rip)     # 0xa480
movq 0x73b2(%rip), %rbx      # 0x9fd8
leaq 0x4514(%rip), %rsi      # 0x7141
movq %rbx, %rdi
callq 0x2220
movsd 0x7833(%rip), %xmm0     # 0xa470
movq %rax, %rdi
callq 0x22f0
movq %rax, %rdi
callq 0x2100
leaq 0x44f6(%rip), %rsi      # 0x714a
movq %rbx, %rdi
callq 0x2220
movsd 0x7814(%rip), %xmm0     # 0xa478
movq %rax, %rdi
callq 0x22f0
movq %rax, %rdi
callq 0x2100
leaq 0x44d8(%rip), %rsi      # 0x7153
movq %rbx, %rdi
callq 0x2220
movsd 0x77f5(%rip), %xmm0     # 0xa480
movq %rax, %rdi
callq 0x22f0
movq %rax, %rdi
callq 0x2100
leaq 0x44ba(%rip), %rsi      # 0x715c
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
nopl (%rax)