#include "PublicSDKMotion.h"

#define PRESS_WINDOW_SIZE 10

struct vel_struct {
	int time_stamp;

	float vel;
};

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

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

	void calGlobalPos(deque<shoes_data_cell>& shoes_data_vector, deque<shoes_data_cell>& step_data_vector,
		float* global_pos, 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);

	int getStepVel();

	void  check_vel_vector(int time_stamp, deque<vel_struct>& vel_vector);
	float cal_mean_vel(int end_time, int start_time, float max_val);
	float get_vel_mean(int time_stamp, deque<vel_struct>& vel_vector, deque<shoes_data_cell>& step_data_vector);


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

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

	deque<vel_struct> left_vel_vector;
	deque<vel_struct> right_vel_vector;

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


	int left_jump_cancel_update_vel;
	int right_jump_cancel_update_vel;

	float step_vel;

	int both_zupt_count = 0;


};