#include "PublicSDKMotion.h"


struct acc_pos_struct
{
	float pos_x;
	float pos_y;
	float pos_z;

	float acc_xy_norm;

	int zupt;
};

struct double_press_struct
{
	int front_press;
	int back_press;

	int up_flag;
	int get_flag;
};

class TestGame : public PublicSDKMotion
{
public:
	TestGame();

	void setData(deque<acc_pos_struct>& shoes_data_vector,  float pos_x, float pos_y, float pos_z, float acc_x, float acc_y, float acc_z, float heading, float pitch, float roll, int zupt);

	void calGlobalPos(deque<shoes_data_cell>& shoes_data_vector, deque<shoes_data_cell>& step_data_vector,
		float* global_pos, int LEFT_OR_RIGHT);

	//void ProcessResult(int zupt, int& zupt_count, int& has_init, float heading,
	//	float& rotate_heading, float* global_pos, deque<shoes_data_cell>& step_data_vector,
	//	deque<globalPos_struct>& global_pos_vector, int LEFT_OR_RIGHT);

	virtual void 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);


	virtual void getResult(int* dec);

	virtual float getGamePos(int left_or_right, int index);

private:
	deque<acc_pos_struct> left_shoes_data_vector;
	deque<acc_pos_struct> right_shoes_data_vector;

	float left_step_heading;
	float right_step_heading;

	float left_global_pos[3];
	float right_global_pos[3];

	float left_cmd_global_pos[3];
	float right_cmd_global_pos[3];

	deque<shoes_data_cell> left_step_data_vector;
	deque<shoes_data_cell> right_step_data_vector;
	
	deque<double_press_struct> left_press_vector;
	deque<double_press_struct> right_press_vector;

	float left_step_pos[3];
	float right_step_pos[3];

	int last_left_zupt;
	int last_right_zupt;

	int left_step;
	int right_step;

	int left_step_length;
	int right_step_length;

	int result[4];

};