#ifndef STEPDETECTION_H
#define STEPDETECTION_H


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

#include <cmath>
#include <vector>

#ifdef WITH_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>
	#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
#endif

#include "../../Assertions.h"
#include "../../math/MovingAverageTS.h"
#include "../../math/dsp/FIRComplex.h"


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

private:

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

	Timestamp blockUntil;
	bool waitForUp = false;

	const Timestamp blockTime;								// 150-250 looks good
    const float upperThreshold = +0.4*0.6f;					// + is usually smaller than down (look at graphs)
    const float lowerThreshold = -1.5*0.6f;					// the 0.8 is for testing!


	#ifdef WITH_DEBUG_PLOT
		K::Gnuplot gp;
		K::GnuplotPlot plot;
		K::GnuplotPlotElementLines lineDet;
		K::GnuplotPlotElementPoints pointDet;
		K::GnuplotPlotElementLines lineX;
		K::GnuplotPlotElementLines lineY;
		K::GnuplotPlotElementLines lineZ;
		Timestamp plotRef;
		Timestamp lastPlot;
	#endif


public:

	/** ctor */
	StepDetection(const Timestamp blockTime = Timestamp::fromMS(200)) : blockTime(blockTime), avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {

		#ifdef WITH_DEBUG_PLOT
			gp << "set autoscale xfix\n";
			plot.setTitle("Step Detection");
			plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000"); lineX.setTitle("accX");
			plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00"); lineY.setTitle("accY");
			plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff"); lineZ.setTitle("accZ");
			plot.add(&lineDet); lineDet.getStroke().getColor().setHexStr("#000000");
			plot.add(&pointDet); pointDet.setPointSize(2); pointDet.setPointType(7);
		#endif

	}

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

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

		// difference between long-term-average (gravity) and very-short-time average
		const float delta = avgShort.get() - avgLong.get();

		bool step = false;


		if (blockUntil > ts) {

			step = false;

		} else {

			// wait for a rising edge
			if (waitForUp && delta > upperThreshold) {
				blockUntil = ts + blockTime;				// block some time
				waitForUp = false;
			}

			// wait for a falling edge
			if (!waitForUp && delta < lowerThreshold) {
				blockUntil = ts + blockTime;				// block some time
				waitForUp = true;
				step = true;
			}

		}


		#ifdef WITH_DEBUG_PLOT

			if (plotRef.isZero()) {plotRef = ts;}
			const Timestamp tsPlot = (ts-plotRef);
			const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);

			//lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
			lineX.add( K::GnuplotPoint2(tsPlot.ms(), acc.x) );
			lineY.add( K::GnuplotPoint2(tsPlot.ms(), acc.y) );
			lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) );
			lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) );

			if (step) {
				pointDet.add( K::GnuplotPoint2(tsPlot.ms(), 0) );
			}

			if (lastPlot + Timestamp::fromMS(50) < tsPlot) {

				lastPlot = tsPlot;
				auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
				lineX.removeIf(remove);
				lineY.removeIf(remove);
				lineZ.removeIf(remove);
				lineDet.removeIf(remove);
				pointDet.removeIf(remove);

				gp.draw(plot);
				gp.flush();
				usleep(100);

			}

		#endif

		return step;


	}

};


#endif // STEPDETECTION_H