Indoor/sensors/activity/ActivityButterPressurePercent.h

#ifndef ACTIVITYBUTTERPRESSUREPERCENT_H
#define ACTIVITYBUTTERPRESSUREPERCENT_H

#include "../../data/Timestamp.h"
#include "../../math/filter/Butterworth.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/distribution/Normal.h"
#include <KLib/Assertions.h>
#include <vector>

#include "../pressure/BarometerData.h"

#include "Activity.h"

/**
 * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering
 * activity recognition based on a small window given by matlabs diff(window)
 *
 * todo: if an elevator is detected, first we have a short time the stairs are more prober.
 */
class ActivityButterPressurePercent {

public:

    struct ActivityProbabilities{
        float elevatorDown;
        float stairsDown;
        float stay;
        float stairsUp;
        float elevatorUp;

        ActivityProbabilities(float elevatorDown, float stairsDown,
                              float stay, float stairsUp, float elevatorUp) :
            elevatorDown(elevatorDown), stairsDown(stairsDown),
            stay(stay), stairsUp(stairsUp), elevatorUp(elevatorUp) {;}

        ActivityProbabilities() :
            elevatorDown(0.01f), stairsDown(0.01f),
            stay(0.96f), stairsUp(0.01f), elevatorUp(0.01f) {;}
    };


    struct History {
        Timestamp ts;
        BarometerData data;
        History(const Timestamp ts, const BarometerData data) : ts(ts), data(data) {;}
    };

private:

    std::vector<History> output;
    bool initialize;

    ActivityProbabilities currentActivity;

    /** change this values for much success */
    const unsigned long diffSize = 20; //the number values used for finding the activity.
    Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.05f,2);
    FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));

    const float variance = 0.02f;

    const float muStairs = 0.04f;
    const float muStay = 0.00f;
    const float muEleveator = 0.08f;

    std::vector<float> densities = std::vector<float>(5, 1);
    std::vector<float> densitiesOld = std::vector<float>(5, 1);

public:


	/** ctor */
    ActivityButterPressurePercent() : currentActivity(ActivityProbabilities(0.01f, 0.01f, 0.96f, 0.01f, 0.01f)){
        initialize = true;
	}


	/** add new sensor readings that were received at the given timestamp */
    ActivityProbabilities add(const Timestamp& ts, const BarometerData& baro) {

        //init
        if(initialize){
            butter.stepInitialization(baro.hPa);
            initialize = false;

            return currentActivity;
        }

        bool newInterpolatedValues = false;

        //interpolate & butter
        auto callback = [&] (const Timestamp ts, const float val) {
            float interpValue = val;

            //butter
            float butterValue = butter.process(interpValue);
            output.push_back(History(ts, BarometerData(butterValue)));

            newInterpolatedValues = true;

        };
        ffi.add(ts, baro.hPa, callback);

        if(newInterpolatedValues == true){

            //getActivity
            if(output.size() > diffSize){
                //diff
                std::vector<float> diff;
                for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){

                    float diffVal = output[i+1].data.hPa - output[i].data.hPa;

                    diff.push_back(diffVal);
                }

                float sum = 0;
                for(float val : diff){
                    sum += val;
                }

                float actValue = sum;

                //calculate the probabilites of walking down/up etc...
                densitiesOld = densities;

                //in one building there is an ultra fast elevator, therefore we need to clip the activity value...
                if(actValue > muEleveator){
                    actValue = muEleveator;
                }
                if(actValue < -muEleveator){
                    actValue = -muEleveator;
                }

                float densityElevatorDown = Distribution::Normal<float>::getProbability(muEleveator, variance, actValue);
                float densityStairsDown = Distribution::Normal<float>::getProbability(muStairs, variance, actValue);
                float densityStay = Distribution::Normal<float>::getProbability(muStay, variance, actValue);
                float densityStairsUp = Distribution::Normal<float>::getProbability(-muStairs, variance, actValue);
                float densityElevatorUp = Distribution::Normal<float>::getProbability(-muEleveator, variance, actValue);

                _assertTrue( (densityElevatorDown == densityElevatorDown), "the probability of densityElevatorDown is null!");
                _assertTrue( (densityStairsDown == densityStairsDown), "the probability of densityStairsDown is null!");
                _assertTrue( (densityStay == densityStay), "the probability of densityStay is null!");
                _assertTrue( (densityStairsUp == densityStairsUp), "the probability of densityStairsUp is null!");
                _assertTrue( (densityElevatorUp == densityElevatorUp), "the probability of densityElevatorUp is null!");

                _assertTrue( (densityElevatorDown != 0.0f), "the probability of densityElevatorDown is null!");
                _assertTrue( (densityStairsDown != 0.0f), "the probability of densityStairsDown is null!");
                _assertTrue( (densityStay != 0.0f), "the probability of densityStay is null!");
                _assertTrue( (densityStairsUp != 0.0f), "the probability of densityStairsUp is null!");
                _assertTrue( (densityElevatorUp != 0.0f), "the probability of densityElevatorUp is null!");

                //wenn aufzug / treppe der größte wert, werden für x timestamps auf die jeweilige katerogie multipliziert.
                densities[0] = densityElevatorDown;
                densities[1] = densityStairsDown;
                densities[2] = densityStay;
                densities[3] = densityStairsUp;
                densities[4] = densityElevatorUp;

                //normalize
                float densitySum = densities[0] + densities[1] + densities[2] + densities[3] + densities[4];

                for(unsigned long i = 0; i < densities.size(); ++i){
                    densities[i] /= densitySum;

                    //values cant be zero!
                    densities[i] = (densities[i] > 0.0f ? densities[i] : 0.01f);
                }

                currentActivity = ActivityProbabilities(densities[0], densities[1], densities[2], densities[3], densities[4]);

            }
        }

        //retruns for every call, indepedent of callback.
        return currentActivity;
	}

    /** get the current Activity */
    ActivityProbabilities getCurrentActivity() {
        return currentActivity;
	}


};

#endif // ACTIVITYBUTTERPRESSUREPERCENT_H