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

int16_t left_time;
uint16_t left_press_front;
uint16_t left_press_back;

int32_t left_pos[3];
int16_t left_att[3];
int16_t left_acc[3];
int16_t left_zupt;

int16_t right_time;

uint16_t right_press_front;
uint16_t right_press_back;

int32_t  right_pos[3];
int16_t  right_att[3];
int16_t  right_acc[3];
int16_t  right_zupt;

uint8_t rssi;

uint8_t right_rssi;
uint8_t left_rssi;

uint16_t right_laser_distance;
uint16_t left_laser_distance;

void decode_shoes_data_right(uint8_t* buff,uint8_t right_len)
{
	right_laser_distance = ((uint16_t)buff[0] << 8) | ((uint16_t)buff[1] << 0);
	
	right_pos[0] = ((int32_t)buff[2] << 24) | ((int32_t)buff[3] << 16) | ((int32_t)buff[4] << 8) | ((int32_t)buff[5] << 0);
	right_pos[1] = ((int32_t)buff[6] << 24) | ((int32_t)buff[7] << 16) | ((int32_t)buff[8] << 8) | ((int32_t)buff[9] << 0);
	right_pos[2] = ((int32_t)buff[10] << 24) | ((int32_t)buff[11] << 16) | ((int32_t)buff[12] << 8) | ((int32_t)buff[13] << 0);
	
	right_att[0] = ((int16_t)buff[14] << 8) | ((int16_t)buff[15] << 0);
	right_att[1] = ((int16_t)buff[16] << 8) | ((int16_t)buff[17] << 0);
	right_att[2] = ((int16_t)buff[18] << 8) | ((int16_t)buff[19] << 0);
	
	right_zupt = ((uint8_t)buff[20]);
	
	right_acc[0]  = ((int16_t)buff[21] << 8) | ((int16_t)buff[22] << 0);
	right_acc[1]  = ((int16_t)buff[23] << 8) | ((int16_t)buff[24] << 0);
	right_acc[2]  = ((int16_t)buff[25] << 8) | ((int16_t)buff[26] << 0);
	
	right_rssi	= (uint8_t) buff[27];
	
	right_time =  ((uint16_t)buff[28] << 8) | ((uint16_t)buff[29] << 0);
	
	
	right_press_front = ((uint16_t)buff[30] << 8) | ((uint16_t)buff[31] << 0);
	right_press_back  = ((uint16_t)buff[32] << 8) | ((uint16_t)buff[33] << 0);

}

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

	left_rssi	= (uint8_t) buff[27];
	
	left_time = ((uint16_t)buff[28] << 8) | ((uint16_t)buff[29] << 0);
	
	left_press_front = ((uint16_t)buff[30] << 8) | ((uint16_t)buff[31] << 0);
	left_press_back = ((uint16_t)buff[32] << 8) | ((uint16_t)buff[33] << 0);

}

void special_motion(int time_stamp, uint16_t h_press, uint16_t s_press, int16_t h_pos[3], int16_t s_pos[3], 
										float left_acc_z, float right_acc_z, int16_t *down, int16_t *jump)
{
	
	/*
		处理蹲的动作
	*/
	short down_tag = 0;
	int front_down = 0;
	int back_down  = 0;
	if(0)
	{
		
		down_tag = press_down_detect_new(time_stamp, left_laser_distance, right_laser_distance, left_acc_z, right_acc_z, 
		(float)left_att[1]/10000, (float)right_att[1]/10000, rssi);
		
	}
	else
	{
		
		 press_down_detect_press(0, (float)left_press_front, (float)left_press_back,
									(float)right_press_front, (float)right_press_back, &front_down, &back_down);
		
		if(front_down == 1 || back_down == 1)
		{
			down_tag = 1;
		}
	}

	if (down_tag)
	{
		/*
		gpio_mt_run(300);
		*/
		down_send_count = 10;
	}
	
	if(down_send_count > 0)
	{
		is_down = 1;
		down_send_count --;
	}
	else
	{
		is_down = 0;
	}
	
	/*
		处理跳的动作
	*/
	
	short jump_tag  = 0;
	
	jump_tag  = press_jump_detect(left_acc_z, right_acc_z);
	
	
		/*
	*利用zupt来避免跳
	*/
	if(left_zupt || right_zupt)
	{
		jump_tag =0;
	}
	
	
	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 = is_down;
	*jump   = is_jump;
}



uint8_t buf[64];

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 + is_down * 2 + is_jump * 1);
	
	buf[L++] = (uint8_t)(rssi >> 0);
	
	buf[L++] = (uint8_t)(package_time);
	
	buf[L++] = (uint8_t)(right_press_front>>8);
	buf[L++] = (uint8_t)(right_press_front>>0);
	
	buf[L++] = (uint8_t)(right_press_back>>8);
	buf[L++] = (uint8_t)(right_press_back>>0);
	
	buf[L++] = (uint8_t)(left_press_front>>8);
	buf[L++] = (uint8_t)(left_press_front>>0);
	
	buf[L++] = (uint8_t)(left_press_back>>8);
	buf[L++] = (uint8_t)(left_press_back>>0);
	
	package_time ++;
}

void package_to_buff()
{
	inter_package_data();
}

void integrate_left_right(uint8_t* left_buff,uint8_t left_len, uint8_t* right_buff, uint8_t right_len)
{
	//把底层的数据解开
	if(LEFT_IS_HOST)
	{
		decode_shoes_data_right(right_buff, right_len);
		decode_shoes_data_left(left_buff, left_len);
	}
	else
	{
		decode_shoes_data_right(left_buff, left_len);
		decode_shoes_data_left(right_buff, right_len);
	}
	
		
	//整合左右脚的数据
	
	static int32_t last_pos_left[3];
	static int32_t last_pos_right[3];
	
	if(left_zupt)
	{
		memcpy(last_pos_left, left_pos, 3 * sizeof(int32_t));
	}
//	
	if(right_zupt)
	{
		memcpy(last_pos_right, right_pos, 3 * sizeof(int32_t));
	}
	
	int16_t left_pos_offset[3];
	int16_t right_pos_offset[3];
	
	for(int i = 0; i < 3 ; i ++)
	{
		left_pos_offset[i] = left_pos[i] - last_pos_left[i];
		right_pos_offset[i] = right_pos[i] - last_pos_right[i];
	}
	
	short *down = 0;
	short *jump = 0;
	
	rssi = left_rssi;
	
	special_motion(left_time, left_press_front,  right_press_front, left_pos_offset,  right_pos_offset, (float)left_acc[2]/2048.0f , (float)right_acc[2]/2048.0f, down, jump);
	
	//整合完，再组合成一包
	
	package_to_buff();
	
}

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