WLG/utility/calculation.cpp

#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 (size_t 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 (size_t i = 0; i < vecData.size(); i++) {
        vecFFTrealData.push_back(outHilFFt[i][0]);
        vecFFTimageData.push_back(outHilFFt[i][1]);
    }
    fftw_free(inHilFFt);
    fftw_free(outHilFFt);
}

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

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);
}

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 * M_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 * M_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());

    for (size_t j = 0; j < vecData.size(); j++) {
        inFFt[j][0] = (double)vecData[j];
        inFFt[j][1] = 0;
    }
    FFT(vecData.size(), inFFt, outFFt);
    for (size_t 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> vecFFTrealData, vecFFTimageData;
    std::vector<float> vecRealData, vecImageData;
    std::vector<float> veciFFtData;
    std::vector<float> veciFFtData2;
    std::vector<float> vecHilbertData;
    _FFT(vecData, vecFFTrealData, vecFFTimageData);
    for (size_t i = 0; i < vecFFTrealData.size(); i++) {
        if (i > (size_t)StartFrequency && i < (size_t)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 (size_t 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

    // 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, ifftData;
    _FFT(vecData, realshiftfft, imageshiftfft);

    //在频域上进行5-1000 Hz的带通滤波
    for (size_t i = 0; i < vecData.size() / 2 + 1; ++i) {
        double frequency = i * resolution; // 计算当前频率分量
        if (frequency < 5 || frequency > 1000) {
            // 将5 Hz 到 1000 Hz 之外的频率成分设置为0
            realshiftfft[i] = 0.0;
            realshiftfft[i] = 0.0;
            imageshiftfft[i] = 0.0;
            imageshiftfft[i] = 0.0;
        }
    }

    for (size_t k = 1; k < realshiftfft.size() + 1; k++) {
        double frequency = k * resolution;                                                   // 计算当前频率分量
        realvalue.push_back((realshiftfft.at(k - 1) / (frequency * 2 * M_PI)) * 1000 * 2);   //单位转换mm/s,*1000  *2  精度损失
        imagevalue.push_back((imageshiftfft.at(k - 1) / (frequency * 2 * M_PI)) * 1000 * 2); //单位转换mm/s,*1000
    }

    _iFFT(realvalue, imagevalue, retData);
}


std::vector<float> Calculation::fftInterpolate(const std::vector<float>& input, size_t outputSize) {
    size_t inputSize = input.size();
    double in[inputSize];
    for (int i = 0; i < inputSize; i++) {
        in[i] = input[i];
    }
    // 1. FFTW 初始化
    fftw_complex *freqDomain = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * inputSize);
    fftw_complex *paddedFreqDomain = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * outputSize);
    fftw_plan forwardPlan = fftw_plan_dft_r2c_1d(inputSize, in, freqDomain, FFTW_ESTIMATE);
    
    // 2. 执行 FFT
    fftw_execute(forwardPlan);

    // 3. 频域插值：扩展频谱
    size_t halfSize = inputSize / 2 + 1; // 实数FFT的对称部分
    for (size_t i = 0; i < halfSize; ++i) {
        paddedFreqDomain[i][0] = freqDomain[i][0]; // 实部
        paddedFreqDomain[i][1] = freqDomain[i][1]; // 虚部
    }
    for (size_t i = halfSize; i < outputSize - halfSize; ++i) {
        paddedFreqDomain[i][0] = 0.0; // 实部填零
        paddedFreqDomain[i][1] = 0.0; // 虚部填零
    }
    for (size_t i = outputSize - halfSize; i < outputSize; ++i) {
        paddedFreqDomain[i][0] = freqDomain[inputSize - (outputSize - i)][0];
        paddedFreqDomain[i][1] = freqDomain[inputSize - (outputSize - i)][1];
    }

    // 4. IFFT 变换回时域
    std::vector<double> output(outputSize);
    fftw_plan inversePlan = fftw_plan_dft_c2r_1d(outputSize, paddedFreqDomain, output.data(), FFTW_ESTIMATE);
    fftw_execute(inversePlan);
    
    // 5. 缩放输出结果
    for (double& val : output) {
        val /= outputSize;
    }
    std::vector<float> output2(outputSize);
    for (int i = 0; i < outputSize; i++) {
        output2[i] = output[i];
    }
    // 清理 FFTW
    fftw_destroy_plan(forwardPlan);
    fftw_destroy_plan(inversePlan);
    fftw_free(freqDomain);
    fftw_free(paddedFreqDomain);

    return output2;
}