// File: 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
subq $0x40, %rsp
leaq 0xe709(%rip), %rdi      # 0x124f8
movl $0x1e, %esi
movl $0x20, %edx
movl $0xe4, %ecx
callq 0x4200
leaq 0xe6ee(%rip), %rdi      # 0x124f8
movl $0x1e, %esi
movl $0x22, %edx
xorl %ecx, %ecx
callq 0x4200
movsd 0x9235(%rip), %xmm0     # 0xd058
movsd %xmm0, -0x8(%rbp)
leaq -0x30(%rbp), %rdi
movsd 0x921c(%rip), %xmm0     # 0xd050
callq 0x4fd0
movq 0xe198(%rip), %rdi      # 0x11fd8
leaq 0x92dc(%rip), %rsi      # 0xd123
callq 0x3270
movq %rax, %rdi
movq 0xe16a(%rip), %rsi      # 0x11fc0
callq 0x32b0
leaq -0x30(%rbp), %rdi
callq 0x5070
movl $0x0, -0x34(%rbp)
cmpl $0xa, -0x34(%rbp)
jge 0x3f57
leaq -0xe(%rbp), %rcx
leaq 0xe678(%rip), %rdi      # 0x124f8
movl $0x1e, %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 0xe62a(%rip), %xmm1     # 0x124e0
movsd 0x919a(%rip), %xmm2     # 0xd058
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xe612(%rip)     # 0x124e0
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 0xe5f9(%rip), %xmm1     # 0x124e8
movsd 0x9161(%rip), %xmm2     # 0xd058
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xe5e1(%rip)     # 0x124e8
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 0xe5c8(%rip), %xmm1     # 0x124f0
movsd 0x9128(%rip), %xmm2     # 0xd058
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xe5b0(%rip)     # 0x124f0
leaq -0x30(%rbp), %rdi
callq 0x5090
movl -0x34(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x34(%rbp)
jmp 0x3e6b
movsd 0xe581(%rip), %xmm0     # 0x124e0
movsd 0x90f9(%rip), %xmm1     # 0xd060
divsd %xmm1, %xmm0
movsd %xmm0, 0xe56d(%rip)     # 0x124e0
movsd 0xe56d(%rip), %xmm0     # 0x124e8
movsd 0x90dd(%rip), %xmm1     # 0xd060
divsd %xmm1, %xmm0
movsd %xmm0, 0xe559(%rip)     # 0x124e8
movsd 0xe559(%rip), %xmm0     # 0x124f0
movsd 0x90c1(%rip), %xmm1     # 0xd060
divsd %xmm1, %xmm0
movsd %xmm0, 0xe545(%rip)     # 0x124f0
movq 0xe026(%rip), %rdi      # 0x11fd8
leaq 0x9190(%rip), %rsi      # 0xd149
callq 0x3270
movq %rax, %rdi
movsd 0xe517(%rip), %xmm0     # 0x124e0
callq 0x3480
movq %rax, %rdi
movq 0xdfe8(%rip), %rsi      # 0x11fc0
callq 0x32b0
movq 0xdff4(%rip), %rdi      # 0x11fd8
leaq 0x9167(%rip), %rsi      # 0xd152
callq 0x3270
movq %rax, %rdi
movsd 0xe4ed(%rip), %xmm0     # 0x124e8
callq 0x3480
movq %rax, %rdi
movq 0xdfb6(%rip), %rsi      # 0x11fc0
callq 0x32b0
movq 0xdfc2(%rip), %rdi      # 0x11fd8
leaq 0x913e(%rip), %rsi      # 0xd15b
callq 0x3270
movq %rax, %rdi
movsd 0xe4c3(%rip), %xmm0     # 0x124f0
callq 0x3480
movq %rax, %rdi
movq 0xdf84(%rip), %rsi      # 0x11fc0
callq 0x32b0
movq 0xdf90(%rip), %rdi      # 0x11fd8
leaq 0x9115(%rip), %rsi      # 0xd164
callq 0x3270
movq %rax, %rdi
movq 0xdf62(%rip), %rsi      # 0x11fc0
callq 0x32b0
addq $0x40, %rsp
popq %rbp
retq
nopl (%rax)