// File: mrlordkaj[P]i2cdevlib-pi/MPU6050/MPU6050.cpp
// Function: MPU6050::GetCurrentFIFOPacket(unsigned char*, unsigned char)
int8_t MPU6050::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof
     int16_t fifoC;
     // This section of code is for when we allowed more than 1 packet to be acquired
     uint32_t BreakTimer = micros();
     do {
         if ((fifoC = getFIFOCount())  > length) {

             if (fifoC > 200) { // if you waited to get the FIFO buffer to > 200 bytes it will take longer to get the last packet in the FIFO Buffer than it will take to  reset the buffer and wait for the next to arrive
                 resetFIFO(); // Fixes any overflow corruption
                 fifoC = 0;
                 while (!(fifoC = getFIFOCount()) && ((micros() - BreakTimer) <= (11000))); // Get Next New Packet
                 } else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
                 uint8_t Trash[BUFFER_LENGTH];
                 while ((fifoC = getFIFOCount()) > length) {  // Test each time just in case the MPU is writing to the FIFO Buffer
                     fifoC = fifoC - length; // Save the last packet
                     uint16_t  RemoveBytes;
                     while (fifoC) { // fifo count will reach zero so this is safe
                         RemoveBytes = min((int)fifoC, BUFFER_LENGTH); // Buffer Length is different than the packet length this will efficiently clear the buffer
                         getFIFOBytes(Trash, (uint8_t)RemoveBytes);
                         fifoC -= RemoveBytes;
                     }
                 }
             }
         }
         if (!fifoC) return 0; // Called too early no data or we timed out after FIFO Reset
         // We have 1 packet
         if ((micros() - BreakTimer) > (11000)) return 0;
     } while (fifoC != length);
     getFIFOBytes(data, length); //Get 1 packet
     return 1;
}

// Assembly:
subq $0x68, %rsp
movb %dl, %al
movq %rdi, 0x58(%rsp)
movq %rsi, 0x50(%rsp)
movb %al, 0x4f(%rsp)
movq 0x58(%rsp), %rax
movq %rax, 0x10(%rsp)
callq 0x22d40
movl %eax, 0x48(%rsp)
movq 0x10(%rsp), %rdi
callq 0x22880
movw %ax, 0x4c(%rsp)
cwtl
movzbl 0x4f(%rsp), %ecx
cmpl %ecx, %eax
jle 0x40945
movswl 0x4c(%rsp), %eax
cmpl $0xc8, %eax
jle 0x408b5
movq 0x10(%rsp), %rdi
callq 0x23250
movw $0x0, 0x4c(%rsp)
movq 0x10(%rsp), %rdi
callq 0x22880
movw %ax, %cx
movw %cx, 0x4c(%rsp)
xorl %eax, %eax
cmpw $0x0, %cx
movb %al, 0xf(%rsp)
jne 0x408a4
callq 0x22d40
subl 0x48(%rsp), %eax
cmpl $0x2af8, %eax           # imm = 0x2AF8
setbe %al
movb %al, 0xf(%rsp)
movb 0xf(%rsp), %al
testb $0x1, %al
jne 0x408ae
jmp 0x408b0
jmp 0x40871
jmp 0x40943
jmp 0x408b7
movq 0x10(%rsp), %rdi
callq 0x22880
movw %ax, 0x4c(%rsp)
cwtl
movzbl 0x4f(%rsp), %ecx
cmpl %ecx, %eax
jle 0x40941
movswl 0x4c(%rsp), %eax
movzbl 0x4f(%rsp), %ecx
subl %ecx, %eax
movw %ax, 0x4c(%rsp)
cmpw $0x0, 0x4c(%rsp)
je 0x4093c
movswl 0x4c(%rsp), %eax
cmpl $0x20, %eax
jge 0x408fe
movswl 0x4c(%rsp), %eax
movl %eax, 0x8(%rsp)
jmp 0x40909
movl $0x20, %eax
movl %eax, 0x8(%rsp)
jmp 0x40909
movq 0x10(%rsp), %rdi
movl 0x8(%rsp), %eax
movw %ax, 0x1e(%rsp)
leaq 0x20(%rsp), %rsi
movw 0x1e(%rsp), %ax
movzbl %al, %edx
callq 0x23540
movzwl 0x1e(%rsp), %ecx
movswl 0x4c(%rsp), %eax
subl %ecx, %eax
movw %ax, 0x4c(%rsp)
jmp 0x408e1
jmp 0x408b7
jmp 0x40943
jmp 0x40945
cmpw $0x0, 0x4c(%rsp)
jne 0x40954
movb $0x0, 0x67(%rsp)
jmp 0x40998
callq 0x22d40
subl 0x48(%rsp), %eax
cmpl $0x2af8, %eax           # imm = 0x2AF8
jbe 0x4096b
movb $0x0, 0x67(%rsp)
jmp 0x40998
jmp 0x4096d
movswl 0x4c(%rsp), %eax
movzbl 0x4f(%rsp), %ecx
cmpl %ecx, %eax
jne 0x40837
movq 0x10(%rsp), %rdi
movq 0x50(%rsp), %rsi
movzbl 0x4f(%rsp), %edx
callq 0x23540
movb $0x1, 0x67(%rsp)
movb 0x67(%rsp), %al
addq $0x68, %rsp
retq
nopw %cs:(%rax,%rax)