DataPlayer/ChaosDataPlayer/Calculation.cpp

#include "Calculation.hpp"
#define PI 3.1415926535
Calculation::Calculation()
{
}

Calculation::~Calculation()
{
}

/************************************************************************/
/* 一维数据的复数快速傅里叶变换                                             */
/************************************************************************/
void Calculation::FFT(int n, fftw_complex* in, fftw_complex* out)
{
    if (in == NULL || out == NULL) return;
    fftw_plan p;
    p = fftw_plan_dft_1d(n, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
    fftw_execute(p);
    fftw_destroy_plan(p);
    fftw_cleanup();
}

/************************************************************************/
/* 一维数据的实数快速傅里叶变换                                             */
/************************************************************************/
void Calculation::FFT_R(int n, QVector<float> & vecData, fftw_complex* out)
{
    double in[n];
    for (int i = 0; i < n; i++) {
        in[i] = vecData[i];
    }

    for (int i = 0; i < n; i++)
    {
        out[i][0] = (double)vecData[i];
        out[i][1] = 0;
    }
    //create a DFT plan and execute it
    fftw_plan plan = fftw_plan_dft_r2c_1d(n, in, out, FFTW_ESTIMATE);
    fftw_execute(plan);
    //destroy the plan to prevent a memory leak
    fftw_destroy_plan(plan);
    fftw_cleanup();
}

/************************************************************************/
/* 一维数据的快速傅里叶逆变换                                          */
/************************************************************************/
void Calculation::iFFT(int n, fftw_complex* in, fftw_complex* out)
{
    if (in == NULL || out == NULL) return;
    fftw_plan p;
    p = fftw_plan_dft_1d(n, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);
    fftw_execute(p);
    fftw_destroy_plan(p);
    fftw_cleanup();
}

//************************************
// Method:    caculateAmp_Pha
// FullName:  Calculation::caculateAmp_Pha
// Access:    public static
// Returns:   void
// Qualifier:
// Parameter: int n
// Parameter: fftw_complex * in
// Parameter: int frequency
// Parameter: double & amplitude
// Parameter: double & phase
// 函数功能是计算特定频率的幅值和相位，原来的讨论中是传入一个特定的频率，然后在给定的频率左右范围内找幅值和相位
// 目前的函数实现是计算FFT变换后特定点的幅值和相位
// 然后还有一个地方需要修改，即给定频率和FFT变换结果序列
//************************************
void Calculation::caculateAmp_Pha(int n, fftw_complex* in, int frequency, double &amplitude, double &phase)
{
    int  index = frequency;
    amplitude = 2 * sqrt((in[index][0] / n) * (in[index][0] / n) + (in[index][1] / n) * (in[index][1] / n));
    phase = 180 * atan(in[index][1] / in[index][0]) / M_PI;
}


void Calculation::absVec(QVector<float> & vecAbsData,QVector<float> & vecData)
{
    for (int i = 0; i < vecData.size(); i++) {
        vecAbsData.push_back(fabs(vecData[i]));
    }
    return;
}


float Calculation::mean(QVector<float> & vecData)
{
    double meanTemp = 0;
    for (int i = 0; i < vecData.size(); i++) {
        meanTemp += (double)vecData[i];
    }
    return meanTemp / vecData.size();
}


void Calculation::drop_mean(QVector<float> & vecDropMeanData, QVector<float> & vecData)
{
    float fMean = mean(vecData);
    for (int i = 0; i < vecData.size(); i++) {
        vecDropMeanData.push_back(vecData[i] - fMean);
    }
    return;
}

float Calculation::srm(QVector<float> & vecData)
{
    double dSrmTemp = 0;
    for (int i = 0; i < vecData.size(); i++){
        dSrmTemp = dSrmTemp + sqrt(vecData[i]);
    }
    dSrmTemp = dSrmTemp / vecData.size();
    return dSrmTemp * dSrmTemp;
}


float Calculation::rms(QVector<float> & vecData)
{
    double rmsTemp = 0;
    for (int i = 0; i < vecData.size(); i++) {
        rmsTemp = rmsTemp += (vecData[i] * vecData[i]);
    }
    rmsTemp = rmsTemp / vecData.size();
    return sqrt(rmsTemp);
}


float Calculation::variance(QVector<float> & vecDropMeanData)
{
    double varianceTemp = 0;
    for (int i = 0; i < vecDropMeanData.size(); i++) {
        varianceTemp = varianceTemp += (vecDropMeanData[i] * vecDropMeanData[i]);
    }
    return varianceTemp/vecDropMeanData.size();
}

float Calculation::skew_state(QVector<float> & vecDropMeanData, float fVariance)
{
    double tempSkew = 0;
    for (int i = 0; i < vecDropMeanData.size(); i++) {
        tempSkew = tempSkew + pow(vecDropMeanData[i], 3);
    }
    tempSkew = tempSkew / vecDropMeanData.size();
    tempSkew = tempSkew / pow(fVariance, 1.5);
    return tempSkew;
}


float Calculation::kurtosis(QVector<float> & vecDropMeanData, float fVariance)
{
    double tempkurtosis = 0;
    for (int i = 0; i < vecDropMeanData.size(); i++) {
        tempkurtosis = tempkurtosis + pow(vecDropMeanData[i], 4);
    }
    tempkurtosis = tempkurtosis / vecDropMeanData.size();
    tempkurtosis = tempkurtosis / pow(fVariance, 2);
    return tempkurtosis;
}

void Calculation::hilbert(QVector<double> & vecData, QVector<double> & vecHilbertData, int N)
{

    double in[N];
    for (int i = 0; i < N; i++) {
        in[i] = vecData[i];
    }
    fftw_complex *out;
    out  = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);

    for (int i = 0; i < N; ++i)
    {
        out[i][0] = (double)vecData[i];
        out[i][1] = 0;
    }
    //create a DFT plan and execute it
    fftw_plan plan = fftw_plan_dft_r2c_1d(N, in, out, FFTW_ESTIMATE);
    fftw_execute(plan);
    //destroy the plan to prevent a memory leak
    fftw_destroy_plan(plan);
    int hN = N >> 1;			// half of the length (N /2)
    int numRem = hN;		// the number of remaining elements
    // multiply the appropriate values by 2
    // (those that should be multiplied by 1 are left intact because they wouldn't change)
    for (int i = 1; i < hN; ++i) // 1,2,...,N/2 - 1 的项乘以2
    {
        out[i][0] *= 2;
        out[i][1] *= 2;
    }
    // if the length is even, the number of remaining elements decreases by 1
    if (N % 2 == 0)
        numRem--;
    // if it's odd and greater than 1, the middle value must be multiplied by 2
    else if (N > 1)  // 奇数非空
    {
        out[hN][0] *= 2;
        out[hN][1] *= 2;
    }
    // set the remaining values to 0
    // (multiplying by 0 gives 0, so we don't care about the multiplicands)
    memset(&out[hN + 1][0], 0, numRem * sizeof(fftw_complex));
    // create an IDFT plan and execute it
    plan = fftw_plan_dft_1d(N, out, out, FFTW_BACKWARD, FFTW_ESTIMATE);
    fftw_execute(plan);
    // do some cleaning
    fftw_destroy_plan(plan);
    fftw_cleanup();
    // scale the IDFT output
    for (int i = 0; i < N; ++i)
    {
        out[i][0] /= N;
        out[i][1] /= N;
    }

    for( int n=0; n<N; n++ )//输出
    {
      //  xr[n]=cos(n*pi/6);//原始信号
      //  y_r[n] = s_i[n];
        complex complex_after;
        complex_after.real = out[n][1];
        complex_after.imag = out[n][0];
        float amp = sqrt(complex_after.real * complex_after.real +complex_after.imag * complex_after.imag);
        vecHilbertData.push_back(amp);
      //  printf("%d    %f\n",n,vecHilbertData[n]);
    }
    fftw_free(out);
}

void Calculation::fftShift(fftw_complex* in, int l)
{
    double temp;
    double temp2;
    for (int j = 0;j<l/2;j++) {
        temp = in[j+l/2][0];
        temp2 = in[j+l/2][1];
        in[j+l/2][0] = in[j][0];
        in[j+l/2][1] = in[j][1];
        in[j][0] = temp;
        in[j][1] = temp2;
    }
}


void Calculation::FFTSpec(QVector<double> & vecData, QVector<double> & vecFFTSpecData)
{
    fftw_complex *inFFt, *outFFt;
    inFFt  = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
    outFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());

    for (int j = 0; j < vecData.size(); j++) {
        inFFt[j][0] = (double)vecData[j];
        inFFt[j][1] = 0;
    }
    FFT(vecData.size(),inFFt, outFFt);
    for(int j = 0; j < vecData.size()/2; j++) {
        vecFFTSpecData.push_back(sqrt(outFFt[j][0]*outFFt[j][0] + outFFt[j][1]*outFFt[j][1])*2/vecData.size());
    }
    fftw_free(inFFt);
    fftw_free(outFFt);
}
void Calculation::_iFFT( QVector<double> & vecrealData,QVector<double> & vecimageData,QVector<double> & veciFFTData)
{
    fftw_complex *inFFt, *outFFt;
    int N = vecrealData.size();
    inFFt  = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);
    outFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);

    for (int j = 0; j < N; j++) {
        inFFt[j][0] = (double)vecrealData[j];
        inFFt[j][1] = (double)vecimageData[j];
    }
    iFFT(N,inFFt, outFFt);
    for (int i = 0; i < N; i++) {
        outFFt[i][0] *= 1./N;
        outFFt[i][1] *= 1./N;
        veciFFTData.push_back(outFFt[i][0]);
    }
    fftw_free(inFFt);
    fftw_free(outFFt);
}

void Calculation::_FFT(QVector<double> & vecData, QVector<double> & vecFFTrealData,QVector<double> & vecFFTimageData)
{
    fftw_complex *inHilFFt, *outHilFFt;
    inHilFFt  = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
    outHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());

    for (int j = 0; j < vecData.size(); j++) {
        inHilFFt[j][0] = (double)vecData[j];
        inHilFFt[j][1] = 0;
    }

    FFT(vecData.size(), inHilFFt, outHilFFt);
    //fftShift(outHilFFt,  vecData.size());
    for (int i = 0; i < vecData.size(); i++) {
        vecFFTrealData.push_back(outHilFFt[i][0]);
        vecFFTimageData.push_back(outHilFFt[i][1]);
    }
    fftw_free(inHilFFt);
    fftw_free(outHilFFt);
}


void Calculation::_fft(QVector<double> & vecData, QVector<double> & vecFFTrealData,QVector<double> & vecFFTimageData)
{
    fftw_complex *inHilFFt, *outHilFFt;
    inHilFFt  = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
    outHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());

    for (int j = 0; j < vecData.size(); j++) {
        inHilFFt[j][0] = (double)vecData[j];
        inHilFFt[j][1] = 0;
    }

    FFT(vecData.size(), inHilFFt, outHilFFt);
    fftShift(outHilFFt,  vecData.size());
    for (int i = 0; i < vecData.size(); i++) {
        vecFFTrealData.push_back(outHilFFt[i][0]);
        vecFFTimageData.push_back(outHilFFt[i][1]);
    }
    fftw_free(inHilFFt);
    fftw_free(outHilFFt);
}

void Calculation::envSpec(QVector<double> & vecData, QVector<double> & vecEnvSpecData,int StartFrequency,int EndFrequency,bool PolarPlot)
{

    QVector<double> vecFFTrealData,vecFFTimageData;
    QVector<double> vecRealData,vecImageData;
    QVector<double> veciFFtData;
    QVector<double> veciFFtData2;
    QVector<double> vecHilbertData;
    _FFT(vecData,vecFFTrealData,vecFFTimageData);
    for(int i = 0; i < vecFFTrealData.size();i++){

        if(i < StartFrequency || i > EndFrequency){
             vecFFTrealData.replace(i,0.0);
             vecFFTimageData.replace(i,0.0);
        }
    }
    _iFFT(vecFFTrealData,vecFFTimageData,veciFFtData);

    for(int j = 0; j < veciFFtData.size();j++){
        veciFFtData2.push_back(veciFFtData[j]*2);
    }

    if(!PolarPlot){
        hilbert(veciFFtData2,vecHilbertData,veciFFtData2.size());
        FFTSpec(vecHilbertData, vecEnvSpecData);
    }else{

        vecEnvSpecData = veciFFtData2;
    }

}
//w(n) = 0.5 – 0.5*cos(2*πn/N); for n=0,1,2,…………………………N-1
void Calculation::Hanning(QVector<double> & vecData,QVector<double> & vecHanningData)
{
    int N = vecData.size();

    float* w = NULL;
    w = (float*)calloc(N, sizeof(float));
    int half, i, idx;
    if (N % 2 == 0)
    {
        half = N / 2;
        for (i = 0; i < half; i++) //CALC_HANNING   Calculates Hanning window samples.
             w[i] = 0.5 * (1 - cos(2 * PI * (i + 1) / (N + 1)));


        idx = half - 1;
        for (i = half; i < N; i++) {
            w[i] = w[idx];
            idx--;
        }
     }
     else
     {
         half = (N + 1) / 2;
         for (i = 0; i < half; i++) //CALC_HANNING   Calculates Hanning window samples.
            w[i] = 0.5 * (1 - cos(2 * PI * (i + 1) / (N + 1)));

         idx = half - 2;
         for (i = half; i < N; i++) {
             w[i] = w[idx];
             idx--;
         }
     }
    for(int j = 0; j < N;j++){
        vecHanningData.push_back(w[j]);
    }
    free(w);
}

double Calculation::Phase(QVector<double> & vecData)
{
    fftw_complex *inHilFFt, *outHilFFt;
    inHilFFt  = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
    outHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());

    for (int j = 0; j < vecData.size(); j++) {
        inHilFFt[j][0] = (double)vecData[j];
        inHilFFt[j][1] = 0;
    }

    FFT(vecData.size(), inHilFFt, outHilFFt);
    QVector<double> vecFFTrealData;
    QVector<double>  vecFFTimageData;
    for (int i = 0; i < vecData.size(); i++) {
        vecFFTrealData.push_back(outHilFFt[i][0]);
        vecFFTimageData.push_back(outHilFFt[i][1]);
    }
    fftw_free(inHilFFt);
    fftw_free(outHilFFt);

    double Phase1 = atan2(vecFFTrealData[0],vecFFTimageData[0]) *180/PI;
    double Phase2 = atan2(vecFFTrealData[vecData.size()-1],vecFFTimageData[vecData.size()-1]) *180/PI;
    return Phase2 - Phase1;
}

void Calculation::_Integration(QVector<double> & vecData,QVector<double>& retData,double& RMS)
{
    QVector<double> realshiftfft;
    QVector<double> imageshiftfft;
    QVector<double> realvalue,imagevalue;
    _FFT(vecData,realshiftfft,imageshiftfft);

    for (int i = 0; i <  10; i++) {
        realshiftfft[i] = 0;
        imageshiftfft[i] = 0;
    }
    for (int i = 1000; i <  realshiftfft.size(); i++) {
        realshiftfft[i] = 0;
        imageshiftfft[i] = 0;
    }
    qDebug()<< realshiftfft.size() << endl;
    qDebug()<< imageshiftfft.size() << endl;

    for(int k = 1; k < realshiftfft.size()+1;k++){
        realvalue.push_back((realshiftfft.at(k-1)/(k*2*PI))*1000 * 2);//单位转换mm/s,*1000  *2  精度损失
        imagevalue.push_back((imageshiftfft.at(k-1)/(k*2*PI))*1000 * 2);//单位转换mm/s,*1000
    }

    _iFFT(realvalue,imagevalue,retData);

    float Sum = 0.0;
    for(int j = 0; j < retData.size();j++){
        float fData = retData[j]*retData[j];
        Sum += fData;
    }

    RMS = sqrt(Sum/(float)retData.size());  //有效值

}

void Calculation::_Differentiation(QVector<double> & vecData,QVector<double>& retData)
{
    retData.push_back(vecData[0]);
    for(int j = 1; j < vecData.size()-3;j++)
    {
         retData.push_back((vecData.at(j+1)-vecData.at(j-1))/(double)(2*1/(double)vecData.size()));
    }
    retData.push_back(vecData.at(vecData.size()-1));
}

QVector<double> Calculation::ComputeDenCoeffs(int FilterOrder, double Lcutoff, double Ucutoff)
{
    int k;            // loop variables
    double theta;     // PI * (Ucutoff - Lcutoff) / 2.0
    double cp;        // cosine of phi
    double st;        // sine of theta
    double ct;        // cosine of theta
    double s2t;       // sine of 2*theta
    double c2t;       // cosine 0f 2*theta
    QVector<double> RCoeffs(2 * FilterOrder);     // z^-2 coefficients
    QVector<double> TCoeffs(2 * FilterOrder);     // z^-1 coefficients
    QVector<double> DenomCoeffs;     // dk coefficients
    double PoleAngle;      // pole angle
    double SinPoleAngle;     // sine of pole angle
    double CosPoleAngle;     // cosine of pole angle
    double a;         // workspace variables

    cp = cos(PI * (Ucutoff + Lcutoff) / 2.0);
    theta = PI * (Ucutoff - Lcutoff) / 2.0;
    st = sin(theta);
    ct = cos(theta);
    s2t = 2.0*st*ct;        // sine of 2*theta
    c2t = 2.0*ct*ct - 1.0;  // cosine of 2*theta

    for (k = 0; k < FilterOrder; ++k)
    {
        PoleAngle = PI * (double)(2 * k + 1) / (double)(2 * FilterOrder);
        SinPoleAngle = sin(PoleAngle);
        CosPoleAngle = cos(PoleAngle);
        a = 1.0 + s2t*SinPoleAngle;
        RCoeffs[2 * k] = c2t / a;
        RCoeffs[2 * k + 1] = s2t*CosPoleAngle / a;
        TCoeffs[2 * k] = -2.0*cp*(ct + st*SinPoleAngle) / a;
        TCoeffs[2 * k + 1] = -2.0*cp*st*CosPoleAngle / a;
    }

    DenomCoeffs = TrinomialMultiply(FilterOrder, TCoeffs, RCoeffs);

    DenomCoeffs[1] = DenomCoeffs[0];
    DenomCoeffs[0] = 1.0;
    for (k = 3; k <= 2 * FilterOrder; ++k)
        DenomCoeffs[k] = DenomCoeffs[2 * k - 2];

    for (int i = DenomCoeffs.size() - 1; i > FilterOrder * 2 + 1; i--)
        DenomCoeffs.pop_back();

    return DenomCoeffs;
}

QVector<double> Calculation::TrinomialMultiply(int FilterOrder, QVector<double>& b, QVector<double>& c)
{
    int i, j;
    QVector<double> RetVal(4 * FilterOrder);

    RetVal[2] = c[0];
    RetVal[3] = c[1];
    RetVal[0] = b[0];
    RetVal[1] = b[1];

    for (i = 1; i < FilterOrder; ++i)
    {
        RetVal[2 * (2 * i + 1)] += c[2 * i] * RetVal[2 * (2 * i - 1)] - c[2 * i + 1] * RetVal[2 * (2 * i - 1) + 1];
        RetVal[2 * (2 * i + 1) + 1] += c[2 * i] * RetVal[2 * (2 * i - 1) + 1] + c[2 * i + 1] * RetVal[2 * (2 * i - 1)];

        for (j = 2 * i; j > 1; --j)
        {
            RetVal[2 * j] += b[2 * i] * RetVal[2 * (j - 1)] - b[2 * i + 1] * RetVal[2 * (j - 1) + 1] +
                c[2 * i] * RetVal[2 * (j - 2)] - c[2 * i + 1] * RetVal[2 * (j - 2) + 1];
            RetVal[2 * j + 1] += b[2 * i] * RetVal[2 * (j - 1) + 1] + b[2 * i + 1] * RetVal[2 * (j - 1)] +
                c[2 * i] * RetVal[2 * (j - 2) + 1] + c[2 * i + 1] * RetVal[2 * (j - 2)];
        }

        RetVal[2] += b[2 * i] * RetVal[0] - b[2 * i + 1] * RetVal[1] + c[2 * i];
        RetVal[3] += b[2 * i] * RetVal[1] + b[2 * i + 1] * RetVal[0] + c[2 * i + 1];
        RetVal[0] += b[2 * i];
        RetVal[1] += b[2 * i + 1];
    }

    return RetVal;
}

QVector<double> Calculation::ComputeNumCoeffs(int FilterOrder, double Lcutoff, double Ucutoff, QVector<double>& DenC)
{
    QVector<double> TCoeffs;
    QVector<double> NumCoeffs(2 * FilterOrder + 1);
    QVector<std::complex<double>> NormalizedKernel(2 * FilterOrder + 1);

    QVector<double> Numbers;
    for (double n = 0; n < FilterOrder * 2 + 1; n++)
        Numbers.push_back(n);
    int i;

    TCoeffs = ComputeHP(FilterOrder);

    for (i = 0; i < FilterOrder; ++i)
    {
        NumCoeffs[2 * i] = TCoeffs[i];
        NumCoeffs[2 * i + 1] = 0.0;
    }
    NumCoeffs[2 * FilterOrder] = TCoeffs[FilterOrder];

    double cp[2];
    double Bw, Wn;
    cp[0] = 2 * 2.0*tan(PI * Lcutoff / 2.0);
    cp[1] = 2 * 2.0*tan(PI * Ucutoff / 2.0);

    Bw = cp[1] - cp[0];
    //center frequency
    Wn = sqrt(cp[0] * cp[1]);
    Wn = 2 * atan2(Wn, 4);
    double kern;
    const std::complex<double> result = std::complex<double>(-1, 0);

    for (int k = 0; k< FilterOrder * 2 + 1; k++)
    {
        NormalizedKernel[k] = std::exp(-sqrt(result)*Wn*Numbers[k]);
    }
    double b = 0;
    double den = 0;
    for (int d = 0; d < FilterOrder * 2 + 1; d++)
    {
        b += real(NormalizedKernel[d] * NumCoeffs[d]);
        den += real(NormalizedKernel[d] * DenC[d]);
    }
    for (int c = 0; c < FilterOrder * 2 + 1; c++)
    {
        NumCoeffs[c] = (NumCoeffs[c] * den) / b;
    }

    for (int i = NumCoeffs.size() - 1; i > FilterOrder * 2 + 1; i--)
        NumCoeffs.pop_back();

    return NumCoeffs;
}

QVector<double> Calculation::ComputeLP(int FilterOrder)
{
    QVector<double> NumCoeffs(FilterOrder + 1);
    int m;
    int i;

    NumCoeffs[0] = 1;
    NumCoeffs[1] = FilterOrder;
    m = FilterOrder / 2;
    for (i = 2; i <= m; ++i)
    {
        NumCoeffs[i] = (double)(FilterOrder - i + 1)*NumCoeffs[i - 1] / i;
        NumCoeffs[FilterOrder - i] = NumCoeffs[i];
    }
    NumCoeffs[FilterOrder - 1] = FilterOrder;
    NumCoeffs[FilterOrder] = 1;

    return NumCoeffs;
}

QVector<double> Calculation::ComputeHP(int FilterOrder)
{
    QVector<double> NumCoeffs;
    int i;

    NumCoeffs = ComputeLP(FilterOrder);

    for (i = 0; i <= FilterOrder; ++i)
        if (i % 2) NumCoeffs[i] = -NumCoeffs[i];

    return NumCoeffs;
}

//vector<double> filter(int ord, vector<double> a, vector<double> b, vector<double> x)
//{
//	int np = x.size();
//	vector<double> y(np);
//
//	int i, j;
//	y[0] = b[0] * x[0];
//	for (i = 1; i<ord + 1; i++)
//	{
//		y[i] = 0.0;
//		for (j = 0; j<i + 1; j++)
//			y[i] = y[i] + b[j] * x[i - j];
//		for (j = 0; j<i; j++)
//			y[i] = y[i] - a[j + 1] * y[i - j - 1];
//	}
//	for (i = ord + 1; i<np + 1; i++)
//	{
//		y[i] = 0.0;
//		for (j = 0; j<ord + 1; j++)
//			y[i] = y[i] + b[j] * x[i - j];
//		for (j = 0; j<ord; j++)
//			y[i] = y[i] - a[j + 1] * y[i - j - 1];
//	}
//
//	return y;
//}

QVector<double> Calculation::filter(QVector<double>&x, QVector<double>& coeff_b, QVector<double>& coeff_a)
{
    int len_x = x.size();
    int len_b = coeff_b.size();
    int len_a = coeff_a.size();

    QVector<double> zi(len_b);

    QVector<double> filter_x(len_x);

    if (len_a == 1)
    {
        for (int m = 0; m<len_x; m++)
        {
            filter_x[m] = coeff_b[0] * x[m] + zi[0];
            for (int i = 1; i<len_b; i++)
            {
                zi[i - 1] = coeff_b[i] * x[m] + zi[i];//-coeff_a[i]*filter_x[m];
            }
        }
    }
    else
    {
        for (int m = 0; m<len_x; m++)
        {
            filter_x[m] = coeff_b[0] * x[m] + zi[0];
            for (int i = 1; i<len_b; i++)
            {
                zi[i - 1] = coeff_b[i] * x[m] + zi[i] - coeff_a[i] * filter_x[m];
            }
        }
    }

    return filter_x;
}

//vector<double> bandpass(vector<double> input, double lowpass, double highpass, double fps)
//{
//	double N = input.size();
//	cv::Mat x = cv::Mat::zeros(1, input.size(), CV_64FC1);
//
//	for (int i = 0; i < input.size(); i++)
//	{
//		x.at<double>(0, i) = input[i];
//	}
//
//	Mat x_fre;
//	dft(x, x_fre, DFT_COMPLEX_OUTPUT);
//
//	Mat W = Mat::zeros(1, input.size(), CV_64FC1);
//
//	for (int i = 0; i < input.size(); i++)
//	{
//		if ((double)i / N *)
//	}
//}

void Calculation::ButterWorth(QVector<double>&inData,double *FrequencyBands,QVector<double>& outData)
{
    int FiltOrd = 4;
//    double s =  inData.size();
//    double a = cos(M_PI*(FrequencyBands[0]+FrequencyBands[1])/s)/cos(M_PI*(FrequencyBands[0]-FrequencyBands[1])/s);
//      double a2 = a*a;
//      double b = tan(M_PI*(FrequencyBands[0]-FrequencyBands[1])/s);
//      double b2 = b*b;
//      double r;
//      int n = FiltOrd/4;
//      double *A = (double *)malloc(n*sizeof(double));
//      double *d1 = (double *)malloc(n*sizeof(double));
//      double *d2 = (double *)malloc(n*sizeof(double));
//      double *d3 = (double *)malloc(n*sizeof(double));
//      double *d4 = (double *)malloc(n*sizeof(double));
//      double *w0 = (double *)calloc(n, sizeof(double));
//      double *w1 = (double *)calloc(n, sizeof(double));
//      double *w2 = (double *)calloc(n, sizeof(double));
//      double *w3 = (double *)calloc(n, sizeof(double));
//      double *w4 = (double *)calloc(n, sizeof(double));
//      double x;

//      for(int i=0; i<n; ++i){
//        r = sin(M_PI*(2.0*i+1.0)/(4.0*n));
//        s = b2 + 2.0*b*r + 1.0;
//        A[i] = 1.0/s;
//        d1[i] = 4.0*a*(1.0+b*r)/s;
//        d2[i] = 2.0*(b2-2.0*a2-1.0)/s;
//        d3[i] = 4.0*a*(1.0-b*r)/s;
//        d4[i] = -(b2 - 2.0*b*r + 1.0)/s;}
//      r = 4.0*a;
//      s = 4.0*a2+2.0;

//      for(int j = 0; j < inData.size(); j++){
//        for(int i=0; i<n; ++i){
//          w0[i] = d1[i]*w1[i] + d2[i]*w2[i]+ d3[i]*w3[i]+ d4[i]*w4[i] + inData[j];
//          x = A[i]*(w0[i] - r*w1[i] + s*w2[i]- r*w3[i] + w4[i]);
//          w4[i] = w3[i];
//          w3[i] = w2[i];
//          w2[i] = w1[i];
//          w1[i] = w0[i];
//        }
//        outData.push_back(x);
//        //printf("%lf\n", x);
//      }

//      free(A);free(A);free(d1);free(d2);free(d3);free(d4);
//      free(w0);free(w1);free(w2);free(w3);free(w4);
    QVector<double> a,b;

    a = ComputeDenCoeffs(FiltOrd, FrequencyBands[0], FrequencyBands[1]);

    b = ComputeNumCoeffs(FiltOrd, FrequencyBands[0], FrequencyBands[1], a);

    outData = filter(inData,b,a);
}