#include "special_motion.h"
#include "motion_config.h"

int down_send_count = 0;
int is_down = 0;

int jump_send_count = 0;
int is_jump = 0;

int down_res = 0;
int jump_res = 0;

int16_t left_time;
uint16_t left_press;
int32_t left_pos[3];
int16_t left_att[3];
int16_t left_acc[3];
int16_t left_gyr[3];
int16_t left_zupt;

int16_t right_time;
uint16_t right_press;
int32_t  right_pos[3];
int16_t  right_att[3];
int16_t  right_acc[3];
int16_t  right_gyr[3];
int16_t  right_zupt;

uint8_t rssi;

uint8_t right_rssi;
uint8_t left_rssi;

uint16_t right_front_mag;
uint16_t left_front_mag;

uint16_t right_back_mag;
uint16_t left_back_mag;


void decode_shoes_data_right(uint8_t* buff,uint8_t right_len)
{

	right_pos[0] = ((int32_t)buff[0] << 24) | ((int32_t)buff[1] << 16) | ((int32_t)buff[2] << 8) | ((int32_t)buff[3] << 0);
	right_pos[1] = ((int32_t)buff[4] << 24) | ((int32_t)buff[5] << 16) | ((int32_t)buff[6] << 8) | ((int32_t)buff[7] << 0);
	right_pos[2] = ((int32_t)buff[8] << 24) | ((int32_t)buff[9] << 16) | ((int32_t)buff[10] << 8) | ((int32_t)buff[11] << 0);
	
	right_att[0] = ((int16_t)buff[12] << 8) | ((int16_t)buff[13] << 0);
	right_att[1] = ((int16_t)buff[14] << 8) | ((int16_t)buff[15] << 0);
	right_att[2] = ((int16_t)buff[16] << 8) | ((int16_t)buff[17] << 0);
	
	right_front_mag = ((uint16_t)buff[18] << 8) | ((uint16_t)buff[19] << 0);
	
	right_back_mag  = ((uint16_t)buff[20] << 8) | ((uint16_t)buff[21] << 0);
	
	right_zupt = ((uint8_t)buff[22]);

	right_rssi	= (uint8_t) buff[23];
	
	right_time =  ((uint16_t)buff[24] << 8) | ((uint16_t)buff[25] << 0);
	
	right_acc[0]  = ((int16_t)buff[26] << 8) | ((int16_t)buff[27] << 0);
	right_acc[1]  = ((int16_t)buff[28] << 8) | ((int16_t)buff[29] << 0);
	right_acc[2]  = ((int16_t)buff[30] << 8) | ((int16_t)buff[31] << 0);
	
	right_gyr[0]  = ((int16_t)buff[32] << 8) | ((int16_t)buff[33] << 0);
	right_gyr[1]  = ((int16_t)buff[34] << 8) | ((int16_t)buff[35] << 0);
	right_gyr[2]  = ((int16_t)buff[36] << 8) | ((int16_t)buff[37] << 0);
}

void decode_shoes_data_left(uint8_t* buff,uint8_t left_len)
{
	left_pos[0] = ((int32_t)buff[0] << 24) | ((int32_t)buff[1] << 16) | ((int32_t)buff[2] << 8) | ((int32_t)buff[3] << 0);
	left_pos[1] = ((int32_t)buff[4] << 24) | ((int32_t)buff[5] << 16) | ((int32_t)buff[6] << 8) | ((int32_t)buff[7] << 0);
	left_pos[2] = ((int32_t)buff[8] << 24) | ((int32_t)buff[9] << 16) | ((int32_t)buff[10] << 8) | ((int32_t)buff[11] << 0);
	
	left_att[0] = ((int16_t)buff[12] << 8) | ((int16_t)buff[13] << 0);
	left_att[1] = ((int16_t)buff[14] << 8) | ((int16_t)buff[15] << 0);
	left_att[2] = ((int16_t)buff[16] << 8) | ((int16_t)buff[17] << 0);
	
	left_front_mag = ((uint16_t)buff[18] << 8) | ((uint16_t)buff[19] << 0);
	
	left_back_mag  = ((uint16_t)buff[20] << 8) | ((uint16_t)buff[21] << 0);
	
	left_zupt = ((uint8_t)buff[22]);

	left_rssi	= (uint8_t) buff[23];
	
	left_time =  ((uint16_t)buff[24] << 8) | ((uint16_t)buff[25] << 0);
	
	left_acc[0]  = ((int16_t)buff[26] << 8) | ((int16_t)buff[27] << 0);
	left_acc[1]  = ((int16_t)buff[28] << 8) | ((int16_t)buff[29] << 0);
	left_acc[2]  = ((int16_t)buff[30] << 8) | ((int16_t)buff[31] << 0);
	
	left_gyr[0]  = ((int16_t)buff[32] << 8) | ((int16_t)buff[33] << 0);
	left_gyr[1]  = ((int16_t)buff[34] << 8) | ((int16_t)buff[35] << 0);
	left_gyr[2]  = ((int16_t)buff[36] << 8) | ((int16_t)buff[37] << 0);
	
}


uint8_t buf[82];

uint8_t L=0;

void inter_package_data()
{
	
	static uint8_t package_time;
	
	L=0;
	
	buf[L++] = (uint8_t)(right_pos[0]>>24);
	buf[L++] = (uint8_t)(right_pos[0]>>16);
	buf[L++] = (uint8_t)(right_pos[0]>>8);
	buf[L++] = (uint8_t)(right_pos[0]>>0);
	
	buf[L++] = (uint8_t)(right_pos[1]>>24);
	buf[L++] = (uint8_t)(right_pos[1]>>16);
	buf[L++] = (uint8_t)(right_pos[1]>>8);
	buf[L++] = (uint8_t)(right_pos[1]>>0);
	
	buf[L++] = (uint8_t)(right_pos[2]>>24);
	buf[L++] = (uint8_t)(right_pos[2]>>16);
	buf[L++] = (uint8_t)(right_pos[2]>>8);
	buf[L++] = (uint8_t)(right_pos[2]>>0);
	
	
	buf[L++] = (uint8_t)(left_pos[0]>>24);
	buf[L++] = (uint8_t)(left_pos[0]>>16);
	buf[L++] = (uint8_t)(left_pos[0]>>8);
	buf[L++] = (uint8_t)(left_pos[0]>>0);
	
	buf[L++] = (uint8_t)(left_pos[1]>>24);
	buf[L++] = (uint8_t)(left_pos[1]>>16);
	buf[L++] = (uint8_t)(left_pos[1]>>8);
	buf[L++] = (uint8_t)(left_pos[1]>>0);
	
	buf[L++] = (uint8_t)(left_pos[2]>>24);
	buf[L++] = (uint8_t)(left_pos[2]>>16);
	buf[L++] = (uint8_t)(left_pos[2]>>8);
	buf[L++] = (uint8_t)(left_pos[2]>>0);
	
	buf[L++] = (uint8_t)(right_att[0]>>8);
	buf[L++] = (uint8_t)(right_att[0]>>0);
	
	buf[L++] = (uint8_t)(right_att[1]>>8);
	buf[L++] = (uint8_t)(right_att[1]>>0);
	
	buf[L++] = (uint8_t)(right_att[2]>>8);
	buf[L++] = (uint8_t)(right_att[2]>>0);
	
	buf[L++] = (uint8_t)(left_att[0]>>8);
	buf[L++] = (uint8_t)(left_att[0]>>0);
	
	buf[L++] = (uint8_t)(left_att[1]>>8);
	buf[L++] = (uint8_t)(left_att[1]>>0);
	
	buf[L++] = (uint8_t)(left_att[2]>>8);
	buf[L++] = (uint8_t)(left_att[2]>>0);
	
	buf[L++] = (uint8_t)(right_acc[0]>>8);
	buf[L++] = (uint8_t)(right_acc[0]>>0);
	
	buf[L++] = (uint8_t)(right_acc[1]>>8);
	buf[L++] = (uint8_t)(right_acc[1]>>0);
	
	buf[L++] = (uint8_t)(right_acc[2]>>8);
	buf[L++] = (uint8_t)(right_acc[2]>>0);
	
	buf[L++] = (uint8_t)(left_acc[0]>>8);
	buf[L++] = (uint8_t)(left_acc[0]>>0);
	
	buf[L++] = (uint8_t)(left_acc[1]>>8);
	buf[L++] = (uint8_t)(left_acc[1]>>0);
	
	buf[L++] = (uint8_t)(left_acc[2]>>8);
	buf[L++] = (uint8_t)(left_acc[2]>>0);

	
	int is_girl = 0;
	
	if(GIRL_SHOES)
			is_girl = 1;
	
	
	buf[L++] = (uint8_t)(is_girl * 16 + right_zupt * 8 + left_zupt * 4 + down_res * 2 + jump_res * 1);
	
	buf[L++] = (uint8_t)(left_rssi >> 0);
	
	buf[L++] = (uint8_t)(package_time);
	
	buf[L++] = (uint8_t)(right_front_mag>>8);
	buf[L++] = (uint8_t)(right_front_mag>>0);
	
	buf[L++] = (uint8_t)(right_back_mag>>8);
	buf[L++] = (uint8_t)(right_back_mag>>0);
	
	buf[L++] = (uint8_t)(left_front_mag>>8);
	buf[L++] = (uint8_t)(left_front_mag>>0);
	
	buf[L++] = (uint8_t)(left_back_mag>>8);
	buf[L++] = (uint8_t)(left_back_mag>>0);
	
	buf[L++] = (uint8_t)(right_gyr[0]>>8);
	buf[L++] = (uint8_t)(right_gyr[0]>>0);
	
	buf[L++] = (uint8_t)(right_gyr[1]>>8);
	buf[L++] = (uint8_t)(right_gyr[1]>>0);
	
	buf[L++] = (uint8_t)(right_gyr[2]>>8);
	buf[L++] = (uint8_t)(right_gyr[2]>>0);
	
	buf[L++] = (uint8_t)(left_gyr[0]>>8);
	buf[L++] = (uint8_t)(left_gyr[0]>>0);
	
	buf[L++] = (uint8_t)(left_gyr[1]>>8);
	buf[L++] = (uint8_t)(left_gyr[1]>>0);
	
	buf[L++] = (uint8_t)(left_gyr[2]>>8);
	buf[L++] = (uint8_t)(left_gyr[2]>>0);
	
	package_time ++;
}

//滑动滤波--均值滤波
uint16_t cal_mean_val(uint16_t *filter_window, int length)
{
	uint32_t filter_window_sum = 0;
		
	for(int i = 0; i < length; i++)
	{
		filter_window_sum += filter_window[i];
	}
	
	return filter_window_sum/length;
}


void special_motion(int time_stamp, uint16_t front_mag_left,
	uint16_t back_mag_left, uint16_t front_mag_right, uint16_t back_mag_right,
	int16_t h_pos[3], int16_t s_pos[3], int16_t *down, int16_t *jump)
{

	static int32_t last_left_pos[3];
	static int32_t last_right_pos[3];
	
	static int left_pos_offset[3];
	static int right_pos_offset[3];
	
	static int time_count = 0;
	
	static uint16_t front_mag_left_filter_window[5];
	static uint16_t back_mag_left_filter_window[5];
	
	static uint16_t front_mag_right_filter_window[5];
	static uint16_t back_mag_right_filter_window[5];
	
	static float front_mag_left_norm;
	
	static float back_mag_left_norm;
	
	static float front_mag_right_norm;
	
	static float back_mag_right_norm;
	
	//PU鞋垫数据通常都带有小抖动，现在需要滤波, 让数据更加好看
	
	memcpy(front_mag_left_filter_window, front_mag_left_filter_window + 1, 4 * sizeof(uint16_t));
	memcpy(back_mag_left_filter_window, back_mag_left_filter_window + 1, 4 * sizeof(uint16_t));
	
	memcpy(front_mag_right_filter_window, front_mag_right_filter_window + 1, 4 * sizeof(uint16_t));
	memcpy(back_mag_right_filter_window, back_mag_right_filter_window + 1, 4 * sizeof(uint16_t));
	
	front_mag_left_filter_window[4] = front_mag_left;
	back_mag_left_filter_window[4] = back_mag_left;
	
	front_mag_right_filter_window[4] = front_mag_right;
	back_mag_right_filter_window[4] = back_mag_right;
	
	if(time_count > 4)
	{
		front_mag_left_norm = cal_mean_val(front_mag_left_filter_window, 5);
		
		back_mag_left_norm =  cal_mean_val(back_mag_left_filter_window, 5);
		
		front_mag_right_norm  =  cal_mean_val(front_mag_right_filter_window, 5);
		
		back_mag_right_norm =  cal_mean_val(back_mag_right_filter_window, 5);
	}
	else
	{
		front_mag_left_norm = left_front_mag;
	
		back_mag_left_norm =  left_back_mag;
	
		front_mag_right_norm =  right_front_mag;
	
		back_mag_right_norm =  right_back_mag;
	}

	
	
	time_count ++;
	
	
	float left_acc_z = left_acc[2]/2048.f;
	
	float right_acc_z = right_acc[2]/2048.f;
	
	/*
		处理蹲的动作
	*/
	int front_down_tag = 0;
	int back_down_tag = 0;


	press_down_detect_new(time_stamp, front_mag_left_norm, back_mag_left_norm,
									front_mag_right_norm, back_mag_right_norm,left_zupt,right_zupt,
									left_acc[0]/2048.f, left_acc[1]/2048.f, left_acc[2]/2048.f, right_acc[0]/2048.f, right_acc[1]/2048.f, right_acc[2]/2048.f,
									&front_down_tag, &back_down_tag);

//	is_down = front_down_tag + back_down_tag * 2;
	
	if (front_down_tag || back_down_tag)
	//if (front_down_tag)
	{
		//连续发送30次防止丢包
		down_send_count = 20;
	}
	
	if(down_send_count > 0)
	{
		is_down = 1;
		down_send_count --;
	}
	else
	{
		is_down = 0;
	}
	
	/*
		处理跳的动作
	*/
	
	if(left_zupt)
	{
		memcpy(last_left_pos, left_pos, 3 * sizeof(int32_t));
	}
	
	if(right_zupt)
	{
		memcpy(last_right_pos, right_pos, 3 * sizeof(int32_t));
	}
	
	for(int i = 0; i < 3; i++)
	{
		left_pos_offset[i] = left_pos[i] - last_left_pos[i];
		right_pos_offset[i] = right_pos[i] - last_right_pos[i];
	}
	
	
	 short jump_tag  = 0;
	
//	 int normal_jump_tag  =  att_jump_detect(left_att[1] / 10000.f, right_att[1] / 10000.f, left_zupt, right_zupt);
//	
	 int unnormal_jump_tag = press_jump_detect( left_acc_z,  right_acc_z,  left_zupt,  right_zupt, (int) front_mag_left_norm, (int) front_mag_right_norm);
	
	 int pos_jump_tag = pos_jump_detect(left_pos_offset, right_pos_offset, left_zupt, right_zupt);
		
	 if(pos_jump_tag || unnormal_jump_tag)
	 {
		 jump_tag = 1;
	 }

	//jump_tag  = press_jump_detect(left_acc_z, right_acc_z, left_zupt, right_zupt, front_mag_left_norm, front_mag_right_norm, back_mag_left_norm, back_mag_right_norm);
	
	if (jump_tag)
	{
		/*
		gpio_mt_run(300);
		*/
		jump_send_count = 10;
	}
	
	//连续发20次蹲，终端再处理
	if(jump_send_count > 0)
	{
		is_jump = 1;
		jump_send_count --;
	}
	else
	{
		is_jump = 0;
	}
	
	

	
	down_res = is_down;
	
	jump_res  = is_jump;
	

}



//uint8_t buf[64];

//uint8_t L=0;

void imu_package_to_pack(uint8_t* left_buff, uint8_t left_len, uint8_t* right_buff, uint8_t right_len)
{
	static uint8_t package_time;
	
	int index = 0; 
	int left_index = 0;
	int right_index = 0;
	while(left_index < left_len)
	{
		buf[index ++] =  left_buff[left_index ++];
	}
	while(right_index < right_len)
	{
		buf[index ++] =  right_buff[right_index ++];
	}
	
	buf[index ++] = (uint8_t) is_down;
	buf[index ++] = (uint8_t) is_jump;
	buf[index ++] = (uint8_t) (package_time ++);

	
	L = index;
}

void integrate_left_right(uint8_t* left_buff,uint8_t left_len, uint8_t* right_buff, uint8_t right_len)
{
	static uint16_t time_stamp;
	
	static int16_t 	left_pos_offset[3];
	static int16_t	right_pos_offset[3];
	static int32_t 	last_left_pos[3];
	static int32_t 	last_right_pos[3];
	

	//将双脚的数据读取出来
	
	decode_shoes_data_right(right_buff, right_len);
	decode_shoes_data_left(left_buff,  left_len);
	
	
	//先处理跳蹲再发过去
	int16_t down = 0;
	int16_t jump = 0;
	
	//获取位置的偏移量再判断跳
	if(left_zupt == 1)
	{
		memcpy(last_left_pos, left_pos, 3 * sizeof(int32_t));
	}
	if(right_zupt == 1)
	{
		memcpy(last_right_pos, right_pos, 3 * sizeof(int32_t));
	}
	
	for(int i = 0; i < 3; i++)
	{
		left_pos_offset[i] = left_pos[i] - last_left_pos[i];
		right_pos_offset[i] = right_pos[i] - last_right_pos[i];
	}
	
	
	special_motion(time_stamp, left_front_mag,	left_back_mag, right_front_mag, right_back_mag,
	left_pos_offset,  right_pos_offset ,  &down,  &jump);
	
	
	inter_package_data();
	
	
}

//获取包裹的接口
void get_game_package(uint8_t* buff_address, uint8_t * buff_len)
{
	 memcpy(buff_address, buf, L);
	
	*buff_len = L;
	
}