/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#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"
#include "Reachable.h"
#include "ReachableSampler.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(const WalkParams& params) const = 0;

	};

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

		Grid<Node>& grid;
		std::vector<WalkEvaluator<Node>*> evals;

	public:

		/** ctor */
		WalkerDirectDestination(Grid<Node>& grid) : grid(grid) {
			;
		}

		/** 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(const WalkParams& params) const override {

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

			Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(params.start)), "start-point not found on grid");


			static std::mt19937 rndGen;

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

			// calculate a walk's probability
			auto getP = [&] (const Point3 dst) {
				double p = 1;
				const PotentialWalk pWalk(params, params.start, dst, params.start.getDistance(dst));
				for (const WalkEvaluator<Node>* eval : evals) {
					const double p1 = eval->getProbability(pWalk);
					p *= p1;
				}
				return p;
			};

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

//			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;

//			// get all nodes that satisfy above constraints
//			const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set);

			struct Cond {
				const float maxDist_m;
				const Node* startNode;
				Cond(float maxDist_m, const Node* startNode) : maxDist_m(maxDist_m), startNode(startNode) {;}
				bool visit(const Node& n) const {
					return (startNode->getDistanceInMeter(n)) < maxDist_m;
				}
			};
			Cond cond(params.getDistanceInMeter(grid) + secBuffer_m, startNode);
			std::vector<const Node*>  reachableNodes = ReachableByConditionUnsorted<Node, Cond>::get(grid, *startNode, cond);

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

			// get the to-be-reached destination's position (using start+distance+heading)
			const Point2 dir = res.heading.asVector();
			const Point2 dst = params.start.xy() + (dir * params.getDistanceInMeter(grid));

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

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



				if (grid.hasNodeFor(gp)) {
					res.position = p3;					// update position
					//res.heading;						// keep as-is
					res.probability *= getP(p3);		// 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;

			const PotentialWalk pWalk(params, start, end, start.getDistance(end));

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

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

			res.heading = Heading(start.xy(), end.xy());
			res.probability *= getP(end);
			res.position = end;

			if (!grid.hasNodeFor(grid.toGridPoint(res.position))) {

				std::cout << "issue:" << grid.toGridPoint(res.position).asString() << std::endl;

				std::cout << "issue:" << res.position.asString() << std::endl;

				for (int i = -80; i <= +80; i+=10) {
					Point3 pos = res.position + Point3(0,0,i/100.0f);
					std::cout << pos.asString() <<  " ----- " << res.position.asString() << std::endl;
					std::cout << (grid.toGridPoint(pos)).asString() << std::endl;
					std::cout << grid.hasNodeFor(grid.toGridPoint(pos)) << std::endl;
					std::cout << std::endl;
				}

				int i = 0; (void) i;

			}

#if (GRID_MODE == GM_BOX)
			Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid");
#endif

			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;

		Grid<Node>& grid;
		const float gridSize_m;

		mutable std::minstd_rand rndGen;
		mutable std::uniform_real_distribution<float> dFinal;

	public:

		/** ctor */
		WalkerWeightedRandom(Grid<Node>& grid) :
			grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.48f, +gridSize_m*0.48f) {
			;
		}

		/** 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(const WalkParams& params) const override {

			const float walkDist_m = params.getDistanceInMeter(grid);

			Assert::isNot0(walkDist_m, "walking distance must be > 0");

			const GridPoint gpStart = grid.toGridPoint(params.start);
			const Node* startNode = grid.getNodePtrFor(gpStart);
			if (!startNode) {throw Exception("start node not found!");}

			const float maxDist = walkDist_m + gridSize_m;
			const int depth = std::ceil(walkDist_m / gridSize_m) + 1;

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

			const Point3 start = params.start;


			struct RICond {
				const GridPoint gpStart;
				const float maxDist;
				RICond(const GridPoint gpStart, const float maxDist) : gpStart(gpStart), maxDist(maxDist) {;}
				bool visit (const Node& n) const {
					const float dist_m = n.getDistanceInMeter(gpStart);
					return dist_m < maxDist;
				}
			};
			RICond riCond(gpStart, maxDist);

			// iterate over all reachable nodes that satisfy a certain criteria (e.g. max distance)
			ReachableIteratorUnsorted<Node, RICond> ri(grid, *startNode, riCond);

			int numVisitedNodes = 0;


			#define MODE	2

			#if (MODE == 1)

				double bestNodeP = 0;
				const Node* bestNode = nullptr;

				ReachableByDepthUnsorted<Node> reach(grid);
				std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);

				for (const Node* dstNode : nodes) {
					const Point3 nodeCenter = Helper::gpToP3(*dstNode);
					const float walkDist_m = nodeCenter.getDistance(start);//*1.05;
					double p = 1.0;
					for (const WalkEvaluator<Node>* eval : evals) {
						const double p1 = eval->getProbability(start, nodeCenter, walkDist_m, params);
						p *= p1;
					}
					if (p > bestNodeP) {
						bestNodeP = p;
						bestNode = dstNode;
					}
				}

//				while(ri.hasNext()) {
//					const Node* dstNode = &ri.next();
//					const Point3 nodeCenter = Helper::gpToP3(*dstNode);
//					double p = 1.0;
//					for (const WalkEvaluator<Node>* eval : evals) {
//						const double p1 = eval->getProbability(start, nodeCenter, params);
//						p *= p1;
//					}
//					if (p > bestNodeP) {
//						bestNodeP = p;
//						bestNode = dstNode;
//					}
//				}

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

					const Point3 nodeCenter = Helper::gpToP3(*bestNode);

					// random position within destination-node
					const float ox = dFinal(rndGen);
					const float oy = dFinal(rndGen);

					// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
					const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);
					const float walkDist_m = end.getDistance(start);//*1.05;

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

					if (p > bestP) {bestP = p; best = end;}

				}

			#elif (MODE == 2)

				ReachableByDepthUnsorted<Node> reach(grid);
				std::unordered_set<const Node*> nodes = reach.get(*startNode, depth);

				// all reachable nodes
				//while(ri.hasNext()) {
					//const Node* dstNode = &ri.next();

				for (const Node* dstNode : nodes) {

					++numVisitedNodes;

					const Point3 nodeCenter = Helper::gpToP3(*dstNode);

					// try multiple locations within each reachable node
					for (int i = 0; i < 3; ++i) {

						// random position within destination-node
						const float ox = dFinal(rndGen);
						const float oy = dFinal(rndGen);

						// destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is)
						const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z);

						// 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");
						const float walkDist_m = end.getDistance(start);//*1.05;

						const PotentialWalk pWalk(params, start, end, walkDist_m);

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

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

					}

				}

			#elif (MODE == 3)

				using Reachable = ReachableByDepthWithDistanceSorted<Node>;
				using ReachableNode = typename Reachable::Entry;
				Reachable reach(grid);
				std::vector<ReachableNode> reachableNodes = reach.get(*startNode, depth);

				using Sampler = ReachableSamplerByDepth<Node>;
				using SamplerResult = typename Sampler::SampleResult;
				Sampler sampler(grid, reachableNodes);

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

					const SamplerResult sample = sampler.sample();

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

					if (p > bestP) {bestP = p; best = sample.pos;}

				}

			#endif

			//std::cout << numVisitedNodes << std::endl;

			//double drawProb = 0; const Point3 end = drawer.get(drawProb);
			const Point3 end = best;
			WalkResult res;
			if (start == end) {
				res.probability = 0;
			} else {
				res.heading = Heading(start.xy(), end.xy());
				//res.probability = drawProb;		// when using DrawList
				res.probability = bestP;	// when using 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