// File: mrlordkaj[P]i2cdevlib-pi/MPU6050/MPU6050.cpp
// Function: MPU6050::writeMemoryBlock(unsigned char const*, unsigned short, unsigned char, unsigned char, bool, bool)
bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
    setMemoryBank(bank);
    setMemoryStartAddress(address);
    uint8_t chunkSize;
    uint8_t *verifyBuffer=0;
    uint8_t *progBuffer=0;
    uint16_t i;
    uint8_t j;
    if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
    if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
    for (i = 0; i < dataSize;) {
        // determine correct chunk size according to bank position and data size
        chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;

        // make sure we don't go past the data size
        if (i + chunkSize > dataSize) chunkSize = dataSize - i;

        // make sure this chunk doesn't go past the bank boundary (256 bytes)
        if (chunkSize > 256 - address) chunkSize = 256 - address;
        
        if (useProgMem) {
            // write the chunk of data as specified
            for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j);
        } else {
            // write the chunk of data as specified
            progBuffer = (uint8_t *)data + i;
        }

        I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);

        // verify data if needed
        if (verify && verifyBuffer) {
            setMemoryBank(bank);
            setMemoryStartAddress(address);
            I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
                /*Serial.print("Block write verification error, bank ");
                Serial.print(bank, DEC);
                Serial.print(", address ");
                Serial.print(address, DEC);
                Serial.print("!\nExpected:");
                for (j = 0; j < chunkSize; j++) {
                    Serial.print(" 0x");
                    if (progBuffer[j] < 16) Serial.print("0");
                    Serial.print(progBuffer[j], HEX);
                }
                Serial.print("\nReceived:");
                for (uint8_t j = 0; j < chunkSize; j++) {
                    Serial.print(" 0x");
                    if (verifyBuffer[i + j] < 16) Serial.print("0");
                    Serial.print(verifyBuffer[i + j], HEX);
                }
                Serial.print("\n");*/
                free(verifyBuffer);
                if (useProgMem) free(progBuffer);
                return false; // uh oh.
            }
        }

        // increase byte index by [chunkSize]
        i += chunkSize;

        // uint8_t automatically wraps to 0 at 256
        address += chunkSize;

        // if we aren't done, update bank (if necessary) and address
        if (i < dataSize) {
            if (address == 0) bank++;
            setMemoryBank(bank);
            setMemoryStartAddress(address);
        }
    }
    if (verify) free(verifyBuffer);
    if (useProgMem) free(progBuffer);
    return true;
}

// Assembly:
subq $0x58, %rsp
movl %r9d, 0xc(%rsp)
movl %r8d, 0x10(%rsp)
movl %ecx, %eax
movl 0xc(%rsp), %ecx
movl %eax, 0x14(%rsp)
movl %edx, %eax
movl 0x10(%rsp), %edx
movq %rsi, %r8
movl 0x14(%rsp), %esi
movq %rdi, %r9
movw %ax, %di
movb 0x60(%rsp), %al
movq %r9, 0x48(%rsp)
movq %r8, 0x40(%rsp)
movw %di, 0x3e(%rsp)
movb %sil, 0x3d(%rsp)
movb %dl, 0x3c(%rsp)
andb $0x1, %cl
movb %cl, 0x3b(%rsp)
andb $0x1, %al
movb %al, 0x3a(%rsp)
movq 0x48(%rsp), %rdi
movq %rdi, 0x18(%rsp)
xorl %ecx, %ecx
movzbl 0x3d(%rsp), %esi
movl %ecx, %edx
callq 0x22f00
movq 0x18(%rsp), %rdi
movzbl 0x3c(%rsp), %esi
callq 0x23200
movq $0x0, 0x30(%rsp)
movq $0x0, 0x28(%rsp)
testb $0x1, 0x3b(%rsp)
je 0x41943
movl $0x10, %edi
callq 0x22e40
movq %rax, 0x30(%rsp)
testb $0x1, 0x3a(%rsp)
je 0x41959
movl $0x10, %edi
callq 0x22e40
movq %rax, 0x28(%rsp)
movw $0x0, 0x26(%rsp)
movzwl 0x26(%rsp), %eax
movzwl 0x3e(%rsp), %ecx
cmpl %ecx, %eax
jge 0x41b58
movb $0x10, 0x39(%rsp)
movzwl 0x26(%rsp), %eax
movzbl 0x39(%rsp), %ecx
addl %ecx, %eax
movzwl 0x3e(%rsp), %ecx
cmpl %ecx, %eax
jle 0x4199c
movzwl 0x3e(%rsp), %eax
movzwl 0x26(%rsp), %ecx
subl %ecx, %eax
movb %al, 0x39(%rsp)
movzbl 0x39(%rsp), %eax
movzbl 0x3c(%rsp), %edx
movl $0x100, %ecx            # imm = 0x100
subl %edx, %ecx
cmpl %ecx, %eax
jle 0x419c1
movzbl 0x3c(%rsp), %ecx
movl $0x100, %eax            # imm = 0x100
subl %ecx, %eax
movb %al, 0x39(%rsp)
testb $0x1, 0x3a(%rsp)
je 0x41a11
movb $0x0, 0x25(%rsp)
movzbl 0x25(%rsp), %eax
movzbl 0x39(%rsp), %ecx
cmpl %ecx, %eax
jge 0x41a0f
movq 0x40(%rsp), %rax
movzwl 0x26(%rsp), %ecx
movslq %ecx, %rcx
addq %rcx, %rax
movzbl 0x25(%rsp), %ecx
movslq %ecx, %rcx
movb (%rax,%rcx), %dl
movq 0x28(%rsp), %rax
movzbl 0x25(%rsp), %ecx
movb %dl, (%rax,%rcx)
movb 0x25(%rsp), %al
addb $0x1, %al
movb %al, 0x25(%rsp)
jmp 0x419cd
jmp 0x41a26
movq 0x40(%rsp), %rax
movzwl 0x26(%rsp), %ecx
movslq %ecx, %rcx
addq %rcx, %rax
movq %rax, 0x28(%rsp)
movq 0x18(%rsp), %rax
movb (%rax), %dl
movb 0x39(%rsp), %al
movq 0x28(%rsp), %rcx
movl $0x6f, %esi
movzbl %dl, %edi
movzbl %al, %edx
callq 0x22290
testb $0x1, 0x3b(%rsp)
je 0x41aee
cmpq $0x0, 0x30(%rsp)
je 0x41aee
movq 0x18(%rsp), %rdi
xorl %ecx, %ecx
movzbl 0x3d(%rsp), %esi
movl %ecx, %edx
callq 0x22f00
movq 0x18(%rsp), %rdi
movzbl 0x3c(%rsp), %esi
callq 0x23200
movq 0x18(%rsp), %rax
movb (%rax), %dil
movb 0x39(%rsp), %dl
movq 0x30(%rsp), %rcx
movl $0x6f, %esi
movq 0x1a47c(%rip), %rax     # 0x5bf18
movzbl %dil, %edi
movzbl %dl, %edx
movzwl (%rax), %r8d
callq 0x22b50
movq 0x28(%rsp), %rdi
movq 0x30(%rsp), %rsi
movzbl 0x39(%rsp), %eax
movl %eax, %edx
callq 0x22450
cmpl $0x0, %eax
je 0x41aec
movq 0x30(%rsp), %rdi
callq 0x22cf0
testb $0x1, 0x3a(%rsp)
je 0x41ae2
movq 0x28(%rsp), %rdi
callq 0x22cf0
movb $0x0, 0x57(%rsp)
jmp 0x41b7f
jmp 0x41aee
movzbl 0x39(%rsp), %ecx
movzwl 0x26(%rsp), %eax
addl %ecx, %eax
movw %ax, 0x26(%rsp)
movzbl 0x39(%rsp), %ecx
movzbl 0x3c(%rsp), %eax
addl %ecx, %eax
movb %al, 0x3c(%rsp)
movzwl 0x26(%rsp), %eax
movzwl 0x3e(%rsp), %ecx
cmpl %ecx, %eax
jge 0x41b53
movzbl 0x3c(%rsp), %eax
cmpl $0x0, %eax
jne 0x41b31
movb 0x3d(%rsp), %al
addb $0x1, %al
movb %al, 0x3d(%rsp)
movq 0x18(%rsp), %rdi
xorl %ecx, %ecx
movzbl 0x3d(%rsp), %esi
movl %ecx, %edx
callq 0x22f00
movq 0x18(%rsp), %rdi
movzbl 0x3c(%rsp), %esi
callq 0x23200
jmp 0x41960
testb $0x1, 0x3b(%rsp)
je 0x41b69
movq 0x30(%rsp), %rdi
callq 0x22cf0
testb $0x1, 0x3a(%rsp)
je 0x41b7a
movq 0x28(%rsp), %rdi
callq 0x22cf0
movb $0x1, 0x57(%rsp)
movb 0x57(%rsp), %al
andb $0x1, %al
addq $0x58, %rsp
retq
nopw (%rax,%rax)