// File: mrlordkaj[P]i2cdevlib-pi/MPU6050/MPU6050.cpp
// Function: MPU6050::writeDMPConfigurationSet(unsigned char const*, unsigned short, bool)
bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
    uint8_t *progBuffer = 0;
	uint8_t success, special;
    uint16_t i, j;
    if (useProgMem) {
        progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary
    }

    // config set data is a long string of blocks with the following structure:
    // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
    uint8_t bank, offset, length;
    for (i = 0; i < dataSize;) {
        if (useProgMem) {
            bank = pgm_read_byte(data + i++);
            offset = pgm_read_byte(data + i++);
            length = pgm_read_byte(data + i++);
        } else {
            bank = data[i++];
            offset = data[i++];
            length = data[i++];
        }

        // write data or perform special action
        if (length > 0) {
            // regular block of data to write
            /*Serial.print("Writing config block to bank ");
            Serial.print(bank);
            Serial.print(", offset ");
            Serial.print(offset);
            Serial.print(", length=");
            Serial.println(length);*/
            if (useProgMem) {
                if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length);
                for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j);
            } else {
                progBuffer = (uint8_t *)data + i;
            }
            success = writeMemoryBlock(progBuffer, length, bank, offset, true);
            i += length;
        } else {
            // special instruction
            // NOTE: this kind of behavior (what and when to do certain things)
            // is totally undocumented. This code is in here based on observed
            // behavior only, and exactly why (or even whether) it has to be here
            // is anybody's guess for now.
            if (useProgMem) {
                special = pgm_read_byte(data + i++);
            } else {
                special = data[i++];
            }
            /*Serial.print("Special command code ");
            Serial.print(special, HEX);
            Serial.println(" found...");*/
            if (special == 0x01) {
                // enable DMP-related interrupts
                
                //setIntZeroMotionEnabled(true);
                //setIntFIFOBufferOverflowEnabled(true);
                //setIntDMPEnabled(true);
                I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32);  // single operation

                success = true;
            } else {
                // unknown special command
                success = false;
            }
        }
        
        if (!success) {
            if (useProgMem) free(progBuffer);
            return false; // uh oh
        }
    }
    if (useProgMem) free(progBuffer);
    return true;
}

// Assembly:
subq $0x48, %rsp
movb %cl, %al
movw %dx, %cx
movq %rdi, 0x38(%rsp)
movq %rsi, 0x30(%rsp)
movw %cx, 0x2e(%rsp)
andb $0x1, %al
movb %al, 0x2d(%rsp)
movq 0x38(%rsp), %rax
movq %rax, 0x8(%rsp)
movq $0x0, 0x20(%rsp)
testb $0x1, 0x2d(%rsp)
je 0x41c67
movl $0x8, %edi
callq 0x22e40
movq %rax, 0x20(%rsp)
movw $0x0, 0x1c(%rsp)
movzwl 0x1c(%rsp), %eax
movzwl 0x2e(%rsp), %ecx
cmpl %ecx, %eax
jge 0x41ee0
testb $0x1, 0x2d(%rsp)
je 0x41cf2
movq 0x30(%rsp), %rax
movw 0x1c(%rsp), %cx
movw %cx, %dx
addw $0x1, %dx
movw %dx, 0x1c(%rsp)
movzwl %cx, %ecx
movslq %ecx, %rcx
movb (%rax,%rcx), %al
movb %al, 0x19(%rsp)
movq 0x30(%rsp), %rax
movw 0x1c(%rsp), %cx
movw %cx, %dx
addw $0x1, %dx
movw %dx, 0x1c(%rsp)
movzwl %cx, %ecx
movslq %ecx, %rcx
movb (%rax,%rcx), %al
movb %al, 0x18(%rsp)
movq 0x30(%rsp), %rax
movw 0x1c(%rsp), %cx
movw %cx, %dx
addw $0x1, %dx
movw %dx, 0x1c(%rsp)
movzwl %cx, %ecx
movslq %ecx, %rcx
movb (%rax,%rcx), %al
movb %al, 0x17(%rsp)
jmp 0x41d52
movq 0x30(%rsp), %rax
movw 0x1c(%rsp), %cx
movw %cx, %dx
addw $0x1, %dx
movw %dx, 0x1c(%rsp)
movzwl %cx, %ecx
movb (%rax,%rcx), %al
movb %al, 0x19(%rsp)
movq 0x30(%rsp), %rax
movw 0x1c(%rsp), %cx
movw %cx, %dx
addw $0x1, %dx
movw %dx, 0x1c(%rsp)
movzwl %cx, %ecx
movb (%rax,%rcx), %al
movb %al, 0x18(%rsp)
movq 0x30(%rsp), %rax
movw 0x1c(%rsp), %cx
movw %cx, %dx
addw $0x1, %dx
movw %dx, 0x1c(%rsp)
movzwl %cx, %ecx
movb (%rax,%rcx), %al
movb %al, 0x17(%rsp)
movzbl 0x17(%rsp), %eax
cmpl $0x0, %eax
jle 0x41e41
testb $0x1, 0x2d(%rsp)
je 0x41ddd
movzbl 0x17(%rsp), %eax
movl %eax, %ecx
movl $0x8, %eax
cmpq %rcx, %rax
jae 0x41d8e
movq 0x20(%rsp), %rdi
movzbl 0x17(%rsp), %eax
movl %eax, %esi
callq 0x231f0
movq %rax, 0x20(%rsp)
movw $0x0, 0x1a(%rsp)
movzwl 0x1a(%rsp), %eax
movzbl 0x17(%rsp), %ecx
cmpl %ecx, %eax
jge 0x41ddb
movq 0x30(%rsp), %rax
movzwl 0x1c(%rsp), %ecx
movslq %ecx, %rcx
addq %rcx, %rax
movzwl 0x1a(%rsp), %ecx
movslq %ecx, %rcx
movb (%rax,%rcx), %dl
movq 0x20(%rsp), %rax
movzwl 0x1a(%rsp), %ecx
movb %dl, (%rax,%rcx)
movw 0x1a(%rsp), %ax
addw $0x1, %ax
movw %ax, 0x1a(%rsp)
jmp 0x41d95
jmp 0x41df2
movq 0x30(%rsp), %rax
movzwl 0x1c(%rsp), %ecx
movslq %ecx, %rcx
addq %rcx, %rax
movq %rax, 0x20(%rsp)
movq 0x8(%rsp), %rdi
movq 0x20(%rsp), %rsi
movzbl 0x17(%rsp), %eax
movw %ax, %cx
movb 0x19(%rsp), %al
movl $0x1, %r9d
xorl %edx, %edx
movzwl %cx, %edx
movzbl %al, %ecx
movzbl 0x18(%rsp), %r8d
movl $0x0, (%rsp)
callq 0x234d0
andb $0x1, %al
movb %al, 0x1f(%rsp)
movzbl 0x17(%rsp), %ecx
movzwl 0x1c(%rsp), %eax
addl %ecx, %eax
movw %ax, 0x1c(%rsp)
jmp 0x41ebc
testb $0x1, 0x2d(%rsp)
je 0x41e6d
movq 0x30(%rsp), %rax
movw 0x1c(%rsp), %cx
movw %cx, %dx
addw $0x1, %dx
movw %dx, 0x1c(%rsp)
movzwl %cx, %ecx
movslq %ecx, %rcx
movb (%rax,%rcx), %al
movb %al, 0x1e(%rsp)
jmp 0x41e8d
movq 0x30(%rsp), %rax
movw 0x1c(%rsp), %cx
movw %cx, %dx
addw $0x1, %dx
movw %dx, 0x1c(%rsp)
movzwl %cx, %ecx
movb (%rax,%rcx), %al
movb %al, 0x1e(%rsp)
movzbl 0x1e(%rsp), %eax
cmpl $0x1, %eax
jne 0x41eb5
movq 0x8(%rsp), %rax
movl $0x38, %esi
movl $0x32, %edx
movzbl (%rax), %edi
callq 0x22ad0
movb $0x1, 0x1f(%rsp)
jmp 0x41eba
movb $0x0, 0x1f(%rsp)
jmp 0x41ebc
cmpb $0x0, 0x1f(%rsp)
jne 0x41edb
testb $0x1, 0x2d(%rsp)
je 0x41ed4
movq 0x20(%rsp), %rdi
callq 0x22cf0
movb $0x0, 0x47(%rsp)
jmp 0x41ef6
jmp 0x41c6e
testb $0x1, 0x2d(%rsp)
je 0x41ef1
movq 0x20(%rsp), %rdi
callq 0x22cf0
movb $0x1, 0x47(%rsp)
movb 0x47(%rsp), %al
andb $0x1, %al
addq $0x48, %rsp
retq
nopw %cs:(%rax,%rax)