a8k_optalgo/a8k_opt_algo.cpp

#include "a8k_opt_algo.hpp"

#include <stdarg.h>

void zos_log(const char* fmt, ...);

#define ZLOGI(fmt, ...) zos_log("INFO " fmt "\n", ##__VA_ARGS__);
#define ZLOGD(fmt, ...) zos_log("DEBU " fmt "\n", ##__VA_ARGS__);
#define ZLOGE(fmt, ...) zos_log("ERRO " fmt "\n", ##__VA_ARGS__);
#define ZLOGW(fmt, ...) zos_log("WARN " fmt "\n", ##__VA_ARGS__);

namespace a8k_opt_algo {
using namespace std;

typedef enum {
  ktopt,
  kfopt,
} opt_type_t;

static void algo_assert(bool condition, const char* msg) {
  if (!condition) {
    ZLOGE("algo_assert:%s\n", msg);
    throw std::runtime_error(msg);
  }
}
bool feq(float a, float b, float epsilon) {
  float dv = a - b;
  if (dv < 0) dv = -dv;
  return dv <= epsilon;
}

opt_type_t     m_opttype;
PorcessContext m_cxt;

static vector<float> sub_sampling(vector<float>& inputRaw, int nSubSampleRate);
vector<float>        super_sampling(vector<float>& inputRaw, int32_t nInputLength, int32_t nUpSampleRate);

static vector<float> 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> 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> 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;
}

/**
 * @brief 最小二乘法求解曲线斜率
 *
 * @param val Y轴数据
 * @param size Y轴数据长度
 * @return float 斜率
 */

void 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 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 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> 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;
}
vector<float> 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;
}

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

/***********************************************************************************************************************
 *                                                      ALGO_IMPL                                                      *
 ***********************************************************************************************************************/

int32_t findPeakTurnPoint(vector<float>& data, int32_t search_start, int32_t suggest_search_end) {
  int32_t search_end = 0;
  for (int32_t i = search_start; i < suggest_search_end; i++) {
    if (data[i] > m_cxt.agvline) {
      search_end = i;
    }
  }

  int32_t peakTurnPos = 0;
  float   maxdiff2    = 0;
  for (int32_t i = search_start; i < search_end; i++) {
    if (m_cxt.diffX2[i] > maxdiff2) {
      maxdiff2    = m_cxt.diffX2[i];
      peakTurnPos = i;
    }
  }
  return peakTurnPos;
}

float computePeakArea(vector<float>& data, int32_t start, int32_t end) {
  float area = 0;
  for (int i = start; i < end; i++) {
    area += data[i];
  }

  float baselinearea = 0;
  baselinearea       = (data[start] + data[end]) * abs(end - start) / 2;

  return abs(area - baselinearea);
}

void findpeak(vector<float>& data, int32_t search_start, int32_t search_end, PeakInfo& retpeak) {
  // find peak
  ZLOGI("find peak in [%d %d]", search_start, search_end);
  retpeak.find_peak      = false;
  retpeak.area           = 0;
  retpeak.peak_pos       = 0;
  retpeak.peak_start_pos = 0;
  retpeak.peak_end_pos   = 0;

  float max     = 0;
  int   peakpos = 0;
  float midpos  = search_start + (search_end - search_start) / 2;
  for (int i = search_start; i < search_end; i++) {
    if (data[i] > max) {
      max     = data[i];
      peakpos = i;
    }
  }

  if (max < m_cxt.agvline) {
    ZLOGI("invalid peak:%f, max < m_cxt.agvline:%f", max, m_cxt.agvline);
    retpeak.find_peak = false;
    return;
  } else if (peakpos > midpos + 15) {
    ZLOGI("invalid peak:%d, peakpos > midpos + 15:%d", peakpos, midpos + 15);
    retpeak.find_peak = false;
    return;
  } else if (peakpos < midpos - 15) {
    ZLOGI("invalid peak:%d, peakpos < midpos - 15:%d", peakpos, midpos - 15);
    retpeak.find_peak = false;
    return;
  }

  // find_peak_start
  // 从pos向前找20个点，从低于均值线的坐标开始找，找到diff2的最大值
  retpeak.peak_pos       = peakpos;
  retpeak.peak_start_pos = findPeakTurnPoint(data, peakpos - 20, peakpos) - 4;//-4 是经验数值
  retpeak.peak_end_pos   = findPeakTurnPoint(data, peakpos, peakpos + 20) + 4;//+4 是经验数值
  retpeak.area           = computePeakArea(data, retpeak.peak_start_pos, retpeak.peak_end_pos);
  retpeak.find_peak      = true;

  // find_peak_end
  // 从pos向后找20个点，找到diff2的最大值
}

void a8k_opt_algo_process(vector<float>& ogigin_val, OptAlgoResult& result) {
  //
  vector<float> super     = super_sampling(ogigin_val, ogigin_val.size(), 5);
  vector<float> subsample = sub_sampling(super, 24);
  ZLOGI("subsample size:%d", subsample.size());

  m_cxt.raw          = subsample;
  m_cxt.avg          = subsample;
  m_cxt.diff         = least_square_method_differentiate(m_cxt.avg, 5);   // 最小二乘法求曲线斜率集合
  m_cxt.diffX2       = least_square_method_differentiate(m_cxt.diff, 5);  // 最小二乘法求曲线二次斜率集合
  m_cxt.agvline      = find_avg_line(subsample);
  result.displayData = m_cxt.avg;

  // findPeak

  for (size_t i = 0; i < 5; i++) {
    findpeak(m_cxt.avg, 20, 60, result.pin040);
    findpeak(m_cxt.avg, 60, 100, result.pin080);
    findpeak(m_cxt.avg, 100, 140, result.pin120);
    findpeak(m_cxt.avg, 140, 180, result.pin160);
    findpeak(m_cxt.avg, 180, 220, result.pin200);
  }
}

ecode_t A8kOptAlgoProcess(vector<float> ogigin_val, OptAlgoResult& result) {  //
  if (ogigin_val.size() != 1200) {
    return kOptErr_pointNumError;
  }
  a8k_opt_algo_process(ogigin_val, result);
  return kOptErr_suc;
}

void A8kOptAlgoGetProcessContext(PorcessContext& context) { context = m_cxt; }

/***********************************************************************************************************************
 *                                               T_A8kOptAlgoPreProcess                                                *
 ***********************************************************************************************************************/
int32_t t_detector_gain_to_raw_gain(float scan_gain) {
  // opamp_gain = (((100.0 * (float) scan_gain_raw) / 255) + 2.4) / 4.7;
  int32_t scan_gain_raw = 0;
  scan_gain_raw         = (scan_gain * 4.7 - 2.4) * 256. / 100. + 0.5;
  if (scan_gain_raw < 1) scan_gain_raw = 1;
  if (scan_gain_raw > 255) scan_gain_raw = 255;
  return scan_gain_raw;
}
float t_detector_raw_gain_to_gain(int32_t gain) {
  float scan_gain = 0;
  scan_gain       = (((100.0 * (float)gain) / 256) + 2.4) / 4.7;
  return scan_gain;
}

ecode_t T_A8kOptAlgoPreProcess(vector<float> ogigin_val, int32_t now_scan_gain, int32_t expectResultRangeStart, int32_t expectResultRangeEnd, OptAlgoPreProcessResult& result) {
  if (ogigin_val.size() != 1200) {
    return kOptErr_pointNumError;
  }

  int32_t adcgoal = expectResultRangeEnd;
  int32_t maxadc  = ogigin_val[0];

  result.scanAgain       = false;
  result.suggestScanGain = now_scan_gain;

  for (int32_t i = 1; i < ogigin_val.size(); i++) {
    if (maxadc < ogigin_val[i]) {
      maxadc = ogigin_val[i];
    }
  }

  if (maxadc > expectResultRangeStart) {
    result.scanAgain = false;
    return kOptErr_suc;
  }

  float nowgain     = t_detector_raw_gain_to_gain(now_scan_gain);
  float gain_adjust = 0;
  if (maxadc != 0) {
    gain_adjust = nowgain * ((float)adcgoal / (float)maxadc);
  } else {
    gain_adjust = nowgain;
  }
  result.suggestScanGain = t_detector_gain_to_raw_gain(gain_adjust);
  result.scanAgain       = true;
  return kOptErr_suc;
}

/***********************************************************************************************************************
 *                                               F_A8kOptAlgoPreProcess                                                *
 ***********************************************************************************************************************/
float f_detector_raw_gain_to_gain(int32_t gain) {
  float scan_gain = 0;
  scan_gain       = (((100. / 256.) * (float)gain) + 0.125) / 2.15 + 1.;
  return scan_gain;
}
int32_t f_detector_gain_to_raw_gain(float scan_gain) {
  //    int32_t scan_gain     = (((100. / 256.) * (float)scan_gain_raw) + 0.125) / 2.15 + 1.;
  int32_t scan_gain_raw = 0;
  scan_gain_raw         = ((scan_gain - 1.0) * 2.15 - 0.125) * 255. / 100. + 0.5;
  if (scan_gain_raw < 1) scan_gain_raw = 1;
  if (scan_gain_raw > 255) scan_gain_raw = 255;
  return scan_gain_raw;
}
ecode_t F_A8kOptAlgoPreProcess(vector<float> ogigin_val, int32_t now_scan_gain, int32_t expectResultRangeStart, int32_t expectResultRangeEnd, OptAlgoPreProcessResult& result) {
  if (ogigin_val.size() != 1200) {
    return kOptErr_pointNumError;
  }

  int32_t adcgoal = expectResultRangeEnd;
  int32_t maxadc  = ogigin_val[0];

  result.scanAgain       = false;
  result.suggestScanGain = now_scan_gain;

  for (int32_t i = 1; i < ogigin_val.size(); i++) {
    if (maxadc < ogigin_val[i]) {
      maxadc = ogigin_val[i];
    }
  }

  if (maxadc > expectResultRangeStart) {
    result.scanAgain = false;
    return kOptErr_suc;
  }

  float nowgain     = f_detector_raw_gain_to_gain(now_scan_gain);
  float gain_adjust = 0;
  if (maxadc != 0) {
    gain_adjust = nowgain * ((float)adcgoal / (float)maxadc);
  } else {
    gain_adjust = nowgain;
  }
  result.suggestScanGain = f_detector_gain_to_raw_gain(gain_adjust);
  result.scanAgain       = true;
  return kOptErr_suc;
}

}  // namespace a8k_opt_algo