#ifndef WALKMODULERELATIVEPRESSURE_H
#define WALKMODULERELATIVEPRESSURE_H


#include "WalkModule.h"
#include "WalkStateHeading.h"

#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../Assertions.h"


/**
 * a walk-state the contains the pressure relative to time t0
 */
struct WalkStateRelativePressure {

	/** the pressure level [relative to time t0] one SHOULD messure at this state */
	float pressureRelToT0;

	int dirLock = 0;

	/** ctor */
	WalkStateRelativePressure(const float pressureRelToT0) : pressureRelToT0(pressureRelToT0) {;}

};


/**
 * uses the pressure relative to t=0 within the control-data
 * to estimate the likelyhood for any z-changes during the transition
 */
template <typename Node, typename WalkState, typename Control> class WalkModuleRelativePressureControl : public WalkModule<Node, WalkState> {

private:

	Control* ctrl;

	/** pressure-change (hPa) per meter */
	const float hPaPerMeter = 0.126f;		// given an average hPa of 938

public:

	/** ctor */
	WalkModuleRelativePressureControl(Control* ctrl, const float hPaPerMeter) : ctrl(ctrl), hPaPerMeter(hPaPerMeter) {
		;
	}

	virtual void updateBefore(WalkState& state) override {
		(void) state;
	}

	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

//		// e.g. walking down from the 3rd to the second floor
//		// startZ = 10 meter, endZ = 7 meter -> deltaZ = 3 meter;
//		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
//		const int deltaZ_cm = startNode.z_cm - endNode.z_cm;
//		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;

//		// update the states pressure
//		state.pressureRelToT0 += deltaPressure;

//		// sanity checks
//		Assert::isNotNaN(deltaPressure, "detected NaN!");
//		Assert::isNotNaN(state.pressureRelToT0, "detected NaN!");

//		static int xx = 0;
//		if (++xx % 1024 == 0) {
//			++xx;
//		}

	}

	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {

		// e.g. walking down from the 3rd to the second floor
		// startZ = 10 meter, endZ = 7 meter -> deltaZ = 3 meter;
		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
		const int deltaZ_cm = curNode.z_cm - nextNode.z_cm;
		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
		const float expectedPressure = state.pressureRelToT0 + deltaPressure;

		// update the states pressure
		state.pressureRelToT0 = expectedPressure;

		// sanity checks
		Assert::isNotNaN(deltaPressure, "detected NaN!");
		Assert::isNotNaN(state.pressureRelToT0, "detected NaN!");

		if(std::abs(state.dirLock) > 0) {
			if (state.dirLock > 0) {--state.dirLock;}
			if (state.dirLock < 0) {++state.dirLock;}
		} else {
			if (deltaZ_cm > 0) {state.dirLock = +25;}
			if (deltaZ_cm < 0) {state.dirLock = -25;}
		}

	}

	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {

		(void) startNode;

		// get the values from control-data
		const float curRelPressure = ctrl->barometer.hPaRelativeToT0;
		const float pressureSigma = ctrl->barometer.estimatedSigma * 1.50f;

		// not yet available/calibrated/possible -> skip evaluation!
		if (pressureSigma == 0) {return 1;}

		// e.g. walking down from the 3rd to the second floor
		// startZ = 10 meter, endZ = 7 meter -> deltaZ = +3 meter;
		// deltaPressure is POSITIVE (pressure increases) as we walk downstairs
		const int deltaZ_cm = curNode.z_cm - potentialNode.z_cm;
		const float deltaPressure = (deltaZ_cm / 100.0f) * hPaPerMeter;
		const float expectedPressure = state.pressureRelToT0 + deltaPressure;

		const float oldErr = std::abs(state.pressureRelToT0 - curRelPressure);
		const float newErr = std::abs(expectedPressure - curRelPressure);

		if (std::abs(state.dirLock) > 0) {
			if (state.dirLock > 0) {
				if (deltaZ_cm < 0) {return 0;}
				if (deltaZ_cm == 0) {return 0.1;}
				return 0.9;
			} else {
				if (deltaZ_cm > 0) {return 0;}
				if (deltaZ_cm == 0) {return 0.1;}
				return 0.9;
			}
		}


		if (oldErr > pressureSigma) {
			if (deltaZ_cm == 0) {return 0.001;}
		}

		if (newErr > oldErr) {return 0.002;}
		if (newErr == oldErr) {return 0.005;}
		if (newErr < oldErr) {return 0.99;}
		throw 1;



//		// compare control-data with potential transition
//		double prob = Distribution::Normal<double>::getProbability(curRelPressure, pressureSigma, expectedPressure);
////		prob = std::pow(prob, 20);

//		// sanity checks
//		Assert::isNotNaN(prob, "detected NaN!");
//		Assert::isNotNaN(deltaPressure, "detected NaN!");
//		Assert::isNotNaN(expectedPressure, "detected NaN!");

//		return prob;

	}


};

#endif // WALKMODULERELATIVEPRESSURE_H