// File: /workspace/github_cpp_makefile_star2_O1/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, %r13d
movl %esi, 0x14(%rsp)
cmpl $0x3b, %esi
jne 0x30044
movzbl (%rbx), %edi
leaq 0x1(%rbx), %r8
movq 0x16f2f(%rip), %rax     # 0x46f48
movzwl (%rax), %r9d
movl $0x6, %ebp
movl $0x75, %esi
movl $0x6, %edx
movl $0x6, %ecx
callq 0x1f930
cmpb $0x38, 0x1(%rbx)
movl $0x77, %r13d
cmovbl %ebp, %r13d
cmpl $0x77, %r13d
sete %al
orb $0x2, %al
movb %al, 0x7(%rsp)
movq 0x16f28(%rip), %rdi     # 0x46f80
movl $0x3e, %esi
callq 0x1f730
xorl %r15d, %r15d
leaq 0x4(%rsp), %r12
movl %r13d, %ebp
movzbl (%rbx), %edi
movq 0x16ed1(%rip), %rax     # 0x46f48
movzwl (%rax), %r8d
movzbl %bpl, %esi
movl $0x1, %edx
movq %r12, %rcx
callq 0x1f230
movswl 0x4(%rsp), %eax
movss 0x6033(%rip), %xmm0     # 0x360cc
cmpl $0x13, %r13d
je 0x300b2
movl %eax, %ecx
andl $0x1, %ecx
movw %cx, 0x1e(%rsp,%r15,2)
movss 0x601e(%rip), %xmm0     # 0x360d0
xorps %xmm1, %xmm1
cvtsi2ss %eax, %xmm1
mulss %xmm0, %xmm1
movss %xmm1, 0x24(%rsp,%r15,4)
incq %r15
addb 0x7(%rsp), %bpl
cmpq $0x3, %r15
jne 0x3006d
testb %r14b, %r14b
movss 0x8(%rsp), %xmm0
je 0x303fe
movzbl %r14b, %eax
movl %eax, 0x18(%rsp)
xorl %eax, %eax
movl 0x14(%rsp), %edx
cmpb $0x3b, %dl
sete %al
leaq 0x5ff4(%rip), %rcx      # 0x360f0
movsd (%rcx,%rax,8), %xmm1
movsd %xmm1, 0x38(%rsp)
movzbl %dl, %eax
movq %rax, 0x48(%rsp)
xorl %eax, %eax
leaq 0x4(%rsp), %r14
movq %rax, 0x30(%rsp)
movl %r13d, %ebp
movss %xmm0, 0x8(%rsp)
cvtss2sd %xmm0, %xmm0
movsd %xmm0, 0x50(%rsp)
xorl %eax, %eax
xorl %r12d, %r12d
movq %rax, 0x40(%rsp)
movl %r12d, 0x10(%rsp)
xorl %r13d, %r13d
movl %ebp, %r12d
xorl %r15d, %r15d
movzbl (%rbx), %edi
movq 0x48(%rsp), %rax
addl %r13d, %eax
movq 0x16df0(%rip), %rcx     # 0x46f48
movzwl (%rcx), %r8d
movzbl %al, %esi
movl $0x1, %edx
movq %r14, %rcx
callq 0x1f230
movswl 0x4(%rsp), %eax
xorps %xmm1, %xmm1
cvtsi2ss %eax, %xmm1
movaps %xmm1, %xmm0
addss 0x5f51(%rip), %xmm0     # 0x360d4
cmpq $0x4, %r13
je 0x3018c
movaps %xmm1, %xmm0
cmpb $0x3b, 0x14(%rsp)
je 0x30196
movaps %xmm1, %xmm0
xorps %xmm1, %xmm1
cvtss2sd %xmm0, %xmm1
mulsd 0x5f13(%rip), %xmm1     # 0x360b8
mulsd 0x50(%rsp), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x24(%rsp,%r13,2), %xmm2
addsd %xmm1, %xmm2
xorps %xmm1, %xmm1
cvtsd2ss %xmm2, %xmm1
movss %xmm1, 0x24(%rsp,%r13,2)
movaps %xmm0, %xmm2
mulss 0xc(%rsp), %xmm2
subss %xmm2, %xmm1
cmpl $0x13, %ebp
jne 0x30204
mulss 0x5efb(%rip), %xmm1     # 0x360dc
xorl %eax, %eax
ucomiss 0x5efa(%rip), %xmm1     # 0x360e4
setb %al
cvtss2sd %xmm1, %xmm1
leaq 0x5f18(%rip), %rcx      # 0x36110
addsd (%rcx,%rax,8), %xmm1
cvttsd2si %xmm1, %rax
jmp 0x3023d
mulss 0x5ecc(%rip), %xmm1     # 0x360d8
xorl %eax, %eax
ucomiss 0x5ecf(%rip), %xmm1     # 0x360e4
setb %al
cvtss2sd %xmm1, %xmm1
leaq 0x5eed(%rip), %rcx      # 0x36110
addsd (%rcx,%rax,8), %xmm1
cvttsd2si %xmm1, %rax
movw %ax, 0x4(%rsp)
andl $0xfffe, %eax           # imm = 0xFFFE
orw 0x1e(%rsp,%r13), %ax
movaps %xmm0, %xmm1
xorps 0x5eb9(%rip), %xmm1     # 0x36100
maxss %xmm1, %xmm0
movl %r15d, %ecx
xorps %xmm1, %xmm1
cvtsi2ss %rcx, %xmm1
addss %xmm0, %xmm1
cvttss2si %xmm1, %r15
movw %ax, 0x4(%rsp)
movzbl (%rbx), %edi
movzbl %r12b, %r12d
movl %r12d, %esi
movl $0x1, %edx
movq %r14, %rcx
callq 0x1f7d0
addq $0x2, %r13
addb 0x7(%rsp), %r12b
cmpq $0x6, %r13
jne 0x30146
movq 0x40(%rsp), %r13
cmpl $0x63, %r13d
jne 0x302bd
cmpl $0x3e8, %r15d           # imm = 0x3E8
movl 0x10(%rsp), %r12d
jbe 0x302c2
movq 0x16cd2(%rip), %rdi     # 0x46f80
movl $0x2a, %esi
callq 0x1f730
xorl %r13d, %r13d
jmp 0x302c2
movl 0x10(%rsp), %r12d
movl %r15d, %eax
xorps %xmm0, %xmm0
cvtsi2sd %rax, %xmm0
mulsd 0x38(%rsp), %xmm0
xorl %eax, %eax
movsd 0x5de3(%rip), %xmm1     # 0x360c0
ucomisd %xmm0, %xmm1
seta %al
addl %eax, %r12d
cmpl $0x63, %r15d
ja 0x302fa
cmpl $0xb, %r13d
jl 0x302fa
cmpw $0x9, %r12w
jg 0x30312
movl $0x1, %edi
callq 0x1f460
leal 0x1(%r13), %eax
cmpl $0x63, %r13d
jl 0x30133
movq 0x16c67(%rip), %rdi     # 0x46f80
movl $0x2e, %esi
callq 0x1f730
movss 0xc(%rsp), %xmm0
mulss 0x5daf(%rip), %xmm0     # 0x360e0
movss %xmm0, 0xc(%rsp)
movl %ebp, %r13d
movl %r13d, %r12d
xorl %r15d, %r15d
movss 0x24(%rsp,%r15,4), %xmm0
cmpl $0x13, %r13d
jne 0x30378
mulss 0x5d87(%rip), %xmm0     # 0x360dc
xorl %eax, %eax
ucomiss 0x5d86(%rip), %xmm0     # 0x360e4
setb %al
cvtss2sd %xmm0, %xmm0
leaq 0x5da4(%rip), %rcx      # 0x36110
addsd (%rcx,%rax,8), %xmm0
cvttsd2si %xmm0, %rax
jmp 0x303b1
mulss 0x5d58(%rip), %xmm0     # 0x360d8
xorl %eax, %eax
ucomiss 0x5d5b(%rip), %xmm0     # 0x360e4
setb %al
cvtss2sd %xmm0, %xmm0
leaq 0x5d79(%rip), %rcx      # 0x36110
addsd (%rcx,%rax,8), %xmm0
cvttsd2si %xmm0, %rax
movw %ax, 0x4(%rsp)
andl $0xfffe, %eax           # imm = 0xFFFE
orw 0x1e(%rsp,%r15,2), %ax
movw %ax, 0x4(%rsp)
movzbl (%rbx), %edi
movzbl %r12b, %r12d
movl %r12d, %esi
movl $0x1, %edx
movq %r14, %rcx
callq 0x1f7d0
incq %r15
addb 0x7(%rsp), %r12b
cmpq $0x3, %r15
jne 0x30340
movss 0x8(%rsp), %xmm0
mulss 0x5cf3(%rip), %xmm0     # 0x360e0
movq 0x30(%rsp), %rax
incl %eax
cmpl 0x18(%rsp), %eax
jne 0x30116
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
nop