#include "app_fft.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "string.h"
#include <math.h>
#include "stdlib.h"
#include "hal_sc7a20.h"

 
#define FFT_SIZE 128 
#define PI 3.14159265358979
creal_T S0[FFT_SIZE];
creal_T S1[FFT_SIZE];
creal_T S2[FFT_SIZE];
QueueHandle_t FFT_DataQueueHandle;


  

static void cmul(creal_T a,creal_T b,creal_T *c)
{
   c->re = a.re * b.re - a.im * b.im;
   c->im = a.re * b.im + a.im * b.re;
}
static void cadd(creal_T a,creal_T b,creal_T *c)
{
   c->re = a.re + b.re;
   c->im = a.im + b.im;
}
static void csub(creal_T a,creal_T b,creal_T *c)
{
	 c->re = a.re - b.re;
	 c->im = a.im - b.im;
}

static void RaderReverse(creal_T *X, uint16_T N)
{
    uint16_T i;
    uint16_T j;
    uint16_T k;
    creal_T temp;
    j = N/2;
    for(i = 1; i < N - 1; i++)
    {
        if(i < j)
        {
            temp = X[j];
            X[j] = X[i];
            X[i] = temp;
        }
        k = N/2;
        while(k <= j)
        {
            j = j - k;
            k = k/2;
        }
        j = j + k;
    }
}

static int fft_cmp(const void*p1,const void*p2)
{
	return (*(struct _fft_Freq*)p2).amp>(*(struct _fft_Freq*)p1).amp ? 1 : -1;
}


static void fft(fft_type *fft_data,creal_T *X, creal_T *W, uint16_T N, real_T *returnFreq, real_T *returnP) // N为偶数
{ 
    uint16_T i = 0;
    uint16_T j = 0;
    uint16_T k = 0;
    uint16_T l = 0;
    creal_T up;
    creal_T down;
    creal_T product;

    // 变换
    RaderReverse(X, N);

    for (i = 0; i < log(N) / log(2); i++)
    {
        l = 1 << i;
        for (j = 0; j < N; j += 2 * l)
        {
            for (k = 0; k < l; k++)
            {
                cmul(X[j + k + l], W[N * k / 2 / l], &product);
                cadd(X[j + k], product, &up);
                csub(X[j + k], product, &down);
                X[j + k] = up;
                X[j + k + l] = down;
            }
        }
    }

    // 计算幅值
    for (i = 0; i < N/2+1; i++)
    {
        if(i==0||i==N/2)
        {
            fft_data[i].amp = sqrt((X[i].im) * (X[i].im) + (X[i].re) * (X[i].re)) / N;
        }
        else
        {
            fft_data[i].amp = 2*sqrt((X[i].im) * (X[i].im) + (X[i].re) * (X[i].re)) / N;
        }
    }
    // 从大到小 排序
    qsort(fft_data,sizeof(fft_data) / sizeof(fft_data[0]),sizeof(fft_data[0]),fft_cmp);
    for (i = 0; i < 5; i++)
    {
        returnFreq[i] = fft_data[i].freq;
        returnP[i] = fft_data[i].amp;
    }
}



void fft_task(void *pvParameters)
{	
	uint16_t i = 0;
	uint16_t freq = 500;	
	creal_T W[FFT_SIZE];
	fft_type fft_data[FFT_SIZE/2+1];
	int16_t sc7a20_data_test[3];	
	fft_data_t  fft_data_sort;
	
	
	// 计算频率
	for (i = 0; i < FFT_SIZE / 2 + 1; i++)
	{
			fft_data[i].freq = freq * i / FFT_SIZE;
	}

	for (i = 0; i < FFT_SIZE; i++)
	{
			W[i].re = cos(2 * PI / FFT_SIZE * i);
			W[i].im = -sin(2 * PI / FFT_SIZE * i);
	}	
	
	FFT_DataQueueHandle = xQueueCreate(1, sizeof(fft_type));
	
	
  SC7A20_Init(); 
	
	while(1)
	{
		for(int i = 0;i < FFT_SIZE;i++)
		{
			SC7A20_GetInfo(sc7a20_data_test); 
			S0[i].re = (real_T)sc7a20_data_test[0];
			S0[i].im=0;
			S1[i].re = (real_T)sc7a20_data_test[1];
			S1[i].im=0;
			S2[i].re = (real_T)sc7a20_data_test[2];
			S2[i].im=0;
			
			//memcpy(xyzData,sc7a20_data_test,sizeof(xyzData));  //原始值发送
			vTaskDelay(1);	
		}
		 
		fft(fft_data,S0,W,FFT_SIZE,fft_data_sort.returnFreq[0],fft_data_sort.returnP[0]);
		
		fft(fft_data,S1,W,FFT_SIZE,fft_data_sort.returnFreq[1],fft_data_sort.returnP[1]);
		
		fft(fft_data,S2,W,FFT_SIZE,fft_data_sort.returnFreq[2],fft_data_sort.returnP[2]);
		 
		xQueueOverwrite(FFT_DataQueueHandle,&fft_data);
	
 		
		vTaskDelay(1000);						
	}
}