
#include <iostream>
#include<deque>

#include "pub.h"

#define RESULT_LEN 10

using namespace std;

struct shoes_data_cell {
	//鞋子传上来的时间戳
	int time_stamp;

	//鞋子当前的位置
	float pos_x;
	float pos_y;
	float pos_z;

	//鞋子的姿态
	float heading;
	float pitch;
	float roll;

	int zupt;

	int rssi;

};

/*
* 公用接口类， 代码复用抽取
*/
class PublicInterface
{
public :
	int result[RESULT_LEN]; //返回到手机命令数据

	float left_global_pos[3];//描绘游戏过程中全局位置
	float right_global_pos[3];

	int max_acc_unzupt_left[3];//记录空中时候加速度的最大值最小值
	int min_acc_unzupt_left[3];

	int max_acc_unzupt_right[3];
	int min_acc_unzupt_right[3];

	PublicInterface() {
		memset(result, 0, RESULT_LEN * sizeof(int));

		memset(left_global_pos, 0, 3 * sizeof(float));

		memset(right_global_pos, 0, 3 * sizeof(float));
	};

	void getResult(int* dec); //返回到手机命令数据接口

	void setData(deque<shoes_data_cell>& shoes_data_vector, int time_stamp, float pos_x, float pos_y, float pos_z,
		float heading, float pitch, float roll, int zupt, int rssi);//缓存空中的数据到shoes_data_vector中， 头尾都触地时候的数据

	/*
	* 计算全局位置的普通函数， 利用鞋子传上来的空中数据shoes_data_vector，换算为旋转过后的空中数据
	* main_heading， 将传上来的方向旋转至鞋子的朝向
	* extra_heading， 将当前鞋子朝向的轨迹数据，额外再做旋转，一般旋转大概估计身体朝向的位置
	*/
	void calGlobalPos(deque<shoes_data_cell>& shoes_data_vector, deque<shoes_data_cell>& step_data_vector, float* global_pos, 
		float main_heading, float extra_heading, int LEFT_OR_RIGHT);

	/*
	* 获取全局位置的普通函数, 用于python脚本分析bug时候，查看游戏内的全局位置有没有符合期望。
	*/
	float getGamePos(int left_or_right, int index);


	/*
	* 寻找非触地时候的最大加速度和最小加速度
	*/
	void setPolarAccUnzupt(int* max_acc, int* min_acc, int* acc, int zupt);

	/*
	* 判断空中轨迹中加速度的最大值、最小债
	*/
	bool unzuptValid(int* max_acc, int* min_acc);

	/*
	* 根据小于rssi_thresh触地信号的个数累计到zupt_count，估计当前鞋子的方向以及将global_pos设置为0
	*/
	void resetZerosPointByRssi(int zupt, int zupt_count_thresh, int& zupt_count, int& has_init, float heading, float& rotate_heading, int rssi, int rssi_thresh,  float* global_pos);

};