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

#include "math.h"

#include "detect_step_by_mag.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;

uint32_t step_count;

int16_t ShoeTouchFlag2;

int16_t Rcounter2;
int16_t Tcounter=0;
int16_t FKEY2;
int16_t BKEY2;
//int32_t RealFrontLiftPoint=30000;
//int32_t RealBackLiftPoint=30000;
int RealFrontLiftPoint;
int RealBackLiftPoint;

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)(RealFrontLiftPoint>>24);
	//foot_data_buf[foot_data_len++] = (uint8_t)(RealFrontLiftPoint>>16);
	//foot_data_buf[foot_data_len++] = (uint8_t)(RealFrontLiftPoint>>8);
	//foot_data_buf[foot_data_len++] = (uint8_t)(RealFrontLiftPoint>>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)(RealBackLiftPoint>>24);
	//foot_data_buf[foot_data_len++] = (uint8_t)(RealBackLiftPoint>>16);
	//foot_data_buf[foot_data_len++] = (uint8_t)(RealBackLiftPoint>>8);
	//foot_data_buf[foot_data_len++] = (uint8_t)(RealBackLiftPoint>>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)(FKEY2>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(FKEY2>>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)(ShoeTouchFlag2>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(ShoeTouchFlag2>>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)(BKEY2>>8);
	foot_data_buf[foot_data_len++] = (uint8_t)(BKEY2>>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)
{
	/*
		½âÎöÊý¾Ý
	*/
	//static int16_t 
	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", (int)(acc[0]*1000), (int)(acc[1]*1000),(int)(acc[2]*1000));
	
	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.9397 * pos_tmp[0] + 0.3420 *  pos_tmp[1])*100.0f);
////	pos[1] =  (int32_t)((-0.3420 * pos_tmp[0] + 0.9397 *  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]; 
  int16_t result;
	if(RealTimeStep(front_mag, back_mag,_acc))
	//if(detect_step_by_mag(front_mag, _acc[2]))
	{
		step_count++;
	}
	SEGGER_RTT_printf(0,"step_count: %d \r\n", step_count);
	
	ShoeTouchFlag2 = Get_ShoeTouchFlag();
	Rcounter = Get_Rcounter();
	Tcounter = Get_Tcounter();
	RealFrontLiftPoint=Get_RealFrontLiftPoint();
  RealBackLiftPoint=Get_RealBackLiftPoint();
  FKEY2=Get_FKEY();
	BKEY2=Get_BKEY();

	foot_data_to_package();	

	
}