#ifndef INDOOR_GW3_REACHABLESAMPLER_H
#define INDOOR_GW3_REACHABLESAMPLER_H

#include "../../../math/random/RandomGenerator.h"

#include "Reachable.h"
#include "Helper.h"

namespace GW3 {

	template <typename Node> class ReachableSamplerByDepth {

	public:

		using Entry = typename ReachableByDepthWithDistanceSorted<Node>::Entry;

		struct SampleResult {
			Point3 pos;
			float walkDistToStart_m;
			SampleResult(const Point3 pos, const float dist_m) : pos(pos), walkDistToStart_m(dist_m) {;}
		};

	private:

		const Grid<Node>& grid;
		const float gridSize_m;

		const std::vector<Entry>& reachableNodes;

		mutable Random::RandomGenerator gen;

		mutable std::uniform_real_distribution<float> dOffset;


	public:

		/** ctor */
		ReachableSamplerByDepth(const Grid<Node>& grid, const std::vector<Entry>& reachableNodes) :
			grid(grid), reachableNodes(reachableNodes), gridSize_m(grid.getGridSize_cm() / 100.0f), dOffset(-gridSize_m*0.48f, +gridSize_m*0.48f) {
			;
		}

		SampleResult sample() {

			std::uniform_int_distribution<int> dIdx(0, reachableNodes.size() - 1);

			const int idx = dIdx(gen);

			const Entry* e = &reachableNodes[idx];
			const Entry* ePrev1 = (e->prevIdx == -1) ? (nullptr) : (&reachableNodes[e->prevIdx]);
			const Node* nDst = e->node;

			// center of the destination node
			const Point3 nodeCenter = Helper<Node>::gpToP3(*nDst);

			// random position within destination-node
			const float ox = dOffset(gen);
			const float oy = dOffset(gen);

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

			// calculate end's walking-distance towards the start
			float distToStart_m;
			if (ePrev1) {
				distToStart_m = ePrev1->walkDistToStart_m + (Helper<Node>::gpToP3(*(ePrev1->node)).getDistance(end));
			} else {
				distToStart_m = nodeCenter.getDistance(end);
			}

			// done
			return SampleResult(end, distToStart_m);

		}


	};

}

#endif // REACHABLESAMPLER_H