// File: /workspace/github_cpp_makefile_star2_O2/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
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movq %rdi, %r13
leaq 0x10(%rbp), %r15
movq %r15, %rdi
callq 0x38dc
movss 0x152c(%rip), %xmm0     # 0x4040
movq %r13, %rdi
movl %eax, %esi
callq 0x31b0
movss %xmm0, -0x2c(%rbp)
leaq 0x68(%r13), %rbx
movq %r15, %rdi
callq 0x38e6
movss 0x150d(%rip), %xmm0     # 0x4044
movq %rbx, %rdi
movl %eax, %esi
callq 0x31b0
movss %xmm0, -0x40(%rbp)
leaq 0xd0(%r13), %r12
movq %r15, %rdi
callq 0x38f0
movq %r12, %rdi
movl %eax, %esi
movss 0x14de(%rip), %xmm0     # 0x4040
callq 0x31b0
movss %xmm0, -0x3c(%rbp)
leaq 0x138(%r13), %r14
movq %r14, -0x50(%rbp)
movq %r15, %rdi
callq 0x38fa
movq %r14, %rdi
movl %eax, %esi
movss 0x14b4(%rip), %xmm0     # 0x4040
callq 0x31b0
movss %xmm0, -0x38(%rbp)
leaq 0x1a0(%r13), %r14
movq %r14, -0x58(%rbp)
movq %r15, %rdi
callq 0x3904
movq %r14, %rdi
movl %eax, %esi
movss 0x148a(%rip), %xmm0     # 0x4040
callq 0x31b0
movss %xmm0, -0x30(%rbp)
leaq 0x208(%r13), %r14
movq %r14, -0x48(%rbp)
movq %r15, %rdi
callq 0x390e
movq %r14, %rdi
movl %eax, %esi
movss 0x1460(%rip), %xmm0     # 0x4040
callq 0x31b0
movss %xmm0, -0x34(%rbp)
movq %r15, %rdi
callq 0x38d4
movq 0x43d7(%rip), %rdi      # 0x6fd0
movl %eax, %esi
callq 0x21d0
leaq 0x1455(%rip), %r14      # 0x405c
movq %rax, %rdi
movq %r14, %rsi
callq 0x2130
movq %rax, %rdi
movss -0x2c(%rbp), %xmm0
callq 0x20f0
movq %rax, %rdi
movq %r14, %rsi
callq 0x2130
movq %rax, %rdi
movss -0x40(%rbp), %xmm0
callq 0x20f0
movq %rax, %rdi
movq %r14, %rsi
callq 0x2130
movq %rax, %rdi
movss -0x3c(%rbp), %xmm0
callq 0x20f0
movq %rax, %rdi
movq %r14, %rsi
callq 0x2130
movq %rax, %rdi
movss -0x38(%rbp), %xmm0
callq 0x20f0
movq %rax, %rdi
movq %r14, %rsi
callq 0x2130
movq %rax, %rdi
movss -0x30(%rbp), %xmm0
callq 0x20f0
movq %rax, %rdi
movq %r14, %rsi
callq 0x2130
movq %rax, %rdi
movss -0x34(%rbp), %xmm0
callq 0x20f0
movq %rax, %rdi
callq 0x20b0
movq %r15, %rdi
callq 0x38d4
movq %r13, %rdi
movl %eax, %esi
movss -0x2c(%rbp), %xmm0
callq 0x31da
movq %r15, %rdi
callq 0x38d4
movq %rbx, %rdi
movl %eax, %esi
movss -0x40(%rbp), %xmm0
callq 0x31da
movq %r15, %rdi
callq 0x38d4
movq %r12, %rdi
movl %eax, %esi
movss -0x3c(%rbp), %xmm0
callq 0x31da
movq %r15, %rdi
callq 0x38d4
movq -0x50(%rbp), %rdi
movq %r13, %rbx
movl %eax, %esi
movss -0x38(%rbp), %xmm0
callq 0x31da
movq %r15, %rdi
callq 0x38d4
movq -0x58(%rbp), %rdi
movq %rdi, %r13
movl %eax, %esi
movss -0x30(%rbp), %xmm0
callq 0x31da
movq %r15, %rdi
callq 0x38d4
movq -0x48(%rbp), %rdi
movl %eax, %esi
movss -0x34(%rbp), %xmm0
callq 0x31da
cmpl $0x2, 0x270(%rbx)
jne 0x2db4
leaq 0x10(%rbp), %rdi
callq 0x38d4
subl 0x274(%rbx), %eax
cmpl $0x15, %eax
jl 0x2db4
movq 0x427a(%rip), %rdi      # 0x6fd0
leaq 0x1301(%rip), %rsi      # 0x405e
callq 0x2130
leaq 0x130f(%rip), %rsi      # 0x4078
movq %rax, %rdi
callq 0x2130
movq %rax, %r14
movq %r15, %rdi
callq 0x38d4
movq %r14, %rdi
movl %eax, %esi
callq 0x21d0
leaq 0x1305(%rip), %rsi      # 0x4092
movq %rax, %rdi
callq 0x2130
movl $0x5, 0x270(%rbx)
movq %r15, %rdi
callq 0x38d4
movl %eax, 0x274(%rbx)
pushq $0x5
jmp 0x303a
movq %r12, %rdi
callq 0x3250
testb %al, %al
je 0x2f09
movq %r12, %rdi
callq 0x3260
movl %eax, %r14d
movq %r12, %rdi
callq 0x326a
movss %xmm0, -0x2c(%rbp)
leaq 0x10(%rbp), %rdi
callq 0x38d4
cmpl $0x1, %r14d
jne 0x2e80
movss -0x2c(%rbp), %xmm0
ucomiss 0x1251(%rip), %xmm0     # 0x404c
jbe 0x2f09
movl %eax, %ecx
subl 0x274(%rbx), %ecx
cmpl $0x97, %ecx
jl 0x2f09
movl %eax, 0x274(%rbx)
movq 0x41ae(%rip), %rdi      # 0x6fd0
leaq 0x1235(%rip), %rsi      # 0x405e
callq 0x2130
leaq 0x1278(%rip), %rsi      # 0x40ad
movq %rax, %rdi
callq 0x2130
movq %rax, %r14
movq %r15, %rdi
callq 0x38d4
movq %r14, %rdi
movl %eax, %esi
callq 0x21d0
leaq 0x1239(%rip), %rsi      # 0x4092
movq %rax, %rdi
callq 0x2130
movl $0x2, 0x270(%rbx)
movq %r15, %rdi
callq 0x38d4
movl %eax, 0x274(%rbx)
pushq $0x2
jmp 0x303a
movss 0x11c0(%rip), %xmm0     # 0x4048
ucomiss -0x2c(%rbp), %xmm0
jbe 0x2f09
movl %eax, %ecx
subl 0x274(%rbx), %ecx
cmpl $0x97, %ecx
jl 0x2f09
movl %eax, 0x274(%rbx)
movq 0x4125(%rip), %rdi      # 0x6fd0
leaq 0x11ac(%rip), %rsi      # 0x405e
callq 0x2130
leaq 0x1207(%rip), %rsi      # 0x40c5
movq %rax, %rdi
callq 0x2130
movq %rax, %r14
movq %r15, %rdi
callq 0x38d4
movq %r14, %rdi
movl %eax, %esi
callq 0x21d0
leaq 0x11b0(%rip), %rsi      # 0x4092
movq %rax, %rdi
callq 0x2130
movl $0x1, 0x270(%rbx)
movq %r15, %rdi
callq 0x38d4
movl %eax, 0x274(%rbx)
pushq $0x1
jmp 0x303a
movq %r13, %rdi
callq 0x3250
testb %al, %al
je 0x2fb3
movq %r13, %rdi
callq 0x3260
movss -0x30(%rbp), %xmm0
ucomiss 0x1123(%rip), %xmm0     # 0x4050
jbe 0x2fb3
movss 0x1119(%rip), %xmm0     # 0x4054
ucomiss -0x30(%rbp), %xmm0
jbe 0x2fb3
testl %eax, %eax
jne 0x2fb3
cmpl $0x1, 0x270(%rbx)
jne 0x2fb3
movq 0x407b(%rip), %rdi      # 0x6fd0
leaq 0x1102(%rip), %rsi      # 0x405e
callq 0x2130
leaq 0x1179(%rip), %rsi      # 0x40e1
movq %rax, %rdi
callq 0x2130
movq %rax, %r14
movq %r15, %rdi
callq 0x38d4
movq %r14, %rdi
movl %eax, %esi
callq 0x21d0
leaq 0x1106(%rip), %rsi      # 0x4092
movq %rax, %rdi
callq 0x2130
movl $0x3, 0x270(%rbx)
movq %r15, %rdi
callq 0x38d4
movl %eax, 0x274(%rbx)
pushq $0x3
jmp 0x303a
movq %r13, %rdi
callq 0x3260
movss -0x30(%rbp), %xmm0
ucomiss 0x1091(%rip), %xmm0     # 0x4058
jbe 0x304e
testl %eax, %eax
jne 0x304e
cmpl $0x3, 0x270(%rbx)
jne 0x304e
movq 0x3fef(%rip), %rdi      # 0x6fd0
leaq 0x1076(%rip), %rsi      # 0x405e
callq 0x2130
leaq 0x1107(%rip), %rsi      # 0x40fb
movq %rax, %rdi
callq 0x2130
movq %rax, %r14
movq %r15, %rdi
callq 0x38d4
movq %r14, %rdi
movl %eax, %esi
callq 0x21d0
leaq 0x107a(%rip), %rsi      # 0x4092
movq %rax, %rdi
callq 0x2130
movl $0x4, 0x270(%rbx)
movq %r15, %rdi
callq 0x38d4
movl %eax, 0x274(%rbx)
pushq $0x4
popq %r14
movl %r14d, %eax
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq -0x50(%rbp), %rdi
callq 0x3280
xorl %r14d, %r14d
testb %al, %al
je 0x303c
movq %r13, %rdi
callq 0x3280
testb %al, %al
je 0x303c
movq -0x48(%rbp), %rdi
callq 0x3280
testb %al, %al
je 0x303c
cmpl $0xa, 0x270(%rbx)
je 0x303c
movq 0x3f49(%rip), %rdi      # 0x6fd0
leaq 0xfd0(%rip), %rsi       # 0x405e
callq 0x2130
leaq 0x107a(%rip), %rsi      # 0x4114
movq %rax, %rdi
callq 0x2130
movq %rax, %r14
movq %r15, %rdi
callq 0x38d4
movq %r14, %rdi
movl %eax, %esi
callq 0x21d0
leaq 0xfd4(%rip), %rsi       # 0x4092
movq %rax, %rdi
callq 0x2130
movl $0xa, 0x270(%rbx)
movq %r15, %rdi
callq 0x38d4
movl %eax, 0x274(%rbx)
pushq $0xa
jmp 0x303a
nop