#ifndef ACTIVITYBUTTERPRESSURE_H
#define ACTIVITYBUTTERPRESSURE_H

#include "../../data/Timestamp.h"
#include "../../math/filter/Butterworth.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/MovingAVG.h"

#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)
 */
class ActivityButterPressure {

public:

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

    private:

    std::vector<History> output;
    Activity currentActivity;
    MovingAVG<float> mvAvg = MovingAVG<float>(20);

    /** change this values for much success
    *
    *   Nexus 6:
    *   butter =  Filter::ButterworthLP<float>(10,0.1f,2);
    *   threshold = 0.025;
    *   diffSize = 20;
    *   FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));
    */
    const bool additionalLowpassFilter = false;
    const unsigned long diffSize = 20; //the number values used for finding the activity.
    const float threshold = 0.025f; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction!
    Filter::ButterworthLP<float> butter =  Filter::ButterworthLP<float>(10,0.05f,2);
    Filter::ButterworthLP<float> butter2 =  Filter::ButterworthLP<float>(10,0.05f,2);

    FixedFrequencyInterpolator<float> ffi = FixedFrequencyInterpolator<float>(Timestamp::fromMS(100));

public:


	/** ctor */
    ActivityButterPressure() : currentActivity(STAY){
        ;
    }


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

        //init
        static bool firstCall = false;
        if(!firstCall){
            butter.stepInitialization(baro.hPa);
            firstCall = true;

            butter2.stepInitialization(0);
            return STAY;
        }

        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 = 0;
                if(additionalLowpassFilter == true){
                    //additional butter/moving average for the results
                    //mvAvg.add(sum);
                    //actValue = mvAvg.get();
                    actValue = butter2.process(sum);
                }else{
                    actValue = sum;
                }

                if(actValue > threshold){
                    currentActivity = Activity::WALKING_DOWN;
                }
                else if (actValue < -threshold){
                    currentActivity = Activity::WALKING_UP;
                }
                else{
                    currentActivity = Activity::WALKING;
                }
            }
        }

        return currentActivity;

	}

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

#endif // ACTIVITYBUTTERPRESSURE_H