// File: skyetheguy[P]gait_cycle_phase_detection/gaitCycle.cpp
// Function: gaitCycle::processReadings(sensorData)
int gaitCycle::processReadings(sensorData currentData)
{
	float xAccelFilter = xAccelSignal.lowPassFilter(currentData.getXAccel(), 0.2);
	float yAccelFilter = yAccelSignal.lowPassFilter(currentData.getYAccel(), 0.9);
	float zAccelFilter = zAccelSignal.lowPassFilter(currentData.getZAccel(), 0.2);

	float xGyroFilter = xGyroSignal.lowPassFilter(currentData.getXGyro(), 0.2);
	float yGyroFilter = yGyroSignal.lowPassFilter(currentData.getYGyro(), 0.2);
	float zGyroFilter = zGyroSignal.lowPassFilter(currentData.getZGyro(), 0.2);

	cout << currentData.getTime() << "," << xAccelFilter << "," << yAccelFilter << "," << zAccelFilter << "," << xGyroFilter << "," << yGyroFilter << "," << zGyroFilter << endl;

	xAccelSignal.updateState(currentData.getTime(), xAccelFilter);
	yAccelSignal.updateState(currentData.getTime(), yAccelFilter);
	zAccelSignal.updateState(currentData.getTime(), zAccelFilter);

	xGyroSignal.updateState(currentData.getTime(), xGyroFilter);
	yGyroSignal.updateState(currentData.getTime(), yGyroFilter);
	zGyroSignal.updateState(currentData.getTime(), zGyroFilter);

	// ***************************
	// CHECK FOR MID SWING
	// ***************************
	if (lastState == TOE_OFF && (currentData.getTime() - lastStateChangeTime) > 20 )
	{
		cout << "\n=======================\n" << " ** MID SWING ** @time = " << currentData.getTime() << "\n=======================\n\n" ;
		lastState = MID_SWING;
		lastStateChangeTime = currentData.getTime();
		return MID_SWING;
	}


	// ***************************
	// CHECK FOR HEEL STRIKE or TOE-OFF/TAKE OFF USING SPIKES IN Z-ACCEL
	// ***************************
	if (zAccelSignal.changedDir())
	{
		if (zAccelSignal.getDir() == DECREASING) // IF DIRECTION CHANGED TO DECREASING WE HAVE A MAX POINT
		{
			// HANDLE MAX POINT
			//cout << "MAX POINT @time " << zAccelSignal.getLastTime() << " value " << zAccelSignal.getLastReading() << endl;

			if (isToeOff(zAccelSignal.getLastReading(), currentData.getTime()))
			{
				cout << "\n=======================\n" << " ** TOE OFF ** @time = " << currentData.getTime() << "\n=======================\n\n" ;
				lastState = TOE_OFF;
				lastStateChangeTime = currentData.getTime();
				return TOE_OFF;
			}
		}
		else // IF DIRECTION CHANGED TO INCREASING WE HAVE A MIN POINT
		{
			// HANDLE MIN POINT
			//cout << "MIN POINT @time " << zAccelSignal.getLastTime() << " value " << zAccelSignal.getLastReading() << endl;


			if (isHeelStrike(zAccelSignal.getLastReading(), currentData.getTime()))
			{
				cout << "\n=======================\n" << " ** HEEL STRIKE ** @time = " << currentData.getTime() << "\n=======================\n\n" ;
				lastState = HEEL_STRIKE;
				lastStateChangeTime = currentData.getTime();
				return HEEL_STRIKE;
			}
		}
	}

	// ***************************
	// CHECK FOR FOOT FLAT
	// ***************************
	if (yGyroSignal.changedDir() && yGyroSignal.getDir() == INCREASING && yGyroFilter < 4000 && yGyroFilter > -1000 && lastState == HEEL_STRIKE)
	{
		cout << "\n=======================\n" << " ** FOOT FLAT ** @time = " << currentData.getTime() << "\n=======================\n\n" ;
		lastState = FOOT_FLAT;
		lastStateChangeTime = currentData.getTime();
		return FOOT_FLAT;
	}

	// ***************************
	// CHECK FOR HEEL OFF
	// ***************************
	if (yGyroSignal.getDir() == INCREASING && yGyroFilter > 2500 && lastState == FOOT_FLAT)
	{
		cout << "\n=======================\n" << " ** HEEL OFF ** @time = " << currentData.getTime() << "\n=======================\n\n" ;
		lastState = HEEL_OFF;
		lastStateChangeTime = currentData.getTime();
		return HEEL_OFF;
	}



	// ***************************
	// CHECK FOR NOT MOVING
	// ***************************
	if (xGyroSignal.isFlat() && yGyroSignal.isFlat() && zGyroSignal.isFlat() && lastState != NOT_MOVING)
	{
		cout << "\n=======================\n" << " ** NOT MOVING ** @time = " << currentData.getTime() << "\n=======================\n\n" ;
		lastState = NOT_MOVING;
		lastStateChangeTime = currentData.getTime();
		return NOT_MOVING;
	}
	


	
	// NOT WORKING PROPERLY YET
	/*else 
	{
		if (!zGyroSignal.isFlat() && !xGyroSignal.isFlat() && lastState == 10)
		{
			cout << "\n=======================\n" << " ** STARTED MOVING ** " << currentData.getTime() << "\n=======================\n\n" ;
			lastState = 11;
			return 11;
		}
	}*/


	return 0;


}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0xd0, %rsp
movq %rdi, %rax
leaq 0x10(%rbp), %rdi
movq %rdi, -0x40(%rbp)
movq %rax, -0x10(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0x30(%rbp)
callq 0x5e90
movq -0x30(%rbp), %rdi
movl %eax, %esi
movss 0x236b(%rip), %xmm0     # 0x6040
callq 0x4a10
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movss %xmm0, -0x14(%rbp)
addq $0x68, %rax
movq %rax, -0x88(%rbp)
callq 0x5eb0
movq -0x88(%rbp), %rdi
movl %eax, %esi
movss 0x233c(%rip), %xmm0     # 0x6044
callq 0x4a10
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movss %xmm0, -0x18(%rbp)
addq $0xd0, %rax
movq %rax, -0x80(%rbp)
callq 0x5ed0
movq -0x80(%rbp), %rdi
movl %eax, %esi
movss 0x2309(%rip), %xmm0     # 0x6040
callq 0x4a10
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movss %xmm0, -0x1c(%rbp)
addq $0x138, %rax            # imm = 0x138
movq %rax, -0x78(%rbp)
callq 0x5ef0
movq -0x78(%rbp), %rdi
movl %eax, %esi
movss 0x22da(%rip), %xmm0     # 0x6040
callq 0x4a10
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movss %xmm0, -0x20(%rbp)
addq $0x1a0, %rax            # imm = 0x1A0
movq %rax, -0x70(%rbp)
callq 0x5f10
movq -0x70(%rbp), %rdi
movl %eax, %esi
movss 0x22ab(%rip), %xmm0     # 0x6040
callq 0x4a10
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movss %xmm0, -0x24(%rbp)
addq $0x208, %rax            # imm = 0x208
movq %rax, -0x68(%rbp)
callq 0x5f30
movq -0x68(%rbp), %rdi
movl %eax, %esi
movss 0x227c(%rip), %xmm0     # 0x6040
callq 0x4a10
movq -0x40(%rbp), %rdi
movss %xmm0, -0x28(%rbp)
callq 0x5e80
movl %eax, %esi
movq 0x61f0(%rip), %rdi      # 0x9fd0
callq 0x2200
movq %rax, %rdi
leaq 0x226d(%rip), %rsi      # 0x605c
callq 0x2150
movq %rax, %rdi
movss -0x14(%rbp), %xmm0
callq 0x2110
movq %rax, %rdi
leaq 0x2251(%rip), %rsi      # 0x605c
callq 0x2150
movq %rax, %rdi
movss -0x18(%rbp), %xmm0
callq 0x2110
movq %rax, %rdi
leaq 0x2235(%rip), %rsi      # 0x605c
callq 0x2150
movq %rax, %rdi
movss -0x1c(%rbp), %xmm0
callq 0x2110
movq %rax, %rdi
leaq 0x2219(%rip), %rsi      # 0x605c
callq 0x2150
movq %rax, %rdi
movss -0x20(%rbp), %xmm0
callq 0x2110
movq %rax, %rdi
leaq 0x21fd(%rip), %rsi      # 0x605c
callq 0x2150
movq %rax, %rdi
movss -0x24(%rbp), %xmm0
callq 0x2110
movq %rax, %rdi
leaq 0x21e1(%rip), %rsi      # 0x605c
callq 0x2150
movq %rax, %rdi
movss -0x28(%rbp), %xmm0
callq 0x2110
movq %rax, %rdi
movq 0x6129(%rip), %rsi      # 0x9fc0
callq 0x2170
movq -0x40(%rbp), %rdi
callq 0x5e80
movq -0x30(%rbp), %rdi
movl %eax, %esi
movss -0x14(%rbp), %xmm0
callq 0x4a70
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
addq $0x68, %rax
movq %rax, -0x60(%rbp)
callq 0x5e80
movq -0x60(%rbp), %rdi
movl %eax, %esi
movss -0x18(%rbp), %xmm0
callq 0x4a70
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
addq $0xd0, %rax
movq %rax, -0x58(%rbp)
callq 0x5e80
movq -0x58(%rbp), %rdi
movl %eax, %esi
movss -0x1c(%rbp), %xmm0
callq 0x4a70
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
addq $0x138, %rax            # imm = 0x138
movq %rax, -0x50(%rbp)
callq 0x5e80
movq -0x50(%rbp), %rdi
movl %eax, %esi
movss -0x20(%rbp), %xmm0
callq 0x4a70
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
addq $0x1a0, %rax            # imm = 0x1A0
movq %rax, -0x48(%rbp)
callq 0x5e80
movq -0x48(%rbp), %rdi
movl %eax, %esi
movss -0x24(%rbp), %xmm0
callq 0x4a70
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
addq $0x208, %rax            # imm = 0x208
movq %rax, -0x38(%rbp)
callq 0x5e80
movq -0x38(%rbp), %rdi
movl %eax, %esi
movss -0x28(%rbp), %xmm0
callq 0x4a70
movq -0x30(%rbp), %rax
cmpl $0x2, 0x270(%rax)
jne 0x401d
movq -0x40(%rbp), %rdi
callq 0x5e80
movq -0x30(%rbp), %rcx
subl 0x274(%rcx), %eax
cmpl $0x14, %eax
jle 0x401d
movq 0x602a(%rip), %rdi      # 0x9fd0
leaq 0x20b1(%rip), %rsi      # 0x605e
callq 0x2150
movq %rax, %rdi
leaq 0x20bc(%rip), %rsi      # 0x6078
callq 0x2150
movq -0x40(%rbp), %rdi
movq %rax, -0x90(%rbp)
callq 0x5e80
movq -0x90(%rbp), %rdi
movl %eax, %esi
callq 0x2200
movq %rax, %rdi
leaq 0x20a9(%rip), %rsi      # 0x6092
callq 0x2150
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movl $0x5, 0x270(%rax)
callq 0x5e80
movl %eax, %ecx
movq -0x30(%rbp), %rax
movl %ecx, 0x274(%rax)
movl $0x5, -0x4(%rbp)
jmp 0x4441
movq -0x30(%rbp), %rdi
addq $0xd0, %rdi
callq 0x4b10
testb $0x1, %al
jne 0x4036
jmp 0x41c8
movq -0x30(%rbp), %rdi
addq $0xd0, %rdi
callq 0x4b40
cmpl $0x1, %eax
jne 0x410c
movq -0x30(%rbp), %rdi
addq $0xd0, %rdi
callq 0x4b60
movq -0x40(%rbp), %rdi
movss %xmm0, -0x94(%rbp)
callq 0x5e80
movq -0x30(%rbp), %rdi
movss -0x94(%rbp), %xmm0
movl %eax, %esi
callq 0x4450
testb $0x1, %al
jne 0x4089
jmp 0x4107
movq 0x5f40(%rip), %rdi      # 0x9fd0
leaq 0x1fc7(%rip), %rsi      # 0x605e
callq 0x2150
movq %rax, %rdi
leaq 0x2007(%rip), %rsi      # 0x60ad
callq 0x2150
movq -0x40(%rbp), %rdi
movq %rax, -0xa0(%rbp)
callq 0x5e80
movq -0xa0(%rbp), %rdi
movl %eax, %esi
callq 0x2200
movq %rax, %rdi
leaq 0x1fbf(%rip), %rsi      # 0x6092
callq 0x2150
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movl $0x2, 0x270(%rax)
callq 0x5e80
movl %eax, %ecx
movq -0x30(%rbp), %rax
movl %ecx, 0x274(%rax)
movl $0x2, -0x4(%rbp)
jmp 0x4441
jmp 0x41c6
movq -0x30(%rbp), %rdi
addq $0xd0, %rdi
callq 0x4b60
movq -0x40(%rbp), %rdi
movss %xmm0, -0xa4(%rbp)
callq 0x5e80
movq -0x30(%rbp), %rdi
movss -0xa4(%rbp), %xmm0
movl %eax, %esi
callq 0x44b0
testb $0x1, %al
jne 0x4146
jmp 0x41c4
movq 0x5e83(%rip), %rdi      # 0x9fd0
leaq 0x1f0a(%rip), %rsi      # 0x605e
callq 0x2150
movq %rax, %rdi
leaq 0x1f62(%rip), %rsi      # 0x60c5
callq 0x2150
movq -0x40(%rbp), %rdi
movq %rax, -0xb0(%rbp)
callq 0x5e80
movq -0xb0(%rbp), %rdi
movl %eax, %esi
callq 0x2200
movq %rax, %rdi
leaq 0x1f02(%rip), %rsi      # 0x6092
callq 0x2150
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movl $0x1, 0x270(%rax)
callq 0x5e80
movl %eax, %ecx
movq -0x30(%rbp), %rax
movl %ecx, 0x274(%rax)
movl $0x1, -0x4(%rbp)
jmp 0x4441
jmp 0x41c6
jmp 0x41c8
movq -0x30(%rbp), %rdi
addq $0x1a0, %rdi            # imm = 0x1A0
callq 0x4b10
testb $0x1, %al
jne 0x41e1
jmp 0x42ad
movq -0x30(%rbp), %rdi
addq $0x1a0, %rdi            # imm = 0x1A0
callq 0x4b40
cmpl $0x0, %eax
jne 0x42ad
movss 0x1e46(%rip), %xmm0     # 0x6048
ucomiss -0x24(%rbp), %xmm0
jbe 0x42ad
movss -0x24(%rbp), %xmm0
movss 0x1e33(%rip), %xmm1     # 0x604c
ucomiss %xmm1, %xmm0
jbe 0x42ad
movq -0x30(%rbp), %rax
cmpl $0x1, 0x270(%rax)
jne 0x42ad
movq 0x5d9a(%rip), %rdi      # 0x9fd0
leaq 0x1e21(%rip), %rsi      # 0x605e
callq 0x2150
movq %rax, %rdi
leaq 0x1e95(%rip), %rsi      # 0x60e1
callq 0x2150
movq -0x40(%rbp), %rdi
movq %rax, -0xb8(%rbp)
callq 0x5e80
movq -0xb8(%rbp), %rdi
movl %eax, %esi
callq 0x2200
movq %rax, %rdi
leaq 0x1e19(%rip), %rsi      # 0x6092
callq 0x2150
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movl $0x3, 0x270(%rax)
callq 0x5e80
movl %eax, %ecx
movq -0x30(%rbp), %rax
movl %ecx, 0x274(%rax)
movl $0x3, -0x4(%rbp)
jmp 0x4441
movq -0x30(%rbp), %rdi
addq $0x1a0, %rdi            # imm = 0x1A0
callq 0x4b40
cmpl $0x0, %eax
jne 0x4367
movss -0x24(%rbp), %xmm0
movss 0x1d7d(%rip), %xmm1     # 0x6050
ucomiss %xmm1, %xmm0
jbe 0x4367
movq -0x30(%rbp), %rax
cmpl $0x3, 0x270(%rax)
jne 0x4367
movq 0x5ce0(%rip), %rdi      # 0x9fd0
leaq 0x1d67(%rip), %rsi      # 0x605e
callq 0x2150
movq %rax, %rdi
leaq 0x1df5(%rip), %rsi      # 0x60fb
callq 0x2150
movq -0x40(%rbp), %rdi
movq %rax, -0xc0(%rbp)
callq 0x5e80
movq -0xc0(%rbp), %rdi
movl %eax, %esi
callq 0x2200
movq %rax, %rdi
leaq 0x1d5f(%rip), %rsi      # 0x6092
callq 0x2150
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movl $0x4, 0x270(%rax)
callq 0x5e80
movl %eax, %ecx
movq -0x30(%rbp), %rax
movl %ecx, 0x274(%rax)
movl $0x4, -0x4(%rbp)
jmp 0x4441
movq -0x30(%rbp), %rdi
addq $0x138, %rdi            # imm = 0x138
callq 0x4ba0
testb $0x1, %al
jne 0x4380
jmp 0x443a
movq -0x30(%rbp), %rdi
addq $0x1a0, %rdi            # imm = 0x1A0
callq 0x4ba0
testb $0x1, %al
jne 0x4399
jmp 0x443a
movq -0x30(%rbp), %rdi
addq $0x208, %rdi            # imm = 0x208
callq 0x4ba0
testb $0x1, %al
jne 0x43b2
jmp 0x443a
movq -0x30(%rbp), %rax
cmpl $0xa, 0x270(%rax)
je 0x443a
movq 0x5c0a(%rip), %rdi      # 0x9fd0
leaq 0x1c91(%rip), %rsi      # 0x605e
callq 0x2150
movq %rax, %rdi
leaq 0x1d38(%rip), %rsi      # 0x6114
callq 0x2150
movq -0x40(%rbp), %rdi
movq %rax, -0xc8(%rbp)
callq 0x5e80
movq -0xc8(%rbp), %rdi
movl %eax, %esi
callq 0x2200
movq %rax, %rdi
leaq 0x1c89(%rip), %rsi      # 0x6092
callq 0x2150
movq -0x40(%rbp), %rdi
movq -0x30(%rbp), %rax
movl $0xa, 0x270(%rax)
callq 0x5e80
movl %eax, %ecx
movq -0x30(%rbp), %rax
movl %ecx, 0x274(%rax)
movl $0xa, -0x4(%rbp)
jmp 0x4441
movl $0x0, -0x4(%rbp)
movl -0x4(%rbp), %eax
addq $0xd0, %rsp
popq %rbp
retq
nopl (%rax)