sensors.h

#pragma once
 
#include "ustd_platform.h"
#include "ustd_array.h"
 
namespace ustd {
#define SENSOR_VALUE_INVALID -999999.0
 
class sensorprocessor {
  public:
    unsigned int noVals = 0;
    unsigned int smoothInterval;
    unsigned int pollTimeSec;
    double sum = 0.0;
    double eps;
    bool first = true;
    double meanVal = 0;
    double lastVal = SENSOR_VALUE_INVALID;
    unsigned long last;
 
    sensorprocessor(unsigned int smoothInterval = 5, int unsigned pollTimeSec = 60,
                    double eps = 0.1)
        : smoothInterval{smoothInterval}, pollTimeSec{pollTimeSec}, eps{eps} {
        reset();
    }
 
    bool filter(double *pvalue) {
        meanVal = (meanVal * noVals + (*pvalue)) / (noVals + 1);
        if (noVals < smoothInterval) {
            ++noVals;
        }
        double delta = lastVal - meanVal;
        if (delta < 0.0) {
            delta = (-1.0) * delta;
        }
        if (delta > eps || first) {
            first = false;
            lastVal = meanVal;
            *pvalue = meanVal;
            last = millis();
            return true;
        } else {
            if (pollTimeSec != 0) {
                if (timeDiff(last, millis()) > pollTimeSec * 1000L) {
                    *pvalue = meanVal;
                    last = millis();
                    lastVal = meanVal;
                    return true;
                }
            }
        }
        return false;
    }
 
    bool filter(long *plvalue) {
        double tval = (double)*plvalue;
        bool ret = filter(&tval);
        if (ret) {
            *plvalue = (long)tval;
        }
        return ret;
    }
 
    void reset() {
        noVals = 0;
        sum = 0.0;
        first = true;
        meanVal = 0;
        lastVal = SENSOR_VALUE_INVALID;
        last = millis();
    }
 
    void update(unsigned int _smoothInterval = 5, int unsigned _pollTimeSec = 60,
                    double _eps = 0.1) {
        smoothInterval = _smoothInterval;
        pollTimeSec = _pollTimeSec;
        eps = _eps;
        reset();
    }
};
 
template <typename T_FLOAT> class numericFunction {
  public:
    ustd::array<T_FLOAT> x, y;
    T_FLOAT minX, minY, maxX, maxY;
    unsigned int len;
    bool dir;
    bool extrapolate;
    numericFunction(const T_FLOAT px[], const T_FLOAT py[], unsigned int count,
                    bool _extrapolate = false) {
        extrapolate = _extrapolate;
        len = 0;
        for (unsigned int i = 0; i < count; i++) {
            if (i > 0) {
                // enforce strict monotony:
                if (px[i] <= x[len - 1])
                    continue;
                if (py[i] == y[len - 1])
                    continue;
 
                if (len == 1)
                    dir = (py[i] > y[len - 1]);
                if ((py[i] > y[len - 1]) != dir)
                    continue;
            }
            x[len] = px[i];
            y[len] = py[i];
            if (i == 0 || minX > x[len])
                minX = x[len];
            if (i == 0 || minY > y[len])
                minY = y[len];
            if (i == 0 || maxX < x[len])
                maxX = x[len];
            if (i == 0 || maxY < y[len])
                maxY = y[len];
            ++len;
        }
    }
 
    static T_FLOAT min(ustd::array<T_FLOAT> ar) {
        T_FLOAT minVal = 0.0;
        for (unsigned int i = 0; i < ar.length(); i++) {
            if (i == 0 || ar[i] < minVal)
                minVal = ar[i];
        }
        return minVal;
    }
 
    static T_FLOAT max(ustd::array<T_FLOAT> ar) {
        T_FLOAT maxVal = 0.0;
        for (unsigned int i = 0; i < ar.length(); i++) {
            if (i == 0 || ar[i] > maxVal)
                maxVal = ar[i];
        }
        return maxVal;
    }
 
    static void rescale(ustd::array<T_FLOAT> *par, T_FLOAT *pminX, T_FLOAT *pmaxX, T_FLOAT newMin,
                        T_FLOAT newMax) {
        T_FLOAT dx;
        T_FLOAT newMinX, newMaxX;
        unsigned int len = (*par).length();
        if (len < 2 || *pminX == *pmaxX)
            dx = 1;
        else
            dx = (*pmaxX - *pminX);
        T_FLOAT ndx = newMax - newMin;
        for (unsigned int i = 0; i < len; i++) {
            T_FLOAT xi = (*par)[i];
            (*par)[i] = (xi - *pminX) / dx * ndx + newMin;
            if (i == 0 || newMinX > xi)
                newMinX = xi;
            if (i == 0 || newMaxX < xi)
                newMaxX = xi;
        }
        *pminX = newMinX;
        *pmaxX = newMaxX;
    }
 
    void rescaleX(T_FLOAT newMin, T_FLOAT newMax) {
        rescale(&x, &minX, &maxX, newMin, newMax);
    }
    void rescaleY(T_FLOAT newMin, T_FLOAT newMax) {
        rescale(&y, &minY, &maxY, newMin, newMax);
    }
 
    static int linsearch(ustd::array<T_FLOAT> &ar, T_FLOAT x) {
        int a = 0, b = ar.length() - 1, n;
        while (b - a > 1) {
            n = (a + b) / 2;
            if (ar[n] == x)
                return n;
            if (x > ar[n])
                a = n;
            else
                b = n;
        }
        return a;
    }
 
    T_FLOAT interpol(T_FLOAT xi) {
        T_FLOAT dx1, dx2, dy;
        if (len == 0)
            return 0.0;
        if (len == 1)
            return x[0];
        if (xi < minX) {
            if (!extrapolate)
                return y[0];
            else {
                dx1 = x[0] - xi;
                dx2 = x[1] - x[0];
                dy = y[1] - y[0];
                return y[0] - dy / dx2 * dx1;
            }
        }
        if (xi > maxX) {
            if (!extrapolate)
                return y[len - 1];
            else {
                dx1 = x[len - 1] - x[len];
                dx2 = xi - x[len - 1];
                dy = y[len - 1] - y[len - 2];
                return y[len - 1] + dy / dx1 * dx2;
            }
        }
        unsigned int n = linsearch(x, xi);
        if (n >= len - 1)
            return y[len - 1];
        dx1 = x[n] - x[n + 1];
        dx2 = xi - x[n];
        dy = y[n + 1] - y[n];
        float yi = y[n] - dy / dx1 * dx2;
        return yi;
    }
 
    T_FLOAT operator()(T_FLOAT x) {
        return interpol(x);
    }
};
 
}  // namespace ustd