#include "CutCake.h"

CutCake::CutCake()
{
	jump_valid_time = 0;
	jump_calm_down = 0;

	last_jump = 0;
}


void CutCake::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)
{

	result[0] = -1;

	if (left_acc_deque.size() > 10)
	{
		left_acc_deque.pop_front();
		right_acc_deque.pop_front();
	}

	left_acc_deque.push_back(left_acc[2]);
	right_acc_deque.push_back(right_acc[2]);

	int max_left_acc = *(std::max_element(left_acc_deque.begin(), left_acc_deque.end()));
	int max_right_acc = *(std::max_element(right_acc_deque.begin(), right_acc_deque.end()));


	if (jump_calm_down == 0 && max_left_acc - left_acc[2] > 2048 && max_right_acc - right_acc[2] > 2048
		&& max_left_acc > 3096 && max_right_acc > 3096 && left_acc[2] < 1024 && right_acc[2] < 1024 &&
		left_acc_deque.front() > 1500 && right_acc_deque.front() > 1500)
	{

		jump = 1;
		//result[0] = MOTION_JUMP;
		left_acc_deque.clear();
		right_acc_deque.clear();

		jump_calm_down = 15;
	}


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

	if (last_jump == 0 && jump == 1)
	{
		result[0] = MOTION_JUMP;
		addMotionCount(JUMP_COUNT);
	}

	last_jump = jump;


}

