#ifndef NAVMESHWALKSEMIDIRECTED_H
#define NAVMESHWALKSEMIDIRECTED_H


#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "../../math/distribution/Uniform.h"

#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"

namespace NM {

	template <typename Tria> class NavMeshWalkSemiDirected {

	private:

		const NavMesh<Tria>& mesh;

		std::vector<NavMeshWalkEval<Tria>*> evals;

	public:


		/** single result */
		struct ResultEntry {
			NavMeshLocation<Tria> location;
			Heading heading;
			double probability;
			ResultEntry() : heading(0) {;}
		};

		/** list of results */
		using ResultList = std::vector<ResultEntry>;

	public:

		/** ctor */
		NavMeshWalkSemiDirected(const NavMesh<Tria>& mesh) : mesh(mesh) {

		}

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



		ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {

			// sanity checks
			params.check();

			ResultEntry re;

			static Distribution::Uniform<float> dHead(-0.10, +0.10, 1337);
			static Distribution::Uniform<float> dDist(-0.10, +0.10, 7331);

			float impactHead = 1;
			float impactDist = 1;
			int runs = 0;

			// construct reachable region once
			// must be large enough to also contain increased distances!
			const float searchRegion = 2.0 + params.getToBeWalkedDistance() * 1.1;
			const NavMeshSub<Tria> reachable(params.start, searchRegion);

			while(true) {

				// not to be found.. plan B
				if (++runs > 8) {
					NM::NavMeshRandom<Tria> rnd = reachable.getRandom();
					re.location = rnd.drawWithin(params.start.pos, params.getToBeWalkedDistance());
					re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());
					break;
				}

				// to-be-walked distance;
				const float modHead = dHead.draw() * impactHead;
				const float modDist = dDist.draw() * impactDist;
				impactHead += 3;
				impactDist += 1.05;

				const float toBeWalkedDist = params.getToBeWalkedDistance() + modDist;
				const Heading head = params.heading + modHead;

				if (toBeWalkedDist < 0.01)			{continue;}
				if (toBeWalkedDist > searchRegion)	{continue;}

				// get the to-be-reached destination's position (using start+distance+heading)
				const Point2 dir = head.asVector();
				const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
				const Tria* dstTria = reachable.getContainingTriangle(dst);

				// is above destination reachable?
				if (dstTria) {

					re.heading = head;											// adjust walked heading
					re.location.pos = dstTria->toPoint3(dst);					// new destination position
					re.location.tria = dstTria;									// new destination triangle
					break;

				}

			}

			// calculate probability
			const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
			re.probability = 1.0;
			for (const NavMeshWalkEval<Tria>* eval : evals) {
				const double p1 = eval->getProbability(pwalk);
				re.probability *= p1;
			}

			// done
			return re;

		}

		ResultList getMany(const NavMeshWalkParams<Tria>& params) {

			// sanity checks
			params.check();

			return {getOne(params)};

		}


	};

}

#endif // NAVMESHWALKSEMIDIRECTED_H