/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef NAVMESHDIJKSTRA_H
#define NAVMESHDIJKSTRA_H

#include "../NavMesh.h"
#include "../../nav/dijkstra/Dijkstra.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include "../NavMeshDebug.h"

namespace NM {

	/** distance/neighbor-to-the target for each of the 3 triangle edge points */
	class NavMeshTriangleDijkstra {

	public:

		/** next hop towards the pedestrian's target */
		struct ToTarget {
			struct NextTarget {
				const NavMeshTriangle* tria = nullptr;
				int pointIndex;
				Point3 point() const {
					switch(pointIndex) {
					case 0: return tria->getP1();
					case 1: return tria->getP2();
					case 2: return tria->getP3();
					case 3: return tria->getCenter();
					default: throw Exception("invalid point index");
					}
				}
				template <typename Tria> ToTarget hop() const {
					const Tria* t = (const Tria*)tria;
					switch(pointIndex) {
					case 0: return t->spFromP1;
					case 1: return t->spFromP2;
					case 2: return t->spFromP3;
					case 3: return t->spFromCenter;
					default: throw Exception("invalid point index");
					}
				}
			} next;
			float distance = 0;
		};

		ToTarget spFromCenter;
		ToTarget spFromP1;
		ToTarget spFromP2;
		ToTarget spFromP3;

		/** interpolate the distance towards the garget for the given point */
		template <typename UserTriangleClass> float getDistanceToDestination(const Point3 p) const {
			// this one is a little bit awkward.. normally NavMeshTriangleDijkstra should extend NavMeshTriangle..
			// however, this often yields issues for user-classes, extending NavMeshTriangle more than once
			StaticAssert::AinheritsB<UserTriangleClass, NavMeshTriangle>();
			const UserTriangleClass* userClass = static_cast<const UserTriangleClass*>(this);	// must inherit NavMeshTriangle
			return userClass->interpolate(p, spFromP1.distance, spFromP2.distance, spFromP3.distance);
		}

		/** get the next neighbor-point/triangle for the given point */
		template <typename Tria> NavMeshTriangleDijkstra::ToTarget nearestHop(const Point3 p) const {
			const Tria* tria = static_cast<const Tria*>(this);
			const Point3 pc = tria->getCenter();
			const Point3 p1 = tria->getP1();
			const Point3 p2 = tria->getP2();
			const Point3 p3 = tria->getP3();
			const float dc = p.getDistance(pc);
			const float d1 = p.getDistance(p1);
			const float d2 = p.getDistance(p2);
			const float d3 = p.getDistance(p3);
//			if (dc < d1 && dc < d2 && dc < d3)  {return NavMeshLocation<Tria>(pc, static_cast<const Tria*>(spFromCenter.next));}
//			if (d1 < dc && d1 < d2 && d1 < d3)  {return NavMeshLocation<Tria>(p1, static_cast<const Tria*>(spFromP1.next));}
//			if (d2 < dc && d2 < d1 && d2 < d3)  {return NavMeshLocation<Tria>(p2, static_cast<const Tria*>(spFromP2.next));}
//			if (d3 < dc && d3 < d1 && d3 < d2)  {return NavMeshLocation<Tria>(p3, static_cast<const Tria*>(spFromP3.next));}
			if (dc < d1 && dc < d2 && dc < d3)  {return spFromCenter;}
			if (d1 < dc && d1 < d2 && d1 < d3)  {return spFromP1;}
			if (d2 < dc && d2 < d1 && d2 < d3)  {return spFromP2;}
			if (d3 < dc && d3 < d1 && d3 < d2)  {return spFromP3;}
			throw Exception("invalid code-path detected");
		}

		/** get the complete path from p towards to pedestrian's destination */
		template <typename Tria> std::vector<NavMeshLocation<Tria>> getPathToDestination(const Point3 p) const {
			std::vector<NavMeshLocation<Tria>> path;
//			NavMeshLocation<Tria> cur(p, static_cast<const Tria*>(this));
//			path.push_back(cur);
//			while(true) {
//				//NavMeshLocation<Tria> step = prev.tria->nextHop(prev.pos);//static_cast<const NavMeshTriangleDijkstra*>(prev.tria)->next(prev.pos);
//				NavMeshLocation<Tria> step = static_cast<const NavMeshTriangleDijkstra*>(prev.tria)->nextHop<Tria>(prev.pos);

//				path.push_back(step);
//				if (step.tria == nullptr) {break;}	// reached end
//				prev = step;
//			}
//			return path;

			// starting point
			NavMeshLocation<Tria> first(p, static_cast<const Tria*>(this));
			path.push_back(first);

			// first hop towards the destination
			NavMeshTriangleDijkstra::ToTarget cur = nearestHop<Tria>(p);

			// iterate all hops towards the destination
			while(cur.next.tria) {
				const NavMeshLocation<Tria> loc(cur.next.point(), (const Tria*)cur.next.tria);
				path.push_back(loc);
				cur = cur.next.hop<Tria>();
			}

			return path;
		}

	};

//	// mapper
//	template <typename Tria> struct Access {
//		int getNumNeighbors(const Tria& t) const {return t.getNumNeighbors();}
//		const Tria* getNeighbor(const Tria& t, const int idx) const {return (Tria*)t.getNeighbor(idx);}
//		float getWeightBetween(const Tria& t1, const Tria& t2) const {return t1.getCenter().getDistance(t2.getCenter());}
//	};

	/** add distance-to-target infos for the triangles */
	class NavMeshDijkstra {


		struct TemporalNode {
			Point3 pt;							// a point within the map
			const NavMeshTriangle* triangle;	// the triangle the point belongs to
			int pointIndex;						// [0-2] (3 edge points) or 3 [center]
			std::vector<TemporalNode*> neighbors;
			TemporalNode(const Point3 pt, const NavMeshTriangle* triangle, int pointIndex) : pt(pt), triangle(triangle), pointIndex(pointIndex) {;}
			bool operator == (const TemporalNode& o ) {return o.pt == pt;}
			bool operator == (const Point3& pos ) {return pt == pos;}
			operator std::string() const {return asString();}
			std::string asString() const {
				return "(" + std::to_string(pt.x) + "," + std::to_string(pt.y) + "," + std::to_string(pt.z) + ")";
			}
		};

		struct NodeComp {
			Point3 pos;
			//bool operator () (const Node* n) {return n->pt == pos;}
			bool operator () (const TemporalNode* n) {return n->pt.getDistance(pos) < 0.0001;}
			NodeComp(const Point3 pos) : pos(pos) {;}
		};

		struct NodeAccess {
			int getNumNeighbors(const TemporalNode& n) const {return n.neighbors.size();}
			const TemporalNode* getNeighbor(const TemporalNode& n, const int idx) const {return n.neighbors[idx];}
			float getWeightBetween(const TemporalNode& n1, const TemporalNode& n2) const {return n1.pt.getDistance(n2.pt);}
		};

	public:

		/** attach distance/triangle-to-target to the ToTarget struct */
		static void set(NavMeshTriangleDijkstra::ToTarget& t, const DijkstraNode<TemporalNode>* n) {
			t.distance = n->cumWeight;
			t.next.tria = (n->previous) ? (n->previous->element->triangle) : (nullptr);
			t.next.pointIndex = (n->previous) ? (n->previous->element->pointIndex) : (-1);
		}

		template <typename Tria> static void stamp(NavMesh<Tria>& mesh, const Point3 dst) {

			// ensure Tria extends NavMeshTriangleDijkstra
			StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();

			// build finer mesh for dijkstra
			std::vector<TemporalNode*> nodes = net(mesh);

			// point3 to mesh location
			NavMeshLocation<Tria> endLoc = mesh.getLocation(dst);
			auto it = std::find_if(nodes.begin(), nodes.end(), NodeComp(endLoc.tria->getCenter()));
			if (it == nodes.end()) {throw Exception("end node not found");}
			TemporalNode* end = *it;


//			Node* end = nodes[0];	// TODO;

			NodeAccess acc;

			// dijkstra
			Dijkstra<TemporalNode> dijkstra;
			dijkstra.build(end, acc);

			for (Tria* t : mesh) {

				auto itCenter = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getCenter()));
				auto it1 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP1()));
				auto it2 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP2()));
				auto it3 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP3()));

				auto* dnCenter = dijkstra.getNode(*itCenter);
				if (dnCenter != nullptr) {set(t->spFromCenter, dnCenter);}

				auto* dn1 = dijkstra.getNode(*it1);
				if (dn1 != nullptr) {set(t->spFromP1, dn1);}

				auto* dn2 = dijkstra.getNode(*it2);
				if (dn2 != nullptr) {set(t->spFromP2, dn2);}

				auto* dn3 = dijkstra.getNode(*it3);
				if (dn3 != nullptr) {set(t->spFromP3, dn3);}

			}



			NavMeshDebug dbg;
			dbg.addMesh(mesh);
			dbg.addDijkstra(mesh);
			dbg.draw();


			int zzz = 0; (void) zzz;

		}


		/**
		 * a normal navigation mesh only connects adjacent triangles (can be thought of as "from center to center")
		 * however, later on, we need a distance estimation for any point within the triangle.
		 * we thus need dijkstra to estimate the distance for every edge of the triangle (to allow for barycentric interpolation)
		 * we thus build a temporal graph which contains all triangle centers and edge-points.
		 * Note: many triangles share the same edge-points!
		 * likewise, all possible connections are drawn.
		 */
		template <typename Tria> static std::vector<TemporalNode*> net(NavMesh<Tria>& mesh) {

			std::vector<TemporalNode*> nodes;

			// 1) add all triangle nodes (center, p1, p2, p3)
			for (const Tria* t : mesh) {

				auto itCenter = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getCenter()));
				if (itCenter == nodes.end()) {nodes.push_back(new TemporalNode(t->getCenter(), t, 3));}

				//Node n1(t->getP1());
				auto it1 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP1()));
				if (it1 == nodes.end()) {nodes.push_back(new TemporalNode(t->getP1(), t, 0));}

				//Node n2(t->getP2());
				auto it2 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP2()));
				if (it2 == nodes.end()) {nodes.push_back(new TemporalNode(t->getP2(), t, 1));}

				//Node n3(t->getP1());
				auto it3 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP3()));
				if (it3 == nodes.end()) {nodes.push_back(new TemporalNode(t->getP3(), t, 2));}

			}

			// 2) connect all possible nodes
			for (const Tria* t : mesh) {

				auto itCenter = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getCenter()));
				auto it1 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP1()));
				auto it2 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP2()));
				auto it3 = std::find_if(nodes.begin(), nodes.end(), NodeComp(t->getP3()));

				(*itCenter)->neighbors.push_back(*it1);
				(*itCenter)->neighbors.push_back(*it2);
				(*itCenter)->neighbors.push_back(*it3);

				(*it1)->neighbors.push_back(*itCenter);
				(*it1)->neighbors.push_back(*it2);
				(*it1)->neighbors.push_back(*it3);

				(*it2)->neighbors.push_back(*itCenter);
				(*it2)->neighbors.push_back(*it1);
				(*it2)->neighbors.push_back(*it3);

				(*it3)->neighbors.push_back(*itCenter);
				(*it3)->neighbors.push_back(*it1);
				(*it3)->neighbors.push_back(*it2);

				// neighbors
				for (const auto* n : *t) {
					auto itCenter2 = std::find_if(nodes.begin(), nodes.end(), NodeComp(n->getCenter()));
					(*itCenter)->neighbors.push_back(*itCenter2);
				}

			}




			K::Gnuplot gp;
			K::GnuplotSplot plot;
			K::GnuplotSplotElementLines lines; plot.add(&lines);

			for (const TemporalNode* n1 : nodes) {
				for (const TemporalNode* n2 : n1->neighbors) {
					const K::GnuplotPoint3 gp1(n1->pt.x, n1->pt.y, n1->pt.z);
					const K::GnuplotPoint3 gp2(n2->pt.x, n2->pt.y, n2->pt.z);
					lines.addSegment(gp1, gp2);
				}
			}

			gp.draw(plot);
			gp.flush();;

			int xxxx = 0; (void) xxxx;

			return nodes;

		}

	};

}

#endif // NAVMESHDIJKSTRA_H