// File: /workspace/github_cpp_makefile_star2_O3/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, %rbx
leaq 0x10(%rbp), %r15
movq %r15, %rdi
callq 0x3be4
movss 0x12d0(%rip), %xmm0     # 0x4068
movq %rbx, %rdi
movl %eax, %esi
callq 0x354a
movss %xmm0, -0x2c(%rbp)
leaq 0x68(%rbx), %r14
movq %r14, -0x60(%rbp)
movq %r15, %rdi
callq 0x3bee
movss 0x12ad(%rip), %xmm0     # 0x406c
movq %r14, %rdi
movl %eax, %esi
callq 0x354a
movss %xmm0, -0x40(%rbp)
leaq 0xd0(%rbx), %r14
movq %r14, -0x58(%rbp)
movq %r15, %rdi
callq 0x3bf8
movq %r14, %rdi
movl %eax, %esi
movss 0x127a(%rip), %xmm0     # 0x4068
callq 0x354a
movss %xmm0, -0x3c(%rbp)
leaq 0x138(%rbx), %r14
movq %r14, -0x50(%rbp)
movq %r15, %rdi
callq 0x3c02
movq %r14, %rdi
movl %eax, %esi
movss 0x1250(%rip), %xmm0     # 0x4068
callq 0x354a
movss %xmm0, -0x38(%rbp)
leaq 0x1a0(%rbx), %r13
movq %r15, %rdi
callq 0x3c0c
movq %r13, %rdi
movl %eax, %esi
movss 0x122a(%rip), %xmm0     # 0x4068
callq 0x354a
movss %xmm0, -0x30(%rbp)
leaq 0x208(%rbx), %r14
movq %r14, -0x48(%rbp)
movq %r15, %rdi
callq 0x3c16
movq %r14, %rdi
movl %eax, %esi
movss 0x1200(%rip), %xmm0     # 0x4068
callq 0x354a
movss %xmm0, -0x34(%rbp)
movq %r15, %rdi
callq 0x3bdc
movq 0x3157(%rip), %rdi      # 0x5fd8
movl %eax, %esi
callq 0x21f0
movq %rax, %r12
leaq 0x11f2(%rip), %r14      # 0x4084
movl $0x1, %edx
movq %rax, %rdi
movq %r14, %rsi
callq 0x2170
xorps %xmm0, %xmm0
cvtss2sd -0x2c(%rbp), %xmm0
movq %r12, %rdi
callq 0x21c0
movq %rax, %r12
movl $0x1, %edx
movq %rax, %rdi
movq %r14, %rsi
callq 0x2170
xorps %xmm0, %xmm0
cvtss2sd -0x40(%rbp), %xmm0
movq %r12, %rdi
callq 0x21c0
movq %rax, %r12
movl $0x1, %edx
movq %rax, %rdi
movq %r14, %rsi
callq 0x2170
xorps %xmm0, %xmm0
cvtss2sd -0x3c(%rbp), %xmm0
movq %r12, %rdi
callq 0x21c0
movq %rax, %r12
movl $0x1, %edx
movq %rax, %rdi
movq %r14, %rsi
callq 0x2170
xorps %xmm0, %xmm0
cvtss2sd -0x38(%rbp), %xmm0
movq %r12, %rdi
callq 0x21c0
movq %rax, %r12
movl $0x1, %edx
movq %rax, %rdi
movq %r14, %rsi
callq 0x2170
xorps %xmm0, %xmm0
cvtss2sd -0x30(%rbp), %xmm0
movq %r12, %rdi
callq 0x21c0
movq %rax, %r12
movl $0x1, %edx
movq %rax, %rdi
movq %r14, %rsi
callq 0x2170
xorps %xmm0, %xmm0
cvtss2sd -0x34(%rbp), %xmm0
movq %r12, %rdi
movq -0x50(%rbp), %r12
callq 0x21c0
movq %rax, %r14
movq (%rax), %rax
movq -0x18(%rax), %rdi
addq %r14, %rdi
movl $0xa, %esi
callq 0x2140
movsbl %al, %esi
movq %r14, %rdi
movq -0x58(%rbp), %r14
callq 0x2030
movq %rax, %rdi
callq 0x20e0
movq %r15, %rdi
callq 0x3bdc
movq %rbx, %rdi
movl %eax, %esi
movss -0x2c(%rbp), %xmm0
callq 0x3574
movq %r15, %rdi
callq 0x3bdc
movq -0x60(%rbp), %rdi
movl %eax, %esi
movss -0x40(%rbp), %xmm0
callq 0x3574
movq %r15, %rdi
callq 0x3bdc
movq %r14, %rdi
movl %eax, %esi
movss -0x3c(%rbp), %xmm0
callq 0x3574
movq %r15, %rdi
callq 0x3bdc
movq %r12, %rdi
movl %eax, %esi
movss -0x38(%rbp), %xmm0
callq 0x3574
movq %r15, %rdi
callq 0x3bdc
movq %r13, %rdi
movl %eax, %esi
movss -0x30(%rbp), %xmm0
callq 0x3574
movq %r15, %rdi
callq 0x3bdc
movq -0x48(%rbp), %rdi
movl %eax, %esi
movss -0x34(%rbp), %xmm0
callq 0x3574
cmpl $0x2, 0x270(%rbx)
jne 0x30b7
movq %r15, %rdi
callq 0x3bdc
subl 0x274(%rbx), %eax
cmpl $0x15, %eax
jl 0x30b7
movq 0x2f92(%rip), %r14      # 0x5fd8
leaq 0x1039(%rip), %rsi      # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0x103f(%rip), %rsi      # 0x40a0
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3bdc
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0x1033(%rip), %rsi      # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0x5, 0x270(%rbx)
movq %r15, %rdi
callq 0x3bdc
movl %eax, 0x274(%rbx)
movl $0x5, %r14d
jmp 0x3449
movq %r14, %rdi
callq 0x364a
testb %al, %al
je 0x3235
movq %r14, %rdi
callq 0x365a
movq %r14, %rdi
movl %eax, %r14d
callq 0x3664
movss %xmm0, -0x2c(%rbp)
movq %r15, %rdi
callq 0x3bdc
cmpl $0x1, %r14d
jne 0x3195
movss -0x2c(%rbp), %xmm0
ucomiss 0xf77(%rip), %xmm0      # 0x4074
jbe 0x3235
movl %eax, %ecx
subl 0x274(%rbx), %ecx
cmpl $0x97, %ecx
jl 0x3235
movl %eax, 0x274(%rbx)
movq 0x2eb4(%rip), %r14      # 0x5fd8
leaq 0xf5b(%rip), %rsi       # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0xf96(%rip), %rsi       # 0x40d5
movl $0x17, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3bdc
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0xf55(%rip), %rsi       # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0x2, 0x270(%rbx)
movq %r15, %rdi
callq 0x3bdc
movl %eax, 0x274(%rbx)
movl $0x2, %r14d
jmp 0x3449
movss 0xed3(%rip), %xmm0      # 0x4070
ucomiss -0x2c(%rbp), %xmm0
jbe 0x3235
movl %eax, %ecx
subl 0x274(%rbx), %ecx
cmpl $0x97, %ecx
jl 0x3235
movl %eax, 0x274(%rbx)
movq 0x2e14(%rip), %r14      # 0x5fd8
leaq 0xebb(%rip), %rsi       # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0xf0e(%rip), %rsi       # 0x40ed
movl $0x1b, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3bdc
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0xeb5(%rip), %rsi       # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0x1, 0x270(%rbx)
movq %r15, %rdi
callq 0x3bdc
movl %eax, 0x274(%rbx)
movl $0x1, %r14d
jmp 0x3449
movq %r13, %rdi
callq 0x364a
testb %al, %al
je 0x32fa
movq %r13, %rdi
callq 0x365a
movss -0x30(%rbp), %xmm0
ucomiss 0xe1f(%rip), %xmm0      # 0x4078
jbe 0x32fa
movss 0xe15(%rip), %xmm0      # 0x407c
ucomiss -0x30(%rbp), %xmm0
jbe 0x32fa
testl %eax, %eax
jne 0x32fa
cmpl $0x1, 0x270(%rbx)
jne 0x32fa
movq 0x2d4f(%rip), %r14      # 0x5fd8
leaq 0xdf6(%rip), %rsi       # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0xe65(%rip), %rsi       # 0x4109
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3bdc
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0xdf0(%rip), %rsi       # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0x3, 0x270(%rbx)
movq %r15, %rdi
callq 0x3bdc
movl %eax, 0x274(%rbx)
movl $0x3, %r14d
jmp 0x3449
movq %r13, %rdi
callq 0x365a
movss -0x30(%rbp), %xmm0
ucomiss 0xd72(%rip), %xmm0      # 0x4080
jbe 0x339d
testl %eax, %eax
jne 0x339d
cmpl $0x3, 0x270(%rbx)
jne 0x339d
movq 0x2cac(%rip), %r14      # 0x5fd8
leaq 0xd53(%rip), %rsi       # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0xddc(%rip), %rsi       # 0x4123
movl $0x18, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3bdc
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0xd4d(%rip), %rsi       # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0x4, 0x270(%rbx)
movq %r15, %rdi
callq 0x3bdc
movl %eax, 0x274(%rbx)
movl $0x4, %r14d
jmp 0x3449
movq %r12, %rdi
callq 0x367a
xorl %r14d, %r14d
testb %al, %al
je 0x3449
movq %r13, %rdi
callq 0x367a
testb %al, %al
je 0x3449
movq -0x48(%rbp), %rdi
callq 0x367a
testb %al, %al
je 0x3449
cmpl $0xa, 0x270(%rbx)
je 0x3449
movq 0x2bfb(%rip), %r14      # 0x5fd8
leaq 0xca2(%rip), %rsi       # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0xd44(%rip), %rsi       # 0x413c
movl $0x1a, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3bdc
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0xc9c(%rip), %rsi       # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0xa, 0x270(%rbx)
movq %r15, %rdi
callq 0x3bdc
movl %eax, 0x274(%rbx)
movl $0xa, %r14d
movl %r14d, %eax
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop