// File: /workspace/github_cpp_makefile_star2_O3/rachit31[P]CubeSatADCS/KalmanPi.cpp
// Function: initializeGyros()
void initializeGyros()
{
     // 119 Hz Sampling, 500 Degrees/second, 31 Hz LPF
	 i2c.writeByte(0x6b, 0x10, 0b01101011);
	 
	 const double pi = 3.141592653589793;
	 const double gyroToW = 500.0/32767.0*pi/180.0; // radians/second/count
	 /* const double gyroToW = 500/(pow(2,15)-1)*pi/180.0; // radians/second/count */
	 
	 uint8_t buffer[6];
	 int16_t data;
	 
	 SleepTimer myTimer(dtGyro);
	 
	 std::cout << "Hold still to initialize Gyro bias" << std::endl;
	
	 myTimer.initialize();
	
	 for (int i = 0; i < 2*119; i++)
	 {
		 i2c.readBuffer(0x6b, 0x18, buffer, 6);
		
		 data = (buffer[1] << 8) | buffer[0]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		 Bw_B[0] += -1.0*gyroToW*data; // computing initial bias, converting to engineering units (radians/second)
		
		 data = (buffer[3] << 8) | buffer[2]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		 Bw_B[1] += gyroToW*data; // computing initial bias, converting to engineering units (radians/second)
		
		 data = (buffer[5] << 8) | buffer[4]; // converting to signed 16 bit value (in raw counts, no units), by bit shifting
		 Bw_B[2] += gyroToW*data; // computing initial bias, converting to engineering units (radians/second)
		
		 myTimer.sleep();
	 }
	
	 Bw_B[0] = Bw_B[0]/(2.0*119.0);
	 Bw_B[1] = Bw_B[1]/(2.0*119.0);
	 Bw_B[2] = Bw_B[2]/(2.0*119.0);
	
	 std::cout << "Bw_B[0]: " << Bw_B[0] << std::endl;
	 std::cout << "Bw_B[1]: " << Bw_B[1] << std::endl;
	 std::cout << "Bw_B[2]: " << Bw_B[2] << std::endl;
	 
	 std::cout << "Gyro bias initialized" << std::endl;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x28, %rsp
leaq 0x8d38(%rip), %r14      # 0xb4a8
movq %r14, %rdi
movl $0x6b, %esi
movl $0x10, %edx
movl $0x6b, %ecx
callq 0x30b8
leaq -0x50(%rbp), %r15
movsd 0x5875(%rip), %xmm0     # 0x8008
movq %r15, %rdi
callq 0x3c60
movq 0x8836(%rip), %rbx      # 0xafd8
leaq 0x58bf(%rip), %rsi      # 0x8068
movl $0x22, %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
movq %r15, %rdi
callq 0x3cde
movl $0xee, %r13d
leaq -0x2e(%rbp), %r12
movq %r14, %rdi
movl $0x6b, %esi
movl $0x18, %edx
movq %r12, %rcx
movl $0x6, %r8d
callq 0x33ae
movswl -0x2e(%rbp), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd 0x57f3(%rip), %xmm0     # 0x8010
addsd 0x8c2b(%rip), %xmm0     # 0xb450
movswl -0x2c(%rbp), %eax
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
movsd %xmm0, 0x8c18(%rip)     # 0xb450
movsd 0x57d8(%rip), %xmm2     # 0x8018
mulsd %xmm2, %xmm1
addsd 0x8c0c(%rip), %xmm1     # 0xb458
movsd %xmm1, 0x8c04(%rip)     # 0xb458
movswl -0x2a(%rbp), %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd %xmm2, %xmm0
addsd 0x8bf5(%rip), %xmm0     # 0xb460
movsd %xmm0, 0x8bed(%rip)     # 0xb460
movq %r15, %rdi
callq 0x3cf0
decl %r13d
jne 0x27ef
movsd 0x8bc4(%rip), %xmm0     # 0xb450
movsd 0x578c(%rip), %xmm1     # 0x8020
divsd %xmm1, %xmm0
movsd %xmm0, 0x8bb0(%rip)     # 0xb450
movsd 0x8bb0(%rip), %xmm0     # 0xb458
divsd %xmm1, %xmm0
movsd %xmm0, 0x8ba4(%rip)     # 0xb458
movsd 0x8ba4(%rip), %xmm0     # 0xb460
divsd %xmm1, %xmm0
movsd %xmm0, 0x8b98(%rip)     # 0xb460
leaq 0x57bc(%rip), %rsi      # 0x808b
movl $0x9, %edx
movq %rbx, %rdi
callq 0x2280
movsd 0x8b6c(%rip), %xmm0     # 0xb450
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 0x5778(%rip), %rsi      # 0x8095
movl $0x9, %edx
movq %rbx, %rdi
callq 0x2280
movsd 0x8b26(%rip), %xmm0     # 0xb458
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 0x5734(%rip), %rsi      # 0x809f
movl $0x9, %edx
movq %rbx, %rdi
callq 0x2280
movsd 0x8ae0(%rip), %xmm0     # 0xb460
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 0x56f0(%rip), %rsi      # 0x80a9
movl $0x15, %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