#include "Calculation.hpp"



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, std::vector<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();
}
void Calculation::_iFFT( std::vector<float> & vecrealData,std::vector<float> & vecimageData,std::vector<float> & 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(std::vector<float> & vecData, std::vector<float> & vecFFTrealData,std::vector<float> & 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);
}
//************************************
// 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;
}


float Calculation::max(std::vector<float> & vecData)
{
	std::vector<float>::iterator it;
	it = std::max_element(vecData.begin(), vecData.end());
	if (it != vecData.end()) {
		return *it;
	}
    return 0;
}

float Calculation::min(std::vector<float> & vecData)
{
	std::vector<float>::iterator it;
	it = std::min_element(vecData.begin(), vecData.end());
	if (it != vecData.end()) {
		return *it;
	}
    return 0;
}


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


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


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

float Calculation::srm(std::vector<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(std::vector<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::getSample_variance(std::vector<float> a)
{
	float ss = 0;
	float s = 0;
	float mx = mean(a);
	int len = a.size();
	for (int i = 0; i < len; i++) {
		s = a[i] - mx;
		ss += pow(s, 2);
	}
	return ss / (len - 1);
}



float Calculation::variance(std::vector<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(std::vector<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(std::vector<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::Hanning(std::vector<float> & vecData,std::vector<float> & 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);
}
void Calculation::hilbert(std::vector<float> & vecData, std::vector<float> & 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(std::vector<float> & vecData, std::vector<float> & 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());
	printf("1---------------------------------------------->%d\n",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());
    }

}

void Calculation::envSpec(std::vector<float> & vecData, std::vector<float> & vecEnvSpecData,int StartFrequency,int EndFrequency)
{
    /*std::vector<float> vecDropMeanData;
    drop_mean(vecDropMeanData, vecData);

    std::vector<float> vecHilbertData;
    hilbert(vecDropMeanData, vecHilbertData, vecDropMeanData.size());

    fftw_complex *inHilFFt, *outHilFFt;
    inHilFFt  = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecHilbertData.size());
    outHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecHilbertData.size());

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

    FFT(vecHilbertData.size(), inHilFFt, outHilFFt);
    fftShift(outHilFFt,  vecHilbertData.size());

    vecEnvSpecData.push_back(0);
    for (int i = vecHilbertData.size() / 2 + 1; i < vecHilbertData.size(); i++) {
        float ftemp = 2 * sqrt(outHilFFt[i][0] * outHilFFt[i][0] + outHilFFt[i][1] * outHilFFt[i][1]) / vecHilbertData.size();
        vecEnvSpecData.push_back(ftemp);
    }*/
	std::vector<float> vecFFTrealData,vecFFTimageData;
	std::vector<float> vecRealData,vecImageData;
	std::vector<float> veciFFtData;
	std::vector<float> veciFFtData2;
	std::vector<float> vecHilbertData;
	_FFT(vecData,vecFFTrealData,vecFFTimageData);
	for(int i = 0; i < vecFFTrealData.size();i++){
		if(i > StartFrequency && i < EndFrequency){
			vecRealData.push_back(vecFFTrealData.at(i));
			vecImageData.push_back(vecFFTimageData.at(i));
		}else{
			vecRealData.push_back(0);
			vecImageData.push_back(0);
		}
	}
	_iFFT(vecRealData,vecImageData,veciFFtData);
	for(int j = 0; j < veciFFtData.size();j++){
		veciFFtData2.push_back(veciFFtData[j]*2);
	}
	hilbert(veciFFtData2,vecHilbertData,veciFFtData2.size());
	FFTSpec(vecHilbertData, vecEnvSpecData);
}


void Calculation::GenerateSin(std::vector<float> & vecData)
{
	double frequency = 800.0; // Frequency of the sine wave in Hz
    double sampling_rate = 12800.0; // Sampling rate in Hz (8 kHz)
    size_t num_samples = 12800; // Total number of samples (1 second of data)
    double dt = 1.0 / sampling_rate; // Time step in seconds

    // Vector to hold the sine wave data
    //std::vector<double> sine_wave(num_samples);

    // Generate the sine wave
    for (size_t i = 0; i < num_samples; ++i) {
        vecData.push_back(std::sin(2 * M_PI * frequency * i * dt));
    }

}

void Calculation::Integration(std::vector<float> & vecData,std::vector<float>& retData,double & resolution)
{

	std::vector<float> realshiftfft;
    std::vector<float> imageshiftfft;
    std::vector<float> realvalue,imagevalue;
    _FFT(vecData,realshiftfft,imageshiftfft);

    for (int i = 0; i <  5 / resolution; i++) {
        realshiftfft[i] = 0;
        imageshiftfft[i] = 0;
    }
    for (int i = 1000 / resolution ; i <  realshiftfft.size(); i++) {
        realshiftfft[i] = 0;
        imageshiftfft[i] = 0;
    }

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

    _iFFT(realvalue,imagevalue,retData);
	

}

/*void acceleration_to_velocity(int fs, int N, float *data, int min_freq, int max_freq, float *out_data) {
    fftwf_execute_dft_r2c(forward_plan[fft_plan_id(N)], data, forward_out);

    float df = fs * 1.0f / N;
    int ni = round(min_freq / df);
    int na = round(max_freq / df);
    float dw = 2 * fcl_pi * df;

    int double_id = get_idle_double_res();
    if (double_id < 0) {
        return;
    }
    float *w11 = get_double_res_ptr(double_id);

    int len = 0;
    int max_f = round((0.5 * fs - df) / df);

    for (int i = 0; i <= max_f; ++i) {
        w11[i] = i * dw;
        ++len;
    }

    for (int i = 0; i < max_f; ++i) {
        w11[len] = -2 * fcl_pi * (0.5 * fs - df) + i * dw;
        ++len;
    }

    forward_out[0][0] = forward_out[0][1] = forward_out[N - 1][0] = forward_out[N - 1][1] = 0;
    float tmp_real, tmp_imag;
    for (int i = 1; i < N - 1; ++i) {
        tmp_real = forward_out[i][1] / w11[i];
        tmp_imag = -forward_out[i][0] / w11[i];
        forward_out[i][0] = tmp_real; // real
        forward_out[i][1] = tmp_imag; // imag
    }

    free_double_res(double_id);

    for (int i = 0; i < N; ++i) {
        backward_in[i][0] = 0;
        backward_in[i][1] = 0;
    }

    for (int i = ni - 1; i < na; ++i) {
        backward_in[i][0] = forward_out[i][0];
        backward_in[i][1] = forward_out[i][1];
    }

    for (int i = N - na; i < N - ni + 1; ++i) {
        backward_in[i][0] = forward_out[i][0];
        backward_in[i][1] = forward_out[i][1];
    }

    fftwf_execute_dft(backward_plan[fft_plan_id(N)], backward_in, backward_out);

    for (int i = 0; i < N; ++i) {
        out_data[i] = backward_out[i][0] / N * 2 * 1000; // *1000 是单位换算, 跟python保持一致
    }
}*/