#include "publicMotion.h"

publicMotion::publicMotion()
{
	motion_time = 0;

	memset(left_front_mag_window, 0, 5 * sizeof(int));

	memset(right_front_mag_window, 0, 5 * sizeof(int));

	memset(left_back_mag_window, 0, 5 * sizeof(int));

	memset(right_back_mag_window, 0, 5 * sizeof(int));

	memset(left_front_mag_mean_window, 0, 15 * sizeof(int));

	memset(right_front_mag_mean_window, 0, 15 * sizeof(int));

	memset(left_back_mag_mean_window, 0, 15 * sizeof(int));

	memset(right_back_mag_mean_window, 0, 15 * sizeof(int));

	left_front_up_time = 0;
	right_front_up_time = 0;

	left_back_up_time = 0;
	right_back_up_time = 0;

}

void publicMotion::meanFiltData(int time_count, int mag_data, int* mag_data_window, int* mean_mag_data)
{
	if (time_count < 7)
	{
		mean_mag_data[time_count % 5] = mag_data;

	}
	else
	{
		float sum = 0;
		for (int i = 0; i < 5; i++)
		{
			sum += mag_data_window[i];
		}

		mean_mag_data[time_count % 15] = sum / 5.;
	}
}

/*
* 均值滤波的窗口大小为10， 当检测到前三个的和 < 后三个数据的和，则视为上升的趋势
* 上升的时候，还要检测上升趋势的最大值，防止两个凸曲线有重叠误判为蹲
*/
void  publicMotion::setSpecialPoint_part(int mag_data, int* mean_data, int* max_val, int* down_point, int* up_time)
{
	int prev_sum = (mean_data[(motion_time - 8) % 15] + mean_data[(motion_time - 7) % 15] + mean_data[(motion_time - 6) % 15]);

	int prev_sum1 = (mean_data[(motion_time - 11) % 15] + mean_data[(motion_time - 10) % 15] + mean_data[(motion_time - 9) % 15]);
	int prev_sum2 = (mean_data[(motion_time - 14) % 15] + mean_data[(motion_time - 13) % 15] + mean_data[(motion_time - 12) % 15]);

	int back_sum = (mean_data[(motion_time - 2) % 15] + mean_data[(motion_time - 1) % 15] + mean_data[(motion_time) % 15]);

	if (prev_sum < back_sum - 100 || prev_sum1 < back_sum - 10 || prev_sum2 < back_sum - 10)
	{
		if (*max_val < mean_data[motion_time % 15])
		{
			*max_val = mean_data[motion_time % 15];
		}

		(*up_time)++;
	}
	else
	{
		*max_val = mean_data[motion_time % 15];

		*down_point = mean_data[motion_time % 15];

		*up_time = 0;
	}
}

void publicMotion::setSpecialPoint(int left_front_data, int right_front_data, int left_back_data, int right_back_data)
{
	if (motion_time < 10)
	{
		left_back_mag_point_val = left_back_data;
		right_back_mag_point_val = right_back_data;

		left_front_mag_point_val = left_front_data;
		right_front_mag_point_val = right_front_data;

		left_back_mag_up_max = left_back_data;
		right_back_mag_up_max = right_back_data;

		left_front_mag_up_max = left_front_data;
		right_front_mag_up_max = right_front_data;
	}
	else
	{

		setSpecialPoint_part(left_front_data, left_front_mag_mean_window, &left_front_mag_up_max, &left_front_mag_point_val, &left_front_up_time);

		setSpecialPoint_part(right_front_data, right_front_mag_mean_window, &right_front_mag_up_max, &right_front_mag_point_val, &right_front_up_time);

		setSpecialPoint_part(left_back_data, left_back_mag_mean_window, &left_back_mag_up_max, &left_back_mag_point_val, &left_back_up_time);

		setSpecialPoint_part(right_back_data, right_back_mag_mean_window, &right_back_mag_up_max, &right_back_mag_point_val, &right_back_up_time);
	}
}

void publicMotion::setMagData(int left_front_data, int right_front_data, int left_back_data, int right_back_data)
{

	left_front_mag_window[motion_time % 5] = left_front_data;

	right_front_mag_window[motion_time % 5] = right_front_data;

	left_back_mag_window[motion_time % 5] = left_back_data;

	right_back_mag_window[motion_time % 5] = right_back_data;

	//均值滤波
	meanFiltData(motion_time, left_front_data, left_front_mag_window, left_front_mag_mean_window);
	meanFiltData(motion_time, right_front_data, right_front_mag_window, right_front_mag_mean_window);

	meanFiltData(motion_time, left_back_data, left_back_mag_window, left_back_mag_mean_window);
	meanFiltData(motion_time, right_back_data, right_back_mag_window, right_back_mag_mean_window);

	setSpecialPoint(left_front_data, right_front_data, left_back_data, right_back_data);


	motion_time++;

}

int publicMotion::getDownData(int left_front_data, int right_front_data, int left_back_data, int right_back_data)
{
	setMagData(left_front_data, right_front_data, left_back_data, right_back_data);

	int  left_front_down_tag = 0;
	int  right_front_down_tag = 0;

	int  left_back_down_tag = 0;
	int  right_back_down_tag = 0;

	if (left_back_data - left_back_mag_point_val > 2000 || (left_back_data - left_back_mag_point_val > 1000 && left_back_mag_point_val > 15000))
	{
		left_back_down_tag = 1;
	}

	if (right_back_data - right_back_mag_point_val > 2000 || (right_back_data - right_back_mag_point_val > 1000 && right_back_mag_point_val > 15000))
	{
		right_back_down_tag = 1;
	}


	if (left_back_down_tag && right_back_down_tag
		&& left_back_data - left_back_mag_up_max > -500 && right_back_data - right_back_mag_up_max > -500 &&
		abs(left_back_up_time - right_back_up_time) < 10 && left_back_up_time > 3 && right_back_up_time > 3)
	{

		return 1;
	}


	if (left_front_data - left_front_mag_point_val > 2000 || (left_front_data - left_front_mag_point_val > 1000 && left_front_mag_point_val > 15000))
	{
		left_front_down_tag = 1;
	}

	if (right_front_data - right_front_mag_point_val > 2000 || (right_front_data - right_front_mag_point_val > 1000 && right_front_mag_point_val > 15000))
	{
		right_front_down_tag = 1;
	}

	if (left_front_down_tag && right_front_down_tag
		&& left_front_data - left_front_mag_up_max > -500 && right_front_data - right_front_mag_up_max > -500
		&& abs(left_front_up_time - right_front_up_time) < 10 && left_front_up_time > 3 && right_front_up_time > 3)
	{

		return 1;
	}


	return 0;
}