a8k_app/lib-algo/opt_algo.cpp

#include "opt_algo.hpp"
#include "logger.hpp"

using namespace opt_algo;
using namespace std;

#define TAG "OptAlgo"

#define BASE_LINE_SLOPE_SAMPLE_POS 240
#define VERSION                    1
static void algo_assert(bool condition, const char* msg) {
  if (!condition) {
    ZLOGE(TAG, "algo_assert:%s", msg);
    throw std::runtime_error(msg);
  }
}

int OptAlgo::getAlgoVersion() { return VERSION; }

int OptAlgo::calculate_peak_num(vector<float>& ogigin_val) {
  float avg     = 0;
  int   peakNum = 0;

  for (size_t i = 0; i < ogigin_val.size(); i++) {
    avg += ogigin_val[i];
  }
  avg = avg / ogigin_val.size();

  bool findPeak = false;
  for (size_t i = 0; i < ogigin_val.size(); i++) {
    if (!findPeak && ogigin_val[i] > avg) {
      findPeak = true;
      peakNum++;
    }

    if (findPeak && ogigin_val[i] < avg) {
      findPeak = false;
    }
  }
  return peakNum;
}

shared_ptr<AlgoResult> OptAlgo::calculate(vector<float> ogigin_val, int peaknum) {
  shared_ptr<AlgoResult> algoResult = make_shared<AlgoResult>();

  algoResult->ogigin_val = ogigin_val;

  algoResult->supper_val            = super_sampling(ogigin_val, ogigin_val.size(), 5);  // pointNum:1200*5=6000
  algoResult->supper_median_val     = median_filtering(algoResult->supper_val, 25);      // pointNum:6000
  algoResult->supper_smooth_sub_val = sub_sampling(algoResult->supper_median_val, 6);    // pointNum:6000/6=1000

  algoResult->lineContext->raw     = sub_sampling(algoResult->supper_median_val, 6);
  algoResult->lineContext->avg     = smooth_windows(algoResult->supper_smooth_sub_val, 13);
  vector<float> diffpreprocess     = least_square_method_differentiate(algoResult->supper_smooth_sub_val, 13);
  algoResult->lineContext->diff    = smooth_windows(diffpreprocess, 13);
  algoResult->lineContext->agvline = find_avg_line(algoResult->supper_smooth_sub_val);

  algoResult->lineContext->raw250  = sub_sampling(algoResult->lineContext->raw, 4);
  algoResult->lineContext->avg250  = sub_sampling(algoResult->lineContext->avg, 4);
  algoResult->lineContext->diff250 = sub_sampling(algoResult->lineContext->diff, 4);

  /**
   * @brief
   *
   *   当前系统中反应的结果一共5种
   *
   *   峰数量2:
   *      80,120
   *   峰数量3:
   *      40,80,120
   *   峰的数量4:
   *      40,80,120,160
   *   峰的数量5:
   *      40,80,120,160,200
   *
   */

  /**
   * @brief
   *    这几取曲线在绝对坐标40(10*4)位置的曲率，因为按照巴迪泰给的项目参考，在前坐标40（250个点的情况），是第一个峰，所以
   * 这里假设坐标10位置是第一个峰的前置位置，然后取曲线在这个位置的斜率，作为基线斜率。
   */

  // int baseline_sample_pos = 0;
  vector<int> slop_smaple_xstart;
  if (peaknum == 2) {
    slop_smaple_xstart.push_back(10 * 4);
    slop_smaple_xstart.push_back(20 * 4);
    slop_smaple_xstart.push_back(200 * 4);
  } else if (peaknum == 3) {
    slop_smaple_xstart.push_back(10 * 4);
    slop_smaple_xstart.push_back(200 * 4);
    slop_smaple_xstart.push_back(235 * 4);

  } else if (peaknum == 4) {
    slop_smaple_xstart.push_back(10 * 4);
    slop_smaple_xstart.push_back(200 * 4);
    slop_smaple_xstart.push_back(235 * 4);

  } else if (peaknum == 5) {
    slop_smaple_xstart.push_back(10 * 4);
    slop_smaple_xstart.push_back(235 * 4);
  }

  linear_least_squares_muti_windos(&algoResult->lineContext->avg[0], (int)algoResult->lineContext->avg.size(), slop_smaple_xstart,
                                   5 * 4,                                    //
                                   algoResult->lineContext->baseline_slope,  //
                                   algoResult->lineContext->baseline_intercept);

  for (size_t i = 0; i < peaknum; i++) {
    Error_t ret = k_ecode_ok;
    if (i == 0) {
      if (peaknum == 2) {
        ret = findpeak(algoResult->lineContext, 80 * 4, 80 * 4, algoResult->peakInfo[0]);
      } else if (peaknum == 3) {
        ret = findpeak(algoResult->lineContext, 40 * 4, 80 * 4, algoResult->peakInfo[0]);
      } else if (peaknum == 4) {
        ret = findpeak(algoResult->lineContext, 40 * 4, 80 * 4, algoResult->peakInfo[0]);
      } else if (peaknum == 5) {
        ret = findpeak(algoResult->lineContext, 40 * 4, 80 * 4, algoResult->peakInfo[0]);
      }
    } else {
      ret = findpeak(algoResult->lineContext, algoResult->peakInfo[i - 1]->peak_end_pos, 80 * 4, algoResult->peakInfo[i]);
    }

    if (ret != k_ecode_ok) {
      algoResult->error_code = ret;
      break;
    }
  }

  algoResult->peakNum = peaknum;
  return algoResult;
}

vector<float> OptAlgo::differentiate(vector<float>& inputRaw) {
  /**
   * @brief
   *  巴迪泰源码，对原始数据添加了一些微小的值，原因未知
   */
  for (int i = 0; i <= inputRaw.size() - 8; i += 8) {
    inputRaw[i + 1] = inputRaw[i + 1] + 0.001f;
    inputRaw[i + 2] = inputRaw[i + 2] + 0.002f;
    inputRaw[i + 3] = inputRaw[i + 3] + 0.003f;
    inputRaw[i + 4] = inputRaw[i + 4] + 0.004f;
    inputRaw[i + 5] = inputRaw[i + 5] + 0.005f;
    inputRaw[i + 6] = inputRaw[i + 6] + 0.004f;
    inputRaw[i + 7] = inputRaw[i + 7] + 0.003f;
    inputRaw[i + 8] = inputRaw[i + 8] + 0.002f;
  }

  /**
   * @brief
   * @Warning: 此处求导和巴迪泰的存在差异，
   * 巴迪泰的是当前数值减去下一个数值，
   * 而此处是当前数值减去上一个数值
   */

  vector<float> differentiateRaw(inputRaw.size(), 0);
  for (size_t i = 1; i < differentiateRaw.size(); i++) {
    differentiateRaw[i] = inputRaw[i] - inputRaw[i - 1];
  }
  differentiateRaw[0] = differentiateRaw[1];
  return differentiateRaw;
}

vector<float> OptAlgo::least_square_method_differentiate(vector<float>& inputRaw, int windows_size) {
  algo_assert(windows_size > 0, "windows_size <= 0");
  algo_assert(windows_size % 2 == 1, "windows_size is not odd");

  vector<float> differentiateRaw(inputRaw.size(), 0);
  vector<float> windowsRaw(windows_size, 0);
  int           windows_size_half = (windows_size - 1) / 2;

  for (int index = windows_size_half; index < inputRaw.size() - windows_size_half; index++) {
    windowsRaw      = getwindowspoint(inputRaw, index, windows_size);
    float intercept = 0;
    linear_least_squares(windowsRaw.data(), windows_size, differentiateRaw[index], intercept);
  }

  for (size_t i = 0; i < windows_size_half; i++) {
    differentiateRaw[i] = differentiateRaw[windows_size_half];
  }

  for (size_t i = inputRaw.size() - windows_size_half; i < inputRaw.size(); i++) {
    differentiateRaw[i] = differentiateRaw[inputRaw.size() - windows_size_half - 1];
  }
  return differentiateRaw;
}
/**
 * @brief 最小二乘法求解曲线斜率
 *
 * @param val Y轴数据
 * @param size Y轴数据长度
 * @return float 斜率
 */

void OptAlgo::linear_least_squares(vector<float>& x, vector<float>& y, float& slope, float& intercept) {
  size_t n    = x.size();
  double sumX = 0.0, sumY = 0.0, sumXY = 0.0, sumXX = 0.0;
  for (size_t i = 0; i < n; ++i) {
    sumX += x[i];
    sumY += y[i];
    sumXY += x[i] * y[i];
    sumXX += x[i] * x[i];
  }
  double xMean = sumX / n;
  double yMean = sumY / n;

  algo_assert(!feq((sumXX - n * xMean * xMean), 0, 0.0001), "sumXX - n * xMean * xMean == 0");
  slope     = (sumXY - n * xMean * yMean) / (sumXX - n * xMean * xMean);
  intercept = yMean - slope * xMean;
  return;
}

void OptAlgo::linear_least_squares(float* y, int size, float& slope, float& intercept) {
  vector<float> xpoint(size, 0);
  vector<float> ypoint(size, 0);

  for (size_t i = 0; i < size; i++) {
    xpoint[i] = i;
    ypoint[i] = y[i];
  }
  return linear_least_squares(xpoint, ypoint, slope, intercept);
}

void OptAlgo::linear_least_squares_muti_windos(float* y, int size, vector<int> startx, int windowssize, float& slope, float& intercept) {
  vector<float> xpoint;
  vector<float> ypoint;

  // ZLOGI(TAG, "xxxxx%d", startx.size());

  for (size_t i = 0; i < startx.size(); i++) {
    int xstart = startx[i];

    for (size_t xindex = xstart; xindex < (xstart + windowssize); xindex++) {
      // ZLOGI(TAG, "xindex:%d y:%f", xindex, y[xindex]);
      xpoint.push_back(xindex);
      ypoint.push_back(y[xindex]);
    }
  }
  return linear_least_squares(xpoint, ypoint, slope, intercept);
}

vector<float> OptAlgo::super_sampling(vector<float>& inputRaw, int32_t nInputLength, int32_t nUpSampleRate) {
  /**
   * @brief
   *
   */
  int           nOutputLength = nInputLength * nUpSampleRate;
  vector<float> upSamplingRaw(nOutputLength, 0);

  for (int si = 0, di = 0; si < nInputLength - 1; di++) {
    float a = upSamplingRaw[di * nUpSampleRate] = (float)inputRaw[si];
    float b = upSamplingRaw[(di + 1) * nUpSampleRate] = (float)inputRaw[++si];

    float nSlope = (b - a) / nUpSampleRate;

    for (int i = 0; i < nUpSampleRate - 1; i++) {
      int baseIndex                = (di * nUpSampleRate) + i;
      upSamplingRaw[baseIndex + 1] = upSamplingRaw[baseIndex] + nSlope;
    }
  }

  return upSamplingRaw;
}

vector<float> OptAlgo::sub_sampling(vector<float>& inputRaw, int nSubSampleRate) {
  int   nSum          = 0;
  float fAvg          = 0;
  int   subIndex      = 0;
  int   nOutputLength = inputRaw.size() / nSubSampleRate;

  vector<float> subSampledRaw(nOutputLength, 0);

  for (int index = 0; index < inputRaw.size(); index++) {
    if (index % nSubSampleRate == 0 && index > 0) {
      fAvg = nSum / nSubSampleRate;
      if (subIndex < subSampledRaw.size()) {
        subSampledRaw[subIndex++] = fAvg;
      } else {
        int empty = 0;
      }
      nSum = 0;
    }
    nSum += inputRaw[index];
  }
  subSampledRaw[subSampledRaw.size() - 1] = subSampledRaw[subSampledRaw.size() - 2];
  return subSampledRaw;
}

vector<float> OptAlgo::smooth_windows(vector<float>& inputRaw, int windows_size) {
  vector<float> smoothRaw(inputRaw.size(), 0);
  int           windows_size_half = (windows_size - 1) / 2;

  for (int index = windows_size_half; index < inputRaw.size() - windows_size_half; index++) {
    float sum = 0;
    for (int i = index - windows_size_half; i <= index + windows_size_half; i++) {
      sum += inputRaw[i];
    }
    smoothRaw[index] = sum / windows_size;
  }

  for (size_t i = 0; i < windows_size_half; i++) {
    smoothRaw[i] = smoothRaw[windows_size_half];
  }

  for (size_t i = inputRaw.size() - windows_size_half; i < inputRaw.size(); i++) {
    smoothRaw[i] = smoothRaw[inputRaw.size() - windows_size_half - 1];
  }

  return smoothRaw;
}
vector<float> OptAlgo::median_filtering(vector<float>& inputRaw, int windows_size) {
  vector<float> medianRaw(inputRaw.size(), 0);
  vector<float> windows(windows_size, 0);

  int windows_size_half = (windows_size - 1) / 2;

  for (int index = windows_size_half; index < inputRaw.size() - windows_size_half; index++) {
    for (int i = 0; i < windows_size; i++) {
      windows[i] = inputRaw[index + i - windows_size_half];
    }
    sort_vector(windows);  // 从小到大顺序排序
    medianRaw[index] = windows[windows_size_half + 1];
  }

  for (size_t i = 0; i < windows_size_half; i++) {
    medianRaw[i] = medianRaw[windows_size_half];
  }

  for (size_t i = inputRaw.size() - windows_size_half; i < inputRaw.size(); i++) {
    medianRaw[i] = medianRaw[inputRaw.size() - windows_size_half - 1];
  }

  return medianRaw;
}

/**
 * @brief 求数据的均值
 *
 * @param inputRaw
 * @return float
 */
float OptAlgo::find_avg_line(vector<float>& inputRaw) {
  float base_min = 500;
  float fsum     = 0;
  int   cnt      = 0;

  int range = inputRaw.size();

  do {
    fsum = cnt = 0;
    for (int i = 1; i < range; i++) {
      if (inputRaw[i] < base_min) {
        fsum += inputRaw[i];
        cnt++;
      }
    }

    base_min = base_min + 50;
  } while (cnt < range - 15 * inputRaw.size() / 250);

  float fbase = fsum / cnt;
  return fbase;
}
bool OptAlgo::feq(float a, float b, float epsilon) {
  float dv = a - b;
  if (dv < 0) dv = -dv;
  return dv <= epsilon;
}

Error_t OptAlgo::findpeak(shared_ptr<LineContext> lineContext, int32_t search_start, int32_t peakwindth,
                          shared_ptr<PeakInfo> retpeak) {  //

  /**
   * @brief 查找峰的位置
   *
   * 思路:
   *    搜索所有大于均值的点，当点满足，其数值大于附近一定数量的点时，认为是峰的位置
   *
   *
   */

  ZLOGI(TAG, "findpeak start:%d windth:%d", search_start, peakwindth);

  // judge_win_size
  int judge_win_size = 2 * (lineContext->avg.size() / 250) + 1;

  int peakoff =  //
      sub_find_peak(lineContext, search_start, peakwindth, judge_win_size);
  if (peakoff < 0) return k_ecode_can_not_find_peak;

  retpeak->peak_pos = peakoff;

  /**
   * @brief 查找峰的起始位置
   *
   * 思路:
   *    从峰的位置开始，向前搜索，找到一个点，其值小于平均值。
   *    然后继续向前搜索，找到一个点，斜率接近基线斜率。
   *
   */
  int peak_start_pos = find_peak_endpoint(lineContext, retpeak->peak_pos, -1, peakwindth / 2);
  int peak_end_pos   = find_peak_endpoint(lineContext, retpeak->peak_pos, 1, peakwindth / 2);

  if (peak_start_pos < 0) return k_ecode_can_not_find_peak_start;
  if (peak_end_pos < 0) return k_ecode_can_not_find_peak_end;

  retpeak->peak_start_pos = peak_start_pos;
  retpeak->peak_end_pos   = peak_end_pos;

  /**
   * @brief
   *  计算峰的面积
   */

  float peak_full_area = 0;
  for (int i = peak_start_pos; i <= peak_end_pos; i++) {
    peak_full_area += lineContext->raw[i];
  }

  float peak_base_line_area = 0;

#if 1
  peak_base_line_area =  //
      (lineContext->raw[peak_start_pos] + lineContext->raw[peak_end_pos]) * 0.5 * (peak_end_pos - peak_start_pos + 1);
#else
  for (int i = peak_start_pos; i <= peak_end_pos; i++) {
    peak_base_line_area += i * lineContext->baseline_slope + lineContext->baseline_intercept;
  }
#endif

  retpeak->peak_full_area      = peak_full_area;
  retpeak->peak_base_line_area = peak_base_line_area;
  retpeak->area                = peak_full_area - peak_base_line_area;
  if (retpeak->area <= 0) retpeak->area = 0;
  retpeak->find_peak = true;

  return k_ecode_ok;
}

int OptAlgo::find_peak_endpoint(shared_ptr<LineContext> lineContext,
                                int                     peakpos,           //
                                int                     search_direction,  //
                                int                     search_windows) {
  /**
   * @brief
   *  通过波峰的位置查找波峰的起始位置
   *
   *  逻辑:
   *    1. 从波峰的位置开始，向前搜索，找到一个点，其值小于平均值。
   *    2. 然后继续向前搜索，找到一个点，斜率接近基线斜率。
   *
   */

  int off = -1;
  ZLOGI(TAG, "find peakend top_pos:%d direction:%d windows:%d", peakpos, search_direction, search_windows);

  //
  algo_assert(search_windows > 0, "search_windows <= 0");
  algo_assert(search_direction == 1 || search_direction == -1, "search_direction != 1 && search_direction != -1");
  //
  int index_dval   = search_direction >= 0 ? 1 : -1;
  int search_start = peakpos;
  int search_end   = peakpos + search_direction * search_windows;

  algo_assert(search_end >= 0, "search_end < 0");
  algo_assert(lineContext->avg.size() > search_end, "lineContext->avg.size() <= search_start");

  for (int i = search_start; i != search_end; i += index_dval) {
    float now = lineContext->avg[i];
    if (now >= lineContext->agvline) continue;

    if (search_direction == 1) {
      if (feq(lineContext->diff[i], lineContext->baseline_slope, 0.3) || lineContext->diff[i] >= lineContext->baseline_slope) {
        off = i;
        break;
      }
    } else {
      if (feq(lineContext->diff[i], lineContext->baseline_slope, 0.3) || lineContext->diff[i] <= lineContext->baseline_slope) {
        off = i;
        break;
      }
    }
  }
  return off;
}

int OptAlgo::sub_find_peak(shared_ptr<LineContext> lineContext, int start_off, int windos_size, int judge_win_size) {  //
  ZLOGI(TAG, "sub_find_peak %d %d %d", start_off, windos_size, judge_win_size);
  float maxv    = 0;
  int   peakoff = -1;
  bool  findmax = false;
  for (size_t index = 0; index < windos_size; index++) {
    int off = index + start_off;
    // 从窗口的一半大小开始判断
    if (findmax && lineContext->avg[off] <= lineContext->agvline) break;
    if (off < judge_win_size / 2) continue;
    // 查找的点要大于基线
    if (lineContext->avg[off] <= lineContext->agvline) continue;
    // 判断的
    if ((off + judge_win_size / 2) > (lineContext->avg.size() - 1)) break;
    // 找到最大的峰值，这里判断用于去除一个波峰中的某个临时的小波峰
    if (maxv > lineContext->avg[off]) continue;

    /**
     * @brief 检查当前位置的点，是否是附近最大的点
     */
    if (is_maxval_in_windows(&(lineContext->avg[off]), judge_win_size)) {
      findmax = true;
      maxv    = lineContext->avg[off];
      peakoff = off;
    }
  }
  return peakoff;
}

bool OptAlgo::is_maxval_in_windows(float* val, int windows_size) {
  algo_assert(windows_size > 0, "windows_size <= 0");
  algo_assert(windows_size % 2 == 1, "windows_size is not odd");

  bool   ret         = true;
  float* valstartpos = val - windows_size / 2;
  for (size_t i = 0; i < windows_size; i++) {
    if (&valstartpos[i] == val) continue;

    if (valstartpos[i] > *val) {
      ret = false;
      break;
    }
  }

  return ret;
}

float OptAlgo::get_avg_in_windows(vector<float>& src, int off, int windows) {
  float sum = 0;
  algo_assert(windows % 2 == 1, "windows is not odd");
  for (int i = off - windows / 2; i <= off + windows / 2; i++) {
    sum += src[i];
  }
  return sum / windows;
}

void OptAlgo::sort_vector(vector<float>& src) {
  // 实现冒泡排序
  for (int i = 0; i < src.size(); i++) {
    for (int j = 0; j < src.size() - i - 1; j++) {
      if (src[j] > src[j + 1]) {
        float temp = src[j];
        src[j]     = src[j + 1];
        src[j + 1] = temp;
      }
    }
  }
}

vector<float> OptAlgo::getwindowspoint(vector<float>& src, int off, int windows) {
  vector<float> ret(windows, 0);
  int           retindex = 0;
  for (int i = off - windows / 2; i <= off + windows / 2; i++) {
    ret[retindex] = src[i];
    retindex++;
  }
  return ret;
}