#include "process_result.h"


int time_stamp = 0;

float gyr[3];
float acc[3];
int16_t h_pos[3];
int16_t h_att[3];
int16_t h_zupt;
int16_t jump_send_count;
int16_t down_send_count;

int16_t is_jump;
int16_t is_down;

extern int16_t s_pos[3];
extern int16_t s_att[3];
extern int8_t s_rssi;
extern int16_t s_zupt;

int left_press_buff[4];

int32_t left_press = 0;
int32_t right_press = 0;

void sen_data_acc(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(accel[0]>>8);
	buf[L++] = (uint8_t)(accel[0]>>0);
	
	buf[L++] = (uint8_t)(accel[1]>>8);
	buf[L++] = (uint8_t)(accel[1]>>0);
	
	buf[L++] = (uint8_t)(accel[2]>>8);
	buf[L++] = (uint8_t)(accel[2]>>0);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}

void sen_data_pos(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	static short count = 0;
//	buf[L++] = (uint8_t)(h_pos[0]>>8);
//	buf[L++] = (uint8_t)(h_pos[0]>>0);
//	
//	buf[L++] = (uint8_t)(h_pos[1]>>8);
//	buf[L++] = (uint8_t)(h_pos[1]>>0);
	
	buf[L++] = (uint8_t)(h_pos[2]>>8);
	buf[L++] = (uint8_t)(h_pos[2]>>0);
	
	buf[L++] = (uint8_t)(s_pos[2]>>8);
	buf[L++] = (uint8_t)(s_pos[2]>>0);
	
	buf[L++] = (uint8_t)(count>>8);
	buf[L++] = (uint8_t)(count>>0);
	count ++;
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}

void send_data_press_gyr(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(gyro[0]>>8);
	buf[L++] = (uint8_t)(gyro[0]>>0);
	
	buf[L++] = (uint8_t)(gyro[1]>>8);
	buf[L++] = (uint8_t)(gyro[1]>>0);
	
	buf[L++] = (uint8_t)(gyro[2]>>8);
	buf[L++] = (uint8_t)(gyro[2]>>0);
	
	buf[L++] = (uint8_t)(h_press>>8);
	buf[L++] = (uint8_t)(h_press>>0);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
	
}

//void send_data_pos_angle(void)
//{
//	uint8_t buf[256];
//	uint8_t L=0;
//	
//	buf[L++] = (uint8_t)(h_pos[0]>>8);
//	buf[L++] = (uint8_t)(h_pos[0]>>0);
//	
//	buf[L++] = (uint8_t)(h_pos[1]>>8);
//	buf[L++] = (uint8_t)(h_pos[1]>>0);
//	
//	buf[L++] = (uint8_t)(h_pos[2]>>8);
//	buf[L++] = (uint8_t)(h_pos[2]>>0);
//	
//	buf[L++] = (uint8_t)(angle[0]>>8);
//	buf[L++] = (uint8_t)(angle[0]>>0);
//	
//	buf[L++] = (uint8_t)(angle[1]>>8);
//	buf[L++] = (uint8_t)(angle[1]>>0);
//	
//	buf[L++] = (uint8_t)(angle[2]>>8);
//	buf[L++] = (uint8_t)(angle[2]>>0);
//	
//	send_protocol(DEX_NUM,CMD_HEART,buf,L);
//}

void send_data_acc(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(accel[0]>>8);
	buf[L++] = (uint8_t)(accel[0]>>0);
	
	buf[L++] = (uint8_t)(accel[1]>>8);
	buf[L++] = (uint8_t)(accel[1]>>0);
	
	buf[L++] = (uint8_t)(accel[2]>>8);
	buf[L++] = (uint8_t)(accel[2]>>0);
	
	buf[L++] = (uint8_t)(h_press>>8);
	buf[L++] = (uint8_t)(h_press>>0);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}
void sen_data_press(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(h_press>>8);
	buf[L++] = (uint8_t)(h_press>>0);
	
	buf[L++] = (uint8_t)(s_press>>8);
	buf[L++] = (uint8_t)(s_press>>0);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}
void sen_data_left_press(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(left_press>>24);
	buf[L++] = (uint8_t)(left_press>>16);
	
	buf[L++] = (uint8_t)(left_press>>8);
	buf[L++] = (uint8_t)(left_press>>0);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}

void send_package_data(uint16_t package_num, int16_t right_shoes_data[3], int16_t left_shoes_data[3],
			int16_t s_zupt, int16_t h_zupt,int16_t is_down, int16_t is_jump, int8_t s_rssi, uint8_t time_stamp)
{
	
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(right_shoes_data[0]>>8);
	buf[L++] = (uint8_t)(right_shoes_data[0]>>0);
	
	buf[L++] = (uint8_t)(right_shoes_data[1]>>8);
	buf[L++] = (uint8_t)(right_shoes_data[1]>>0);
	
	buf[L++] = (uint8_t)(right_shoes_data[2]>>8);
	buf[L++] = (uint8_t)(right_shoes_data[2]>>0);
	
	buf[L++] = (uint8_t)(left_shoes_data[0]>>8);
	buf[L++] = (uint8_t)(left_shoes_data[0]>>0);
	
	buf[L++] = (uint8_t)(left_shoes_data[1]>>8);
	buf[L++] = (uint8_t)(left_shoes_data[1]>>0);
	
	buf[L++] = (uint8_t)(left_shoes_data[2]>>8);
	buf[L++] = (uint8_t)(left_shoes_data[2]>>0);
	
	buf[L++] = (uint8_t)(s_zupt * 8 + h_zupt * 4 + is_down * 2 + is_jump * 1);
	
	buf[L++] = (uint8_t)(s_rssi >> 0);
	
	buf[L++] = (uint8_t)(time_stamp);
	
	send_protocol(DEX_NUM,package_num,buf,L);
	

}

void send_dual_foot_data(void)
{
	static uint8_t time_stamp = 0;
	
	send_package_data( 0x03, s_att, h_att,  s_zupt, h_zupt,  is_down,  is_jump, s_rssi, time_stamp);
	
	send_package_data( 0x04, s_pos, h_pos,  s_zupt, h_zupt,  is_down,  is_jump, s_rssi, time_stamp);
	
	time_stamp ++;
}


/*
void send_dual_foot_data(void)
{
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(h_press>>8);
	buf[L++] = (uint8_t)(h_press>>0);
	
	buf[L++] = (uint8_t)(s_press>>8);
	buf[L++] = (uint8_t)(s_press>>0);
	
	buf[L++] = (uint8_t)(s_pos[2]>>8);
	buf[L++] = (uint8_t)(s_pos[2]>>0);
	
	buf[L++] = (uint8_t)(s_att[0]>>8);
	buf[L++] = (uint8_t)(s_att[0]>>0);
	
	buf[L++] = (uint8_t)(s_att[1]>>8);
	buf[L++] = (uint8_t)(s_att[1]>>0);
	
	buf[L++] = (uint8_t)(s_att[2]>>8);
	buf[L++] = (uint8_t)(s_att[2]>>0);
	
	buf[L++] = (uint8_t)(h_pos[0]>>8);
	buf[L++] = (uint8_t)(h_pos[0]>>0);
	
	buf[L++] = (uint8_t)(h_pos[1]>>8);
	buf[L++] = (uint8_t)(h_pos[1]>>0);
	
	buf[L++] = (uint8_t)(h_pos[2]>>8);
	buf[L++] = (uint8_t)(h_pos[2]>>0);
	
	buf[L++] = (uint8_t)(h_att[0]>>8);
	buf[L++] = (uint8_t)(h_att[0]>>0);
	
	buf[L++] = (uint8_t)(h_att[1]>>8);
	buf[L++] = (uint8_t)(h_att[1]>>0);
	
	buf[L++] = (uint8_t)(h_att[2]>>8);
	buf[L++] = (uint8_t)(h_att[2]>>0);
	
	buf[L++] = (uint8_t)(s_zupt>>0);
	buf[L++] = (uint8_t)(h_zupt>>0);
	
	buf[L++] = (uint8_t)(is_down>>0);
	buf[L++] = (uint8_t)(is_jump>>0);
	
	buf[L++] = (uint8_t)(s_rssi>>0);
	
	send_protocol(DEX_NUM,0x02,0x03,buf,L);
}
*/
void send_data_to_pc(void)
{
	static uint8_t wait_time = 0;
	
	if(wait_time == 5)
	{
		send_dual_foot_data();
		wait_time = 0;
	}
	
	wait_time ++;
	  
}


//void send_motion_to_phone(uint8_t motion,uint16_t ts)
//{
//	uint8_t buf[16];
//	uint8_t L=0;

//	buf[L++] = motion;
//	
//	buf[L++] = (uint8_t)(ts>>8);
//	buf[L++] = (uint8_t)(ts>>0);
//	
//	for(int i = 0; i <5; i++)
//		send_protocol(DEX_NUM,CMD_MOTION,buf,L);
//}


void process_motion()
{
	time_stamp = time_stamp + 1;
	
	left_press  = (((int32_t)h_press & 0x0000ffff)<<8);
	right_press = (((int32_t)s_press & 0x0000ffff)<<8);
	
//	press_minimum_eval(((int32_t)h_press & 0x0000ffff), ((int32_t)s_press & 0x0000ffff),  h_pos[2], s_pos[2]);

	short down_tag = 0;

	down_tag = press_down_detect_new(time_stamp, left_press, right_press);

	if (down_tag)
	{
//		send_motion_to_phone(MOTION_DOWN, time_stamp);
		gpio_mt_run(20);
		down_send_count = 20;
	}
	
	if(down_send_count > 0)
	{
		is_down = 1;
		down_send_count --;
	}
	else
	{
		is_down = 0;
	}
	
	short jump_tag  = 0;
	
	jump_tag  = press_jump_detect(h_pos , s_pos);
	
	if (jump_tag)
	{
//		send_motion_to_phone(MOTION_JUMP, time_stamp);
		gpio_mt_run(20);
		jump_send_count = 20;
	}
	
	//连续发20次蹲，终端再处理
	if(jump_send_count > 0)
	{
		is_jump = 1;
		jump_send_count --;
	}
	else
	{
		is_jump = 0;
	}
	


	gyr[0] = (float)gyro[0] / GYR_LSB;
	gyr[1] = (float)gyro[1] / GYR_LSB;
	gyr[2] = (float)gyro[2] / GYR_LSB;

	acc[0] = (float)accel[0] / ACC_LSB;
	acc[1] = (float)accel[1] / ACC_LSB;
	acc[2] = (float)accel[2] / ACC_LSB;

	unsigned char left_tag = footPDR(time_stamp, gyr, acc, left_press, h_pos, h_att, &h_zupt);

	if (h_zupt > 1 || h_zupt < 0)
	{
//		send_motion_to_phone(MOTION_LEFT, time_stamp);
		gpio_mt_run(20);
	}    
	
	
//	memcpy(pos_n_offset, s_gyro, 3*sizeof(short));
	send_data_to_pc();
}