File evamMedianDecor.h

File List > src > evamMedianDecor.h

Go to the documentation of this file

#pragma once

#include <Arduino.h>
#include <evaHeartbeat.h>

using namespace eva;

namespace evam
{
    constexpr unsigned short kDefaultWindowSize = 5;
    constexpr unsigned short kMinWindowSize = 3;
    constexpr unsigned short kMaxWindowSize = 15;

    struct MedianConfig {
        unsigned short windowSize;

        MedianConfig(unsigned short windowSize) : windowSize(windowSize) {}
    };

    template <class TMotor, unsigned short tWindowSize = kDefaultWindowSize>
    class MedianDecor : public Heartbeat, public TMotor
    {
        static_assert(tWindowSize >= kMinWindowSize && tWindowSize <= kMaxWindowSize, 
                      "tWindowSize out of range");
        static_assert(tWindowSize % 2 == 1, 
                      "tWindowSize must be odd");

    private:
        static constexpr unsigned long kHeartbeatPeriodMs = 10;

        MedianConfig mConfig;

        signed short mBuffer[tWindowSize];
        unsigned short mIndex = 0;
        bool mBufferFull = false;
        signed short mLastSample = 0;

        signed short calculateMedian()
        {
            signed short sorted[tWindowSize];
            unsigned short count = mBufferFull ? tWindowSize : mIndex + 1;

            for (unsigned short i = 0; i < count; i++)
                sorted[i] = mBuffer[i];

            for (unsigned short i = 0; i < count - 1; i++)
            {
                for (unsigned short j = 0; j < count - i - 1; j++)
                {
                    if (sorted[j] > sorted[j + 1])
                    {
                        signed short temp = sorted[j];
                        sorted[j] = sorted[j + 1];
                        sorted[j + 1] = temp;
                    }
                }
            }

            return sorted[count / 2];
        }

    protected:
        void onHeartbeat() override
        {
            mBuffer[mIndex] = mLastSample;
            mIndex++;
            if (mIndex >= tWindowSize)
            {
                mIndex = 0;
                mBufferFull = true;
            }

            if (mBufferFull || mIndex > tWindowSize / 2)
            {
                TMotor::Go(calculateMedian());
            }
        }

    public:
        MedianDecor() : mConfig(tWindowSize), Heartbeat(kHeartbeatPeriodMs)
        {
            for (unsigned short i = 0; i < tWindowSize; i++)
                mBuffer[i] = 0;
        }

        template<typename... Args>
        MedianDecor(MedianConfig config, Args... args) 
            : mConfig(config), Heartbeat(kHeartbeatPeriodMs), TMotor(args...)
        {
            for (unsigned short i = 0; i < tWindowSize; i++)
                mBuffer[i] = 0;
        }

        void Reset()
        {
            mIndex = 0;
            mBufferFull = false;
            for (unsigned short i = 0; i < tWindowSize; i++)
                mBuffer[i] = 0;
        }

        void Go(signed short aValue)
        {
            mLastSample = constrain(aValue, -1000, 1000);
        }
    };
}