#ifndef NAVMESHWALKEVAL_H
#define NAVMESHWALKEVAL_H

#include "NavMeshWalkParams.h"
#include "../NavMeshLocation.h"
#include "../../math/Distributions.h"
#include "../../misc/PerfCheck.h"

namespace NM {

	template <typename Tria> struct NavMeshPotentialWalk {

		NavMeshWalkParams<Tria> requested;

		NavMeshLocation<Tria> end;

		NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested) : requested(requested) {
			;
		}

		NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested, const NavMeshLocation<Tria>& end) : requested(requested), end(end) {
			;
		}

	};

	/**
	 * evaluate a NavMeshWalk from -> to = probability
	 */
	template <typename Tria> class NavMeshWalkEval {

	public:

		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const = 0;

	};







	/**
	 * evaluate the difference between head(start,end) and the requested heading
	 */
	template <typename Tria> class WalkEvalHeadingStartEnd : public NavMeshWalkEval<Tria> {

		const double sigma_rad;
		const double kappa;
		Distribution::VonMises<double> _dist;
		Distribution::LUT<double> dist;

	public:

		// kappa = 1/var = 1/sigma^2
		// https://en.wikipedia.org/wiki/Von_Mises_distribution
		WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
			sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
			;
		}

		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {

			PERF_REGION(4, "WalkEvalHeadingStartEnd");

			Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position");

			const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
			const float diff = head.getDiffHalfRAD(walk.requested.heading);
			//const float diff = Heading::getSignedDiff(params.heading, head);
			//return Distribution::Normal<double>::getProbability(0, sigma, diff);
			return dist.getProbability(diff);

		}

	};

	/**
	 * evaluate the difference between head(start,end) and the requested heading
	 */
	template <typename Tria> class WalkEvalHeadingStartEndNormal : public NavMeshWalkEval<Tria> {

		const double sigma_rad;
		Distribution::Normal<double> dist;

	public:

		WalkEvalHeadingStartEndNormal(const double sigma_rad = 0.04) :
			sigma_rad(sigma_rad), dist(0, sigma_rad) {
			;
		}

		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {

			PERF_REGION(4, "WalkEvalHeadingStartEnd");

			Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position");

			const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
			const float diff = head.getDiffHalfRAD(walk.requested.heading);
			//const float diff = Heading::getSignedDiff(params.heading, head);
			//return Distribution::Normal<double>::getProbability(0, sigma, diff);
			return dist.getProbability(diff);

		}

	};


	/**
	 * evaluate the difference between distance(start, end) and the requested distance
	 */
	template <typename Tria> class WalkEvalDistance : public NavMeshWalkEval<Tria> {

		const double sigma;

		const Distribution::Normal<double> dist;

	public:

		WalkEvalDistance( const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}

		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {

			PERF_REGION(5, "WalkEvalDistance");

			const float requestedDistance_m = walk.requested.getToBeWalkedDistance();
			const float walkedDistance_m = walk.requested.start.pos.getDistance(walk.end.pos);
			const float diff = walkedDistance_m - requestedDistance_m;
			return dist.getProbability(diff);
			//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);

		}

	};


}

#endif // NAVMESHWALKEVAL_H