#include "PublicSDKMotion.h"

#define PRESS_WINDOW_SIZE 10

#define RSSI_LEN 10


struct origin_pos_struct {
	int step_count;

	float original_pos_x;

	float original_pos_y;
};

enum DANCE_GAME_POINT {
	ZERO_POINT = 0,
	RIGHT_UP_POINT = 1,
	LEFT_UP_POINT = 2,
	RIGHT_DOWN_POINT = 3,
	LEFT_DOWN_POINT = 4,

};

//缓存一定时间的rssi，中间缓存一步间的轨迹数据，最后再继续缓存rssi数据
struct shoes_data_rssi_struct
{
	int rssi_count;

	deque<shoes_data_cell> step_data_deque;
};




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

	void set_shoes_data_with_rssi(deque<shoes_data_cell>& step_data_vector, deque<shoes_data_cell>& shoes_data_rssi_vector);

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

private:

	deque<shoes_data_cell> left_shoes_data_vector;
	deque<shoes_data_cell> right_shoes_data_vector;

	deque<globalPos_struct> right_global_pos_vector;
	deque<globalPos_struct> left_global_pos_vector;

	deque<shoes_data_rssi_struct> left_shoes_data_rssi_vector;
	deque<shoes_data_rssi_struct> right_shoes_data_rssi_vector;


	float left_step_heading;
	float right_step_heading;


	deque<shoes_data_cell> left_step_data_vector;
	deque<shoes_data_cell> right_step_data_vector;

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

	int last_left_zupt;
	int last_right_zupt;

	float left_heading;
	float right_heading;

	int left_foot_has_init;
	int right_foot_has_init;

	int left_zupt_count;
	int right_zupt_count;

	int right_cmd_status;
	int left_cmd_status;

	int last_right_cmd_status;
	int last_left_cmd_status;

	int right_cmd_cur_status;
	int left_cmd_cur_status;

	const int rssi_threshold = 32;

	int right_cmd_wait_time;
	int left_cmd_wait_time;

	int zero_point_wait_time;


};