#ifndef INDOOR_GW3_WALKEVALUATOR_H
#define INDOOR_GW3_WALKEVALUATOR_H

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

#include "../../../math/Distributions.h"
#include "../../../grid/Grid.h"

namespace GW3 {

	/** interface for all evaluators that return a probability for a given walk */
	template <typename Node> class WalkEvaluator {

	public:

		/** get the probability for the given walk */
		//virtual double getProbability(const Walk<Node>& walk) const = 0;

		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const = 0;

	};


	/** evaluate the grid-node-importance importance(end) */
	template <typename Node> class WalkEvalEndNodeProbability : public WalkEvaluator<Node> {

		Grid<Node>* grid;

	public:

		WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}

		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const override {

			(void) params;
			(void) pStart;

			const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
			const Node& node = grid->getNodeFor(gp);
			const double p = node.getWalkImportance();
			return p;
			//return std::pow(p,10);

		}

	};



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

		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 Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {

			(void) params;

			if (pStart == pEnd) {
				std::cout << "warn! start-position == end-positon" << std::endl;
				return 0;
			}

			const Heading head(pStart.xy(), pEnd.xy());
			const float diff = head.getDiffHalfRAD(params.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 Node> class WalkEvalDistance : public WalkEvaluator<Node> {

		const double sigma;

		const Distribution::Normal<double> dist;

	public:

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

		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override {

			const float requestedDistance_m = params.distance_m;
			const float walkedDistance_m = walkDist_m;//pStart.getDistance(pEnd);
			const float diff = walkedDistance_m - requestedDistance_m;
			return dist.getProbability(diff);
			//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);

		}

	};

}

#endif // INDOOR_GW3_WALKEVALUATOR_H