#include "process_result.h"

float gyr[3];
float acc[3];
float mag[3];


int time_stamp = 0;

int16_t accel[3];
int16_t gyro[3];
int16_t magn[3];

uint16_t h_press;
uint16_t s_press;

int32_t s_pos[3];
int16_t s_att[3];

int16_t s_gyro[3];
int16_t s_accl[3];
int16_t s_magn[3];

int32_t h_pos[3];
int16_t h_att[3];

int8_t s_rssi;
int16_t s_zupt;
int16_t h_zupt;


void init_MOTION(void)
{
	time_stamp = 0;
}

void process_motion(IMU_DAT_t* p)
{
	/*
		解析数据
	*/
	
	memcpy(accel, p->h.acc, 3 * sizeof(int16_t));
	memcpy(gyro , p->h.gyr, 3 * sizeof(int16_t));
	memcpy(magn , p->h.mag, 3 * sizeof(int16_t));
	
	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;

	
	mag[0] = (float)magn[0];
	mag[1] = (float)magn[1];
	mag[2] = (float)magn[2];
	
	if(SH_NUMBER > 18)
	{
		float mag_temp[3];
		memcpy(mag_temp, mag, 3 * sizeof(float));
		
		mag[0] = 0.64278761f * mag[0] - 0.76604444f * mag[1];
		mag[1] = 0.76604444f * mag[0] + 0.64278761f * mag[1];
		
	}
	
	mag[0] = (mag[0] - magBias[0]) * magScale[0];
	mag[1] = (mag[1] - magBias[1]) * magScale[1];
	mag[2] = (mag[2] - magBias[2]) * magScale[2];
	
	float magTmp = mag[0];
	mag[0] = mag[1];
	mag[1] = magTmp;
	mag[2] = -mag[2];
	
	acc[0] = (acc[0] - accBias[0]) * accScale[0];
	acc[1] = (acc[1] - accBias[1]) * accScale[1];
	acc[2] = (acc[2] - accBias[2]) * accScale[2];
	
	h_press = p->h.press;
	
	s_rssi  = p->s.rssi;
	
	time_stamp = time_stamp + 1; //自定义时间戳

	
	if(IS_HOST)
	{
		
		memcpy(s_pos, p->s.pos, 3 * sizeof(int32_t));
		
		memcpy(s_att, p->s.att, 3 * sizeof(int16_t));
		
		s_press = p->s.press;
		
		s_zupt  = p->s.zupt;
		
		if(CAL_MODE)
		{
			s_gyro[0] = (int16_t)((s_pos[0] & 0xffff0000) >> 16);
			s_gyro[1] = (int16_t)((s_pos[0] & 0x0000ffff) >> 0);
			s_gyro[2] = (int16_t)((s_pos[1] & 0xffff0000) >> 16);
			
			s_accl[0] = (int16_t)((s_pos[1] & 0x0000ffff) >> 0);
			s_accl[1] = (int16_t)((s_pos[2] & 0xffff0000) >> 16);
			s_accl[2] = (int16_t)((s_pos[2] & 0x0000ffff) >> 0);
			
			memcpy(s_magn, p->s.att, 3 * sizeof(int16_t));
			
			send_imu_data_to_ble( h_press,  gyro,  accel,  magn, s_press,  s_gyro,  s_accl,  s_magn, s_rssi);
			
			return;
		}
		
		host_shoes(time_stamp,  gyr, acc, mag, h_press,  s_press, h_pos,  h_att,  s_pos,  s_att, h_zupt, s_zupt, s_rssi);
	}
	else
	{
		if(!CAL_MODE)
		{
			slave_shoes( time_stamp,  gyr,  acc,  mag, h_press,
			s_pos, s_att, s_zupt);
		}
		
		else
		{
			send_imu_data_to_host( h_press, gyro,  accel,  magn, 0);
			
			return;
		}
	}
}