#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 "BarometerData.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 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:
    //just for debugging and plotting
    std::vector<History> input;
    std::vector<History> inputInterp;
    std::vector<History> output;
    std::vector<float> sumHist;
    std::vector<float> mvAvgHist;
    std::vector<ActivityProbabilities> actHist;

    bool initialize;

    ActivityProbabilities currentActivity;

    /** change this values for much success */
    const int 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.08;

    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;
        }

        //input.push_back(History(ts, baro));

        bool newInterpolatedValues = false;

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

            //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(int 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;
                //sumHist.push_back(actValue);

                //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), "the probability of densityElevatorDown is null!");
                //_assertTrue( (densityStairsDown != 0), "the probability of densityStairsDown is null!");
                //_assertTrue( (densityStay != 0), "the probability of densityStay is null!");
                //_assertTrue( (densityStairsUp != 0), "the probability of densityStairsUp is null!");
                //_assertTrue( (densityElevatorUp != 0), "the probability of densityElevatorUp is null!");


                //todo: aging: wahrscheinlichkeit aufzug zu fahren oder treppe zu steigen, wird nicht knall hart auf 0 gesetzt,
                //sobald der sensors nichts mehr hat, sondern wird mit der zeit geringer. größer NV?

                //const Timestamp age = ts - ap.getTimestamp();

                //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;

                //int highestValueIdx = densities.at(distance(densities.begin(), max_element (densities.begin(),densities.end())));
                // if an activity other then staying is detected with a high probability, we are using the previous probability
                // to keep it a little while longer. this prevents hard activity changes and helping the transition and evaluation
                // to not jump between elevators/stairs and the floor and provide somewhat a smooother floorchange.
                // TODO: Put this into the Project and not in Indoor, since this class should only provide the probability of the
                // given activity! Since i had no time, this was the fastest solution for now.
//                if(highestValueIdx != 2){
//                    for(int i = 0; i < densities.size(); ++i){
//                        densities[i] *= densitiesOld[i];
//                    }
//                }


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

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

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

//                densityElevatorDown /= densitySum;
//                densityStairsDown /= densitySum;
//                densityStay /= densitySum;
//                densityStairsUp /= densitySum;
//                densityElevatorUp /= densitySum;

                // if one value is 1.0 and all other are 0.0, fix that by providing a small possibility
//                densityElevatorDown = (densityElevatorDown > 0.0f ? densityElevatorDown : 0.01f);
//                densityStairsDown = (densityStairsDown > 0.0f ? densityStairsDown : 0.01f);
//                densityStay = (densityStay > 0.0f ? densityStay : 0.01f);
//                densityStairsUp = (densityStairsUp > 0.0f ? densityStairsUp : 0.01f);
//                densityElevatorUp = (densityElevatorUp > 0.0f ? densityElevatorUp : 0.01f);

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

            }

            //actHist.push_back(currentActivity);
        }

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

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

    std::vector<float> getSensorHistory(){
        std::vector<float> tmp;

        for(History val : input){
            tmp.push_back(val.data.hPa);
        }

        return tmp;
    }

    std::vector<float> getInterpolatedHistory(){
        std::vector<float> tmp;

        for(History val : inputInterp){
            tmp.push_back(val.data.hPa);
        }

        return tmp;
    }

    std::vector<float> getOutputHistory(){

        std::vector<float> tmp;

        for(History val : output){
            tmp.push_back(val.data.hPa);
        }

        return tmp;
    }

    std::vector<float> getSumHistory(){
        return sumHist;
    }


    std::vector<ActivityProbabilities> getActHistory(){

        return actHist;
    }


};

#endif // ACTIVITYBUTTERPRESSUREPERCENT_H