

#include "BuildHouse.h"

bool isUpDeque(deque<dualPressData>& dual_press,int &min_index, int deque_size)
{
	if (dual_press.size() != deque_size)
	{
		return false;
	}

	int max_left_val = dual_press.front().left_press;
	int max_right_val = dual_press.front().right_press;

	int max_left_index = 0;
	int max_right_index = 0;

	int min_left_val = dual_press.front().left_press;
	int min_right_val = dual_press.front().right_press;

	int min_left_index = 0;
	int min_right_index = 0;


	for (int i = 0; i < dual_press.size(); i++)
	{
		if (max_left_val < dual_press[i].left_press)
		{
			max_left_val = dual_press[i].left_press;
			max_left_index = i;
		}
		if (max_right_val < dual_press[i].right_press)
		{
			max_right_val = dual_press[i].right_press;
			max_right_index = i;
		}

		if (min_left_val > dual_press[i].left_press)
		{
			min_left_val = dual_press[i].left_press;
			min_left_index = i;
		}
		if (min_right_val > dual_press[i].right_press)
		{
			min_right_val = dual_press[i].right_press;
			min_right_index = i;
		}

	}

	if (max_left_val - min_left_val > 50 && max_left_index > min_left_index &&
		max_right_val - min_right_val > 50 && max_right_index > min_right_index)
	{
		min_index = max(min_left_index, min_right_index);

		return true;
	}

	return false;
}

bool isValidAcc(deque<dualAccData>& dual_acc, int start_index,  int thresh_hold)
{
	int left_max_val = dual_acc[start_index].left_acc;
	int left_min_val = dual_acc[start_index].left_acc;

	int right_max_val = dual_acc[start_index].right_acc;
	int right_min_val = dual_acc[start_index].right_acc;

	for (int i = start_index; i < dual_acc.size(); i++)
	{
		if (dual_acc[i].left_acc > left_max_val)
		{
			left_max_val = dual_acc[i].left_acc;
		}
		if (dual_acc[i].right_acc > right_max_val)
		{
			right_max_val = dual_acc[i].right_acc;
		}

		if (dual_acc[i].left_acc < left_min_val)
		{
			left_min_val = dual_acc[i].left_acc;
		}
		if (dual_acc[i].right_acc < right_min_val)
		{
			right_min_val = dual_acc[i].right_acc;
		}
		
	}

	if (left_max_val - left_min_val < thresh_hold && right_max_val - right_min_val < thresh_hold)
	{
		return true;
	}

	return false;
}

void setUptrendDeque(deque<dualPressData> &dual_press_front, deque<dualPressData> & front_press,int deque_size)
{
	if (dual_press_front.size() == deque_size)
	{
		int min_index = 0;

		//if (isUpDeque(dual_press_front, min_index) && isValidAcc(dual_acc))
		if (isUpDeque(dual_press_front, min_index, deque_size))
		{
			//缓存上升区域
			if (front_press.size() == 0)
			{

				for (int i = min_index; i < dual_press_front.size(); i++)
				{
					front_press.push_back(dual_press_front[i]);

				}
			}
			else
			{
				front_press.push_back(dual_press_front.back());

			}

		}
		else
		{
			front_press.clear();
		}
	}
}

void setUptrendDequeFront(deque<dualPressData>& dual_press_front, deque<dualPressData>& front_press_left, deque<dualPressData>& front_press_right, int deque_size)
{
	if (dual_press_front.size() == deque_size)
	{
		int left_min_index = 0, left_max_index = 0;
		int right_min_index = 0, right_max_index = 0;

		int left_min_press = dual_press_front.front().left_press;
		int right_min_press = dual_press_front.front().right_press;
		
		int left_max_press = left_min_press, right_max_press = right_min_press;
		int i = 0;
		std::for_each(dual_press_front.begin(), dual_press_front.end(), [&](dualPressData value) {
			if (left_min_press > value.left_press) {
				left_min_index = i; left_min_press = value.left_press;
			}
			if (right_min_press > value.right_press) {
				right_min_index = i; right_min_press = value.right_press;
			}
			if (left_max_press < value.left_press) {
				left_max_index = i; left_max_press = value.left_press;
			}
			if (right_max_press < value.right_press) {
				right_max_index = i; right_max_press = value.right_press;
			}
			i++;
			});

		if (right_max_index > right_min_index && right_max_press > right_min_press)
		{
			//缓存上升区域
			if (front_press_right.size() == 0)
			{
				for (int i = right_min_index; i < dual_press_front.size(); i++)
				{
					front_press_right.push_back(dual_press_front[i]);
				}
			}
			else
			{
				front_press_right.push_back(dual_press_front.back());
			}

		}
		else
		{
			front_press_right.clear();
		}


		if (left_max_index > left_min_index && left_max_press > left_min_press)
		{
			//缓存上升区域
			if (front_press_left.size() == 0)
			{
				for (int i = left_min_index; i < dual_press_front.size(); i++)
				{
					front_press_left.push_back(dual_press_front[i]);
				}
			}
			else
			{
				front_press_left.push_back(dual_press_front.back());
			}

		}
		else
		{
			front_press_left.clear();
		}
	}
}

void BuildHouse::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 sdk_down = 0;
	int front_down = 0;

	result[0] = 0;
	//处理队列的窗口

	dual_press_front.push_back({ left_front_press , right_front_press , time_stamp });
	dual_press_back.push_back({ left_back_press, right_back_press ,time_stamp });
	
	//加速度窗口
	dual_acc.push_back({ left_acc[2], right_acc[2]});

	if (dual_press_front.size() > 10)
	{
		dual_press_front.pop_front();
	}

	if (dual_press_back.size() > 10)
	{
		dual_press_back.pop_front();
	}

	if (dual_acc.size() > 10)
	{
		dual_acc.pop_front();
	}

	//setUptrendDeque(dual_press_front, front_press, 10);

	setUptrendDequeFront(dual_press_front, front_press_left, front_press_right, 10);
	
	setUptrendDeque(dual_press_back, back_press, 10);

	if (front_press_left.size() > 1 && front_press_right.size() > 1)
	{
		int left_dist = front_press_left.back().left_press - front_press_left.front().left_press;

		int right_dist = front_press_right.back().right_press - front_press_right.front().right_press;

		if ( left_dist > 1500 && right_dist > 1500 
			&& dual_press_front[9].left_press > dual_press_front[8].left_press + 50
			&& dual_press_front[9].right_press > dual_press_front[8].right_press + 50)
		{
			front_down = 1;
		}

	}

	if (back_press.size() > 1)
	{
		int left_dist = back_press.back().left_press - back_press.front().left_press;

		int right_dist = back_press.back().right_press - back_press.front().right_press;

		if(left_dist > 1000 &&  right_dist > 1000 && back_press.back().time - back_press.front().time > 20)
		
		//if (left_dist > 750 && right_dist > 750)
		{
			sdk_down = 1;
		}

	}


	if (refuse_down_motion_time > 0)
	{
		refuse_down_motion_time--;
	}


	if ((sdk_down == 1 && last_down == 0 && cmd_wait_time == 0 && front_down_cmd_wait_time == 0)
		|| (front_down == 1 && last_front_down == 0 && front_down_cmd_wait_time == 0))
	//if (sdk_down == 1 && last_down == 0)
	{
		cmd_wait_time = 60;

		if (front_down == 1 && last_front_down == 0)
		{
			front_down_cmd_wait_time = 40;
		}

		result[0] = MOTION_DOWN;

		addMotionCount(DOWN_COUNT);
	}

	if (cmd_wait_time > 0) cmd_wait_time--;

	if (front_down_cmd_wait_time > 0)  front_down_cmd_wait_time--;

	last_down = sdk_down;

	last_front_down = front_down;
	
}