#include "SingleFootAction.h"


void SingleFootAction::run(int x, int y, int z,int zupt, int rssi, float pitch)
{
	report_siganl_listen(last_z);

	pause_signal_listen(pitch);
	
	direction_signal_listen(last_x, last_y, zupt, rssi);

	set_motion_signal(zupt);

	last_zupt = zupt;

	last_x = x; last_y = y; last_z = z;

}

int SingleFootAction::get_interation_state()
{
	return interation_state;
}

void SingleFootAction::set_motion_signal(int zupt )
{
	if (pause_signal_count >= 200)
	{
		if (pause_signal == 1)
		{
			interation_state = MOTION_STOP;

		}
		else
		{
			interation_state = -1;
		}
	}
	else
	{
		if (last_zupt == 0 && zupt == 1 && can_report_motion)
		{
			interation_state = direction_signal;

			can_report_motion = 0;
		}
		else
		{
			interation_state = -1;
		}
	
	}
}

void  SingleFootAction::direction_signal_listen(int x, int y, int zupt, int rssi)
{	
	float x_offset =  x;
	float y_offset =  y;

	float len_offset = sqrt(x_offset * x_offset + y_offset * y_offset);

	if (last_zupt == 0 && zupt == 1)
	{
		if (left_or_right == RIGHT_FOOT)
		{
			std::cout << "debug "<<"x: " << x << " y: " << y  << endl;
		}
		if (rssi < 30)
		{
			direction_signal = -1;
		}
		else if (len_offset < 15.f)
		{
			return;
		}
		else
		{
			float angle = atan2(y_offset, x_offset);

			if (left_or_right == LEFT_FOOT)
			{
				if (angle > PI * 3 / 4 || angle < -PI * 3 / 4)
				{
					direction_signal = MOTION_LEFT;
				}
				else if (angle < PI * 3 / 4 && angle >PI / 4)
				{
					direction_signal = MOTION_FRONT;
				}
				else if (angle > -PI * 3 / 4 && angle < -PI / 4)
				{
					direction_signal = MOTION_BACK;
				}
				else
				{
					direction_signal = -1;
				}
			}
			else
			{
				if (angle < PI / 4 && angle > -PI / 4)
				{
					direction_signal = MOTION_RIGHT;
				}
				else if (angle < PI * 3 / 4 && angle >PI / 4)
				{
					direction_signal = MOTION_FRONT;
				}
				else if (angle > -PI * 3 / 4 && angle < -PI / 4)
				{
					direction_signal = MOTION_BACK;
				}
				else
				{
					direction_signal = -1;
				}
			}
		}
		
	}
}
void SingleFootAction::report_siganl_listen(int z)
{
	if (z > 1)
	{
		can_report_motion = 1;
	}
}

void SingleFootAction::pause_signal_listen(float pitch)
{
	if (pitch < STOP_ANGLE)
	{
		pause_signal_count++;
	}	
	else
	{
		pause_signal_count = 0;
	}

	if (pause_signal_count == 200)
	{
		pause_signal = 1;
	}
	else
	{
		pause_signal = 0;
	}
}