#ifndef INDOOR_GW3_WALKER_H
#define INDOOR_GW3_WALKER_H

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

#include "../../Grid.h"

#include "Helper.h"
#include "Structs.h"
#include "WalkEvaluator.h"

namespace GW3 {

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

	public:

		/** get a new destination for the given start */
		virtual const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const = 0;

	};

	template <typename Node> class WalkerDirectDestination : public WalkerBase<Node> {

		//Random::RandomGenerator rnd;

		std::vector<WalkEvaluator<Node>*> evals;

	public:

		/** make the code a little more readable */
		using Helper = GW3::Helper<Node>;
		using Walk = typename GW3::Walk<Node>;
		using Walks = typename GW3::Walks<Node>;
		using Nodes = typename GW3::Nodes<Node>;

		/** add a new evaluator to the walker */
		void addEvaluator(WalkEvaluator<Node>* eval) {
			this->evals.push_back(eval);
		}

		/** perform the walk based on the configured setup */
		const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {

			Assert::isNot0(params.distance_m, "walking distance must be > 0");

			static std::mt19937 rndGen;

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

			// include one additional grid-cell (increased distance)
			const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1);
			ReachableSettings set;
			set.limitDistance = true;
			set.dist_m = params.distance_m + secBuffer_m;
			set.limitHeading = false;
			set.heading = params.heading;
			set.maxHeadingDiff_rad = M_PI/2;
			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);

			WalkResult res;
			res.heading = params.heading;
			res.position = params.start;


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

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

				const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
				const GridPoint gp = Helper::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";

				}

			}

			// not found -> try random pick among all reachable nodes
			const float gridSize_m = grid.getGridSize_cm() / 100.0f;
			std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);

			const Node* dstNode = reachableNodes[dNode(rndGen)];

			// random position within destination-node
			std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);
			const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);

			const Point3 start = params.start;
			const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;

			double p = 1;
			for (const WalkEvaluator<Node>* eval : evals) {
				const double p1 = eval->getProbability(start, end, params);
				p *= p1;
			}

			if (start == end) {
				res.probability = 0;
				return res;
			}

			res.heading = Heading(start.xy(), end.xy());
			res.probability = p;
			res.position = end;
			return res;

		}

	};


	/**
	 * 1) get all reachable nodes from "start" (within a certain distance)
	 * 2) randomly pick one of em
	 * 3) scatter positon within the node's square -> end position
	 * 4) evaluate the probability of walking from start -> end
	 */
	template <typename Node> class WalkerWeightedRandom : public WalkerBase<Node> {

		std::vector<WalkEvaluator<Node>*> evals;

	public:

		/** make the code a little more readable */
		using Helper = GW3::Helper<Node>;
		using Walk = typename GW3::Walk<Node>;
		using Walks = typename GW3::Walks<Node>;
		using Nodes = typename GW3::Nodes<Node>;

		/** add a new evaluator to the walker */
		void addEvaluator(WalkEvaluator<Node>* eval) {
			this->evals.push_back(eval);
		}

		/** perform the walk based on the configured setup */
		const WalkResult getDestination(Grid<Node>& grid, const WalkParams& params) const override {

			Assert::isNot0(params.distance_m, "walking distance must be > 0");

			static std::minstd_rand rndGen;

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

//			// include one additional grid-cell (increased distance)
//			const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05);

//			ReachableSettings set;
//			set.limitDistance = true;
//			set.limitHeading = true;
//			set.dist_m = params.distance_m + secBuffer_m;
//			set.heading = params.heading;
//			set.maxHeadingDiff_rad = M_PI/2;
//			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);

			const float gridSize_m = grid.getGridSize_cm() / 100.0f;
			//std::uniform_int_distribution<int> dNode(0, (int)reachableNodes.size() - 1);


			Point3 best;
			double bestP = 0;
//			DrawList<Point3> drawer;

			const Point3 start = params.start;

			// try X random destinations, evaluate them, draw one of em according to probability (reduces the number of "stupid particles")
			//for (int i = 0; i < 500; ++i) {

			//	const Node* dstNode = reachableNodes[dNode(rndGen)];

			std::uniform_real_distribution<float> dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f);



			ReachableIteratorUnsorted<Node> ri(grid, *startNode);
			const float maxDist = params.distance_m * 1.25 + gridSize_m;

			auto skip = [&] (const Node& n) {
				const float dist_m = n.getDistanceInMeter(gpStart);
				return dist_m > maxDist;
			};

			//for (const Node* dstNode : reachableNodes) {
			while(ri.hasNext()) {

				const Node* dstNode = &ri.next(skip);
//				const float dist_m = dstNode->getDistanceInMeter(gpStart);

//				if (dist_m > maxDist) {
//					break;
//				}

				for (int i = 0; i < 25; ++i) {

					// random position within destination-node
					const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0);

					// destination = node-center + offset (within the node's bbox)
					const Point3 end = Helper::gpToP3(*dstNode) + dstOffset;

					// sanity check
					if (start == end) {continue;}
					if (!grid.hasNodeFor(Helper::p3ToGp(end))) {
						std::cout << "random destination is not part of the grid" << std::endl;
						continue;
					}

					//Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid");

					double p = 1;
					for (const WalkEvaluator<Node>* eval : evals) {
						const double p1 = eval->getProbability(start, end, params);
						p *= p1;
					}

					if (p > bestP) {bestP = p; best = end;}
					//drawer.add(end, p);

				}

			}

			//const Point3 end = drawer.get();
			const Point3 end = best;
			WalkResult res;
			if (start == end) {
				res.probability = 0;
			} else {
				res.heading = Heading(start.xy(), end.xy());
				res.probability = bestP;
			}
			res.position = end;
			return res;

		}

	};




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




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

//			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 = Helper::contains(grid, reachableNodes, dst);
//				if (n) {

//					const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f);
//					const GridPoint gp = Helper::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 // INDOOR_GW3_WALKER_H