#include "collect_data.h"
#include <string.h>
#include "math.h"
#include "pdebug.h"

tagSmpValueBuffer g_smpValueBuffer;
tagAnlgeValue g_AnlgeValue;

#if 0
/**
  * @brief fir滤波，更新数据
  * @param  
  * @retval 
  */
void FirFilter_RenewData(int16_t iXValue,int16_t iYValue,int16_t iZValue)
{
	//X
	writeIP = gs_FirFilterParam[0].WriteIP;
	gs_FirFilterParam[0].pDataBuf[writeIP] = iXValue;
	gs_FirFilterParam[0].WriteIP = (writeIP + 1) % gs_FirFilterParam[0].BufLen;
	
	//Y
	writeIP = gs_FirFilterParam[1].WriteIP;
	gs_FirFilterParam[1].pDataBuf[writeIP] = iYValue;
	gs_FirFilterParam[1].WriteIP = (writeIP + 1) % gs_FirFilterParam[1].BufLen;
	
}
#endif

/**
  * @brief 读取采样值
  * @param  
  * @retval 
  */
void GetSmpValue()
{  
  int16_t X_Value = 0,Y_Value = 0,Z_Value;
  for(int i=0; i<SMPVALUE_BUFFER_LEN; i++){ 
      //采集数据
      Lsm6ds3_ReadAccelerationRaw(&X_Value, &Y_Value, &Z_Value);
      g_smpValueBuffer.XValueBuffer[i].iValue = X_Value;
      g_smpValueBuffer.YValueBuffer[i].iValue = Y_Value;
  }
  
   //x轴采样值
	g_smpValueBuffer.XValue.iValue = f_Middle((U_DataType*)g_smpValueBuffer.XValueBuffer);
	 //y轴采样值
	g_smpValueBuffer.YValue.iValue= f_Middle((U_DataType*)g_smpValueBuffer.YValueBuffer);
}

/**
  * @brief 计算角度值
  * @param  
  * @retval 
  */
#define LSB  16393.44
#define PI   (180/3.1415926)
void AD_ComputeAngle(int SmpValue_X,int SmpValue_Y,float *pAngle_X,float *pAngle_Y)
{   
  float x_value = (float)SmpValue_X / LSB;
  float y_value = (float)SmpValue_Y / LSB;
  
  if(x_value >= 1)
     x_value=1;
  if(x_value <= -1)
     x_value=-1;
  
  if(y_value >= 1)
     y_value=1;
  
  if(y_value <= -1)
     y_value=-1;
  
 *pAngle_X = asinf(x_value)*PI;
 *pAngle_Y = asinf(y_value)*PI;  
}

/**
  * @brief 获取角度值 
  * @param  
  * @retval 
  */
void ComputeAngle()
{
	int16_t  X_SmpValue_New,Y_SmpValue_New;
	
	float angle_x_m = 0,angle_y_m = 0; //测量值
	 	
	//当前采样值
	X_SmpValue_New = g_smpValueBuffer.XValue.iValue;
	Y_SmpValue_New = g_smpValueBuffer.YValue.iValue;

#ifdef FILTER_MIDDLE_ENABLE
  u_int16_t  f_Middle(int16_t *ArrDataBuffer);
#endif
  
  
#ifdef FILTER_FIR_ENABLE
	FirFilter_RenewData(X_SmpValue_New,Y_SmpValue_New,Z_SmpValue_New);
	
	X_SmpValue_New = filter_fir_plus(&gs_FirFilterParam[0],gs_FirFilterHParam);
	Y_SmpValue_New = filter_fir_plus(&gs_FirFilterParam[1],gs_FirFilterHParam);
#endif
	
	
	AD_ComputeAngle(X_SmpValue_New,Y_SmpValue_New,&angle_x_m,&angle_y_m);

        if(angle_x_m > 60)
           angle_x_m = 60;

        if(angle_x_m < -60)   
            angle_x_m = -60;

        if(angle_y_m > 60)   
            angle_y_m = 60;

        if(angle_y_m < -60)   
            angle_y_m = -60;

        g_AnlgeValue.Angle_X = angle_x_m;
        g_AnlgeValue.Angle_Y = angle_y_m; 
  
  //term_printf("x_angele: %.2f, y_angle: %.2f\r\n",g_AnlgeValue.Angle_X,g_AnlgeValue.Angle_Y);
	//g_AnlgeValue.isReady = SD_TRUE;
 
}

/**
  * @brief 获取角度值 
  * @param  
  * @retval 
  */
void GetAngleValue()
{
	//if(SD_TRUE == g_ReadSmpIP)
	//{
		GetSmpValue();          
		ComputeAngle();       
		//SaveRealTimeData();
		//g_ReadSmpIP = SD_FALSE;
	//}
}