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

	float lastStepAtDistance = 0;

	Timestamp refStepPattern;
	Interpolator<Timestamp, AccelerometerData> stepPattern;

	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;

public:

	/** ctor with the walker to follow */
	SyntheticSteps(SyntheticWalker* walker) {

		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);
//		}
		stepPattern.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0));
		stepPattern.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3));
		stepPattern.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8));
		stepPattern.add(Timestamp::fromMS(450), 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) override {

		(void) curPos;
		const float nextStepAt = (lastStepAtDistance + stepSize_m);

		// 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 base(0, 4, 9.7);
		const AccelerometerData noise(x, y, z);
		AccelerometerData acc = base + noise;

		// 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 = base + noise*2.5f + step;
			}
		}

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

	}

};

#endif // SYNTHETICSTEPS_H