#include <Figments.h>
#include <BluetoothSerial.h>
#include <Ringbuf.h>

class BluetoothSerialTelemetry : public InputSource {
  public:
    BluetoothSerialTelemetry();
    void onStart() override;
    InputEvent read() override;

    template<typename T, uint8_t Size = 8>
    struct Averager {
        std::array<T, Size> buf;
        unsigned int idx = 0;
        unsigned int count = 0;

        void add(const T &value) {
            buf[idx] = value;
            idx = (idx + 1) % Size;
            if (count < Size) {
                count += 1;
            }
        }

        T value() const {
            if (count == 0) {
                return T{};
            }
            long long int sum = 0;
            for(unsigned int i = 0; i < count; i++) {
                sum += buf[i];
            }
            return sum / count;
        }
    };
  private:
    BluetoothSerial m_serial;
    Ringbuf<char, 32> m_ringbuf;
    CRGB m_color;
    Averager<int16_t, 32> m_value;
};