#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"
#include "GridWalk3Helper.h"
#include "../../../geo/BBox2.h"

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

private:

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

	RandomGenerator rnd;

public:

	/** paremters for the walk */
	struct WalkParams {
		Point3 start;
		float distance_m;
		Heading heading = Heading(0);
	};

	/** result of the random walk */
	struct WalkResult {
		Point3 position;
		Heading heading = Heading(0);
		double probability = 1.0;
	};


	using Helper = GridWalk3Helper<Node>;
	using Walk = typename GridWalk3Helper<Node>::Walk3;
	using Walks = typename GridWalk3Helper<Node>::Walks3;
	using Nodes = typename GridWalk3Helper<Node>::Nodes3;

	GridPoint p3ToGp(const Point3 p) const {
		const Point3 p100 = p*100;
		return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
	}

	Point3 gpToP3(const GridPoint gp) const {
		return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
	}

	/** perform the walk based on the configured setup */
	const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) {
		//return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m);
		return _drawThenCheck(grid, params);

	}

//	/** perform the walk based on the configured setup */
//	const Point3 getDestination(Grid<Node>& grid, const GridPoint start, const params) {

//		//return _getFromPossibleWalks(grid, start, ctrl, dist_m);
//		//return _drawThenCheck(grid, start, ctrl, dist_m);
//		throw "error";
//	}

	/** does the given grid-node contain the provided point-in-question? */
	const bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
		const float gridSize_m = grid.getGridSize_cm() / 100.0f;
		const float d = gridSize_m / 2.0f;
		const Point2 pMin = n->inMeter().xy() - Point2(d, d);
		const Point2 pMax = n->inMeter().xy() + Point2(d, d);
		const BBox2 bbox(pMin, pMax);
		return bbox.contains(pt);
	}

	/** does one of the given grid-node scontains the provided point-in-question? */
	const Node* contains(const Grid<Node>& grid, const Nodes& nodes, Point2 pt) {
		for (const Node* n : nodes) {
			if (contains(grid, n, pt)) {return n;}
		}
		return nullptr;
	}

	const WalkResult _drawThenCheck(Grid<Node>& grid, const WalkParams& params) {

		const GridPoint gpStart = p3ToGp(params.start);
		const Node* startNode = grid.getNodePtrFor(gpStart);

		static Distribution::Normal<float> dDist(1, 0.02);
		static Distribution::Normal<float> dHead(0, 0.01);

		// include one additional grid-cell (increased distance)
		const float secBuffer_m = grid.getGridSize_cm() / 100.0f;
		const float range_m = params.distance_m + secBuffer_m;
		const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m);

		WalkResult res;
		res.heading = params.heading;
		res.position = params.start;
		float realDist_m = params.distance_m;

		int cnt = 0;
		while(true) {

			const Point2 dir = res.heading.asVector();
			const Point2 dst = params.start.xy() + (dir * realDist_m);

			// is dst reachable?
			const Node* n = contains(grid, reachableNodes, dst);
			if (n) {

				const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
				const GridPoint gp = p3ToGp(p3);

				if (grid.hasNodeFor(gp)) {
					res.position = p3;					// update position
					res.heading;						// keep as-is
					res.probability;					// keep as-is
					return res;							// done
				} else {
					std::cout << "WARN dst not found" << std::endl;
					//throw "should not happen";
				}

			}

			// reduce probability with every new run
			++cnt;
			res.probability /= 2;

			// before trying again, modify distance and angle
			//if (1 == 1) {
				res.heading = params.heading + dHead.draw() * cnt;
				realDist_m = params.distance_m * dDist.draw();// * cnt;
			//}

			// reached max retries?
			if (cnt > 8) {
				res.position = params.start;	// reset
				res.heading = params.heading;	// reset
				res.probability = 1e-50;			// unlikely
				return res;
			}	// did not work out....

		}

		throw "error";

	}

//	const Node* _getFromPossibleWalks(Grid<Node>& grid, const GridPoint start, Control& ctrl, const float dist_m) {

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

//		Heading desiredHeading = ctrl.heading;

//		DrawList<Walk> weightedWalks;

//		const Walks walks = Helper::getAllPossibleWalks(grid, startNode, dist_m);
//		for (const Walk& walk : walks) {
//			const double prob = eval(walk, desiredHeading, dist_m);
//			weightedWalks.add(walk, prob);
//		}


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

//	}





	double evalDistance(const Walk& w, const float desiredDist) const {
		const Node* nStart = w.front();
		const Node* nEnd = w.back();
		const float walkDist = nStart->inMeter().getDistance(nEnd->inMeter());
		return Distribution::Normal<double>::getProbability(desiredDist, 0.1, walkDist);
	}

	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 float desiredDistance) const {

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

	}




//	Walk getRandomWalk2(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