#include "process_result.h"
#include "hal_mt.h"
#include "app_flash.h"
#include "app_switchimu.h"

#include "math.h"

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


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;

int8_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;

int32_t step_count;

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;

int nan_flag = 0;

//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 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);
	
	foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>24);
	foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>16);
	foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(step_count>>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(int32_t time_stamp, int16_t _acc[3], int16_t _gry[3],  int16_t front_mag[3], int16_t back_mag[3], uint8_t _rssi)
{
	/*
		解析数据
	*/
	
	if(isnan(_acc[0]) || isnan(_acc[1]) || isnan(_acc[2]) || isnan(_gry[0]) || isnan(_gry[1]) || isnan(_gry[2]) )
	{
		nan_flag = 1;
	}
	
	if(nan_flag == 1)
	{
		SEGGER_RTT_printf(0,"read nan from IMU", foot_data_len);
	}
	

	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 - 8;
	
	if(slave_rssi < 0)
	{
		slave_rssi = 0;
	}
	
	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", _acc[0], _acc[1], _acc[2]);
	
	front_mag_norm = (uint16_t)(abs(front_magn[2]));
	
	back_mag_norm  = (uint16_t)(abs(back_magn[2]));
	
	//SEGGER_RTT_printf(0,"front_mag_norm : %d; back_mag_norm : %d; front_magn[0] : %d; front_magn[1] : %d; front_magn[2] : %d \r\n", front_mag_norm, back_mag_norm,front_magn[0],front_magn[1],front_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);


	//SEGGER_RTT_printf(0,"pos[0] : %d; pos[1] : %d ; pos[2] : %d \r\n", pos[0] , pos[1],  pos[2]);
	
	//SEGGER_RTT_printf(0,"gyro[0] : %d; gyro[1] : %d ; gyro[2] : %d \r\n", gyro[0] , gyro[1],  gyro[2]);
	
		//35号左鞋安装有误差，软件来补锅
//	float pos_tmp[3];
//	
//	pos_tmp[0] = -pos[1] * 0.01f;
//	pos_tmp[1] = pos[0] * 0.01f;


//	
//	pos[0] = (int32_t)((0.9397f * pos_tmp[0] + 0.3420f *  pos_tmp[1])*100.0f);
//	pos[1] =  (int32_t)((-0.3420f * pos_tmp[0] + 0.9397f *  pos_tmp[1])*100.0f);
	
	
	
	int32_t  pos_tmp[3];

	memcpy(pos_tmp, pos, 3 * sizeof(int32_t));

	pos[0] = -pos_tmp[1];
	pos[1] = pos_tmp[0];

	accel[2] = -accel[2]; 
	
	step_count = mFlash.mStep.stepCur[0];
	
	//SEGGER_RTT_printf(0,"step_count: %d \r\n", step_count);

	foot_data_to_package();	

	
}