// File: 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:
subq $0xa8, %rsp
movb %dl, %al
movb %sil, %cl
movq %rdi, 0xa0(%rsp)
movb %cl, 0x9f(%rsp)
movss %xmm0, 0x98(%rsp)
movss %xmm1, 0x94(%rsp)
movb %al, 0x93(%rsp)
movq 0xa0(%rsp), %rax
movq %rax, 0x48(%rsp)
movzbl 0x9f(%rsp), %eax
cmpl $0x3b, %eax
jne 0x42181
movq 0x48(%rsp), %rdi
callq 0x23310
movzbl %al, %edx
movl $0x77, %eax
movl $0x6, %ecx
cmpl $0x38, %edx
cmovll %ecx, %eax
movl %eax, 0x44(%rsp)
jmp 0x4218c
movl $0x13, %eax
movl %eax, 0x44(%rsp)
jmp 0x4218c
movl 0x44(%rsp), %eax
movb %al, 0x92(%rsp)
movzbl 0x92(%rsp), %edx
movl $0x2, %eax
movl $0x3, %ecx
cmpl $0x77, %edx
cmovel %ecx, %eax
movb %al, 0x85(%rsp)
movq 0x19da3(%rip), %rdi     # 0x5bf60
movl $0x3e, %esi
callq 0x22d60
movl $0x0, 0x64(%rsp)
cmpl $0x3, 0x64(%rsp)
jge 0x422a4
movq 0x48(%rsp), %rax
movzbl (%rax), %edi
movzbl 0x92(%rsp), %eax
movl 0x64(%rsp), %ecx
movzbl 0x85(%rsp), %edx
imull %edx, %ecx
addl %ecx, %eax
movq 0x19d16(%rip), %rcx     # 0x5bf18
movzwl (%rcx), %r8d
movzbl %al, %esi
movl $0x1, %edx
leaq 0x90(%rsp), %rcx
callq 0x224e0
movswl 0x90(%rsp), %eax
cvtsi2ss %eax, %xmm0
movss %xmm0, 0x8c(%rsp)
movzbl 0x92(%rsp), %eax
cmpl $0x13, %eax
je 0x42276
movswl 0x90(%rsp), %eax
andl $0x1, %eax
movw %ax, %cx
movslq 0x64(%rsp), %rax
movw %cx, 0x86(%rsp,%rax,2)
movss 0x9b04(%rip), %xmm0     # 0x4bd64
mulss 0x8c(%rsp), %xmm0
movslq 0x64(%rsp), %rax
movss %xmm0, 0x70(%rsp,%rax,4)
jmp 0x42292
movss 0x9ae2(%rip), %xmm0     # 0x4bd60
mulss 0x8c(%rsp), %xmm0
movslq 0x64(%rsp), %rax
movss %xmm0, 0x70(%rsp,%rax,4)
jmp 0x42294
movl 0x64(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x64(%rsp)
jmp 0x421cf
movl $0x0, 0x60(%rsp)
movl 0x60(%rsp), %eax
movzbl 0x93(%rsp), %ecx
cmpl %ecx, %eax
jge 0x4290b
movw $0x0, 0x6e(%rsp)
movl $0x0, 0x5c(%rsp)
cmpl $0x64, 0x5c(%rsp)
jge 0x426e5
movl $0x0, 0x68(%rsp)
movl $0x0, 0x58(%rsp)
cmpl $0x3, 0x58(%rsp)
jge 0x42617
movq 0x48(%rsp), %rax
movzbl (%rax), %edi
movzbl 0x9f(%rsp), %edx
movl 0x58(%rsp), %eax
movl %eax, %ecx
movl %edx, %eax
leal (%rax,%rcx,2), %eax
movq 0x19c01(%rip), %rcx     # 0x5bf18
movzwl (%rcx), %r8d
movzbl %al, %esi
movl $0x1, %edx
leaq 0x90(%rsp), %rcx
callq 0x224e0
movswl 0x90(%rsp), %eax
cvtsi2ss %eax, %xmm0
movss %xmm0, 0x8c(%rsp)
movzbl 0x9f(%rsp), %eax
cmpl $0x3b, %eax
jne 0x42377
cmpl $0x2, 0x58(%rsp)
jne 0x42377
movss 0x8c(%rsp), %xmm0
movss 0x99fe(%rip), %xmm1     # 0x4bd68
subss %xmm1, %xmm0
movss %xmm0, 0x8c(%rsp)
movss 0x8c(%rsp), %xmm0
movd %xmm0, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm0
movss %xmm0, 0x80(%rsp)
movss 0x8c(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jbe 0x423b8
movss 0x8c(%rsp), %xmm0
movss %xmm0, 0x40(%rsp)
jmp 0x423d4
movss 0x8c(%rsp), %xmm0
movd %xmm0, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm0
movss %xmm0, 0x40(%rsp)
movss 0x40(%rsp), %xmm1
movl 0x68(%rsp), %eax
cvtsi2ss %rax, %xmm0
addss %xmm1, %xmm0
cvttss2si %xmm0, %rax
movl %eax, 0x68(%rsp)
movss 0x98(%rsp), %xmm0
mulss 0x80(%rsp), %xmm0
movss %xmm0, 0x7c(%rsp)
movss 0x80(%rsp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x993b(%rip), %xmm1     # 0x4bd58
mulsd %xmm1, %xmm0
movss 0x94(%rsp), %xmm1
cvtss2sd %xmm1, %xmm2
movslq 0x58(%rsp), %rax
movss 0x70(%rsp,%rax,4), %xmm1
cvtss2sd %xmm1, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x70(%rsp,%rax,4)
movzbl 0x92(%rsp), %eax
cmpl $0x13, %eax
je 0x4252b
movss 0x7c(%rsp), %xmm0
movslq 0x58(%rsp), %rax
addss 0x70(%rsp,%rax,4), %xmm0
movss 0x98eb(%rip), %xmm1     # 0x4bd64
divss %xmm1, %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jb 0x424be
movss 0x7c(%rsp), %xmm0
movslq 0x58(%rsp), %rax
addss 0x70(%rsp,%rax,4), %xmm0
movss 0x98c6(%rip), %xmm1     # 0x4bd64
divss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x98a2(%rip), %xmm1     # 0x4bd50
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %rax
movq %rax, 0x38(%rsp)
jmp 0x424f5
movss 0x7c(%rsp), %xmm0
movslq 0x58(%rsp), %rax
addss 0x70(%rsp,%rax,4), %xmm0
movss 0x988d(%rip), %xmm1     # 0x4bd64
divss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x9869(%rip), %xmm1     # 0x4bd50
subsd %xmm1, %xmm0
cvttsd2si %xmm0, %rax
movq %rax, 0x38(%rsp)
movq 0x38(%rsp), %rax
movw %ax, 0x90(%rsp)
movswl 0x90(%rsp), %eax
andl $0xfffe, %eax           # imm = 0xFFFE
movslq 0x58(%rsp), %rcx
movswl 0x86(%rsp,%rcx,2), %ecx
orl %ecx, %eax
movw %ax, 0x90(%rsp)
jmp 0x425cd
movss 0x7c(%rsp), %xmm0
movslq 0x58(%rsp), %rax
addss 0x70(%rsp,%rax,4), %xmm0
movss 0x981c(%rip), %xmm1     # 0x4bd60
divss %xmm1, %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jb 0x42589
movss 0x7c(%rsp), %xmm0
movslq 0x58(%rsp), %rax
addss 0x70(%rsp,%rax,4), %xmm0
movss 0x97f7(%rip), %xmm1     # 0x4bd60
divss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x97d7(%rip), %xmm1     # 0x4bd50
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %rax
movq %rax, 0x30(%rsp)
jmp 0x425c0
movss 0x7c(%rsp), %xmm0
movslq 0x58(%rsp), %rax
addss 0x70(%rsp,%rax,4), %xmm0
movss 0x97be(%rip), %xmm1     # 0x4bd60
divss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x979e(%rip), %xmm1     # 0x4bd50
subsd %xmm1, %xmm0
cvttsd2si %xmm0, %rax
movq %rax, 0x30(%rsp)
movq 0x30(%rsp), %rax
movw %ax, 0x90(%rsp)
movq 0x48(%rsp), %rax
movb (%rax), %sil
movzbl 0x92(%rsp), %eax
movl 0x58(%rsp), %ecx
movzbl 0x85(%rsp), %edx
imull %edx, %ecx
addl %ecx, %eax
movl $0x1, %edx
leaq 0x90(%rsp), %rcx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x22e60
movl 0x58(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x58(%rsp)
jmp 0x422ea
cmpl $0x63, 0x5c(%rsp)
jne 0x42641
cmpl $0x3e8, 0x68(%rsp)      # imm = 0x3E8
jbe 0x42641
movl $0x0, 0x5c(%rsp)
movq 0x19929(%rip), %rdi     # 0x5bf60
movl $0x2a, %esi
callq 0x22d60
movl 0x68(%rsp), %eax
cvtsi2sd %rax, %xmm0
movsd %xmm0, 0x18(%rsp)
movzbl 0x9f(%rsp), %eax
movsd 0x96e0(%rip), %xmm0     # 0x4bd40
movsd 0x9608(%rip), %xmm1     # 0x4bc70
movsd %xmm1, 0x20(%rsp)
cmpl $0x3b, %eax
movsd %xmm0, 0x28(%rsp)
je 0x42685
movsd 0x20(%rsp), %xmm0
movsd %xmm0, 0x28(%rsp)
movsd 0x18(%rsp), %xmm1
movsd 0x28(%rsp), %xmm0
mulsd %xmm0, %xmm1
movsd 0x969b(%rip), %xmm0     # 0x4bd38
ucomisd %xmm1, %xmm0
jbe 0x426b1
movw 0x6e(%rsp), %ax
addw $0x1, %ax
movw %ax, 0x6e(%rsp)
cmpl $0x64, 0x68(%rsp)
jae 0x426cb
cmpl $0xa, 0x5c(%rsp)
jle 0x426cb
movswl 0x6e(%rsp), %eax
cmpl $0xa, %eax
jl 0x426cb
jmp 0x426e5
movl $0x1, %edi
callq 0x228c0
movl 0x5c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x5c(%rsp)
jmp 0x422cf
movq 0x19874(%rip), %rdi     # 0x5bf60
movl $0x2e, %esi
callq 0x22d60
movss 0x98(%rsp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x963d(%rip), %xmm1     # 0x4bd48
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x98(%rsp)
movss 0x94(%rsp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x9617(%rip), %xmm1     # 0x4bd48
mulsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x94(%rsp)
movl $0x0, 0x54(%rsp)
cmpl $0x3, 0x54(%rsp)
jge 0x428f9
movzbl 0x92(%rsp), %eax
cmpl $0x13, %eax
je 0x4281f
movslq 0x54(%rsp), %rax
movss 0x70(%rsp,%rax,4), %xmm0
movss 0x95eb(%rip), %xmm1     # 0x4bd64
divss %xmm1, %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jb 0x427b8
movslq 0x54(%rsp), %rax
movss 0x70(%rsp,%rax,4), %xmm0
movss 0x95cc(%rip), %xmm1     # 0x4bd64
divss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x95a8(%rip), %xmm1     # 0x4bd50
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %rax
movq %rax, 0x10(%rsp)
jmp 0x427e9
movslq 0x54(%rsp), %rax
movss 0x70(%rsp,%rax,4), %xmm0
movss 0x9599(%rip), %xmm1     # 0x4bd64
divss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x9575(%rip), %xmm1     # 0x4bd50
subsd %xmm1, %xmm0
cvttsd2si %xmm0, %rax
movq %rax, 0x10(%rsp)
movq 0x10(%rsp), %rax
movw %ax, 0x90(%rsp)
movswl 0x90(%rsp), %eax
andl $0xfffe, %eax           # imm = 0xFFFE
movslq 0x54(%rsp), %rcx
movswl 0x86(%rsp,%rcx,2), %ecx
orl %ecx, %eax
movw %ax, 0x90(%rsp)
jmp 0x428af
movslq 0x54(%rsp), %rax
movss 0x70(%rsp,%rax,4), %xmm0
movss 0x952e(%rip), %xmm1     # 0x4bd60
divss %xmm1, %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jb 0x42871
movslq 0x54(%rsp), %rax
movss 0x70(%rsp,%rax,4), %xmm0
movss 0x950f(%rip), %xmm1     # 0x4bd60
divss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x94ef(%rip), %xmm1     # 0x4bd50
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %rax
movq %rax, 0x8(%rsp)
jmp 0x428a2
movslq 0x54(%rsp), %rax
movss 0x70(%rsp,%rax,4), %xmm0
movss 0x94dc(%rip), %xmm1     # 0x4bd60
divss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0x94bc(%rip), %xmm1     # 0x4bd50
subsd %xmm1, %xmm0
cvttsd2si %xmm0, %rax
movq %rax, 0x8(%rsp)
movq 0x8(%rsp), %rax
movw %ax, 0x90(%rsp)
movq 0x48(%rsp), %rax
movb (%rax), %sil
movzbl 0x92(%rsp), %eax
movl 0x54(%rsp), %ecx
movzbl 0x85(%rsp), %edx
imull %edx, %ecx
addl %ecx, %eax
movl $0x1, %edx
leaq 0x90(%rsp), %rcx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x22e60
movl 0x54(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x54(%rsp)
jmp 0x4274a
jmp 0x428fb
movl 0x60(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x60(%rsp)
jmp 0x422ac
movq 0x48(%rsp), %rdi
callq 0x23250
movq 0x48(%rsp), %rdi
callq 0x222e0
addq $0xa8, %rsp
retq
nopw (%rax,%rax)