#include "pub.h"

#include "BigFoot.h"


BigFoot::BigFoot()
{
	memset(result, 0, 4 * sizeof(int));

	memset(left_min_acc, 0, 3 * sizeof(int));

	memset(left_max_acc, 0, 3 * sizeof(int));

	memset(right_min_acc, 0, 3 * sizeof(int));

	memset(right_max_acc, 0, 3 * sizeof(int));

	last_left_zupt = 1;

	last_right_zupt = 1;
}

int BigFoot::station_acc(int* max_acc, int* min_acc)
{
	for (int i = 0; i < 3; i++)
	{
		if (max_acc[i] - min_acc[i] > 1024)
		{
			return 0;
		}
	}


	return 1;
}

void BigFoot::getResult(int* matrix)
{
	memcpy(matrix, result, 4 * sizeof(int));
}

void BigFoot::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 girl_shoes)
{


	//需要用加速度过滤伪触地，如果期间是小于0.5g的则视为伪触地，过滤掉
	//station_acc 判断方法

	if (left_zupt && last_left_zupt == 0 && !station_acc(left_max_acc, left_min_acc))
	//if (left_zupt && last_left_zupt == 0 )
	{
		result[0] = 1;
		std::cout << "左脚踩了" << std::endl;
	}
	else
	{
		result[0] = 0;
	}

	if (right_zupt && last_right_zupt == 0 && !station_acc(right_max_acc, right_min_acc))
	{
		result[1] = 1;
		std::cout << "右脚踩了" << std::endl;
	}
	else
	{
		result[1] = 0;
	}
	

	last_left_zupt = left_zupt;

	last_right_zupt = right_zupt;


	if (left_zupt)
	{
		memcpy(left_min_acc, left_acc, 3 * sizeof(int));

		memcpy(left_max_acc, left_acc, 3 * sizeof(int));
	}
	else
	{
		for (int i = 0; i < 3; i++)
		{
			if (left_min_acc[i] > left_acc[i])
			{
				left_min_acc[i] = left_acc[i];
			}

			if (left_max_acc[i] < left_acc[i])
			{
				left_max_acc[i] = left_acc[i];
			}
		}
	}

	if (right_zupt)
	{
		memcpy(right_min_acc, right_acc, 3 * sizeof(int));

		memcpy(right_max_acc, right_acc, 3 * sizeof(int));
	}
	else
	{
		for (int i = 0; i < 3; i++)
		{
			if (right_min_acc[i] > right_acc[i])
			{
				right_min_acc[i] = right_acc[i];
			}

			if (right_max_acc[i] < right_acc[i])
			{
				right_max_acc[i] = right_acc[i];
			}
		}
	}


}