#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.0001f, -0.0041f, 0.0283f};
float acc_scale[3] =  {0.9929f,   0.9950f, 1.0018f};


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);
	
	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;
	
	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;
	}

	front_mag_norm = (uint16_t)(abs(front_magn[2]));
	
	back_mag_norm  = (uint16_t)(abs(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, front_mag_norm , zupt, acc_zero, pos, att);
	
	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];

	foot_data_to_package();	

	
}