// File: /workspace/github_cpp_makefile_star2_O1/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 0x3c70
movss 0x12ac(%rip), %xmm0     # 0x4068
movq %rbx, %rdi
movl %eax, %esi
callq 0x3562
movss %xmm0, -0x2c(%rbp)
leaq 0x68(%rbx), %r14
movq %r14, -0x60(%rbp)
movq %r15, %rdi
callq 0x3c7a
movss 0x1289(%rip), %xmm0     # 0x406c
movq %r14, %rdi
movl %eax, %esi
callq 0x3562
movss %xmm0, -0x40(%rbp)
leaq 0xd0(%rbx), %r14
movq %r14, -0x58(%rbp)
movq %r15, %rdi
callq 0x3c84
movq %r14, %rdi
movl %eax, %esi
movss 0x1256(%rip), %xmm0     # 0x4068
callq 0x3562
movss %xmm0, -0x3c(%rbp)
leaq 0x138(%rbx), %r14
movq %r14, -0x50(%rbp)
movq %r15, %rdi
callq 0x3c8e
movq %r14, %rdi
movl %eax, %esi
movss 0x122c(%rip), %xmm0     # 0x4068
callq 0x3562
movss %xmm0, -0x38(%rbp)
leaq 0x1a0(%rbx), %r13
movq %r15, %rdi
callq 0x3c98
movq %r13, %rdi
movl %eax, %esi
movss 0x1206(%rip), %xmm0     # 0x4068
callq 0x3562
movss %xmm0, -0x30(%rbp)
leaq 0x208(%rbx), %r14
movq %r14, -0x48(%rbp)
movq %r15, %rdi
callq 0x3ca2
movq %r14, %rdi
movl %eax, %esi
movss 0x11dc(%rip), %xmm0     # 0x4068
callq 0x3562
movss %xmm0, -0x34(%rbp)
movq %r15, %rdi
callq 0x3c68
movq 0x3133(%rip), %rdi      # 0x5fd8
movl %eax, %esi
callq 0x21f0
movq %rax, %r12
leaq 0x11ce(%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 0x3c68
movq %rbx, %rdi
movl %eax, %esi
movss -0x2c(%rbp), %xmm0
callq 0x358c
movq %r15, %rdi
callq 0x3c68
movq -0x60(%rbp), %rdi
movl %eax, %esi
movss -0x40(%rbp), %xmm0
callq 0x358c
movq %r15, %rdi
callq 0x3c68
movq %r14, %rdi
movl %eax, %esi
movss -0x3c(%rbp), %xmm0
callq 0x358c
movq %r15, %rdi
callq 0x3c68
movq %r12, %rdi
movl %eax, %esi
movss -0x38(%rbp), %xmm0
callq 0x358c
movq %r15, %rdi
callq 0x3c68
movq %r13, %rdi
movl %eax, %esi
movss -0x30(%rbp), %xmm0
callq 0x358c
movq %r15, %rdi
callq 0x3c68
movq -0x48(%rbp), %rdi
movl %eax, %esi
movss -0x34(%rbp), %xmm0
callq 0x358c
cmpl $0x2, 0x270(%rbx)
jne 0x30db
movq %r15, %rdi
callq 0x3c68
subl 0x274(%rbx), %eax
cmpl $0x15, %eax
jl 0x30db
movq 0x2f6e(%rip), %r14      # 0x5fd8
leaq 0x1015(%rip), %rsi      # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0x101b(%rip), %rsi      # 0x40a0
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3c68
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0x100f(%rip), %rsi      # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0x5, 0x270(%rbx)
movq %r15, %rdi
callq 0x3c68
movl %eax, 0x274(%rbx)
movl $0x5, %r14d
jmp 0x346d
movq %r14, %rdi
callq 0x3662
testb %al, %al
je 0x3259
movq %r14, %rdi
callq 0x3672
movq %r14, %rdi
movl %eax, %r14d
callq 0x367c
movss %xmm0, -0x2c(%rbp)
movq %r15, %rdi
callq 0x3c68
cmpl $0x1, %r14d
jne 0x31b9
movss -0x2c(%rbp), %xmm0
ucomiss 0xf53(%rip), %xmm0      # 0x4074
jbe 0x3259
movl %eax, %ecx
subl 0x274(%rbx), %ecx
cmpl $0x97, %ecx
jl 0x3259
movl %eax, 0x274(%rbx)
movq 0x2e90(%rip), %r14      # 0x5fd8
leaq 0xf37(%rip), %rsi       # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0xf72(%rip), %rsi       # 0x40d5
movl $0x17, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3c68
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0xf31(%rip), %rsi       # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0x2, 0x270(%rbx)
movq %r15, %rdi
callq 0x3c68
movl %eax, 0x274(%rbx)
movl $0x2, %r14d
jmp 0x346d
movss 0xeaf(%rip), %xmm0      # 0x4070
ucomiss -0x2c(%rbp), %xmm0
jbe 0x3259
movl %eax, %ecx
subl 0x274(%rbx), %ecx
cmpl $0x97, %ecx
jl 0x3259
movl %eax, 0x274(%rbx)
movq 0x2df0(%rip), %r14      # 0x5fd8
leaq 0xe97(%rip), %rsi       # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0xeea(%rip), %rsi       # 0x40ed
movl $0x1b, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3c68
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0xe91(%rip), %rsi       # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0x1, 0x270(%rbx)
movq %r15, %rdi
callq 0x3c68
movl %eax, 0x274(%rbx)
movl $0x1, %r14d
jmp 0x346d
movq %r13, %rdi
callq 0x3662
testb %al, %al
je 0x331e
movq %r13, %rdi
callq 0x3672
movss -0x30(%rbp), %xmm0
ucomiss 0xdfb(%rip), %xmm0      # 0x4078
jbe 0x331e
movss 0xdf1(%rip), %xmm0      # 0x407c
ucomiss -0x30(%rbp), %xmm0
jbe 0x331e
testl %eax, %eax
jne 0x331e
cmpl $0x1, 0x270(%rbx)
jne 0x331e
movq 0x2d2b(%rip), %r14      # 0x5fd8
leaq 0xdd2(%rip), %rsi       # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0xe41(%rip), %rsi       # 0x4109
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3c68
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0xdcc(%rip), %rsi       # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0x3, 0x270(%rbx)
movq %r15, %rdi
callq 0x3c68
movl %eax, 0x274(%rbx)
movl $0x3, %r14d
jmp 0x346d
movq %r13, %rdi
callq 0x3672
movss -0x30(%rbp), %xmm0
ucomiss 0xd4e(%rip), %xmm0      # 0x4080
jbe 0x33c1
testl %eax, %eax
jne 0x33c1
cmpl $0x3, 0x270(%rbx)
jne 0x33c1
movq 0x2c88(%rip), %r14      # 0x5fd8
leaq 0xd2f(%rip), %rsi       # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0xdb8(%rip), %rsi       # 0x4123
movl $0x18, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3c68
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0xd29(%rip), %rsi       # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0x4, 0x270(%rbx)
movq %r15, %rdi
callq 0x3c68
movl %eax, 0x274(%rbx)
movl $0x4, %r14d
jmp 0x346d
movq %r12, %rdi
callq 0x3692
xorl %r14d, %r14d
testb %al, %al
je 0x346d
movq %r13, %rdi
callq 0x3692
testb %al, %al
je 0x346d
movq -0x48(%rbp), %rdi
callq 0x3692
testb %al, %al
je 0x346d
cmpl $0xa, 0x270(%rbx)
je 0x346d
movq 0x2bd7(%rip), %r14      # 0x5fd8
leaq 0xc7e(%rip), %rsi       # 0x4086
movl $0x19, %edx
movq %r14, %rdi
callq 0x2170
leaq 0xd20(%rip), %rsi       # 0x413c
movl $0x1a, %edx
movq %r14, %rdi
callq 0x2170
movq %r15, %rdi
callq 0x3c68
movq %r14, %rdi
movl %eax, %esi
callq 0x21f0
leaq 0xc78(%rip), %rsi       # 0x40ba
movl $0x1a, %edx
movq %rax, %rdi
callq 0x2170
movl $0xa, 0x270(%rbx)
movq %r15, %rdi
callq 0x3c68
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