#ifndef STEPDETECTION_H
#define STEPDETECTION_H


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

#include <cmath>
#include <vector>

#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 "../../Assertions.h"
#include "../../math/MovingAverageTS.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 = Timestamp::fromMS(250);		// 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!

public:

	/** ctor */
	StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
		;
	}

	/** 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) {return false;}

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

//		static K::Gnuplot gp;
//		static K::GnuplotPlot plot;
//		static K::GnuplotPlotElementLines lines1; plot.add(&lines1);
//		static K::GnuplotPlotElementLines lines2; plot.add(&lines2); lines2.setColorHex("#0000ff");
//		static Timestamp ref = ts;

//		static int i = 0;

//		//lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
//		lines2.add( K::GnuplotPoint2((ts-ref).ms(), delta) );

//		if (++i % 100 == 0) {
//			gp.draw(plot);
//			gp.flush();
//			usleep(1000*25);
//		}

		return step;


	}


//private:

//	/** low pass acc-magnitude */
//	float avg1 = 0;

//	/** even-more low-pass acc-magnitude */
//	float avg2 = 0;

//private:

//	class Stepper {

//	private:

//		/** block for 300 ms after every step */
//		const Timestamp blockTime = Timestamp::fromMS(300);

//		/** the threshold for detecting a spike as step */
//		const float threshold = 0.30;

//		/** block until the given timestamp before detecting additional steps */
//		Timestamp blockUntil;


//	public:

//		/** is the given (relative!) magnitude (mag - ~9.81) a step? */
//		bool isStep(const Timestamp ts, const float mag) {

//			// still blocking
//			if (ts < blockUntil) {
//				return false;
//			}

//			// threshold reached? -> step!
//			if (mag > threshold) {

//				// block x milliseconds until detecting the next step
//				blockUntil = ts + blockTime;

//				// we have a step
//				return true;

//			}

//			// no step
//			return false;

//		}


//	};

//	Stepper stepper;

//public:

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

//		avg1 = avg1 * 0.91 + acc.magnitude() * 0.09;	// short-time average	[filtered steps]
//		avg2 = avg2 * 0.97 + acc.magnitude() * 0.03;	// long-time average	[gravity]

//		// average maginitude must be > 9.0 to be stable enough to proceed
//		if (avg2 > 9) {

//			// gravity-free magnitude
//			const float avg = avg1 - avg2;

//			// detect steps
//			return stepper.isStep(ts, avg);

//		} else {

//			return false;

//		}

//	}


};


#endif // STEPDETECTION_H