#include "process_result.h"
#include "hal_mt.h"
#include "hal_flash.h"

extern uint8_t isGameMode;

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


uint32_t time_stamp = 0;

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

int32_t pos[3];
int16_t att[3];


int zupt;

uint16_t press;

uint8_t slave_rssi;

uint8_t foot_data_buf[64];

uint8_t foot_data_len;



void init_MOTION(void)
{
	time_stamp = 0;
}

void foot_data_to_package()
{
	static uint16_t package_time = 0;
	
	foot_data_len = 0;
	
	foot_data_buf[foot_data_len++] = (uint8_t)(press>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(press>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>24);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>16);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[0]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>24);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>16);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[1]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>24);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>16);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(pos[2]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(att[0]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(att[0]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(att[1]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(att[1]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(att[2]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(att[2]>>0);
 	
	foot_data_buf[foot_data_len++] = (uint8_t)(zupt>>0);
	
	
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[0]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[0]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[1]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[1]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[2]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(accel[2]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t) slave_rssi;
	
	foot_data_buf[foot_data_len++] = (uint8_t)(package_time>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(package_time>>0);
	
	package_time ++;

}

void get_foot_data(uint8_t *buf, uint8_t *buff_len)
{
	 memcpy(buf, foot_data_buf, foot_data_len);

	* buff_len = foot_data_len;
}

void process_motion(int16_t _acc[3], int16_t _gry[3], int16_t _mag[3], uint16_t _press, uint8_t _rssi)
{
	/*
		½âÎöÊý¾Ý
	*/
	
	
	memcpy(accel, _acc, 3 * sizeof(int16_t));
	
	accel[2] = -accel[2];
	
	memcpy(gyro , _gry, 3 * sizeof(int16_t));

	memcpy(magn , _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];
	
	
//	acc[0] = (acc[0] - 0.0f);
	acc[1] = (acc[1] - 0.020f);
	acc[2] = (acc[2] - 0.050f);
//	acc[1] = (acc[1] - 0.000f);
//	acc[2] = (acc[2] + 0.040f);
	

	
	int acc_x_status = 0;
	int acc_y_status = 0;
	int acc_z_status = 0;
	
//	if(isGameMode == 0)
	if(0)
	{
		detect_step_by_acc(time_stamp, acc, &acc_x_status, &acc_y_status, &acc_z_status, &zupt);
		
	}
	else
	{
		step_detect_by_acc_gyr(acc, gyr, &acc_x_status, &acc_y_status, &acc_z_status, &zupt);
	}
	
	press = _press;
	
	slave_rssi =  _rssi;
	
	acc[2] = -acc[2];
	
	if(isGameMode == 0)
	{
		footPDR(time_stamp, gyr, acc, mag, (((int32_t)_press & 0x0000ffff)<<8), pos, att, 0, 0, zupt);
	}
	else
	{
		footPDR(time_stamp, gyr, acc, mag, (((int32_t)_press & 0x0000ffff)<<8), pos, att, acc_x_status, acc_y_status, acc_z_status);
	}
//	att[0] = acc_x_status;
//	att[1] = acc_y_status;
//	att[2] = acc_z_status;

	
//	if(time_stamp % 100 == 0)
//	{
//		MT_Run(500);
//	}
	
  foot_data_to_package();
	
	time_stamp ++;
	
}