/*
 * © 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 NAVMESHTRIANGLE_H
#define NAVMESHTRIANGLE_H

#include "../geo/Point3.h"
#include "../geo/Point2.h"

// fast barycentric code
// https://stackoverflow.com/questions/25385361/point-within-a-triangle-barycentric-co-ordinates#25386102
// https://i.stack.imgur.com/8VODS.png
// https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates

namespace NM {

	/**
	 * represents one triangle within the NavMesh
	 * each Triangle has up to 3 neighbors (one per edge)
	 *
	 * for performance enhancements,
	 * some memeber attributes are pre-calculated once
	 */
	class NavMeshTriangle {

	private:

		template<typename> friend class NavMesh;

		const Point3 p1;
		const Point3 p2;
		const Point3 p3;
		const uint8_t type;

		NavMeshTriangle* _neighbors[3];
		int _numNeighbors;

	protected:	// precalculated stuff

//		Point2 v0;
//		Point2 v1;
//		float dot00;
//		float dot01;
//		float dot11;
//		double invDenom;

		float area;

		float minZ;
		float maxZ;

		const Point3 center;
		const Point3 v12;
		const Point3 v13;

		const double _det;

	public:

		/** ctor */
		NavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) :
			p1(p1), p2(p2), p3(p3), type(type),
			_neighbors(), _numNeighbors(0),
			center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1),
			_det(1.0*(p2.y - p3.y)*(p1.x - p3.x) + (p3.x - p2.x)*(p1.y - p3.y)) {

			precompute();

		}

		/** get the triangle's type */
		uint8_t getType() const {return type;}

		Point3 getP1() const {return p1;}

		Point3 getP2() const {return p2;}

		Point3 getP3() const {return p3;}


		/** get the number of known neighbors for this triangle */
		int getNumNeighbors() const {return _numNeighbors;}

		/** get the idx-th neighbor */
		const NavMeshTriangle* getNeighbor(const int idx) const {return _neighbors[idx];}


		/** get the distance between the given point and the triangle using approximate tests */
		float getDistanceApx(const Point3 pt) const {

//			const float d1 = pt.getDistance(p1);
//			const float d2 = pt.getDistance(p2);
//			const float d3 = pt.getDistance(p3);
//			const float d4 = pt.getDistance(center);
//			const float d5 = pt.getDistance((p1-p2)/2);
//			const float d6 = pt.getDistance((p2-p3)/2);
//			const float d7 = pt.getDistance((p3-p1)/2);
//			return std::min(d1, std::min(d2, std::min(d3, std::min(d4, std::min(d5, std::min(d6,d7))))));

//			const float d1 = pt.getDistance(p1);
//			const float d2 = pt.getDistance(p2);
//			const float d3 = pt.getDistance(p3);
//			const float d4 = pt.getDistance(center);
//			return std::min(d1, std::min(d2, std::min(d3,d4)));

			float bestD = 99999;
			Point3 bestP;
			Point3 dir12 = p2-p1;
			Point3 dir13 = p3-p1;
			Point3 dir23 = p3-p2;
			for (float f = 0; f < 1; f += 0.05f) {
				const Point3 pos1 = p1 + dir12 * f; const float dist1 = pos1.getDistance(pt);
				const Point3 pos2 = p1 + dir13 * f;	const float dist2 = pos2.getDistance(pt);
				const Point3 pos3 = p2 + dir23 * f;	const float dist3 = pos3.getDistance(pt);
				if (dist1 < bestD) {bestP = pos1; bestD = dist1;}
				if (dist2 < bestD) {bestP = pos2; bestD = dist2;}
				if (dist3 < bestD) {bestP = pos3; bestD = dist3;}
			}

			return bestD;


		}


		bool operator == (const NavMeshTriangle& o) const {
			return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3);
		}

		/** is the triangle plain? (same Z for all points) */
		bool isPlain() const {
			const float d1 = std::abs(p1.z - p2.z);
			const float d2 = std::abs(p2.z - p3.z);
			return (d1 < 0.1) && (d2 < 0.1);
		}

		const NavMeshTriangle* const* begin() const {return &_neighbors[0];}

		const NavMeshTriangle* const* end() const {return &_neighbors[_numNeighbors];}

		Point3 getPoint(const float u, const float v) const {
			return p1 + (v12*u) + (v13*v);
		}


		/** 2D UV */
		void getUV(const Point2 p, float& u, float& v) const {

			// https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
			const Point2 v0 = p2.xy()	- p1.xy();
			const Point2 v1 = p3.xy()	- p1.xy();
			const Point2 v2 = p			- p1.xy();
			const float den = v0.x * v1.y - v1.x * v0.y;
			u = (v2.x * v1.y - v1.x * v2.y) / den;
			v = (v0.x * v2.y - v2.x * v0.y) / den;

		}

		/** 2D UVW */
		void getUVW(const Point2 p, float& u, float& v, float& w) const {
			getUV(p,u,v);
			w = 1-u-v;
		}

		/** 3D UV */
		void getUV(const Point3 p, float& u, float& v) const {

			const Point3 v0 = p2 - p1;
			const Point3 v1 = p3 - p1;
			const Point3 v2 = p  - p1;
			const float d00 = dot(v0, v0);
			const float d01 = dot(v0, v1);
			const float d11 = dot(v1, v1);
			const float d20 = dot(v2, v0);
			const float d21 = dot(v2, v1);
			const float denom = d00 * d11 - d01 * d01;
			u = (d11 * d20 - d01 * d21) / denom;
			v = (d00 * d21 - d01 * d20) / denom;
			//w = 1.0f - v - w;

			int xx = 0; (void) xx;

		}

		/** 3D UVW */
		void getUVW(const Point3 p, float& u, float& v, float& w) const {
			getUV(p,u,v);
			w = 1-u-v;
		}

		/** barycentric interpolation at Point p for val1@p1, val2@p2, val3@p3 */
		template <typename T> T interpolate(const Point3 p, const T val1, const T val2, const T val3) const {

			float u, v, w;
			getUVW(p.xy(),u,v,w);

			return (w*val1) + (u*val2) + (v*val3);

		}




		/** does the triangle contain the given 3D point? */
		bool contains(const Point3 p) const {
			return	(minZ <= p.z) && (maxZ >= p.z) && contains(p.xy());
		}

		/** does the triangle contain the given 2D point? */
		bool contains(const Point2 p) const {

//			const Point2 v2 = p - p1.xy();

//			// Compute dot products
//			float dot02 = dot(v0, v2);
//			float dot12 = dot(v1, v2);

//			// Compute barycentric coordinates
//			float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
//			float v = (dot00 * dot12 - dot01 * dot02) * invDenom;

			float u, v;
			getUV(p, u, v);

			// Check if point is in triangle
			return (u >= 0) && (v >= 0) && (u + v <= 1);

		}

		/** estimate the correct z-value for the given 2D point */
		Point3 toPoint3(const Point2 p) const {

//			const Point2 v2 = p - p1.xy();

//			// Compute dot products
//			float dot02 = dot(v0, v2);
//			float dot12 = dot(v1, v2);

//			// Compute barycentric coordinates
//			float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
//			float v = (dot00 * dot12 - dot01 * dot02) * invDenom;

			float u, v;
			getUV(p, u, v);

			const Point3 res = getPoint(u,v);
			Assert::isNear<float>(res.x, p.x, 1.0f, "TODO: high difference while mapping from 2D to 3D");
			Assert::isNear<float>(res.y, p.y, 1.0f, "TODO: high difference while mapping from 2D to 3D");

			//return res;
			return Point3(p.x, p.y, res.z);	// only use the new z, keep input as-is

		}

		/** nearest point on the triangle */
		Point3 toPoint3Near(const Point2 p) const {

			float u, v;
			getUV(p, u, v);

			if (u < 0) {u = 0;}
			if (u > 1) {u = 1;}
			if (v < 0) {v = 0;}
			if (v > 1) {v = 1;}

			return getPoint(u,v);

		}



		/** get the triangle's size */
		float getArea() const {
			return area;
		}

		/** get the triangle's center-point */
		Point3 getCenter() const {
			return center;
		}

		/** cast to string */
		operator std::string() const {return asString();}

		/** get as string */
		std::string asString() const {
			return "(" + std::to_string(center.x) + "," + std::to_string(center.y) + "," + std::to_string(center.z) + ")";
		}



	private:

		/** perform some pre-calculations to speed things up */
		void precompute() {

#pragma message "TODO, z buffer"
			minZ = std::min(p1.z, std::min(p2.z, p3.z)) - 0.15;	// TODO the builder does not align on the same height as we did
			maxZ = std::max(p1.z, std::max(p2.z, p3.z)) + 0.15;

//			// Compute vectors
//			v0 = p3.xy() - p1.xy();
//			v1 = p2.xy() - p1.xy();

//			// Compute dot products
//			dot00 = dot(v0, v0);
//			dot01 = dot(v0, v1);
//			dot11 = dot(v1, v1);

//			// Compute barycentric coordinates
//			invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)dot01);




			const float a = (p2-p1).length();
			const float b = (p3-p1).length();
			const float c = (p2-p3).length();
			const float s = 0.5f * (a+b+c);
			area = std::sqrt( s * (s-a) * (s-b) * (s-c) );

		}

	protected:

		void addNeighbor(NavMeshTriangle* o) {
			Assert::isBetween(_numNeighbors, 0, 3, "number of neighbors out of bounds");
			_neighbors[_numNeighbors] = o;
			++_numNeighbors;
		}


	};

}

#endif // NAVMESHTRIANGLE_H