#include "send_data.h"

void send_package_data(uint16_t package_num, int32_t right_pos_data[3], int32_t left_pos_data[3], int16_t right_att_data[3], int16_t left_att_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_pos_data[0]>>24);
	buf[L++] = (uint8_t)(right_pos_data[0]>>16);
	buf[L++] = (uint8_t)(right_pos_data[0]>>8);
	buf[L++] = (uint8_t)(right_pos_data[0]>>0);
	
	buf[L++] = (uint8_t)(right_pos_data[1]>>24);
	buf[L++] = (uint8_t)(right_pos_data[1]>>16);
	buf[L++] = (uint8_t)(right_pos_data[1]>>8);
	buf[L++] = (uint8_t)(right_pos_data[1]>>0);
	
	buf[L++] = (uint8_t)(right_pos_data[2]>>24);
	buf[L++] = (uint8_t)(right_pos_data[2]>>16);
	buf[L++] = (uint8_t)(right_pos_data[2]>>8);
	buf[L++] = (uint8_t)(right_pos_data[2]>>0);
	
	
	buf[L++] = (uint8_t)(left_pos_data[0]>>24);
	buf[L++] = (uint8_t)(left_pos_data[0]>>16);
	buf[L++] = (uint8_t)(left_pos_data[0]>>8);
	buf[L++] = (uint8_t)(left_pos_data[0]>>0);
	
	buf[L++] = (uint8_t)(left_pos_data[1]>>24);
	buf[L++] = (uint8_t)(left_pos_data[1]>>16);
	buf[L++] = (uint8_t)(left_pos_data[1]>>8);
	buf[L++] = (uint8_t)(left_pos_data[1]>>0);
	
	buf[L++] = (uint8_t)(left_pos_data[2]>>24);
	buf[L++] = (uint8_t)(left_pos_data[2]>>16);
	buf[L++] = (uint8_t)(left_pos_data[2]>>8);
	buf[L++] = (uint8_t)(left_pos_data[2]>>0);
	
	buf[L++] = (uint8_t)(right_att_data[0]>>8);
	buf[L++] = (uint8_t)(right_att_data[0]>>0);
	
	buf[L++] = (uint8_t)(right_att_data[1]>>8);
	buf[L++] = (uint8_t)(right_att_data[1]>>0);
	
	buf[L++] = (uint8_t)(right_att_data[2]>>8);
	buf[L++] = (uint8_t)(right_att_data[2]>>0);
	
	buf[L++] = (uint8_t)(left_att_data[0]>>8);
	buf[L++] = (uint8_t)(left_att_data[0]>>0);
	
	buf[L++] = (uint8_t)(left_att_data[1]>>8);
	buf[L++] = (uint8_t)(left_att_data[1]>>0);
	
	buf[L++] = (uint8_t)(left_att_data[2]>>8);
	buf[L++] = (uint8_t)(left_att_data[2]>>0);
	
	int is_girl = 0;
	
	if(GIRL_SHOES)
			is_girl = 1;
	
	
	buf[L++] = (uint8_t)(is_girl * 16 + 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_data_to_com(int32_t s_pos[3], int32_t h_pos[3], int16_t s_att[3], int16_t h_att[3] , int16_t s_zupt, int16_t h_zupt, int16_t is_down, int16_t is_jump, int8_t s_rssi)
{
	static uint8_t time_stamp = 0;
	
	if(LEFT_IS_HOST)
		send_package_data( 0x04, s_pos, h_pos, s_att, h_att ,s_zupt, h_zupt,  is_down,  is_jump, s_rssi, time_stamp);
	
	else
		send_package_data( 0x04, h_pos, s_pos, h_att, s_att ,h_zupt, s_zupt,  is_down,  is_jump, s_rssi, time_stamp);
	
	time_stamp ++;
}

void send_data_to_host(uint16_t press, int32_t pos_n_offset[3], int16_t att[3], int16_t zupt)
{
	static uint16_t ts = 0;
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(press>>8);
	buf[L++] = (uint8_t)(press>>0);
	
	buf[L++] = (uint8_t)(pos_n_offset[0]>>24);
	buf[L++] = (uint8_t)(pos_n_offset[0]>>16);
	buf[L++] = (uint8_t)(pos_n_offset[0]>>8);
	buf[L++] = (uint8_t)(pos_n_offset[0]>>0);
	
	buf[L++] = (uint8_t)(pos_n_offset[1]>>24);
	buf[L++] = (uint8_t)(pos_n_offset[1]>>16);
	buf[L++] = (uint8_t)(pos_n_offset[1]>>8);
	buf[L++] = (uint8_t)(pos_n_offset[1]>>0);
	
	buf[L++] = (uint8_t)(pos_n_offset[2]>>24);
	buf[L++] = (uint8_t)(pos_n_offset[2]>>16);
	buf[L++] = (uint8_t)(pos_n_offset[2]>>8);
	buf[L++] = (uint8_t)(pos_n_offset[2]>>0);
	
	buf[L++] = (uint8_t)(att[0]>>8);
	buf[L++] = (uint8_t)(att[0]>>0);
	
	buf[L++] = (uint8_t)(att[1]>>8);
	buf[L++] = (uint8_t)(att[1]>>0);
	
	buf[L++] = (uint8_t)(att[2]>>8);
	buf[L++] = (uint8_t)(att[2]>>0);
 	
	buf[L++] = (uint8_t)(zupt>>0);
	
	buf[L++] = (uint8_t)(ts>>8);
	buf[L++] = (uint8_t)(ts>>0);
	
	ts++;

	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}

void send_acc_mag_to_host(uint16_t press, int16_t accl[3], int16_t magn[3], int16_t zupt)
{
	static uint16_t ts = 0;
	uint8_t buf[256];
	uint8_t L=0;
		
	buf[L++] = 0;
	buf[L++] = 0;
	buf[L++] = (uint8_t)(press>>8);
	buf[L++] = (uint8_t)(press>>0);
	
	buf[L++] = (uint8_t)(accl[0]>>8);
	buf[L++] = (uint8_t)(accl[0]>>0);
	
	buf[L++] = (uint8_t)(accl[1]>>8);
	buf[L++] = (uint8_t)(accl[1]>>0);
	
	buf[L++] = (uint8_t)(accl[2]>>8);
	buf[L++] = (uint8_t)(accl[2]>>0);
	
	buf[L++] = (uint8_t)(magn[0]>>8);
	buf[L++] = (uint8_t)(magn[0]>>0);
	
	buf[L++] = (uint8_t)(magn[1]>>8);
	buf[L++] = (uint8_t)(magn[1]>>0);
	
	buf[L++] = (uint8_t)(magn[2]>>8);
	buf[L++] = (uint8_t)(magn[2]>>0);
 	
	buf[L++] = (uint8_t)(zupt>>0);
	
	ts++;

	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}


void send_imu_data_to_host(uint16_t press, int16_t gyro[3], int16_t accl[3], int16_t magn[3], int16_t zupt)
{
	
	static uint16_t ts = 0;
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(press>>8);
	buf[L++] = (uint8_t)(press>>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)(accl[0]>>8);
	buf[L++] = (uint8_t)(accl[0]>>0);
	
	buf[L++] = (uint8_t)(accl[1]>>8);
	buf[L++] = (uint8_t)(accl[1]>>0);
	
	buf[L++] = (uint8_t)(accl[2]>>8);
	buf[L++] = (uint8_t)(accl[2]>>0);
	
	buf[L++] = (uint8_t)(magn[0]>>8);
	buf[L++] = (uint8_t)(magn[0]>>0);
	
	buf[L++] = (uint8_t)(magn[1]>>8);
	buf[L++] = (uint8_t)(magn[1]>>0);
	
	buf[L++] = (uint8_t)(magn[2]>>8);
	buf[L++] = (uint8_t)(magn[2]>>0);
	
	buf[L++] = (uint8_t)(zupt>>0);
	
	buf[L++] = (uint8_t)(ts>>8);
	buf[L++] = (uint8_t)(ts>>0);
	
	ts++;

	send_protocol(DEX_NUM,CMD_HEART,buf,L);
}

void send_imu_data_to_ble(uint16_t press, int16_t gyro[3], int16_t accl[3], int16_t magn[3], 
	uint16_t s_press, int16_t s_gyro[3], int16_t s_accl[3], int16_t s_magn[3], int8_t rssi )
{
	static uint16_t ts = 0;
	uint8_t buf[256];
	uint8_t L=0;
	
	buf[L++] = (uint8_t)(press>>8);
	buf[L++] = (uint8_t)(press>>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)(accl[0]>>8);
	buf[L++] = (uint8_t)(accl[0]>>0);
	
	buf[L++] = (uint8_t)(accl[1]>>8);
	buf[L++] = (uint8_t)(accl[1]>>0);
	
	buf[L++] = (uint8_t)(accl[2]>>8);
	buf[L++] = (uint8_t)(accl[2]>>0);
	
	buf[L++] = (uint8_t)(magn[0]>>8);
	buf[L++] = (uint8_t)(magn[0]>>0);
	
	buf[L++] = (uint8_t)(magn[1]>>8);
	buf[L++] = (uint8_t)(magn[1]>>0);
	
	buf[L++] = (uint8_t)(magn[2]>>8);
	buf[L++] = (uint8_t)(magn[2]>>0);
	
	buf[L++] = (uint8_t)(s_press>>8);
	buf[L++] = (uint8_t)(s_press>>0);
	
	buf[L++] = (uint8_t)(s_gyro[0]>>8);
	buf[L++] = (uint8_t)(s_gyro[0]>>0);
	
	buf[L++] = (uint8_t)(s_gyro[1]>>8);
	buf[L++] = (uint8_t)(s_gyro[1]>>0);
	
	buf[L++] = (uint8_t)(s_gyro[2]>>8);
	buf[L++] = (uint8_t)(s_gyro[2]>>0);
	
	buf[L++] = (uint8_t)(s_accl[0]>>8);
	buf[L++] = (uint8_t)(s_accl[0]>>0);
	
	buf[L++] = (uint8_t)(s_accl[1]>>8);
	buf[L++] = (uint8_t)(s_accl[1]>>0);
	
	buf[L++] = (uint8_t)(s_accl[2]>>8);
	buf[L++] = (uint8_t)(s_accl[2]>>0);
	
	buf[L++] = (uint8_t)(s_magn[0]>>8);
	buf[L++] = (uint8_t)(s_magn[0]>>0);
	
	buf[L++] = (uint8_t)(s_magn[1]>>8);
	buf[L++] = (uint8_t)(s_magn[1]>>0);
	
	buf[L++] = (uint8_t)(s_magn[2]>>8);
	buf[L++] = (uint8_t)(s_magn[2]>>0);
	
	buf[L++] = (uint8_t)(rssi);
	
	send_protocol(DEX_NUM,CMD_HEART,buf,L);

}