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


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

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.17;

		/** 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 isStep(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