NIM_DSP6748/code/algorithm/emg_util.c

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <float.h>
#include "emg_util.h"
#include <assert.h>


/**
 * 閲嶇疆闄锋尝婊ゆ尝鍣ㄧ姸鎬�
 * 鍔熻兘锛氬皢婊ゆ尝鍣ㄧ殑杈撳叆杈撳嚭鍘嗗彶鍊兼竻闆讹紝纭繚鏂扮殑婊ゆ尝杩囩▼涓嶅彈涔嬪墠鐘舵�佸奖鍝�
 * 鍙傛暟锛歠ilter - 鎸囧悜闄锋尝婊ゆ尝鍣ㄧ粨鏋勪綋鐨勬寚閽�
 */
void notchFilterReset(NotchFilter *filter) {
    // 閲嶇疆杈撳叆鍘嗗彶鍊硷紙x1涓哄墠涓�鏃跺埢杈撳叆锛寈2涓哄墠涓ゆ椂鍒昏緭鍏ワ級
    filter->x1 = filter->x2 = 0.0;
    // 閲嶇疆杈撳嚭鍘嗗彶鍊硷紙y1涓哄墠涓�鏃跺埢杈撳嚭锛寉2涓哄墠涓ゆ椂鍒昏緭鍑猴級
    filter->y1 = filter->y2 = 0.0;
}

/**
 * 闄锋尝婊ゆ尝鍣ㄥ鐞嗗崟涓牱鏈�
 * 鍔熻兘锛氬鍗曚釜杈撳叆鏁版嵁杩涜婊ゆ尝澶勭悊锛屽熀浜嶪IR婊ゆ尝鍣ㄥ樊鍒嗘柟绋�
 * 鍙傛暟锛�
 *   filter - 鎸囧悜闄锋尝婊ゆ尝鍣ㄧ粨鏋勪綋鐨勬寚閽堬紙鍖呭惈绯绘暟鍜屽巻鍙茬姸鎬侊級
 *   input  - 褰撳墠杈撳叆鐨勯噰鏍锋暟鎹�
 * 杩斿洖鍊硷細婊ゆ尝鍚庣殑杈撳嚭鏁版嵁
 */
double notchFilterProcess(NotchFilter *filter, double input) {
    // 鏍规嵁IIR婊ゆ尝鍣ㄥ樊鍒嗘柟绋嬭绠楀綋鍓嶈緭鍑�
    // 鍏紡锛歽[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2]
    double output = 
        filter->b[0] * input +         // 褰撳墠杈撳叆椤�
        filter->b[1] * filter->x1 +    // 鍓嶄竴鏃跺埢杈撳叆椤�
        filter->b[2] * filter->x2 -    // 鍓嶄袱鏃跺埢杈撳叆椤�
        filter->a[1] * filter->y1 -    // 鍓嶄竴鏃跺埢杈撳嚭鍙嶉椤�
        filter->a[2] * filter->y2;     // 鍓嶄袱鏃跺埢杈撳嚭鍙嶉椤�

    // 鏇存柊杈撳叆鍘嗗彶鐘舵�侊紙绉讳綅鎿嶄綔锛屼繚瀛樻渶鏂扮殑涓や釜杈撳叆鍊硷級
    filter->x2 = filter->x1;  // 鍓嶄袱鏃跺埢 = 鍓嶄竴鏃跺埢
    filter->x1 = input;       // 鍓嶄竴鏃跺埢 = 褰撳墠杈撳叆

    // 鏇存柊杈撳嚭鍘嗗彶鐘舵�侊紙绉讳綅鎿嶄綔锛屼繚瀛樻渶鏂扮殑涓や釜杈撳嚭鍊硷級
    filter->y2 = filter->y1;  // 鍓嶄袱鏃跺埢 = 鍓嶄竴鏃跺埢
    filter->y1 = output;      // 鍓嶄竴鏃跺埢 = 褰撳墠杈撳嚭

    return output;
}

/**
 * 闄锋尝婊ゆ尝鍣ㄥ鐞嗘暟缁勬暟鎹�
 * 鍔熻兘锛氬涓�鏁存杈撳叆鏁版嵁杩涜杩炵画婊ゆ尝锛岄�愪釜鏍锋湰璋冪敤鍗曟牱鏈鐞嗗嚱鏁�
 * 鍙傛暟锛�
 *   filter - 鎸囧悜闄锋尝婊ゆ尝鍣ㄧ粨鏋勪綋鐨勬寚閽�
 *   input  - 鎸囧悜杈撳叆鏁版嵁鏁扮粍鐨勬寚閽�
 *   output - 鎸囧悜杈撳嚭鏁版嵁鏁扮粍鐨勬寚閽堬紙闇�鎻愬墠鍒嗛厤鍐呭瓨锛�
 *   length - 杈撳叆/杈撳嚭鏁版嵁鐨勯暱搴︼紙鏍锋湰鏁帮級
 */
void notchFilterProcessArray(NotchFilter *filter, const double *input, double *output, int length) {
    // 閬嶅巻杈撳叆鏁扮粍锛岄�愪釜鏍锋湰杩涜婊ゆ尝
    int i = 0;
    for (i = 0; i < length; i++) {
        output[i] = notchFilterProcess(filter, input[i]);
    }
}




// 搴旂敤涓�涓弻浜岄樁鑺�
double biquad_filter(double input, const double *sos_row, BiquadState *state) {
    double b0 = sos_row[0];
    double b1 = sos_row[1];
    double b2 = sos_row[2];
    double a1 = sos_row[4];  // 娉ㄦ剰绱㈠紩锛氱5涓槸 a1
    double a2 = sos_row[5];  // 绗�6涓槸 a2

    double output = b0 * input + b1 * state->x1 + b2 * state->x2
                             - a1 * state->y1 - a2 * state->y2;

    state->x2 = state->x1;
    state->x1 = input;
    state->y2 = state->y1;
    state->y1 = output;

    return output;
}

// 鍙嶈浆鏁扮粍
void reverse_array(double *data, int n) {
    int i = 0;
    for (i = 0; i < n / 2; i++) {
        double temp = data[i];
        data[i] = data[n - 1 - i];
        data[n - 1 - i] = temp;
    }
}

/**
 * @brief 闆剁浉浣嶅甫閫氭护娉� (绫讳技 MATLAB filtfilt)
 * @param indata: 杈撳叆淇″彿鏁扮粍
 * @param outdata: 杈撳嚭淇″彿鏁扮粍锛堟护娉㈠悗缁撴灉锛�
 * @param len: 淇″彿闀垮害
 * @param sos: 2x6 鐨勪簩闃惰妭绯绘暟鐭╅樀
 * @param num_stages: 鑺傛暟锛堣繖閲屾槸 2锛�
 */
void filtfilt_sos(const double *indata, double *outdata, int len, const double sos[2][6], int num_stages) {
    // === 鎵�鏈夊彉閲忓０鏄庣Щ鍒伴《閮� ===
    double *temp;
    BiquadState *states;
    int i, j;
    double sample;

    temp = (double *)malloc(len * sizeof(double));
    states = (BiquadState *)calloc(num_stages, sizeof(BiquadState));

    // === 鍓嶅悜婊ゆ尝 ===
    for (i = 0; i < len; i++) {
        sample = indata[i];
        for (j = 0; j < num_stages; j++) {
            sample = biquad_filter(sample, sos[j], &states[j]);
        }
        temp[i] = sample;
    }

    memset(states, 0, num_stages * sizeof(BiquadState));

    reverse_array(temp, len);

    for (i = 0; i < len; i++) {
        sample = temp[i];
        for (j = 0; j < num_stages; j++) {
            sample = biquad_filter(sample, sos[j], &states[j]);
        }
        temp[i] = sample;
    }

    reverse_array(temp, len);

    memcpy(outdata, temp, len * sizeof(double));

    free(temp);
    free(states);
}

// 蹇�熸帓搴忥紙鍗囧簭锛�
void quicksort(double *arr, int n) {
    // 鎵�鏈夊彉閲忕Щ鍒伴《閮�
    double pivot, temp;
    int i, j;

    if (n <= 1) return;

    pivot = arr[n / 2];
    i = 0;
    j = n - 1;

    while (i <= j) {
        while (arr[i] < pivot) i++;
        while (arr[j] > pivot) j--;
        if (i <= j) {
            temp = arr[i]; arr[i] = arr[j]; arr[j] = temp;
            i++; j--;
        }
    }

    quicksort(arr, j + 1);
    if (i < n) quicksort(arr + i, n - i);
}

// 璁＄畻涓綅鏁�
double median(double *arr, int n) {
    // 鎵�鏈夊彉閲忕Щ鍒伴《閮�
    double *copy;
    double med;
    int i;

    copy = (double*)malloc(n * sizeof(double));
    if (!copy) return 0.0;  // 鍐呭瓨鍒嗛厤澶辫触

    for (i = 0; i < n; i++) {
        copy[i] = arr[i];
    }

    quicksort(copy, n);

    if (n % 2 == 1) {
        med = copy[n/2];
    } else {
        med = (copy[n/2-1] + copy[n/2]) * 0.5;
    }

    free(copy);
    return med;
}
/**
 * 瀵圭О绱㈠紩鏄犲皠鍑芥暟锛岀敤浜庢ā鎷烳ATLAB涓殑'sym'妯″紡杈圭晫寤舵嫇銆�
 * 
 * @param i 褰撳墠璁＄畻鐨勭储寮曚綅缃��
 * @param N 淇″彿闀垮害銆�
 * @return 鏄犲皠鍚庣殑绱㈠紩浣嶇疆銆�
 */
int symmetric_index(int i, int N) {
    while (i < 0) { // 濡傛灉绱㈠紩灏忎簬0锛屽垯缈昏浆鍒版鍖洪棿
        i = -i - 1;
    }
    while (i >= N) { // 濡傛灉绱㈠紩瓒呭嚭淇″彿闀垮害锛屽垯闀滃儚鍥炰俊鍙峰唴
        i = 2 * N - i - 1;
    }
    return i;
}
// 鍗风Н + 涓嬮噰鏍凤紙瀵圭О杈圭晫锛�
void dwt_conv_down(const double *input, int len_in, const double *filt, int len_filt, double *output) {
    // 鎵�鏈夊彉閲忕Щ鍒伴《閮�
    int out_len;
    int i, j, center, n, idx;
    double sum;

    out_len = (len_in + 1) / 2;  // 淇闀垮害

    for (i = 0; i < out_len; i++) {
        center = 2 * i;
        sum = 0.0;
        for (j = 0; j < len_filt; j++) {
            n = center - (len_filt - 1)/2 + j;
            idx = symmetric_index(n, len_in);
            sum += input[idx] * filt[len_filt - 1 - j];
        }
        output[i] = sum;
    }
}


// 涓婇噰鏍� + 鍗风Н锛堥噸鏋勶級
void idwt_up_conv(const double *input, int len_in, const double *filt, int len_filt, double *output, int len_out) {
    // 鎵�鏈夊彉閲忕Щ鍒伴《閮�
    int i, j, pos_in, pos_out;
    int radius;

    memset(output, 0, len_out * sizeof(double));
    radius = (len_filt - 1) / 2;  // 3 for sym4

    for (i = 0; i < len_in; i++) {
        pos_in = 2 * i;
        for (j = 0; j < len_filt; j++) {
            pos_out = pos_in + j - radius;
            if (pos_out >= 0 && pos_out < len_out) {
                output[pos_out] += input[i] * filt[len_filt - 1 - j];
            }
        }
    }
}

// 杞槇鍊煎嚱鏁�
void soft_threshold(double *vec, int n, double thr) {
    int i;
    for (i = 0; i < n; i++) {
        if (vec[i] > thr)
            vec[i] -= thr;
        else if (vec[i] < -thr)
            vec[i] += thr;
        else
            vec[i] = 0.0;
    }
}

void denoise_emg_wavelab(double *data, int N, double fs, double *denoised) {
    // === 鎵�鏈夊彉閲忓０鏄庣Щ鍒伴《閮� ===
    const int levels = 6;
    double *work, *tmp_low, *tmp_high;
    double *details[6];
    int det_len[6];
    int current_len;
    int lev, out_len;
    double *abs_d6;
    double med_abs, sigma, thr;
    int i;

    // 鍒嗛厤鍐呭瓨
    work = (double*)malloc(N * sizeof(double));
    tmp_low = (double*)malloc(N * sizeof(double));
    tmp_high = (double*)malloc(N * sizeof(double));
    if (!work || !tmp_low || !tmp_high) {
        // 閿欒澶勭悊
        if (work) free(work);
        if (tmp_low) free(tmp_low);
        if (tmp_high) free(tmp_high);
        return;
    }

    memcpy(work, data, N * sizeof(double));
    current_len = N;

    // === 1. 澶氱骇灏忔尝鍒嗚В ===
    for (lev = 0; lev < levels; lev++) {
        out_len = (current_len + 1) / 2;
        dwt_conv_down(work, current_len, sym4_h, 8, tmp_low);
        dwt_conv_down(work, current_len, sym4_g, 8, tmp_high);

        details[lev] = (double*)malloc(out_len * sizeof(double));
        det_len[lev] = out_len;
        memcpy(details[lev], tmp_high, out_len * sizeof(double));
        memcpy(work, tmp_low, out_len * sizeof(double));
        current_len = out_len;
    }

    // === 2. 鍣０浼拌 ===
    abs_d6 = (double*)malloc(det_len[5] * sizeof(double));
   

    for (i = 0; i < det_len[5]; i++) {
        abs_d6[i] = fabs(details[5][i]);
    }
    med_abs = median(abs_d6, det_len[5]);
    sigma = med_abs / 0.6745;
    // thr = sigma * sqrt(2.0 * log((double)N));
    thr = sigma * sqrt(2.0 * log((double)N));
    printf("Threshold: %.15f  sigma:%.15f\n", thr,sigma);
    free(abs_d6);

    // === 3. 杞槇鍊� ===
    for (lev = 0; lev < levels; lev++) {
        soft_threshold(details[lev], det_len[lev], thr);
    }

    // === 4. 閲嶆瀯 ===
    memcpy(tmp_low, work, current_len * sizeof(double));
    for (lev = levels - 1; lev >= 0; lev--) {
        out_len = (lev == 5) ? N : det_len[lev] * 2;
        idwt_up_conv(tmp_low, current_len, sym4_h, 8, work, out_len);
        idwt_up_conv(details[lev], det_len[lev], sym4_g, 8, tmp_high, out_len);
        for (i = 0; i < out_len; i++) {
            work[i] += tmp_high[i];
        }
        memcpy(tmp_low, work, out_len * sizeof(double));
        current_len = out_len;
    }

    memcpy(denoised, work, N * sizeof(double));


    free(work);
    free(tmp_low);
    free(tmp_high);
    for (i = 0; i < levels; i++) {
        if (details[i]) free(details[i]);
    }
}



void remove_dc_offset(const double* src, double* dest, int n) {
    double sum, mean;
    int i;

    if (n <= 0) return;

    sum = 0.0;
    for (i = 0; i < n; i++) {
        sum += src[i];
    }
    mean = sum / n;

    for (i = 0; i < n; i++) {
        dest[i] = src[i] - mean;
    }
}


// ================= 涓诲鐞嗗嚱鏁� =================
void emg_denoised(double *emg_raw, int length, int fs, double *emg_denoised) {
    // === 鎵�鏈夊彉閲忓０鏄庣Щ鍒伴《閮� ===
    double *temp1;
    double *temp2;
    NotchFilter filter;
    int i;

    // 楠岃瘉杈撳叆鏈夋晥鎬�
    if (!emg_raw || length <= 0 || !emg_denoised) {
        fprintf(stderr, "閿欒锛氭棤鏁堣緭鍏ュ弬鏁癨n");
        return;
    }
    
    // 鍒嗛厤涓存椂鍐呭瓨
    temp1 = (double *)malloc(length * sizeof(double));
    temp2 = (double *)malloc(length * sizeof(double));
    
    if (!temp1 || !temp2) {
        fprintf(stderr, "閿欒锛氬唴瀛樺垎閰嶅け璐n");
        if (temp1) free(temp1);
        if (temp2) free(temp2);
        return;
    }
    
    // 鍒濆鍖栧唴瀛�
    memset(temp1, 0, length * sizeof(double));
    memset(temp2, 0, length * sizeof(double));

    // 鍒濆鍖栭櫡娉㈡护娉㈠櫒锛堜娇鐢ㄩ瀹氫箟绯绘暟锛�
    for (i = 0; i < 3; i++) {
        filter.b[i] = NOTCH_B[i];
        filter.a[i] = NOTCH_A[i];
    }

    // 閲嶇疆婊ゆ尝鍣ㄧ姸鎬�
    notchFilterReset(&filter);

    // 鍘婚櫎鐩存祦鍋忕疆
    remove_dc_offset(emg_raw, temp2, length);

    // 闄锋尝婊ゆ尝锛�50Hz 鎴� 60Hz锛�
    notchFilterProcessArray(&filter, temp2, temp1, length);

    // 鍙屽悜浜岄樁鑺傛护娉紙闆剁浉浣嶆护娉級
    filtfilt_sos(temp1, emg_denoised, length, sos, 2);

    // 灏忔尝鍘诲櫔锛堜娇鐢� sym4锛�
    denoise_emg_wavelab(emg_denoised, length, fs, temp2);

    // 灏嗘渶缁堢粨鏋滃鍒跺埌杈撳嚭
    for (i = 0; i < length; i++) {
        emg_denoised[i] = temp2[i];
    }
   
    // 娓呯悊鎵�鏈変复鏃跺唴瀛�
    free(temp1);
    free(temp2);
}
/*
#define DATA_SIZE 2000  // 瀹氫箟鏁版嵁閲忓ぇ灏�
int main1() {
    // 1. 鎵撳紑杈撳叆鏂囦欢
    const char* input_filename = "E:/workspace/C++c/emg/emg_row2000.txt";
    const char* output_filename = "E:/workspace/C++c/emg/emg_denoised_row2000.txt";
    system("chcp 65001 > nul");
    setlocale(LC_ALL, ".UTF8");
    FILE *input_file = fopen(input_filename, "r");
    if (input_file == NULL) {
        perror("鎵撳紑杈撳叆鏂囦欢澶辫触");
        return EXIT_FAILURE;
    }

    // 2. 鍔ㄦ�佸垎閰嶅唴瀛樺瓨鍌ㄦ暟鎹�
    // double *data = (double*)malloc(DATA_SIZE * sizeof(double));
      double inputdata[2000]={0};
      double outputdata[2000]={0};
    if (inputdata == NULL) {
        perror("鍐呭瓨鍒嗛厤澶辫触");
        fclose(input_file);
        return EXIT_FAILURE;
    }

    // 3. 璇诲彇鏁版嵁骞堕獙璇佸畬鏁存��
    int valid_count = 0;
    for (int i = 0; i < DATA_SIZE; i++) {
        if (fscanf(input_file, "%lf", &inputdata[i]) != 1) {
            fprintf(stderr, "绗� %d 涓暟鎹В鏋愬け璐ワ紙鍙兘鏍煎紡閿欒锛塡n", i + 1);
            break;
        }
        valid_count++;
    }
    fclose(input_file);  // 鍏抽棴杈撳叆鏂囦欢

    // 4. 鎵撳嵃鏁版嵁锛堝彲閫夛細闄愬埗鎵撳嵃鍓�10涓級
   
     emg_denoised(inputdata,2000,1000,outputdata);

    //   printf("馃搳 鏁版嵁棰勮锛堝墠10涓級:\n");
    // for (int i = 1000; i < 1100 && i < valid_count; i++) {
    //     printf("Data[%d] = %f\n", i, outputdata[i]);
    // }
    // 5. 淇濆瓨鏁版嵁鍒拌緭鍑烘枃浠�
    FILE *output_file = fopen(output_filename, "w");
    if (output_file == NULL) {
        perror("鍒涘缓杈撳嚭鏂囦欢澶辫触");
        // free(data);
        return EXIT_FAILURE;
    }

    for (int i = 0; i < valid_count; i++) {
        fprintf(output_file, "%.15g\n", outputdata[i]);  // 楂樼簿搴︿繚瀛�
    }
    fclose(output_file);
 
    
    printf("datasave output.txt锛堝叡 %d 涓湁鏁堝�硷級\n", valid_count);

    // 6. 閲婃斁鍐呭瓨
    // free(data);
    return EXIT_SUCCESS;
}
*/