// File: /workspace/github_cpp_makefile_star2_O2/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 $0x68, %rsp
movl %edx, %r13d
movss %xmm1, 0x14(%rsp)
movss %xmm0, 0x18(%rsp)
movq %rdi, %rbx
movl %esi, 0x1c(%rsp)
cmpl $0x3b, %esi
jne 0x2e3f1
movq %rbx, %rdi
callq 0x1fec0
cmpb $0x37, %al
ja 0x2e402
pushq $0x6
jmp 0x2e3f3
pushq $0x13
popq %rax
movl %eax, 0x10(%rsp)
pushq $0x2
popq %rax
movq %rax, 0x20(%rsp)
jmp 0x2e411
pushq $0x3
popq %rax
movq %rax, 0x20(%rsp)
pushq $0x77
popq %rax
movl %eax, 0x10(%rsp)
movq 0x1ab68(%rip), %rdi     # 0x48f80
pushq $0x3e
popq %rsi
callq 0x1fa20
xorl %r14d, %r14d
pushq $0x1
popq %r15
leaq 0xe(%rsp), %r12
movl 0x10(%rsp), %eax
movl %eax, %ebp
cmpq $0x3, %r14
je 0x2e495
movzbl (%rbx), %edi
movq 0x1ab06(%rip), %rax     # 0x48f48
movzwl (%rax), %r8d
movzbl %bpl, %esi
movl %r15d, %edx
movq %r12, %rcx
callq 0x1f320
movswl 0xe(%rsp), %eax
movss 0x5c6a(%rip), %xmm0     # 0x340cc
cmpl $0x13, 0x10(%rsp)
je 0x2e47c
movl %eax, %ecx
andl $0x1, %ecx
movw %cx, 0x36(%rsp,%r14,2)
movss 0x5c54(%rip), %xmm0     # 0x340d0
cvtsi2ss %eax, %xmm1
mulss %xmm0, %xmm1
movss %xmm1, 0x3c(%rsp,%r14,4)
incq %r14
addb 0x20(%rsp), %bpl
jmp 0x2e432
xorl %eax, %eax
movl 0x1c(%rsp), %edx
cmpb $0x3b, %dl
sete %al
movzbl %r13b, %ecx
movl %ecx, 0x28(%rsp)
leaq 0x5c40(%rip), %rcx      # 0x340f0
movsd (%rcx,%rax,8), %xmm0
movsd %xmm0, 0x48(%rsp)
movzbl %dl, %eax
movq %rax, 0x58(%rsp)
xorl %r13d, %r13d
pushq $0x1
popq %r12
movss 0x14(%rsp), %xmm0
cmpl 0x28(%rsp), %r13d
je 0x2e79e
movss %xmm0, 0x14(%rsp)
cvtss2sd %xmm0, %xmm0
movsd %xmm0, 0x60(%rsp)
xorl %ebp, %ebp
xorl %r14d, %r14d
movq %r13, 0x50(%rsp)
cmpl $0x63, %r14d
jg 0x2e6bd
movl %r14d, 0x2c(%rsp)
movl %ebp, 0x30(%rsp)
xorl %r14d, %r14d
movl 0x10(%rsp), %r15d
xorl %r13d, %r13d
movl %r13d, %ebp
cmpq $0x6, %r14
je 0x2e64e
movzbl (%rbx), %edi
movq 0x58(%rsp), %rax
addl %r14d, %eax
movq 0x1aa16(%rip), %rcx     # 0x48f48
movzwl (%rcx), %r8d
movzbl %al, %esi
movl %r12d, %edx
leaq 0xe(%rsp), %rcx
callq 0x1f320
movswl 0xe(%rsp), %eax
cvtsi2ss %eax, %xmm1
movaps %xmm1, %xmm0
addss 0x5b7a(%rip), %xmm0     # 0x340d4
cmpq $0x4, %r14
je 0x2e563
movaps %xmm1, %xmm0
cmpb $0x3b, 0x1c(%rsp)
je 0x2e56d
movaps %xmm1, %xmm0
cvtss2sd %xmm0, %xmm1
mulsd 0x5b3f(%rip), %xmm1     # 0x340b8
mulsd 0x60(%rsp), %xmm1
cvtss2sd 0x3c(%rsp,%r14,2), %xmm2
addsd %xmm1, %xmm2
cvtsd2ss %xmm2, %xmm1
movss %xmm1, 0x3c(%rsp,%r14,2)
movaps %xmm0, %xmm2
mulss 0x18(%rsp), %xmm2
subss %xmm2, %xmm1
cmpl $0x13, 0x10(%rsp)
jne 0x2e5d4
mulss 0x5b2b(%rip), %xmm1     # 0x340dc
xorl %eax, %eax
ucomiss 0x5b2a(%rip), %xmm1     # 0x340e4
setb %al
cvtss2sd %xmm1, %xmm1
leaq 0x5b48(%rip), %rcx      # 0x34110
addsd (%rcx,%rax,8), %xmm1
cvttsd2si %xmm1, %rax
jmp 0x2e608
mulss 0x5afc(%rip), %xmm1     # 0x340d8
xorl %eax, %eax
ucomiss 0x5aff(%rip), %xmm1     # 0x340e4
setb %al
cvtss2sd %xmm1, %xmm1
leaq 0x5b1d(%rip), %rcx      # 0x34110
addsd (%rcx,%rax,8), %xmm1
cvttsd2si %xmm1, %rax
andl $0xfffe, %eax           # imm = 0xFFFE
orw 0x36(%rsp,%r14), %ax
movaps %xmm0, %xmm1
xorps 0x5aee(%rip), %xmm1     # 0x34100
maxss %xmm1, %xmm0
cvtsi2ss %rbp, %xmm1
addss %xmm0, %xmm1
cvttss2si %xmm1, %r13
movw %ax, 0xe(%rsp)
movzbl (%rbx), %edi
movzbl %r15b, %r15d
movl %r15d, %esi
movl %r12d, %edx
leaq 0xe(%rsp), %rcx
callq 0x1fad0
addq $0x2, %r14
addb 0x20(%rsp), %r15b
jmp 0x2e513
movl 0x2c(%rsp), %r14d
cmpl $0x63, %r14d
jne 0x2e674
cmpl $0x3e8, %r13d           # imm = 0x3E8
jbe 0x2e674
movq 0x1a917(%rip), %rdi     # 0x48f80
pushq $0x2a
popq %rsi
callq 0x1fa20
xorl %r14d, %r14d
cvtsi2sd %rbp, %xmm0
mulsd 0x48(%rsp), %xmm0
xorl %eax, %eax
movsd 0x5a37(%rip), %xmm1     # 0x340c0
ucomisd %xmm0, %xmm1
seta %al
movl 0x30(%rsp), %ebp
addl %eax, %ebp
cmpl $0x63, %r13d
movq 0x50(%rsp), %r13
ja 0x2e6ad
cmpl $0xb, %r14d
jl 0x2e6ad
cmpw $0x9, %bp
jg 0x2e6bd
movl %r12d, %edi
callq 0x1f630
incl %r14d
jmp 0x2e4f5
movq 0x1a8bc(%rip), %rdi     # 0x48f80
pushq $0x2e
popq %rsi
callq 0x1fa20
movss 0x18(%rsp), %xmm0
mulss 0x5a06(%rip), %xmm0     # 0x340e0
movss %xmm0, 0x18(%rsp)
movl 0x10(%rsp), %r15d
xorl %r14d, %r14d
cmpq $0x3, %r14
je 0x2e788
movss 0x3c(%rsp,%r14,4), %xmm0
cmpl $0x13, 0x10(%rsp)
jne 0x2e72b
mulss 0x59d4(%rip), %xmm0     # 0x340dc
xorl %eax, %eax
ucomiss 0x59d3(%rip), %xmm0     # 0x340e4
setb %al
cvtss2sd %xmm0, %xmm0
leaq 0x59f1(%rip), %rcx      # 0x34110
addsd (%rcx,%rax,8), %xmm0
cvttsd2si %xmm0, %rax
jmp 0x2e75f
mulss 0x59a5(%rip), %xmm0     # 0x340d8
xorl %eax, %eax
ucomiss 0x59a8(%rip), %xmm0     # 0x340e4
setb %al
cvtss2sd %xmm0, %xmm0
leaq 0x59c6(%rip), %rcx      # 0x34110
addsd (%rcx,%rax,8), %xmm0
cvttsd2si %xmm0, %rax
andl $0xfffe, %eax           # imm = 0xFFFE
orw 0x36(%rsp,%r14,2), %ax
movw %ax, 0xe(%rsp)
movzbl (%rbx), %edi
movzbl %r15b, %r15d
movl %r15d, %esi
movl %r12d, %edx
leaq 0xe(%rsp), %rcx
callq 0x1fad0
incq %r14
addb 0x20(%rsp), %r15b
jmp 0x2e6e8
movss 0x14(%rsp), %xmm0
mulss 0x594a(%rip), %xmm0     # 0x340e0
incl %r13d
jmp 0x2e4d0
movq %rbx, %rdi
callq 0x1fe20
movq %rbx, %rdi
callq 0x1f220
addq $0x68, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop