// File: 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
subq $0x40, %rsp
leaq 0xec09(%rip), %rdi      # 0x124f8
movl $0x6b, %ecx
movl $0x10, %edx
movl %ecx, %esi
callq 0x4200
movsd 0x9710(%rip), %xmm0     # 0xd018
movsd %xmm0, -0x8(%rbp)
movsd 0x96fb(%rip), %xmm0     # 0xd010
movsd %xmm0, -0x10(%rbp)
leaq -0x38(%rbp), %rdi
movsd 0x96e2(%rip), %xmm0     # 0xd008
callq 0x4fd0
movq 0xe6a6(%rip), %rdi      # 0x11fd8
leaq 0x9737(%rip), %rsi      # 0xd070
callq 0x3270
movq %rax, %rdi
movq 0xe678(%rip), %rsi      # 0x11fc0
callq 0x32b0
leaq -0x38(%rbp), %rdi
callq 0x5070
movl $0x0, -0x3c(%rbp)
cmpl $0xee, -0x3c(%rbp)
jge 0x3a4c
leaq -0x16(%rbp), %rcx
leaq 0xeb83(%rip), %rdi      # 0x124f8
movl $0x6b, %esi
movl $0x18, %edx
movl $0x6, %r8d
callq 0x45b0
movzbl -0x15(%rbp), %eax
shll $0x8, %eax
movzbl -0x16(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x18(%rbp)
movswl -0x18(%rbp), %eax
cvtsi2sd %eax, %xmm0
movsd 0xeaf5(%rip), %xmm1     # 0x124a0
movsd 0x9675(%rip), %xmm2     # 0xd028
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xeadd(%rip)     # 0x124a0
movzbl -0x13(%rbp), %eax
shll $0x8, %eax
movzbl -0x14(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x18(%rbp)
movswl -0x18(%rbp), %eax
cvtsi2sd %eax, %xmm0
movsd 0xeac4(%rip), %xmm1     # 0x124a8
movsd 0x9624(%rip), %xmm2     # 0xd010
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xeaac(%rip)     # 0x124a8
movzbl -0x11(%rbp), %eax
shll $0x8, %eax
movzbl -0x12(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x18(%rbp)
movswl -0x18(%rbp), %eax
cvtsi2sd %eax, %xmm0
movsd 0xea93(%rip), %xmm1     # 0x124b0
movsd 0x95eb(%rip), %xmm2     # 0xd010
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xea7b(%rip)     # 0x124b0
leaq -0x38(%rbp), %rdi
callq 0x5090
movl -0x3c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x3c(%rbp)
jmp 0x395d
movsd 0xea4c(%rip), %xmm0     # 0x124a0
movsd 0x95c4(%rip), %xmm1     # 0xd020
divsd %xmm1, %xmm0
movsd %xmm0, 0xea38(%rip)     # 0x124a0
movsd 0xea38(%rip), %xmm0     # 0x124a8
movsd 0x95a8(%rip), %xmm1     # 0xd020
divsd %xmm1, %xmm0
movsd %xmm0, 0xea24(%rip)     # 0x124a8
movsd 0xea24(%rip), %xmm0     # 0x124b0
movsd 0x958c(%rip), %xmm1     # 0xd020
divsd %xmm1, %xmm0
movsd %xmm0, 0xea10(%rip)     # 0x124b0
movq 0xe531(%rip), %rdi      # 0x11fd8
leaq 0x95e5(%rip), %rsi      # 0xd093
callq 0x3270
movq %rax, %rdi
movsd 0xe9e2(%rip), %xmm0     # 0x124a0
callq 0x3480
movq %rax, %rdi
movq 0xe4f3(%rip), %rsi      # 0x11fc0
callq 0x32b0
movq 0xe4ff(%rip), %rdi      # 0x11fd8
leaq 0x95bd(%rip), %rsi      # 0xd09d
callq 0x3270
movq %rax, %rdi
movsd 0xe9b8(%rip), %xmm0     # 0x124a8
callq 0x3480
movq %rax, %rdi
movq 0xe4c1(%rip), %rsi      # 0x11fc0
callq 0x32b0
movq 0xe4cd(%rip), %rdi      # 0x11fd8
leaq 0x9595(%rip), %rsi      # 0xd0a7
callq 0x3270
movq %rax, %rdi
movsd 0xe98e(%rip), %xmm0     # 0x124b0
callq 0x3480
movq %rax, %rdi
movq 0xe48f(%rip), %rsi      # 0x11fc0
callq 0x32b0
movq 0xe49b(%rip), %rdi      # 0x11fd8
leaq 0x956d(%rip), %rsi      # 0xd0b1
callq 0x3270
movq %rax, %rdi
movq 0xe46d(%rip), %rsi      # 0x11fc0
callq 0x32b0
addq $0x40, %rsp
popq %rbp
retq
nop