#ifndef ACTIVITYDETECTOR_H
#define ACTIVITYDETECTOR_H


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

#include "../../data/Timestamp.h"

#include "../../Assertions.h"
#include "../../math/MovingAverageTS.h"
#include "../../math/MovingStdDevTS.h"

#include "../../data/HistoryTS.h"

#include "../activity/Activity.h"

//#define ACT_DET_DEBUG_PLOT

#ifdef ACT_DET_DEBUG_PLOT
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#endif


/**
 * simple step detection based on accelerometer magnitude.
 * magnitude > threshold? -> step!
 * block for several msec until detecting the next one
 */
class ActivityDetector {

private:

	MovingAverageTS<float> avgLong;
	MovingAverageTS<float> avgShort;

	MovingStdDevTS<float> stdDev;
	MovingStdDevTS<float> stdDev2;

	MovingAverageTS<float> baroAvgSlow;
	MovingAverageTS<float> baroAvgFast;

	MovingAverageTS<float> baroAvg;
	HistoryTS<float> baroHistory;

	Activity current;

public:

#ifdef ACT_DET_DEBUG_PLOT
	K::Gnuplot gp;
	K::GnuplotPlot gplot;
	K::GnuplotPlotElementLines l1;
	K::GnuplotPlotElementLines l2;
#endif

	/** ctor */
	ActivityDetector() : avgLong(Timestamp::fromMS(1500), 0), avgShort(Timestamp::fromMS(500), 0),
		stdDev(Timestamp::fromMS(150), 0), stdDev2(Timestamp::fromMS(2000), 0),
		baroAvg(Timestamp::fromMS(500), 0), baroHistory(Timestamp::fromMS(4000)) {
		;

		#ifdef ACT_DET_DEBUG_PLOT
			gplot.add(&l1);
			gplot.add(&l2); l2.getStroke().getColor().setHexStr("#ff0000");
		#endif

	}

	//int xx = 0;

	/** add barometer data */
	void add(const Timestamp ts, const BarometerData& baro) {
		if (baro.isValid()) {
			baroAvg.add(ts, baro.hPa);
			const float avg = baroAvg.get();
			baroHistory.add(ts, avg);
			//l1.add(K::GnuplotPoint2(xx, avg));
			update();
		}
	}

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

	/** does the given data indicate a step? */
	void add(const Timestamp ts, const AccelerometerData& acc) {

		// update averages
		avgLong.add(ts, acc.magnitude());
		avgShort.add(ts, acc.magnitude());
		stdDev.add(ts, acc.magnitude());
		stdDev2.add(ts, acc.magnitude());

//		const float delta = std::abs(avgLong.get() - avgShort.get());

//		static int x = 0; ++x;

//		if (delta < 0.3) {
//			return Activity::STANDING;
//		}

//		if (avgLong.get() > 9.81+0.5) {
//			return Activity::WALKING_UP;
//		} else if (avgLong.get() < 9.81-0.5) {
//			return Activity::WALKING_DOWN;
//		}

//		return Activity::WALKING;

	}


private:

	/** estimate the current activity based on the sensor data */
	void update() {

		// delta in acceleration
		const float delta_acc = std::abs(avgLong.get() - avgShort.get());

		if (delta_acc < 0.015) {
			current = Activity::STANDING;
			return;
		}

		// delta in pressure
		const float delta_hPa = baroHistory.getMostRecent() - baroHistory.getOldest();

		#ifdef ACT_DET_DEBUG_PLOT
			l2.add(K::GnuplotPoint2(xx, delta_hPa));
			gp.draw(gplot);
			gp.flush();
			++xx;
		#endif

		if (std::abs(delta_hPa) < 0.042) {
			current = Activity::WALKING;
			return;
		} else if (delta_hPa > 0) {
			current = Activity::WALKING_DOWN;
			return;
		} else {
			current = Activity::WALKING_UP;
			return;
		}

	}

};

#endif // ACTIVITYDETECTOR_H