#include "xcorr.h"
#include "usart.h"

//float32_t x_rfft_out[1024];
//float32_t y_rfft_out[1024];
//
//float32_t x_temp[1024];
//
//float32_t cmplx_result[1024];


arm_rfft_fast_instance_f32 s;
 

//q15_t x_rfft_out[1024];
//q15_t y_rfft_out[1024];
//
//q15_t x_temp[1024];
//
//q15_t cmplx_result[1024];

float32_t corrx_out[DATA_LEN];

float32_t x_buf[DATA_LEN];
float32_t y_buf[DATA_LEN];

float32_t x_fft_O_f32[DATA_LEN] = {0};
float32_t y_fft_O_f32[DATA_LEN]= {0};
float32_t cmplx_mul_result_f32[DATA_LEN]= {0};
float32_t max_val_f32;
int32_t max_val_index_f32;



//void fft_acc_corr_f32_test()
//{
//    arm_copy_f32((float32_t*)x,x_buf,DATA_LEN);
//    arm_copy_f32((float32_t*)y,y_buf,DATA_LEN);
//    
//    __HAL_TIM_SET_COUNTER(&htim16,0);
//    // 启定时器
//    __HAL_TIM_ENABLE(&htim16);
//    
//    
//    arm_offset_f32(x_buf,-2048,x_buf,DATA_LEN);
//    arm_offset_f32(y_buf,-2048,y_buf,DATA_LEN);  
//
//    arm_rfft_fast_init_f32(&s,DATA_LEN);
//    
//    arm_rfft_fast_f32(&s,x_buf,x_fft_O_f32,0);
//    //arm_rfft_fast_init_f32(&s,DATA_LEN);
//    arm_rfft_fast_f32(&s,y_buf,y_fft_O_f32,0);
//
//    arm_cmplx_conj_f32(y_fft_O_f32,y_fft_O_f32,DATA_LEN);
//    
//   //  __HAL_TIM_DISABLE(&htim7);   
//     
//    //arm_cmplx_mult_cmplx_q15_user(x_fft_O,y_fft_O,cmplx_mul_result,DATA_LEN*2);
//    arm_cmplx_mult_cmplx_f32(x_fft_O_f32,y_fft_O_f32,cmplx_mul_result_f32,DATA_LEN>>1);
//    //arm_rfft_init_q15(&s,DATA_LEN,1,1);
//   
//  //  s.pTwiddleAReal
//    arm_rfft_fast_init_f32(&s,DATA_LEN);
//    arm_rfft_fast_f32(&s,cmplx_mul_result_f32,corrx_out,1);
//   
//    arm_max_f32(corrx_out,DATA_LEN,&max_val_f32,&max_val_index_f32);
//    
//    // 关闭定时
//    __HAL_TIM_DISABLE(&htim16);   
//  
//
//}




//void fft_acc_corr_q15_test()
//{
//    arm_copy_f32((float32_t*)x,x_buf,DATA_LEN);
//    arm_copy_f32((float32_t*)y,y_buf,DATA_LEN);
//    
//    __HAL_TIM_SET_COUNTER(&htim16,0);
//    // 启定时器
//    __HAL_TIM_ENABLE(&htim16);
//    
//    
//    arm_offset_f32(x_buf,-2048,x_buf,DATA_LEN);
//    arm_offset_f32(y_buf,-2048,y_buf,DATA_LEN);  
//
//    arm_rfft_fast_init_f32(&s,DATA_LEN);
//    
//    arm_rfft_fast_f32(&s,x_buf,x_fft_O_f32,0);
//    //arm_rfft_fast_init_f32(&s,DATA_LEN);
//    arm_rfft_fast_f32(&s,y_buf,y_fft_O_f32,0);
//
//    arm_cmplx_conj_f32(y_fft_O_f32,y_fft_O_f32,DATA_LEN);
//    
//   //  __HAL_TIM_DISABLE(&htim7);   
//     
//    //arm_cmplx_mult_cmplx_q15_user(x_fft_O,y_fft_O,cmplx_mul_result,DATA_LEN*2);
//    arm_cmplx_mult_cmplx_f32(x_fft_O_f32,y_fft_O_f32,cmplx_mul_result_f32,DATA_LEN>>1);
//    //arm_rfft_init_q15(&s,DATA_LEN,1,1);
//   
//  //  s.pTwiddleAReal
//    arm_rfft_fast_init_f32(&s,DATA_LEN);
//    arm_rfft_fast_f32(&s,cmplx_mul_result_f32,corrx_out,1);
//   
//    arm_max_f32(corrx_out,DATA_LEN,&max_val_f32,&max_val_index_f32);
//    
//    // 关闭定时
//    __HAL_TIM_DISABLE(&htim16);  
//}

q15_t dc_offset;
float cal_tof(q15_t* x,uint32_t len)
{
    q15_t max_val;
    float32_t echo_p = 0,echo_dt = 0;
    uint32_t max_val_p;
    uint32_t i=0,stop_position = 0;
    // 求取平均值
    arm_mean_q15(x,100,&dc_offset);
    
    // 查找数组中的最大值和最大值所在的索引
    arm_max_q15(x,len,&max_val,&max_val_p);
    
    max_val = max_val - dc_offset ;
    //uint16_t echo_position = 0;
    // 最大值的0.18倍
    uint16_t max_val_zero_1R5 = (max_val*15/100)+dc_offset;
    // 最大值的0.45倍
    uint16_t max_val_zero_4R5 = (max_val*45/100)+dc_offset;
    // 最大值的0.8倍
    uint16_t max_val_zero_8R0 = (max_val*80/100)+dc_offset;
    
    
    //如果最大值位置大于200 则从最大值前200个位置开始寻找起始波形。
    // 优化的地方，从最大值位置开始找到达波,可以最大限度排除偶然噪声干扰，
    // 因为噪声在波形到达出 噪声不是很大
    //优化性能，如果不需要则从数组0位置开始寻找其实波形
    if(max_val_p>=70)
    {
      i = max_val_p-70;
    }else
    {
      i = 0;
    }
    // 在最大值前寻找起始波形
    for( ; i < max_val_p ; i++)
    {
      // 建议判断顶点，但是容易遇到偶然数据异常  类似于 28 29 28 30 29 28这种情况
      //if( x[i-1] < x[i] && x[i]> x[i+1]  )
      
      // 排除以上数据异常情况，但是有可能就无法检测到30 这个顶点。
      // 故需要检测下一个周期的顶点，然后再减去一个周期的时间。
      if( x[i-2]<x[i-1] && x[i-1] <= x[i] && x[i]>=x[i+1] && x[i+1]>x[i+2])
      {
        // 减去偏置电压
        //temp_val_zero = arr[i]-2048;
        // 判断顶点是否在 15%-%45之间。
        if(x[i] >= max_val_zero_1R5 && x[i] <= max_val_zero_4R5 )
        {
          // 如果找到 函数退出
          echo_dt = (x[i-1]-x[i+1])/2.0/(x[i-1]-2*x[i]+x[i+1]);
          echo_p = (float)i+echo_dt;
          return echo_p;
        }
         //  如果15% ~45%之间的数据未找到，则找45-80%的顶点。
         // 判断顶点是否在 45% -- 80% 之间
        if(x[i] >= max_val_zero_4R5 && x[i] <= max_val_zero_8R0)
        {
          // 如果找到 函数推出
          echo_dt = (x[i-1]-x[i+1])/2.0/(x[i-1]-2*x[i]+x[i+1]);
          // 换算成第二个顶点的位置。
          echo_p = (float)i+echo_dt - 12.5;
          return echo_p;
        }
      }
    }
    
    /*
    // 2.5M/0.2k= 12.5  查找最大值前5个周期（63个点）并计算到达时间
    if(max_val_p>=63)
    {
      stop_position = max_val_p-63;
    }
    else
    {
      stop_position = 0;
    }
    
    for(i = max_val_p-5 ; i >= stop_position ; i--)
    {
      if( x[i-2]<x[i-1] && x[i-1] <= x[i] && x[i]>=x[i+1] && x[i+1]>x[i+2])
      {
        // 减去偏置电压
        //temp_val_zero = arr[i]-2048;
        //  如果15% ~45%之间的数据未找到，则找45-80%的顶点。
        // 判断顶点是否在 45% -- 80% 之间
        if(x[i] >= max_val_zero_4R5 && x[i] <= max_val_zero_8R0)
        {
          // 如果找到 函数推出
          echo_dt = (x[i-1]-x[i+1])/2.0/(x[i-1]-2*x[i]+x[i+1]);
          // 换算成第二个顶点的位置。
          echo_p = (float)i+echo_dt - 12.5;
          return  echo_p;
        }
        // 判断顶点是否在 15%-%45之间。
        if(x[i] >= max_val_zero_1R5 && x[i] <= max_val_zero_4R5 )
        {
          // 如果找到 函数退出
          echo_dt = (x[i-1]-x[i+1])/2.0/(x[i-1]-2*x[i]+x[i+1]);
          echo_p = (float)i+echo_dt;
          return  echo_p;
        }
      }
    } */
    return 0;
}


float32_t cal_dtof(q15_t* x , q15_t* y , uint32_t len)
{
  float a,b,c;
  float w_val,x_val,y_val;
  float tof;
  arm_rfft_fast_init_f32(&s,len);
  // 定点数转成float
  arm_q15_to_float(x,x_buf,DATA_LEN);
  // 定点数转成float
  arm_q15_to_float(y,y_buf,DATA_LEN);  
  // 初始化 fft
  arm_rfft_fast_init_f32(&s,DATA_LEN);
  // fft  前一半和后一半是对称的,所以只求解了512组数据
  arm_rfft_fast_f32(&s,x_buf,x_fft_O_f32,0);
  // fft
  arm_rfft_fast_f32(&s,y_buf,y_fft_O_f32,0);
  
  // 复数取共轭 
  arm_cmplx_conj_f32(y_fft_O_f32,y_fft_O_f32,DATA_LEN>>1);
  // fft的数据 
  // 时域卷积==频域相乘                                         1024个数据 复数只有512个         
  arm_cmplx_mult_cmplx_f32(x_fft_O_f32,y_fft_O_f32,cmplx_mul_result_f32,DATA_LEN>>1);
  // ifft
  arm_rfft_fast_f32(&s,cmplx_mul_result_f32,corrx_out,1);
  // 
  arm_max_f32(corrx_out,DATA_LEN,&max_val_f32,&max_val_index_f32);

  
  b =corrx_out[max_val_index_f32];
  if(max_val_index_f32 == 0)
  {
     a =corrx_out[len-1];
     c =corrx_out[max_val_index_f32+1]; 
  }
  else
  if(max_val_index_f32 == len-1)
  {
     a =corrx_out[max_val_index_f32-1];
     c =corrx_out[0]; 
  }else
  {
     a =corrx_out[max_val_index_f32-1];
     c =corrx_out[max_val_index_f32+1]; 
  }
  
  // sin 插值 寻找最大值
  w_val = acosf((a+c)/2.0/b);
  
  float sin_val = sinf(w_val);
  
  y_val = atanf((a-c)/2.0/b/sin_val);
  
  x_val = (0-y_val)/w_val;
  
  tof = max_val_index_f32+x_val;
  
  if(tof>len/2)
    tof= tof-len;
  
  return tof;
}




float xcorr_max_position_uint16_t(q15_t* xcorr_out,q15_t* x , q15_t* y , uint32_t len)
{
  uint32_t i = 0; // y参数移动的次数
  q15_t    max_val=0;
  int32_t  max_p=0;
  q15_t max_val_point[3];
  float w_val,x_val,y_val;

  
   __HAL_TIM_SET_COUNTER(&htim16,0);
  // 启定时器
  __HAL_TIM_ENABLE(&htim16);

  arm_correlate_fast_q15(x,len,y,len,xcorr_out);
    // 关闭定时
  __HAL_TIM_DISABLE(&htim16);
  
 // xcorr(xcorr_out,x,y,len);
  for(i=0;i<len*2-1;i++)
  {
    if(xcorr_out[i]>max_val)
    {
      max_val = xcorr_out[i];
      max_p = i;
    }
  }
  
  // 曲线拟合 找到最大点 当作中间数据
  max_val_point[1] =xcorr_out[max_p];
  max_val_point[0] =xcorr_out[max_p-1];
  max_val_point[2] =xcorr_out[max_p+1]; 

  // sin 插值 寻找最大值
  w_val = acosf((max_val_point[0]+max_val_point[2]-4096)*1.0/(max_val_point[1]-2048)/(max_val_point[1]-2048));
  y_val = atanf((max_val_point[0]-max_val_point[2])/2.0/(max_val_point[1]-2048)/sinf(w_val));
  x_val = (0-y_val)/w_val;
  
  //return ((float)max_p+0.1-0.1);
  return (((float)max_p)-x_val);
  
//  return -((float)max_p);
}