#ifndef WALKMODULESPREAD_H
#define WALKMODULESPREAD_H

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


/**
 * simply try to move away from the starting node as much as possible
 */
template <typename Node, typename WalkState> class WalkModuleSpread : public WalkModule<Node, WalkState> {

private:

	Point3 avg;

public:

	/** ctor */
	WalkModuleSpread() {
		;
	}


	virtual void updateBefore(WalkState& state) override {
		(void) state;
		avg = avg * 0.999 + state.position.inMeter() * 0.001;
	}

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

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

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

		(void) state;
		(void) startNode;
		(void) curNode;

		const float dOld = avg.getDistance(curNode.inMeter());
		const float dNew = avg.getDistance(potentialNode.inMeter());

		if (dNew > dOld) {return 0.8;}
		if (curNode.z_cm != potentialNode.z_cm) {return 0.8;}
		if (dNew == dOld) {return 0.2;}
		return 0;

	}


};


#endif // WALKMODULESPREAD_H