#include "PublicSDKMotion.h"

#define RSSI_LEN 5

struct motion_rssi {

	int rssi;
	int motion;
};


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

	void ProcessResult(float* global_pos, deque<shoes_data_cell>& step_data_vector,
		deque<globalPos_struct>& global_pos_vector, int LEFT_OR_RIGHT);

	int ProcessDualStepVector(deque<shoes_data_cell>& left_cmd_step_data_vector, deque<shoes_data_cell> &right_cmd_step_data_vector,
		float* left_cmd_global_pos, float* right_cmd_global_pos , int rssi);

	bool unzuptValid(int* max_acc, int* min_acc);

	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<int> rssi_vec;
	deque<int> pitch_vec;
	deque<int> roll_vec;
	deque<int> left_acc_vec;
	deque<int> right_acc_vec;

	int pitch_reference_val = 0;
	int roll_reference_val = 0;

	int has_init = 0;


	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;


	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<shoes_data_cell> left_cmd_step_data_vector;
	deque<shoes_data_cell> right_cmd_step_data_vector;

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

	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_wait_time;
	int left_cmd_wait_time;

	int left_init;
	int right_init;

	int left_zupt_extension_time = 0;
	int right_zupt_extension_time = 0;


	const int rssi_threshold = 30;


};