// File: /workspace/github_cpp_makefile_star2_O2/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 $0x18, %rsp
movl %edx, 0x14(%rsp)
movq %rsi, %r14
movq %rdi, %r15
movl %ecx, 0x10(%rsp)
testl %ecx, %ecx
je 0x2e1eb
pushq $0x8
popq %rdi
callq 0x1fab0
movq %rax, %rdx
jmp 0x2e1ed
xorl %edx, %edx
xorl %ebp, %ebp
addl $0x3, %ebp
leal -0x3(%rbp), %r12d
cmpw 0x14(%rsp), %r12w
jae 0x2e2c8
leal -0x1(%rbp), %eax
movl $0xffff, %ecx           # imm = 0xFFFF
andl %ecx, %eax
movzbl (%r14,%rax), %r13d
movzwl %bp, %ebx
testq %r13, %r13
jne 0x2e23e
cmpb $0x1, (%r14,%rbx)
jne 0x2e2c8
movzbl (%r15), %edi
movq %rdx, %rbx
pushq $0x38
popq %rsi
pushq $0x32
popq %rdx
callq 0x1f7f0
movq %rbx, %rdx
addl $0x4, %ebp
jmp 0x2e1f2
leal -0x2(%rbp), %eax
andl %ecx, %eax
movb (%r14,%rax), %sil
movzwl %r12w, %eax
movb (%r14,%rax), %dil
cmpb $0x0, 0x10(%rsp)
je 0x2e293
cmpb $0x9, %r13b
jb 0x2e27e
movb %dil, 0xe(%rsp)
movq %rdx, %rdi
movb %sil, 0xf(%rsp)
movq %r13, %rsi
callq 0x1fdc0
movb 0xe(%rsp), %dil
movb 0xf(%rsp), %sil
movq %rax, %rdx
addq %r14, %rbx
xorl %eax, %eax
cmpq %rax, %r13
je 0x2e299
movb (%rbx,%rax), %cl
movb %cl, (%rdx,%rax)
incq %rax
jmp 0x2e283
addq %r14, %rbx
movq %rbx, %rdx
andl $0x0, (%rsp)
movzbl %dil, %ecx
movzbl %sil, %r8d
movq %r15, %rdi
movq %rdx, %rbx
movq %rdx, %rsi
movl %r13d, %edx
pushq $0x1
popq %r9
callq 0x1ffd0
movq %rbx, %rdx
addl %r13d, %ebp
testb %al, %al
jne 0x2e1ef
cmpb $0x0, 0x10(%rsp)
je 0x2e2d7
movq %rdx, %rdi
callq 0x1f9c0
cmpw 0x14(%rsp), %r12w
setae %al
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop