#ifndef SYNTHETICSTEPS_H
#define SYNTHETICSTEPS_H

#include "SyntheticWalker.h"

#include "../sensors/imu/AccelerometerData.h"

#include "../math/distribution/Normal.h"

/**
 * fakes accelerometer-data
 * based on synthetic walking data
 */
class SyntheticSteps : SyntheticWalker::Listener {

public:

	class Listener {
	public:
		virtual void onSyntheticStepData(const Timestamp ts, const AccelerometerData acc) = 0;
	};

private:

	/** the walker to listen to */
	SyntheticWalker* walker;

	///** the pedestrian's step-size (in meter) */
	//float stepSize_m = 0;

	///** when walking stairs, the step size is much smaller */
	//float stepSizeStair_m = 0;

	float lastStepAtDistance = 0;

	Timestamp refStepPattern;
	Interpolator<Timestamp, AccelerometerData> stepPatternPlain;
	Interpolator<Timestamp, AccelerometerData> stepPatternStair;

	Distribution::Normal<float> dX = Distribution::Normal<float>(0, 0.2);
	Distribution::Normal<float> dY = Distribution::Normal<float>(0, 0.3);
	Distribution::Normal<float> dZ = Distribution::Normal<float>(0, 0.4);


	int stepPatternPos = -1;

	std::vector<Listener*> listeners;

	//float stepSize_m;
	//float stepSizeSigma_m;
	float noiseLevel;
	Distribution::Normal<float> dNextStep;
	Distribution::Normal<float> dNextStepStair;

public:

	/** ctor with the walker to follow */
	SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeStair_m = 0.3, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) :
		//stepSize_m(stepSize_m), drift(drift), stepSizeSigma_m(stepSizeSigma_m),
		noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m), dNextStepStair(stepSizeStair_m, stepSizeSigma_m) {

		walker->addListener(this);
		dX.setSeed(1);
		dY.setSeed(3);
		dZ.setSeed(5);

		// build the step-pattern (how does a step look-like on the accelerometer)
		// TODO: switch to MS?! use interpolator?
//		int duration_ms = 350;
//		for (int i = 0; i < duration_ms; i += 10) {
//			float z = std::sin(i*M_PI*2/duration_ms) * 3.0;
//			if (z < 0) {z *= 0.75;}	// less pronounced in the negative part
//			float y = std::cos(i*M_PI*2/duration_ms) * 0.5;
//			const float x = dO.draw();
//			z += dO.draw()*2;
//			y += dO.draw();
//			AccelerometerData acc(x,y,z);
//			stepPattern.add(Timestamp::fromMS(i), acc);
//		}

		stepPatternPlain.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
		stepPatternPlain.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
		stepPatternPlain.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8));
		stepPatternPlain.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0));

		stepPatternStair.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
		stepPatternStair.add(Timestamp::fromMS(200), AccelerometerData(0, 0.6, 4));
		stepPatternStair.add(Timestamp::fromMS(300), AccelerometerData(0.5, -0.6, -3.5));
		stepPatternStair.add(Timestamp::fromMS(350), AccelerometerData(0, 0, 0));

	}

	/** attach a listener to this provider */
	void addListener(Listener* l) {
		this->listeners.push_back(l);
	}

protected:


	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos, const SyntheticWalker::Type type) override {

		(void) curPos;
		const float distAdd = (type == SyntheticWalker::Type::FLOOR) ? (dNextStep.draw()) : (dNextStepStair.draw());
		const auto stepPattern = (type == SyntheticWalker::Type::FLOOR) ? (stepPatternPlain) : (stepPatternStair);
		const float nextStepAt = lastStepAtDistance + distAdd;

		// 1st, start with random noise on the accelerometer
		const float x = dX.draw();
		const float y = dY.draw();
		const float z = dZ.draw();
		const AccelerometerData aBase(0, 4, 9.7);
		const AccelerometerData aNoise(x, y, z);
		AccelerometerData acc = aBase + aNoise * noiseLevel;

		// is it time to inject a "step" into the accelerometer data?
		if (walkedDistance > nextStepAt) {
			lastStepAtDistance = walkedDistance;
			refStepPattern = walkedTime;
		}

		// overlay the noise with a step-pattern (see ctor)
		if (refStepPattern.ms() > 0) {
			Timestamp curPatPos = walkedTime - refStepPattern;
			if (curPatPos >= stepPattern.getMaxKey()) {
				refStepPattern = Timestamp::fromMS(0);
			} else {
				const AccelerometerData step = stepPattern.get(curPatPos);
				acc = aBase + (aNoise * noiseLevel) + step;
			}
		}

		for (Listener* l : listeners) {l->onSyntheticStepData(walkedTime, acc);}

	}

};

#endif // SYNTHETICSTEPS_H