#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 "../activity/Activity.h"

#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>

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


public:

	K::Gnuplot gp;
	K::GnuplotPlot gplot;
	K::GnuplotPlotElementLines l1;
	K::GnuplotPlotElementLines l2;

	/** ctor */
	ActivityDetector() : avgLong(Timestamp::fromMS(1500), 0), avgShort(Timestamp::fromMS(500), 0),
		stdDev(Timestamp::fromMS(200), 0), stdDev2(Timestamp::fromMS(2000), 0) {
		;
		gplot.add(&l1);
		gplot.add(&l2); l2.getStroke().getColor().setHexStr("#ff0000");
	}

	/** add barometer data */
	void add(const Timestamp ts, const BarometerData& baro) {
		if (baro.isValid()) {
			baroAvgSlow.add(ts, baro.hPa);
			baroAvgFast.add(ts, baro.hPa);
		}
	}

	Activity get() {

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

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

		// delta in pressure
		const float delta_hPa = baroAvgFast.get() - baroAvgSlow.get();

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

	}

	/** 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 (x % 10 == 0) {
////			l1.add({x, avgLong.get()});
////			l2.add({x, avgShort.get()});
////			gp.draw(gplot);
////			gp.flush();
////		}

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

	}





};

#endif // ACTIVITYDETECTOR_H