#include "SingleFootAction.h"


void SingleFootAction::run(uint32_t _time, int x, int y, int z,int zupt, int rssi, float pitch, float *acc)
{
	//std::cout << "pitch : " << pitch <<endl;

	/*
	int enter_signal_result = -1;
	int direction_signal_result = -1;

	can_listen_enter_signal(acc, pitch);

	int enter_signal_tmp = enter_signal_listen(acc, pitch);

	if (last_enter_signal != enter_signal_tmp && enter_signal_tmp > 0 && enter_signal_cooling == 0)
	{
		enter_signal = enter_signal_tmp;

		enter_signal_result = ENTER_KEY;
		
		enter_signal_cooling = 20;

		std::cout << "输出垫脚信号 : " << enter_signal << endl;
	}

	if (enter_signal_tmp > 0)
	{
		enter_signal_cooling = 20;
	}

	last_enter_signal = enter_signal_tmp;
	

	can_listen_direction_signal(rssi);
	
	direction_signal = direction_signal_listen(last_x, last_y, zupt, rssi);

	if (direction_signal > 0 && can_report_motion == 1)
	{
		can_report_motion = 0;

		if (pitch < 0.78f)
		{
			direction_signal_result = direction_signal;
		}
		std::cout << "direction_signal : " << direction_signal << endl;
	}
	*/

	direction_signal_listen_new(_time, last_x, last_y, zupt, rssi, pitch);

	int zupt_trigger = 0;

	if (last_zupt == 0 && zupt == 1)
	{
		zupt_trigger = 1;
	}

	enter_signal_listen_new(_time, acc, pitch, zupt_trigger);

	last_zupt = zupt;

	last_x = x; last_y = y; last_z = z;
	/*
	if (enter_signal_cooling > 0)
	{
		enter_signal_cooling--;
	}

	if (enter_signal_result != -1 )
	{
		if (footActionState.FootState == ENTER_KEY)
		{
			footActionState.StateCount ++;
			footActionState.Triggering_time = _time;
		}
		else
		{
			footActionState.FootState = ENTER_KEY;
			footActionState.StateCount = 1;
			footActionState.Triggering_time = _time;
		}
	}

	if (zupt == 1)
	{
		footActionState.Triggering_time = _time;
	}
	*/
}

void SingleFootAction::reset_interation_state()
{
	footActionState.FootState = 0;
	footActionState.StateCount = 0;
	footActionState.Triggering_time = 0;
}

float cal_acc_var(float* acc, int length)
{
	//计算均值
	float mean_val = 0;

	for (int i = 0; i < length; i++)
	{
		mean_val += acc[i];
	}
	
	mean_val /= length;

	//计算总体方差

	float var_val = 0;
	for (int i = 0; i < length; i++)
	{
		var_val += ((acc[i] - mean_val) * (acc[i] - mean_val));
	}

	return var_val / length;

}

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.2f)
	{
		return 1;
	}

	return 0;
}

int SingleFootAction::enter_signal_listen_new(uint32_t _time, float* acc, float pitch, int zupt_trigger)
{
	memcpy(acc_x_buff, acc_x_buff + 1, 9 * sizeof(float));

	acc_x_buff[9] = acc[0];

	//计算加速度的总体方差（n）

	float acc_var = cal_acc_var(acc_x_buff, 10);

	acc_x_max = acc_x_max > acc[0] ? acc_x_max : acc[0];
	acc_x_min = acc_x_min < acc[0] ? acc_x_min : acc[0];

	if(acc_is_valid(acc_x_buff, 10))
	{
		station_count ++;
	}
	else
	{
		station_count = 0;
	}
	
	//x轴速度剧烈抖动，而且俯仰角大于45度，则证明是踮脚
	if (station_count > 20 && pitch > 0.6f && acc_x_max  - acc_x_min > 1.0f)
	{
		//can_report_enter_signal = 0;
		//std::cout << "抖动状态下判断脚掂起来" << endl;
		acc_x_max = acc[0];
		acc_x_min = acc[0];

		if (footActionState.FootState != ENTER_KEY)
		{
			std::cout << "抖动状态下判断脚掂起来" << endl;
			footActionState.FootState = ENTER_KEY;
			footActionState.StateCount = 1;

		}
		else
		{
			if (_time - footActionState.Triggering_time > 10)
			{
				//std::cout << "抖动状态下判断脚掂起来" << endl;
				footActionState.StateCount ++ ;

				std::cout << "test" << endl;
			}
		}

		footActionState.Triggering_time = _time;
	}
	
	return -1;
}

int SingleFootAction::enter_signal_listen(float *acc, float pitch)
{	
	memcpy(acc_x_buff, acc_x_buff + 1, 9 * sizeof(float));

	acc_x_buff[9] = acc[0];

    //计算加速度的总体方差（n）

	float acc_var = cal_acc_var(acc_x_buff, 10);

	
	//x轴速度剧烈抖动，而且俯仰角大于45度，则证明是踮脚
	if (acc_var > 1.0f && pitch > 0.78f)
	{
		can_report_enter_signal = 0;
		//std::cout << "抖动状态下判断脚掂起来" << endl;
		return 1;
	}

	//如果抖动检测不到，转入平稳状态， 判断俯仰角持续两秒

	float acc_norm = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);

	//std::cout << "acc_norm : " << acc[0] << ", " << acc[1] << ", " << acc[2] << endl;

	if (acc_norm < 1.1f && acc_norm > 0.9f && pitch > 0.78f)
	{
		big_pitch_count++;
	}
	else
	{
		big_pitch_count = 0;
	}

	if (big_pitch_count > 90 && can_report_enter_signal)
	{
		can_report_enter_signal = 0;
		big_pitch_count = 0;

		std::cout << "平稳状态下判断脚掂起来" << endl;

		return 1;
	}

	return -1;

}

int SingleFootAction::can_listen_enter_signal(float* acc, float pitch)
{
	if (sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]) > 1.5f || fabsf(pitch) < 0.2f)
	{
		can_report_enter_signal = 1;
	}

	return 0;
}

FootActionState SingleFootAction::get_interation_state()
{
	return footActionState;
}

int  SingleFootAction::direction_signal_listen_new(uint32_t _time, int x, int y, int zupt, int rssi, float pitch)
{
	float x_offset = x;
	float y_offset = y;

	if (pitch > 0.5f)
	{
		return -1;
	}


	if (last_zupt == 0 && zupt == 1)
	{
		if (_time - footActionState.Triggering_time > 10)
		{
			if (left_or_right == LEFT_FOOT)
			{
				footActionState.FootState = BACK_LEFT;
			}
			else
			{
				footActionState.FootState = BACK_RIGHT;
			}

			footActionState.StateCount++;
		}

	}

	if (zupt)
	{
		footActionState.Triggering_time = _time;
	}

	return -1;
}

/*
int  SingleFootAction::direction_signal_listen_new(uint32_t _time, int x, int y, int zupt, int rssi, float pitch)
{
	float x_offset = x;
	float y_offset = y;

	if (pitch > 0.78f)
	{
		return -1;
	}


	if ( last_zupt == 0 && zupt == 1)
	{
		if (_time - footActionState.FootState < 10)
		{
			return -1;
		}
		//触发判断大概的位置, 短距离通过RSSI判断
		if (rssi < 30)
		{
			//先判断当前状态是什么状态，再判断次数
			std::cout << "rssi 小于阈值" << endl;

			if (footActionState.FootState == 0 || (footActionState.FootState != BACK_LEFT && footActionState.FootState != BACK_RIGHT))
			{
				if (left_or_right == LEFT_FOOT)
				{
					footActionState.FootState = BACK_LEFT;
				}
				else
				{
					footActionState.FootState = BACK_RIGHT;
				}
				footActionState.StateCount = 1;

				footActionState.Triggering_time = _time;

			}

			else
			{
				
				footActionState.StateCount  ++;

				footActionState.Triggering_time = _time;
			}
			

		}
		//长距离移动判断， 通过IMU算出来的距离来判断
		else if (sqrt(x_offset * x_offset + y_offset * y_offset) > 10.f)
		{

			if (left_or_right == LEFT_FOOT)
			{
				x_offset -= 5;
			}
			else
			{
				x_offset += 5;
			}

			float angle = atan2(y_offset, x_offset);

			std::cout << "距离 大于阈值" << endl;

			if (left_or_right == LEFT_FOOT && angle > PI / 2.0 && angle < PI)
			{
				if (footActionState.FootState == FRONT_LEFT)
				{
					footActionState.StateCount++;

					footActionState.Triggering_time = _time;
				}
				else
				{
					footActionState.FootState = FRONT_LEFT;

					footActionState.StateCount = 1;

					footActionState.Triggering_time = _time;
				}
			}
			else if (left_or_right == RIGHT_FOOT && angle < PI / 2.0 && angle >0.0f)
			{
				if (footActionState.FootState == FRONT_RIGHT)
				{
					footActionState.StateCount++;

					footActionState.Triggering_time = _time;
				}
				else
				{
					footActionState.FootState = FRONT_RIGHT;

					footActionState.StateCount = 1;

					footActionState.Triggering_time = _time;
				}
			}

		}

		else 
		{
			std::cout << "距离、rssi 不符合阈值" << endl;
			//rssi > 35 && length < 0.15
			footActionState.StateCount++;

			footActionState.Triggering_time = _time;
		}
	}

	return -1;
}
*/

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

	if (left_or_right == LEFT_FOOT)
	{
		x_offset -= 5;
	}
	else
	{
		y_offset += 5;
	}

	//当触地、双脚靠拢的时候，判断归位，触发位置的判断
	if (zupt == 1 && rssi < 35)
	{
		has_reset_signal = 1;
	}

	if (has_reset_signal == 1 && last_zupt == 0 && zupt == 1)
	{
		//触发判断大概的位置
		if (rssi < 30 && on_floor_count == 0)
		{
			//短距离通过RSSI判断
			if (left_or_right == LEFT_FOOT)
			{
				cmd_stage = BACK_LEFT;
			}
			else
			{
				cmd_stage = BACK_RIGHT;
			}

			time_s = clock();

			on_floor_count = 1;

			return -1;
			
		}
		//长距离移动判断， 通过IMU算出来的距离来判断
		else if (sqrt(x_offset * x_offset + y_offset * y_offset) > 15.f)
		{
			float angle = atan2(y_offset, x_offset);

			if (left_or_right == LEFT_FOOT && angle > PI / 2.0 && angle < PI)
			{
				cmd_stage = FRONT_LEFT;
				on_floor_count = 1;
				time_s = clock();

				return -1;
			}
			else if (left_or_right == RIGHT_FOOT && angle < PI / 2.0 && angle >0.0f)
			{
				cmd_stage = FRONT_RIGHT;
				on_floor_count = 1;
				time_s = clock();

				return -1;
			}
		}
		
		/*
		//第一次触地，触发命令类别的判断
		if (on_floor_count == 0)
		{
			on_floor_count ++;

			

			//记录触发的时间，用以确认的时候，判断是不是在有效时间里面
			if (cmd_stage == FRONT_LEFT || cmd_stage == FRONT_RIGHT || cmd_stage == BACK_LEFT || cmd_stage == BACK_RIGHT)
			{
				time_s = clock();
			}

			return -1;
		}
		*/
		else if (on_floor_count == 1)
		{
			//已经确认了位置

			//先判断是否在有效时间内
			int result = -1;

			double time_interval = (double)(clock() - time_s) / CLOCKS_PER_SEC;
			
			if (time_interval > 1.5)
			{
				std::cout << "debug : 两步之间超过限制 -- " << endl;
			}

			if (time_interval < 0.06)
			{
				std::cout << "抹去0.06s的噪声" << endl;
				
				return result;
			}

			if (time_interval < 1.5)
			{

				if ((cmd_stage == FRONT_LEFT || cmd_stage == FRONT_RIGHT) && sqrt(x_offset * x_offset + y_offset * y_offset) < 15.f)
				{
					result =  cmd_stage;
				}
				else if ((cmd_stage == BACK_LEFT || cmd_stage == BACK_RIGHT) && rssi < 35)
				{
					result = cmd_stage;
				}
				else
				{
					std::cout << "出现其他问题了" << std::endl;
				}

			}

			on_floor_count = 0;
			has_reset_signal = 0;
			cmd_stage = 0;

			return result;
		}
	}

	return -1;
}
void SingleFootAction::can_listen_direction_signal(int rssi)
{
	if (rssi < 30)
	{
		can_report_motion = 1;
	}
}