a8k_app/lib-algo/a8k_opt_algo.cpp

#include "a8k_opt_algo.hpp"

#include <stdarg.h>
#ifdef BUILD_IN_QT
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__);

#else
#define ZLOGI(fmt, ...) ;
#define ZLOGD(fmt, ...) ;
#define ZLOGE(fmt, ...) ;
#define ZLOGW(fmt, ...) ;

#endif

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

 

    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 findPeakStartTurnPoint(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 search_end = suggest_search_end;
        int32_t peakTurnPos = search_start;
        float   maxdiff2 = m_cxt.diffX2[search_start];
        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;
    }

    int32_t findPeakEndTurnPoint(vector<float>& data, int32_t search_start, int32_t suggest_search_end) {
        int32_t search_end = suggest_search_end;
        int32_t peakTurnPos = search_start;
        float   maxdiff2 = m_cxt.diffX2[search_start];
        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;
        }

       
        retpeak.peak_pos = peakpos;
        retpeak.peak_start_pos = findPeakStartTurnPoint(data, peakpos - 20, peakpos) - 4;
        retpeak.peak_end_pos = findPeakEndTurnPoint(data, peakpos, peakpos + 20) + 4; 
        retpeak.area = computePeakArea(data, retpeak.peak_start_pos, retpeak.peak_end_pos);
        retpeak.find_peak = true;

    }

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

        result.input = ogigin_val;
        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