// File: /workspace/github_cpp_makefile_star2_O3/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:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x28, %rsp
movl %ecx, %r14d
movl %edx, %ebp
movq %rsi, %r15
movq %rdi, 0x20(%rsp)
testl %ecx, %ecx
je 0x2fd02
movl $0x8, %edi
callq 0x1f7b0
movq %rax, 0x10(%rsp)
jmp 0x2fd0b
movq $0x0, 0x10(%rsp)
xorl %ebx, %ebx
movl $0xffff, %r12d          # imm = 0xFFFF
cmpw %bp, %bx
jae 0x2fe1c
addl $0x3, %ebx
leal -0x1(%rbx), %eax
andl %r12d, %eax
movzbl (%r15,%rax), %r13d
testq %r13, %r13
jne 0x2fd65
movzwl %bx, %eax
cmpb $0x1, (%r15,%rax)
jne 0x2fe13
movq 0x20(%rsp), %rax
movzbl (%rax), %edi
movl $0x38, %esi
movl $0x32, %edx
callq 0x1f5d0
leal 0x4(%rbx), %eax
incl %ebx
cmpw %bp, %bx
movl %eax, %ebx
jb 0x2fd1f
jmp 0x2fe1c
leal -0x3(%rbx), %eax
leal -0x2(%rbx), %ecx
movl $0xffff, %edx           # imm = 0xFFFF
andl %edx, %ecx
movl $0xffff, %r12d          # imm = 0xFFFF
andl %edx, %eax
movb (%r15,%rcx), %dl
movb (%r15,%rax), %dil
movl %r14d, %eax
movzwl %bx, %r14d
movl %eax, 0x1c(%rsp)
testb %al, %al
je 0x2fdd6
cmpb $0x9, %r13b
jb 0x2fdbb
movb %dil, 0xe(%rsp)
movq 0x10(%rsp), %rdi
movq %r13, %rsi
movb %dl, 0xf(%rsp)
callq 0x1f9a0
movb 0xe(%rsp), %dil
movb 0xf(%rsp), %dl
movq %rax, 0x10(%rsp)
addq %r15, %r14
xorl %eax, %eax
movq 0x10(%rsp), %rsi
movb (%r14,%rax), %cl
movb %cl, (%rsi,%rax)
incq %rax
cmpq %rax, %r13
jne 0x2fdc5
jmp 0x2fddc
addq %r15, %r14
movq %r14, %rsi
movl $0x0, (%rsp)
movzbl %dil, %ecx
movzbl %dl, %r8d
movq 0x20(%rsp), %rdi
movq %rsi, 0x10(%rsp)
movl %r13d, %edx
movl $0x1, %r9d
callq 0x1fae0
addl %r13d, %ebx
testb %al, %al
movl 0x1c(%rsp), %r14d
jne 0x2fd13
xorl %ebx, %ebx
movl $0x0, %eax
jmp 0x2fe20
movb $0x1, %bl
movb $0x1, %al
testb %r14b, %r14b
je 0x2fe31
movq 0x10(%rsp), %rdi
callq 0x1f6e0
movl %ebx, %eax
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq