// File: /workspace/github_cpp_makefile_star2_O3/mrlordkaj[P]i2cdevlib-pi/MPU6050/MPU6050.cpp
// Function: MPU6050::PID(unsigned char, float, float, unsigned char)
void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
	uint8_t SaveAddress = (ReadAddress == 0x3B)?((getDeviceID() < 0x38 )? 0x06:0x77):0x13;

	int16_t  Data;
	float Reading;
	int16_t BitZero[3];
	uint8_t shift =(SaveAddress == 0x77)?3:2;
	float Error, PTerm, ITerm[3];
	int16_t eSample;
	uint32_t eSum ;
	Serial.write('>');
	for (int i = 0; i < 3; i++) {
		I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word)
		Reading = Data;
		if(SaveAddress != 0x13){
			BitZero[i] = Data & 1;										 // Capture Bit Zero to properly handle Accelerometer calibration
			ITerm[i] = ((float)Reading) * 8;
			} else {
			ITerm[i] = Reading * 4;
		}
	}
	for (int L = 0; L < Loops; L++) {
		eSample = 0;
		for (int c = 0; c < 100; c++) {// 100 PI Calculations
			eSum = 0;
			for (int i = 0; i < 3; i++) {
				I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word)
				Reading = Data;
				if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384;	//remove Gravity
				Error = -Reading;
				eSum += abs(Reading);
				PTerm = kP * Error;
				ITerm[i] += (Error * 0.001) * kI;				// Integral term 1000 Calculations a second = 0.001
				if(SaveAddress != 0x13){
					Data = round((PTerm + ITerm[i] ) / 8);		//Compute PID Output
					Data = ((Data)&0xFFFE) |BitZero[i];			// Insert Bit0 Saved at beginning
				} else Data = round((PTerm + ITerm[i] ) / 4);	//Compute PID Output
				I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data);
			}
			if((c == 99) && eSum > 1000){						// Error is still to great to continue 
				c = 0;
				Serial.write('*');
			}
			if((eSum * ((ReadAddress == 0x3B)?.05: 1)) < 5) eSample++;	// Successfully found offsets prepare to  advance
			if((eSum < 100) && (c > 10) && (eSample >= 10)) break;		// Advance to next Loop
			delay(1);
		}
		Serial.write('.');
		kP *= .75;
		kI *= .75;
		for (int i = 0; i < 3; i++){
			if(SaveAddress != 0x13) {
				Data = round((ITerm[i] ) / 8);		//Compute PID Output
				Data = ((Data)&0xFFFE) |BitZero[i];	// Insert Bit0 Saved at beginning
			} else Data = round((ITerm[i]) / 4);
			I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data);
		}
	}
	resetFIFO();
	resetDMP();
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x58, %rsp
movl %edx, %r14d
movss %xmm1, 0x8(%rsp)
movss %xmm0, 0xc(%rsp)
movq %rdi, %rbx
movl $0x13, 0x4(%rsp)
movl %esi, 0x14(%rsp)
cmpl $0x3b, %esi
jne 0x2ff83
movzbl (%rbx), %edi
leaq 0x1(%rbx), %r8
movq 0x16feb(%rip), %rax     # 0x46f48
movzwl (%rax), %r9d
movl $0x6, 0x4(%rsp)
movl $0x75, %esi
movl $0x6, %edx
movl $0x6, %ecx
callq 0x1f930
cmpb $0x37, 0x1(%rbx)
ja 0x2ff8f
movl $0x2, %eax
movq %rax, 0x18(%rsp)
jmp 0x2ffa1
movl $0x3, %eax
movq %rax, 0x18(%rsp)
movl $0x77, 0x4(%rsp)
movq 0x16fd8(%rip), %rdi     # 0x46f80
movl $0x3e, %esi
callq 0x1f730
xorl %r15d, %r15d
leaq 0x2(%rsp), %r12
movl 0x4(%rsp), %eax
movl %eax, %ebp
movzbl (%rbx), %edi
movq 0x16f7e(%rip), %rax     # 0x46f48
movzwl (%rax), %r8d
movzbl %bpl, %esi
movl $0x1, %edx
movq %r12, %rcx
callq 0x1f230
movswl 0x2(%rsp), %eax
movss 0x60e0(%rip), %xmm0     # 0x360cc
cmpl $0x13, 0x4(%rsp)
je 0x30006
movl %eax, %ecx
andl $0x1, %ecx
movw %cx, 0x26(%rsp,%r15,2)
movss 0x60ca(%rip), %xmm0     # 0x360d0
xorps %xmm1, %xmm1
cvtsi2ss %eax, %xmm1
mulss %xmm0, %xmm1
movss %xmm1, 0x2c(%rsp,%r15,4)
incq %r15
addb 0x18(%rsp), %bpl
cmpq $0x3, %r15
jne 0x2ffc0
testb %r14b, %r14b
movss 0x8(%rsp), %xmm0
je 0x30341
movzbl %r14b, %eax
movl %eax, 0x20(%rsp)
xorl %eax, %eax
movl 0x14(%rsp), %edx
cmpb $0x3b, %dl
sete %al
leaq 0x60a0(%rip), %rcx      # 0x360f0
movsd (%rcx,%rax,8), %xmm1
movsd %xmm1, 0x40(%rsp)
movzbl %dl, %ebp
xorl %eax, %eax
leaq 0x2(%rsp), %r14
movq %rax, 0x38(%rsp)
movss %xmm0, 0x8(%rsp)
cvtss2sd %xmm0, %xmm0
movsd %xmm0, 0x50(%rsp)
xorl %eax, %eax
xorl %r15d, %r15d
movq %rax, 0x48(%rsp)
movl %r15d, 0x10(%rsp)
xorl %r15d, %r15d
movl 0x4(%rsp), %r12d
xorl %r13d, %r13d
movzbl (%rbx), %edi
leal (%r15,%rbp), %eax
movq 0x16ea6(%rip), %rcx     # 0x46f48
movzwl (%rcx), %r8d
movzbl %al, %esi
movl $0x1, %edx
movq %r14, %rcx
callq 0x1f230
movswl 0x2(%rsp), %eax
xorps %xmm1, %xmm1
cvtsi2ss %eax, %xmm1
movaps %xmm1, %xmm0
addss 0x6007(%rip), %xmm0     # 0x360d4
cmpq $0x4, %r15
je 0x300d6
movaps %xmm1, %xmm0
cmpb $0x3b, 0x14(%rsp)
je 0x300e0
movaps %xmm1, %xmm0
xorps %xmm1, %xmm1
cvtss2sd %xmm0, %xmm1
mulsd 0x5fc9(%rip), %xmm1     # 0x360b8
mulsd 0x50(%rsp), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x2c(%rsp,%r15,2), %xmm2
addsd %xmm1, %xmm2
xorps %xmm1, %xmm1
cvtsd2ss %xmm2, %xmm1
movss %xmm1, 0x2c(%rsp,%r15,2)
movaps %xmm0, %xmm2
mulss 0xc(%rsp), %xmm2
subss %xmm2, %xmm1
cmpl $0x13, 0x4(%rsp)
jne 0x30150
mulss 0x5faf(%rip), %xmm1     # 0x360dc
xorl %eax, %eax
ucomiss 0x5fae(%rip), %xmm1     # 0x360e4
setb %al
cvtss2sd %xmm1, %xmm1
leaq 0x5fcc(%rip), %rcx      # 0x36110
addsd (%rcx,%rax,8), %xmm1
cvttsd2si %xmm1, %rax
jmp 0x30184
mulss 0x5f80(%rip), %xmm1     # 0x360d8
xorl %eax, %eax
ucomiss 0x5f83(%rip), %xmm1     # 0x360e4
setb %al
cvtss2sd %xmm1, %xmm1
leaq 0x5fa1(%rip), %rcx      # 0x36110
addsd (%rcx,%rax,8), %xmm1
cvttsd2si %xmm1, %rax
andl $0xfffe, %eax           # imm = 0xFFFE
orw 0x26(%rsp,%r15), %ax
movaps %xmm0, %xmm1
xorps 0x5f72(%rip), %xmm1     # 0x36100
maxss %xmm1, %xmm0
movl %r13d, %ecx
xorps %xmm1, %xmm1
cvtsi2ss %rcx, %xmm1
addss %xmm0, %xmm1
cvttss2si %xmm1, %r13
movw %ax, 0x2(%rsp)
movzbl (%rbx), %edi
movzbl %r12b, %r12d
movl %r12d, %esi
movl $0x1, %edx
movq %r14, %rcx
callq 0x1f7d0
addq $0x2, %r15
addb 0x18(%rsp), %r12b
cmpq $0x6, %r15
jne 0x30094
movq 0x48(%rsp), %r12
cmpl $0x63, %r12d
jne 0x30204
cmpl $0x3e8, %r13d           # imm = 0x3E8
movl 0x10(%rsp), %r15d
jbe 0x30209
movq 0x16d8b(%rip), %rdi     # 0x46f80
movl $0x2a, %esi
callq 0x1f730
xorl %r12d, %r12d
jmp 0x30209
movl 0x10(%rsp), %r15d
movl %r13d, %eax
xorps %xmm0, %xmm0
cvtsi2sd %rax, %xmm0
mulsd 0x40(%rsp), %xmm0
xorl %eax, %eax
movsd 0x5e9c(%rip), %xmm1     # 0x360c0
ucomisd %xmm0, %xmm1
seta %al
addl %eax, %r15d
cmpl $0x63, %r13d
ja 0x30241
cmpl $0xb, %r12d
jl 0x30241
cmpw $0x9, %r15w
jg 0x3025a
movl $0x1, %edi
callq 0x1f460
leal 0x1(%r12), %eax
cmpl $0x63, %r12d
jl 0x3007f
movq 0x16d1f(%rip), %rdi     # 0x46f80
movl $0x2e, %esi
callq 0x1f730
movss 0xc(%rsp), %xmm0
mulss 0x5e67(%rip), %xmm0     # 0x360e0
movss %xmm0, 0xc(%rsp)
movl 0x4(%rsp), %r12d
xorl %r15d, %r15d
movss 0x2c(%rsp,%r15,4), %xmm0
cmpl $0x13, 0x4(%rsp)
jne 0x302c0
mulss 0x5e3f(%rip), %xmm0     # 0x360dc
xorl %eax, %eax
ucomiss 0x5e3e(%rip), %xmm0     # 0x360e4
setb %al
cvtss2sd %xmm0, %xmm0
leaq 0x5e5c(%rip), %rcx      # 0x36110
addsd (%rcx,%rax,8), %xmm0
cvttsd2si %xmm0, %rax
jmp 0x302f4
mulss 0x5e10(%rip), %xmm0     # 0x360d8
xorl %eax, %eax
ucomiss 0x5e13(%rip), %xmm0     # 0x360e4
setb %al
cvtss2sd %xmm0, %xmm0
leaq 0x5e31(%rip), %rcx      # 0x36110
addsd (%rcx,%rax,8), %xmm0
cvttsd2si %xmm0, %rax
andl $0xfffe, %eax           # imm = 0xFFFE
orw 0x26(%rsp,%r15,2), %ax
movw %ax, 0x2(%rsp)
movzbl (%rbx), %edi
movzbl %r12b, %r12d
movl %r12d, %esi
movl $0x1, %edx
movq %r14, %rcx
callq 0x1f7d0
incq %r15
addb 0x18(%rsp), %r12b
cmpq $0x3, %r15
jne 0x30287
movss 0x8(%rsp), %xmm0
mulss 0x5db0(%rip), %xmm0     # 0x360e0
movq 0x38(%rsp), %rax
incl %eax
cmpl 0x20(%rsp), %eax
jne 0x30065
movzbl (%rbx), %edi
movl $0x6a, %esi
movl $0x2, %edx
movl $0x1, %ecx
callq 0x1f980
movzbl (%rbx), %edi
movl $0x6a, %esi
movl $0x3, %edx
movl $0x1, %ecx
callq 0x1f980
addq $0x58, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq