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

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


uint32_t time_stamp = 0;

int16_t accel[3];
int16_t gyro[3];
int16_t front_magn[3];
int16_t back_magn[3];

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

int16_t zupt;

uint8_t slave_rssi;

uint8_t foot_data_buf[64];

uint8_t foot_data_len;

int16_t front_zero;

int16_t back_zero;

int16_t acc_zero;

uint16_t front_mag_norm;
uint16_t back_mag_norm;

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

int low_power_dectect;
int low_power_step;
uint16_t low_power_mag;

int zupt_wait_time;
//left
//float acc_bias[3] = {0.091139f, 0.101830f, -0.048524f};
//float acc_scale[3] =  {0.980197f,   1.003513f,  0.987654f};
//right
float acc_bias[3] = {0.047714f, 0.026054f, -0.008199f};
float acc_scale[3] =  {1.003853f,   1.007566f, 0.986378f};

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)(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)(front_mag_norm>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(front_mag_norm>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(back_mag_norm>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(back_mag_norm>>0);
 	
	foot_data_buf[foot_data_len++] = (uint8_t)(zupt>>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);

	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);
	
//	SEGGER_RTT_printf(0,"test footPDR \n");
	
	package_time ++;
}

void imu_data_to_package()
{
	static uint16_t package_time = 0;
	
	foot_data_len = 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)(gyro[0]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(gyro[0]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(gyro[1]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(gyro[1]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(gyro[2]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(gyro[2]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[0]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[0]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[1]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[1]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[2]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(front_magn[2]>>0);
	
	
	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[0]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[0]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[1]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[1]>>0);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[2]>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(back_magn[2]>>0);
	
//	SEGGER_RTT_printf(0," left_sh , foot_data_len = %d\n", foot_data_len);
	
	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 front_mag[3], int16_t back_mag[3], uint8_t _rssi)
{
	/*
		解析数据
	*/
	

	memcpy(accel, _acc, 3 * sizeof(int16_t));
	
	memcpy(gyro , _gry, 3 * sizeof(int16_t));
	
	memcpy(front_magn , front_mag, 3 * sizeof(int16_t));
	
	memcpy(back_magn,	 back_mag, 3 * sizeof(int16_t));
	
	slave_rssi = _rssi + 4;
	
	for(int i = 0; i < 3; i++)
	{
		gyr[i] = gyro[i] / GYR_LSB;
	}
	
	for(int i = 0; i < 3; i++)
	{
		acc[i] = accel[i] / ACC_LSB;
		
//		acc[i] = (acc[i] + acc_bias[i]) * acc_scale[i];
	}
	
	
	
//	SEGGER_RTT_printf(0,"PDR gyro[0]:%d ;gyro[1] : %d ;gyro[2]: %d \n", gyro[0], gyro[1],gyro[2]);

	//SEGGER_RTT_printf(0,"PDR acc[0]:%d ;acc[1] : %d ;acc[2]: %d \n", (int)(acc[0]*1000), (int)(acc[1]*1000),(int)(acc[2]*1000));
	
	front_mag_norm = (uint16_t)(sqrt((float) (front_magn[0] * front_magn[0] + front_magn[1] * front_magn[1] + front_magn[2] * front_magn[2])));
	
	back_mag_norm  = (uint16_t)(sqrt((float) (back_magn[0] * back_magn[0] + back_magn[1] * back_magn[1] + back_magn[2] * back_magn[2])));
	
	
	//检测零速
	detect_zero_vel(front_magn, back_magn, accel, &front_zero, &back_zero, &acc_zero);
	
	//轨迹计算
	if(front_zero > 0 || back_zero > 0 )
	{
		zupt = 1;
	}
	else
	{
		zupt = 0;
	}
	
	footPDR(time_stamp, gyr, acc, 0 , zupt, acc_zero, pos, att);
	
	accel[2] = -accel[2]; 

	
	if(CAL_MODE)
		imu_data_to_package();
	else
		foot_data_to_package();	
	
	time_stamp ++;
	
	
}