#ifndef STEPDETECTION2_H
#define STEPDETECTION2_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

#ifdef WITH_DEBUG_OUTPUT
	#include <fstream>
#endif

#include "../../Assertions.h"
#include "../../math/dsp/FIRComplex.h"
#include "../../math/FixedFrequencyInterpolator.h"
#include "../../math/LocalMaxima.h"


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

	static constexpr int sRate_hz = 75;
	static constexpr int every_ms = 1000 / sRate_hz;

private:

	FixedFrequencyInterpolator<AccelerometerData> interpol;
	FIRComplex fir;
	LocalMaxima locMax;

	const float threshold = 0.5;

	#ifdef WITH_DEBUG_PLOT
		K::Gnuplot gp;
		K::GnuplotPlot plot;
		K::GnuplotPlotElementLines lineMag;
		K::GnuplotPlotElementPoints pointDet;
		Timestamp plotRef;
		Timestamp lastPlot;
	#endif

	#ifdef WITH_DEBUG_OUTPUT
		std::ofstream outFiltered;
		std::ofstream outSteps;
	#endif


public:

	/** ctor */
	StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(5) {

		fir.lowPass(0.66, 40);	// allow deviation of +/- 0.66Hz
		fir.shiftBy(2);			// typical step freq ~2Hz

		#ifdef WITH_DEBUG_PLOT
			gp << "set autoscale xfix\n";
			plot.setTitle("Step Detection");
			plot.add(&lineMag); lineMag.getStroke().getColor().setHexStr("#000000");
			plot.add(&pointDet); pointDet.setPointSize(2); pointDet.setPointType(7);
		#endif

		#ifdef WITH_DEBUG_OUTPUT
			outFiltered = std::ofstream("/tmp/sd2_filtered.dat");
			outSteps = std::ofstream("/tmp/sd2_steps.dat");
		#endif

	}

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

		bool step = false;

		auto onResample = [&] (const Timestamp ts, const AccelerometerData data) {

			const float mag = data.magnitude();

			const std::complex<float> c = fir.append(mag);
			const float real = c.real();
			if (real != real) {return;}
			const float fMag = real;

			LocalMaxima::Res res = locMax.add(fMag);
			step = (res.isMax) && (res.val > threshold);

			#ifdef WITH_DEBUG_OUTPUT
				if (step) {
					outSteps << ts.ms() << " " << fMag << "\n";
					outSteps.flush();
				}
				outFiltered << ts.ms() << " " << fMag << "\n";
			#endif

			#ifdef WITH_DEBUG_PLOT

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

				lineMag.add( K::GnuplotPoint2(tsPlot.ms(), fMag) );

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

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

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

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

				}

			#endif

		};

		interpol.add(ts, acc, onResample);

		return step;


	}

};


#endif // STEPDETECTION2_H