#pragma once

#include "Interaction.h"
#include "pub.h"

Interaction::Interaction()
{
	 left_Triggering_time = 0;
	 right_Triggering_time = 0;

	 last_left_zupt = 1;
	 last_right_zupt = 1;

	 memset(left_acc_x_buff, 0, 10 * sizeof(float));
	 memset(right_acc_x_buff, 0, 10 * sizeof(float));

	 memset(left_pitch_buff, 0, 10 * sizeof(float));
	 memset(right_pitch_buff, 0, 10 * sizeof(float));

	 left_station_count = 0;
	 right_station_count = 0;

	 left_angle_has_reset = 0;
	 right_angle_has_reset = 0;

	 cmd = -1;
}

int Interaction::foot_on_floor(int _time,  int& triggerring_time, int zupt, int &last_zupt, int rssi, float pitch, int left_or_right)
{
	if (pitch > 0.2f)
	{
		return -1;
	}

	int result = -1;

	if (last_zupt == 0 && zupt == 1)
	{
		if (_time - triggerring_time > 25)
		{
			if (left_or_right == LEFT_FOOT)
			{
				result = BACK_LEFT;
			}
			else
			{
				std::cout << "_time - triggerring_time  : " << _time - triggerring_time << endl;
				result = BACK_RIGHT;
			}
		}
	}

	if (zupt)
	{
		triggerring_time = _time;
	}

	last_zupt = zupt;

	return result;
}


int acc_is_valid(float* acc_window, int length)
{
	//要求窗口窗口长度大于10
	//先判断最后五个元素是平稳的，阈值设置为0.06
	float station_max = acc_window[length - 1];
	float station_min = acc_window[length - 1];

	float window_max = acc_window[length - 1];
	float window_min = acc_window[length - 1];

	int window_max_index = length - 1;
	int window_min_index = length - 1;

	for (int i = length - 1; i >= 0; i--)
	{
		if (acc_window[i] > window_max)
		{
			window_max = acc_window[i];
			window_max_index = i;
		}

		if (acc_window[i] < window_min)
		{
			window_min = acc_window[i];
			window_min_index = i;
		}
	}


	for (int i = length - 1; i >= length - 3; i--)
	{
		if (acc_window[i] > station_max)
		{
			station_max = acc_window[i];
		}

		if (acc_window[i] < station_min)
		{
			station_min = acc_window[i];
		}
	}

	if (window_max - window_min < 0.1f)
	{
		return 1;
	}

	return 0;
}
int Interaction::enter_key_listen( float* acc, float* acc_x_buff,int &station_count,float *pitch_buff,float pitch, int& angle_has_reset)
{
	memcpy(acc_x_buff, acc_x_buff + 1, 9 * sizeof(float));

	acc_x_buff[9] = sqrt(acc[0] * acc[0]  + acc[1] * acc[1] + acc[2] * acc[2]);

	memcpy(pitch_buff, pitch_buff + 1, 9 * sizeof(float));

	pitch_buff[9] = pitch;

	//先判断俯仰角是否正常着地，这样才能判断确认键
	if (pitch < 0.2f)
	{
		angle_has_reset = 1;
	}

	if (acc_is_valid(acc_x_buff, 10) && *max_element(pitch_buff, pitch_buff + 10) - *min_element(pitch_buff, pitch_buff + 10) < 0.05)
	{
		station_count++;
	}
	else
	{
		station_count = 0;
	}

	//x轴速度剧烈抖动，而且俯仰角大于45度，则证明是踮脚
	if (station_count > 10 && pitch > 0.3f  && angle_has_reset == 1)
	{
		angle_has_reset = 0;

		return 1;
	}

	return -1;
}

void Interaction::setPolarAccUnzupt(int* max_acc, int* min_acc, int* acc, int zupt)
{
	if (zupt != 1)
	{
		for (int i = 0; i < 3; i++)
		{
			if (max_acc[i] < acc[i])
			{
				max_acc[i] = acc[i];
			}

			if (min_acc[i] > acc[i])
			{
				min_acc[i] = acc[i];
			}
		}
	}
	else
	{
		for (int i = 0; i < 3; i++)
		{
			max_acc[i] = acc[i];
			min_acc[i] = acc[i];
		}
	}

}

bool Interaction::unzuptValid(int* max_acc, int* min_acc)
{
	if (max_acc[0] - min_acc[0] > 1024 || max_acc[1] - min_acc[1] > 1024 || max_acc[2] - min_acc[2] > 1024)
	{
		return true;
	}

	return false;
}

void Interaction::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
	int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
	int jump, int down, int rssi)
{
	int interative_cmd = -1;

	float left_acc_f[3];
	float right_acc_f[3];

	for (int i = 0; i < 3; i++)
	{
		left_acc_f[i] = left_acc[i] / 2048.f;
		right_acc_f[i] = right_acc[i] / 2048.f;
	}

	if (foot_on_floor(time_stamp, left_Triggering_time, left_zupt, last_left_zupt, rssi, left_att[1] * 0.0001f, LEFT_FOOT) != -1
		&& unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
	{
		std::cout << "left interation" << endl;
		interative_cmd = BACK_LEFT;
	}

	if (foot_on_floor(time_stamp, right_Triggering_time, right_zupt, last_right_zupt, rssi, right_att[1] * 0.0001f, RIGHT_FOOT) != -1
		&& unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
	{
		std::cout << "right interation" << endl;
		interative_cmd = BACK_RIGHT;
	}

	if (enter_key_listen(left_acc_f, left_acc_x_buff, left_station_count, left_pitch_buff, left_att[1] * 0.0001f, left_angle_has_reset) != -1)
	{
		std::cout << "cancel key" << endl;
		interative_cmd = CANCLE_KEY;
	}

	if (enter_key_listen(right_acc_f, right_acc_x_buff, right_station_count, right_pitch_buff, right_att[1] * 0.0001f, right_angle_has_reset) != -1)
	{
		std::cout << "OK key" << endl;
		interative_cmd = ENTER_KEY; 
	}

	setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
	setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);

	cmd = interative_cmd;
}

int Interaction::get_interative_cmd()
{
	return cmd;
}