
#include "online_motion.h"

int online_motion::online_detect_down(int right_front_mag, int left_front_mag, int right_back_mag, int  left_back_mag,
	int* right_acc, int* left_acc)
{
	if (right_back_mag_queue.size() > 20)
	{
		right_back_mag_queue.pop_front();
		left_back_mag_queue.pop_front();

		right_front_mag_queue.pop_front();
		left_front_mag_queue.pop_front();
	}
	right_back_mag_queue.push_back(right_back_mag);
	left_back_mag_queue.push_back(left_back_mag);

	right_front_mag_queue.push_back(right_front_mag);
	left_front_mag_queue.push_back(left_front_mag);

	deque<int> right_back_temp{ right_back_mag };
	deque<int> left_back_temp{ left_back_mag };

	deque<int> right_front_temp{ right_front_mag };
	deque<int> left_front_temp{ left_front_mag };

	for (int i = right_back_mag_queue.size() - 2; i >= 0; i--)
	{
		if ((right_back_mag_queue[i] > right_back_temp.front() || left_back_mag_queue[i] > left_back_temp.front())
			&& i > right_back_mag_queue.size() / 2 - 1)
		{
			break;
		}

		if (right_back_mag_queue[i] < right_back_temp.back())
		{
			right_back_temp.push_back(right_back_mag_queue[i]);
		}

		if (left_back_mag_queue[i] < left_back_temp.back())
		{
			left_back_temp.push_back(left_back_mag_queue[i]);
		}
	}

	for (int i = right_back_mag_queue.size() - 2; i >= 0; i--)
	{

		if (right_front_mag_queue[i] > right_front_temp.front() || left_front_mag_queue[i] > left_front_temp.front())
		{
			break;
		}

		if (right_front_mag_queue[i + 1] > right_front_mag_queue[i] - 50 && left_front_mag_queue[i + 1] > left_front_mag_queue[i] - 50)
		{
			right_front_temp.push_back(right_front_mag_queue[i]);
			left_front_temp.push_back(left_front_mag_queue[i]);
		}

	}

	//if (left_back_temp.size() > 5 && right_back_temp.size() > 5
	//	&& left_back_temp.front() > left_back_temp.back() + 2000
	//	&& right_back_temp.front() > right_back_temp.back() + 2000)
	if ( left_back_temp.front() > left_back_temp.back() + 1500
		&& right_back_temp.front() > right_back_temp.back() + 1500)
	{
		return 1;
	}

	if (left_back_temp.front() > left_back_temp.back() + 750 && right_back_temp.front() > right_back_temp.back() + 750
		&& left_front_temp.front() > left_front_temp.back() + 750 && right_front_temp.front() > right_front_temp.back() + 750)
	{
		return 1;
	}

	if (left_back_temp.front() > left_back_temp.back() + 500 && right_back_temp.front() > right_back_temp.back() + 500
		&& left_front_temp.front() > left_front_temp.back() + 1000 && right_front_temp.front() > right_front_temp.back() + 1000)
	{
		return 1;
	}

	if (left_front_temp.front() > left_front_temp.back() + 2000 && right_front_temp.front() > right_front_temp.back() + 2000)
	{
		return 1;
	}


	return 0;
}