#include "GyverBME280.h"

class Barometer {
  public:
    Barometer(GyverBME280 *bme) {
        _bme = bme;
    };

    ~Barometer() {
        delete this->_bme;
    }

    bool initialize(void) {
        _bme->setMode(NORMAL_MODE);
        _bme->setFilter(FILTER_COEF_16);
        _bme->setTempOversampling(OVERSAMPLING_2);
        _bme->setPressOversampling(OVERSAMPLING_16);
        _bme->setStandbyTime(STANDBY_500US);
        return _bme->begin();
    }

    void measureBaseAltitudeSync(void) {
        for (int i = 0; i < _calibrationIterationsCount; ++i) {
            _startedAltitude += altitude();
            delay(1);
        }
        _startedAltitude /= _calibrationIterationsCount;
    }

    void measureBaseAltitudeAsync(void) {
        _isStartCalibration = true;
    }

    float rawAltitude() {
        return pressureToAltitude(_bme->readPressure()) * 100;
    }

    float rawFlightHeight() {
        return rawAltitude() - _startedAltitude;
    }

    float altitude() {
        return _filterRA(pressureToAltitude(_bme->readPressure()) * 100);  // convert from m to cm
    }

    float flightHeight() {
        return altitude() - _startedAltitude;
    }

    void tick() {
        if (_isStartCalibration) {
            if (_isStartCalibration && millis() - _calibrationTimer >= _calibrationIterationDelay) {
                _startedAltitude += altitude();
            }
            if (++_calibrationIterationsCounter >= _calibrationIterationsCount) {
                _startedAltitude /= _calibrationIterationsCount;
                _isStartCalibration = false;
            }
        }
    }

  private:
    GyverBME280 *_bme;
    float _startedAltitude = 0;
    bool _isStartCalibration = false;
    int _calibrationTimer;
    int _calibrationIterationsCounter = 0;
    const int _calibrationIterationsCount = 1500;
    const int _calibrationIterationDelay = 1;  // [ms]

    float _filterRA(float newVal) {
        // running average filter
        constexpr auto WINDOW_SIZE = 100;
        static int t = 0;
        static float vals[WINDOW_SIZE];
        static float average = 0;
        if (++t >= WINDOW_SIZE)
            t = 0;
        average -= vals[t];
        average += newVal;
        vals[t] = newVal;
        return ((float)average / WINDOW_SIZE);
    }
};