#ifndef NAVMESHWALKSINKORSWIM_H
#define NAVMESHWALKSINKORSWIM_H

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

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

namespace NM {

	/**
	 * try to move to the requested location
	 * and, if not, return null
	 */
	template <typename Tria> class NavMeshWalkSinkOrSwim {

	public:

		struct Config {
			Distribution::Uniform<float>* distanceVariation = nullptr;
			Distribution::Uniform<float>* headingVariation = nullptr;
			void check() {
				Assert::isNotNull(distanceVariation,	"distanceVariation must not be null");
				Assert::isNotNull(headingVariation,		"headingVariation must not be null");
			}
		};

	private:

		const NavMesh<Tria>& mesh;

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

		Config cfg;

		int hits = 0;
		int misses = 0;

	public:


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

		ResultEntry lastRes;

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

	public:

		/** ctor without config */
		NavMeshWalkSinkOrSwim(const NavMesh<Tria>& mesh) : mesh(mesh), cfg() {

		}

		/** ctor with config */
		NavMeshWalkSinkOrSwim(const NavMesh<Tria>& mesh, Config cfg) : mesh(mesh), cfg(cfg) {
			cfg.check();
		}

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

			// variation?
			const float distVar =		(cfg.distanceVariation) ? (cfg.distanceVariation->draw()) : (0);
			const float headingVar =	(cfg.headingVariation) ? (cfg.headingVariation->draw()) : (0);

			// to-be-walked distance;
			const float toBeWalkedDist = params.getToBeWalkedDistance() + distVar;
			const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;

			// construct reachable region
			NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);

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

			const Tria* dstTria = reachable.getContainingTriangle(dst);

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

				re.heading = params.heading;					// heading was OK -> keep
				re.location.pos = dstTria->toPoint3(dst);		// new destination position
				re.location.tria = dstTria;						// new destination triangle
				re.probability = 1;
				++hits;

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

				lastRes = re;

			} else {

//				re.heading = params.heading;					// keep
//				re.location = params.start;						// keep
//				re.probability = 0;								// kill
				re = lastRes;
				//re.probability *= 0.1;
				++misses;

			}

			const int total = (hits + misses);
			if (total % 10000 == 0) {
				//std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
			}

			// done
			return re;

		}

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

			// sanity checks
			params.check();

			return {getOne(params)};

		}


	};

}

#endif // NAVMESHWALKSINKORSWIM_H