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

}

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

}


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

	
	package_time ++;
}



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 int16_t left_pos_offset[3];
	static int16_t right_pos_offset[3];
	
	float front_mag_left_norm = front_mag_left;
	
	float back_mag_left_norm = back_mag_left;
	
	float front_mag_right_norm = front_mag_right;
	
	float back_mag_right_norm = back_mag_right;
	
	
	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_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)
	{
		/*
		gpio_mt_run(300);
		*/
		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;
	
	jump_tag  = press_jump_detect(left_acc_z,right_acc_z, left_zupt, right_zupt, front_mag_left, front_mag_right);
	
	
	
	if (jump_tag)
	{
		/*
		gpio_mt_run(300);
		*/
		jump_send_count = 20;
	}
	
	//连续发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];
	
	//直接发送IMU数据过去
	if(CAL_MODE)
	{
		imu_package_to_pack(left_buff, left_len, right_buff, right_len);
	}
	else
	{
		//将双脚的数据读取出来
		
		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;
	
}