#ifndef GRIDWALKER3_H
#define GRIDWALKER3_H

#include "../../../data/Timestamp.h"
#include "../../Grid.h"
#include "../../../math/DrawList.h"
#include "../../../math/Distributions.h"
#include "../../../math/Stats.h"
#include "../../../geo/Heading.h"
#include "../../../math/stats/Variance.h"

/**
 * modular grid-walker that takes various sub-components to determine
 * p(e) and thus randomly pick edges
 */
template <typename Node, typename WalkState, typename Control> class GridWalker3 {

private:

	/** all modules to evaluate */
//	std::vector<WalkModule<Node, WalkState>*> modules;

	RandomGenerator rnd;

public:


	/** one walk along several nodes */
	struct Walk : public std::vector<const Node*> {

	};

//	struct Walks : std::vector<Walk> {

//	};


	/** perform the walk based on the configured setup */
	const Node* getDestination(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {

		const Node* startNode = grid.getNodePtrFor(start);

		Heading desiredHeading = ctrl.heading;

		DrawList<Walk> walks;

		for (int i = 0; i < 100; ++i) {
			const Walk walk = getRandomWalk(grid, startNode, dist_m);
			const double prob = eval(walk, desiredHeading);
			walks.add(walk, prob);
		}

		Walk res = walks.get();
		const Node* dst = res.back();
		return dst;

	}

	double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const {
		const Node* nStart = w.front();
		const Node* nEnd = w.back();
		if (nStart == nEnd) {
			std::cout << "warn! start == end" << std::endl;
			return 0;
		}
		Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm);
		const float diff = head.getDiffHalfRAD(desiredHeading);
		return Distribution::Normal<double>::getProbability(0, 0.3, diff);
	}

	double evalHeadingChanges(const Walk& w) const {
		Stats::Variance<float> var;
		for (int i = 0; i < w.size()-2; ++i) {
			const Node* n0 = w[i+0];
			const Node* n1 = w[i+1];
			const Node* n2 = w[i+2];
			Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm);
			Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm);
			const float diff = h1.getDiffHalfRAD(h2);
			var.add(diff);
		}
		const float totalVar = var.get();
		return Distribution::Normal<double>::getProbability(0, 0.3, totalVar);
	}

	double eval(const Walk& w, const Heading desiredHeading) const {

		return 1.0
				* evalHeadingStartEnd(w, desiredHeading)
				// * evalHeadingChanges(w);
		;

	}

	Walk getRandomWalk(Grid<Node>& grid, const Node* start, const float dist_m) const {

		Walk walk;

		float dist = 0;

		const Node* cur = start;
		while(true) {

			walk.push_back(cur);
			if (dist > dist_m) {break;}

			const int numNeighbors = cur->getNumNeighbors();
			//std::cout << "neighbors: " << numNeighbors << std::endl;
			int idx = rand() % numNeighbors;
			const Node* next = &grid.getNeighbor(*cur, idx);
			dist += next->inMeter().getDistance(cur->inMeter());
			cur = next;

		}

		return walk;

	}

};

#endif // GRIDWALKER3_H