#include "H5.h"
#include <string.h>

//last_jump and now_jump need wait 200ms

#define WAIT_TIME 10

H5::H5()
{
	memset(resultMatrix, -1, 4 * sizeof(int));

	last_left_zupt = 1;
	last_right_zupt = 1;

	CANCLE_DOWN = 0;
	CANCLE_JUMP = 0;

	valid_down_wait_time = 0;
	valid_jump_wait_time = 0;

	last_jump = 0;

	last_down = 0;
}

void H5::Process(int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_press,
	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_press,
	int jump, int down, int rssi)
{

	//»ñÈ¡´¥µØ
	if (last_left_zupt == 0 && left_zupt == 1)
	{
		resultMatrix[0] = MOTION_LEFT;
	}
	else
	{
		resultMatrix[0] = -1;
	}

	if (last_right_zupt == 0 && right_zupt == 1)
	{
		resultMatrix[1] = MOTION_RIGHT;
	}
	else
	{
		resultMatrix[1] = -1;
	}

	//filter down jump

	if (last_jump == 0 && jump == 1)
	{
		resultMatrix[2] = MOTION_JUMP;

		valid_jump_wait_time = WAIT_TIME;
	}
	else
	{
		resultMatrix[2] = -1;
	}

	//reset jump wait time when both foot isn't on the floor
	if ((valid_jump_wait_time > 0 && left_zupt == 0 && right_zupt == 0) || jump == 1)
	{
		valid_jump_wait_time = WAIT_TIME;
	}

	if (valid_jump_wait_time > 0)
	{
		CANCLE_JUMP == 1;

		valid_jump_wait_time--;
	}
	else
	{
		CANCLE_JUMP = 0;
	}
	

	//same process with jump
	if (last_down == 0 && down == 1 && CANCLE_DOWN == 0)
	{
		resultMatrix[3] = MOTION_DOWN;

		valid_down_wait_time = WAIT_TIME;
	}
	else
	{
		resultMatrix[3] = -1;
	}

	if ((valid_jump_wait_time > 0) || down == 1)
	{
		valid_down_wait_time = WAIT_TIME;
	}

	if (valid_down_wait_time > 0)
	{
		CANCLE_DOWN = 1;

		valid_down_wait_time--;
	}
	else
	{
		CANCLE_DOWN = 0;
	}


	last_left_zupt = left_zupt;
	last_right_zupt = right_zupt;
	
	last_jump = jump;
	last_down = down;
}

void H5::getResult(int* dec)
{
	memcpy(dec, resultMatrix, 4 * sizeof(int));
}