#ifndef ACTIVITYDETECTION_H
#define ACTIVITYDETECTION_H

#include <string>
#include "Activities.h"
#include <vector>

#include <Indoor/math/MovingAVG.h>
#include <Indoor/math/MovingMedian.h>

#include <KLib/math/statistics/Statistics.h>


#include <Indoor/math/Distributions.h>

/**
 * classification of pedestrian activities using sensor inputs.
 * sensor inputs are passed into this element
 * recognized acitivities are the output
 *
 */
class ActivityDetection {

private:

	/** the currently detected activity */
	Activity current = Activity::UNKNOWN;

	K::Statistics<float> mag;
	K::Statistics<float> hpa;

	MovingMedian<float> hpaAvg;

public:

	ActivityDetection() : hpaAvg(3) {;}

	/** add accelerometer values */
	void addAccel(float accel[3]) {

		float magnitude = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
		mag.add(magnitude - 9.81f);

	}

	/** add barometer values */
	void addBaro(float hpa) {
		hpaAvg.add(hpa);
		float smoothed = hpaAvg.get();
		this->hpa.add(smoothed);
		if (this->hpa.getCount() > 50) {analyze();}
	}

	struct ActClass {
		Activity act;
		Distribution::Normal<float> barometer;
		Distribution::Normal<float> magnitude;
		ActClass(const Activity act, float muMag, float muBaro, float varMag, float varBaro) :
			act(act), barometer(muBaro, std::sqrt(varBaro)), magnitude(muMag, std::sqrt(varMag)) {
			;
		}
		float getProbability(const float hpaRange, const float varMag) const {
			return	barometer.getProbability(hpaRange) *
					magnitude.getProbability(varMag);
		}
	};

	void analyze() {

//		std::vector<ActClass> classes = {
//			ActClass(Activity::STANDING, 0.042118, 0.01, 0.002322, 0.001),
//			ActClass(Activity::STAIRS, 3.561667, 0.035, 1.392631, 0.002),		// stair up
//			ActClass(Activity::STAIRS, 5.529414, -0.035, 3.416315, 0.002),		// stair down
//			ActClass(Activity::WALKING, 1.669506, 0.01, 0.273880, 0.001),
//		};

//		auto comp = [&] (const ActClass& a, const ActClass& b) {
//			return a.getProbability(hpa.getRange(), mag.getStdDev()) < b.getProbability(hpa.getRange(), mag.getStdDev());
//		};

//		auto it = std::max_element(classes.begin(), classes.end(), comp);

//		current = (*it).act;


		if (mag.getStdDev() < 0.3) {
			current = Activity::STANDING;
		} else {
			if (hpa.getRange() > 0.035) {
				current = Activity::STAIRS;
			} else {
				current = Activity::WALKING;
			}
		}

//		current = (Activity) idx;

		mag.reset();
		hpa.reset();

	}

	/** get the currently detected activity */
	Activity getCurrentActivity() const {
		return current;
	}

	std::string toString() const {
		switch (current) {
			case Activity::UNKNOWN:		return "unknown";
			case Activity::STANDING:	return "standing";
			case Activity::WALKING:		return "walking";
			case Activity::STAIRS:		return "stairs";
			case Activity::ELEVATOR:	return "elevator";
		}
		throw "should not happen";
	}

};

#endif // ACTIVITYDETECTION_H