#ifndef INDOOR_SYNTHETICTURNS_H
#define INDOOR_SYNTHETICTURNS_H

#include "SyntheticWalker.h"

#include "../sensors/imu/AccelerometerData.h"
#include "../sensors/imu/GyroscopeData.h"
#include "../geo/Heading.h"

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

/**
 * simulates acceleromter and gyroscope data
 * based on synthetic walking data
 *
 * @brief The SyntheticTurns class
 */
class SyntheticTurns : public SyntheticWalker::Listener {

public:

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

private:

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

	Distribution::Normal<float> dAccX = Distribution::Normal<float>(0, 2.5);
	Distribution::Normal<float> dAccY = Distribution::Normal<float>(0, 1.5);
	Distribution::Normal<float> dAccZ = Distribution::Normal<float>(0, 1);

	Distribution::Normal<float> dGyroX = Distribution::Normal<float>(0, 0.02);
	Distribution::Normal<float> dGyroY = Distribution::Normal<float>(0, 0.02);
	Distribution::Normal<float> dGyroZ = Distribution::Normal<float>(0, 0.02);

	Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003);
	Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
	Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10);	// heading error, slightly biased


	std::vector<Listener*> listeners;

public:

	/** ctor with the walker to follow */
	SyntheticTurns(SyntheticWalker* walker) {
		walker->addListener(this);
		dAccX.setSeed(1);
		dAccY.setSeed(3);
		dAccZ.setSeed(5);
		dGyroX.setSeed(7);
		dGyroY.setSeed(9);
		dGyroZ.setSeed(11);
	}

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

protected:

	Timestamp lastTs;
	Heading desiredHead = Heading(0);
	Heading curHead = Heading(0);
	Point3 lastPos = Point3(NAN, NAN, NAN);
	float change;

	inline float clamp(const float val, const float min, const float max) {
		if (val < min) {return min;}
		if (val > max) {return max;}
		return val;
	}

	void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {

		// time sine last onWalk();
		if (lastTs.isZero()) {lastTs = walkedTime; return;}
		const Timestamp deltaTs = walkedTime - lastTs;
		lastTs = walkedTime;

		if (lastPos.x != lastPos.x) {
			lastPos = curPos;
		} else {
			desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();;
			lastPos = curPos;
		}

		// difference between current-heading and desired-heading
		const float diffRad = Heading::getSignedDiff(curHead, desiredHead);

		// slowly change the current heading to match the desired one
		const float maxChange = dMaxChange.draw();
		const float toChange = clamp(diffRad, -maxChange, +maxChange);
		//if (change < toChange) {change += toChange*0.01;}
		if (change > toChange) {change *= 0.93;}
		if (change < toChange) {change += dChange.draw()/10000;}
		//if (change > toChange) {change -= dChange.draw();}



		curHead += change;

		// convert to gyro's radians-per-second
		const float radPerSec = change * 1000 / deltaTs.ms();;

		const float accX = 0.00 + dAccX.draw();
		const float accY = 0.00 + dAccY.draw();
		const float accZ = 9.81 + dAccZ.draw();
		AccelerometerData acc(accX, accY, accZ);

		const float gyroX = dGyroX.draw();
		const float gyroY = dGyroY.draw();
		const float gyroZ = dGyroZ.draw() + radPerSec;
		GyroscopeData gyro(gyroX, gyroY, gyroZ);

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

	}

};

#endif // INDOOR_SYNTHETICTURNS_H