#include "QuickHull.hpp"
#include "MathUtils.hpp"
#include <cmath>
#include <cassert>
#include <iostream>
#include <algorithm>
#include <limits>
#include "Structs/Mesh.hpp"

namespace quickhull {

	template<>
	float defaultEps() { 
		return 0.0001f;
	}
	
	template<>
	double defaultEps() {
		return 0.0000001; 
	}
	
	/*
	 * Implementation of the algorithm
	 */
	
	template<typename T>
	ConvexHull<T> QuickHull<T>::getConvexHull(const std::vector<Vector3<T>>& pointCloud, bool CCW, bool useOriginalIndices, T epsilon) {
		VertexDataSource<T> vertexDataSource(pointCloud);
		return getConvexHull(vertexDataSource,CCW,useOriginalIndices,epsilon);
	}
	
	template<typename T>
	ConvexHull<T> QuickHull<T>::getConvexHull(const Vector3<T>* vertexData, size_t vertexCount, bool CCW, bool useOriginalIndices, T epsilon) {
		VertexDataSource<T> vertexDataSource(vertexData,vertexCount);
		return getConvexHull(vertexDataSource,CCW,useOriginalIndices,epsilon);
	}
	
	template<typename T>
	ConvexHull<T> QuickHull<T>::getConvexHull(const T* vertexData, size_t vertexCount, bool CCW, bool useOriginalIndices, T epsilon) {
		VertexDataSource<T> vertexDataSource((const vec3*)vertexData,vertexCount);
		return getConvexHull(vertexDataSource,CCW,useOriginalIndices,epsilon);
	}
	
	template<typename FloatType>
	HalfEdgeMesh<FloatType, size_t> QuickHull<FloatType>::getConvexHullAsMesh(const FloatType* vertexData, size_t vertexCount, bool CCW, FloatType epsilon) {
		VertexDataSource<FloatType> vertexDataSource((const vec3*)vertexData,vertexCount);
		buildMesh(vertexDataSource, epsilon);
		return HalfEdgeMesh<FloatType, size_t>(m_mesh, m_vertexData);
	}
	
	template<typename T>
	void QuickHull<T>::buildMesh(const VertexDataSource<T>& pointCloud, T epsilon)
	{
		if (pointCloud.size()==0) {
			m_mesh = MeshBuilder<T>();
			return;
		}
		m_vertexData = pointCloud;
		
		// Very first: find extreme values and use them to compute the scale of the point cloud.
		m_extremeValues = getExtremeValues();
		m_scale = getScale(m_extremeValues);
		
		// Epsilon we use depends on the scale
		m_epsilon = epsilon*m_scale;
		m_epsilonSquared = m_epsilon*m_epsilon;
		
		// Reset diagnostics
		m_diagnostics = DiagnosticsData();

		// The planar case happens when all the points appear to lie on a two dimensional
		// subspace of R^3.
		m_planar = false;
		
		createConvexHalfEdgeMesh();
		if (m_planar) {
			const size_t extraPointIndex = m_planarPointCloudTemp.size() - 1;
			for (auto& he : m_mesh.m_halfEdges) {
				if (he.m_endVertex == extraPointIndex) {
					he.m_endVertex = 0;
				}
			}
			m_vertexData = pointCloud;
			m_planarPointCloudTemp.clear();
		}
	}

	template<typename T>
	ConvexHull<T> QuickHull<T>::getConvexHull(const VertexDataSource<T>& pointCloud, bool CCW, bool useOriginalIndices, T epsilon) {
		buildMesh(pointCloud, epsilon);
		return ConvexHull<T>(m_mesh,m_vertexData, CCW, useOriginalIndices);
	}

	template<typename T>
	void QuickHull<T>::createConvexHalfEdgeMesh() {
		m_visibleFaces.clear();
		m_horizonEdges.clear();
		m_possiblyVisibleFaces.clear();
		
		// Compute base tetrahedron
		setupInitialTetrahedron();
		assert(m_mesh.m_faces.size() == 4);

		// Init face stack with those faces that have points assigned to them
		m_faceList.clear();
		for (size_t i=0;i < 4;i++) {
			auto& f = m_mesh.m_faces[i];
			if (f.m_pointsOnPositiveSide && f.m_pointsOnPositiveSide->size()>0) {
				m_faceList.push_back(i);
				f.m_inFaceStack = 1;
			}
		}

		// Process faces until the face list is empty.
		size_t iter = 0;
		while (!m_faceList.empty()) {
			iter++;
			if (iter == std::numeric_limits<size_t>::max()) {
				// Visible face traversal marks visited faces with iteration counter
				// (to mark that the face has been visited on this iteration) and the max
				// value represents unvisited faces. At this point we have to reset iteration
				// counter. This shouldn't be an issue on 64 bit machines.
				iter = 0;
			}
			
			const size_t topFaceIndex = m_faceList.front();
			m_faceList.pop_front();
			
			auto& tf = m_mesh.m_faces[topFaceIndex];
			tf.m_inFaceStack = 0;

			assert(!tf.m_pointsOnPositiveSide || tf.m_pointsOnPositiveSide->size()>0);
			if (!tf.m_pointsOnPositiveSide || tf.isDisabled()) {
				continue;
			}
			
			// Pick the most distant point to this triangle plane as the point to which we extrude
			const vec3& activePoint = m_vertexData[tf.m_mostDistantPoint];
			const size_t activePointIndex = tf.m_mostDistantPoint;

			// Find out the faces that have our active point on their positive side
			// (these are the "visible faces"). The face on top of the stack of course is
			// one of them. At the same time, we create a list of horizon edges.
			m_horizonEdges.clear();
			m_possiblyVisibleFaces.clear();
			m_visibleFaces.clear();
			m_possiblyVisibleFaces.emplace_back(topFaceIndex,std::numeric_limits<size_t>::max());
			while (m_possiblyVisibleFaces.size()) {
				const auto faceData = m_possiblyVisibleFaces.back();
				m_possiblyVisibleFaces.pop_back();
				auto& pvf = m_mesh.m_faces[faceData.m_faceIndex];
				assert(!pvf.isDisabled());
				
				if (pvf.m_visibilityCheckedOnIteration == iter) {
					if (pvf.m_isVisibleFaceOnCurrentIteration) {
						continue;
					}
				}
				else {
					const Plane<T>& P = pvf.m_P;
					pvf.m_visibilityCheckedOnIteration = iter;
					const T d = P.m_N.dotProduct(activePoint)+P.m_D;
					if (d>0) {
						pvf.m_isVisibleFaceOnCurrentIteration = 1;
						pvf.m_horizonEdgesOnCurrentIteration = 0;
						m_visibleFaces.push_back(faceData.m_faceIndex);
						for (auto heIndex : m_mesh.getHalfEdgeIndicesOfFace(pvf)) {
							if (m_mesh.m_halfEdges[heIndex].m_opp != faceData.m_enteredFromHalfEdge) {
								m_possiblyVisibleFaces.emplace_back( m_mesh.m_halfEdges[m_mesh.m_halfEdges[heIndex].m_opp].m_face,heIndex );
							}
						}
						continue;
					}
					assert(faceData.m_faceIndex != topFaceIndex);
				}

				// The face is not visible. Therefore, the halfedge we entered from
				// is part of the horizon edge.
				pvf.m_isVisibleFaceOnCurrentIteration = 0;
				m_horizonEdges.push_back(faceData.m_enteredFromHalfEdge);
				
				// Store which half edge is the horizon edge. The other half edges of the face
				// will not be part of the final mesh so their data slots can by recycled.
				const auto halfEdges = m_mesh.getHalfEdgeIndicesOfFace(m_mesh.m_faces[m_mesh.m_halfEdges[faceData.m_enteredFromHalfEdge].m_face]);
				const std::int8_t ind = (halfEdges[0]==faceData.m_enteredFromHalfEdge) ? 0 : (halfEdges[1]==faceData.m_enteredFromHalfEdge ? 1 : 2);
				m_mesh.m_faces[m_mesh.m_halfEdges[faceData.m_enteredFromHalfEdge].m_face].m_horizonEdgesOnCurrentIteration |= (1<<ind);
			}
			const size_t horizonEdgeCount = m_horizonEdges.size();

			// Order horizon edges so that they form a loop. This may fail due to numerical
			// inaccuracy in which case we give up trying to solve horizon edge for this point
			// and accept a minor degeneration in the convex hull.
			if (!reorderHorizonEdges(m_horizonEdges)) {
				m_diagnostics.m_failedHorizonEdges++;
				std::cerr << "Failed to solve horizon edge." << std::endl;
				auto it = std::find(tf.m_pointsOnPositiveSide->begin(),
									tf.m_pointsOnPositiveSide->end(),
									activePointIndex);
				tf.m_pointsOnPositiveSide->erase(it);
				if (tf.m_pointsOnPositiveSide->size()==0) {
					reclaimToIndexVectorPool(tf.m_pointsOnPositiveSide);
				}
				continue;
			}

			// Except for the horizon edges, all half edges of the visible faces can be marked as disabled. Their data slots will be reused.
			// The faces will be disabled as well, but we need to remember the points that were on the positive side of them - therefore
			// we save pointers to them.
			m_newFaceIndices.clear();
			m_newHalfEdgeIndices.clear();
			m_disabledFacePointVectors.clear();
			size_t disableCounter = 0;
			for (auto faceIndex : m_visibleFaces) {
				auto& disabledFace = m_mesh.m_faces[faceIndex];
				auto halfEdges = m_mesh.getHalfEdgeIndicesOfFace(disabledFace);
				for (size_t j=0;j<3;j++) {
					if ((disabledFace.m_horizonEdgesOnCurrentIteration & (1<<j)) == 0) {
						if (disableCounter < horizonEdgeCount*2) {
							// Use on this iteration
							m_newHalfEdgeIndices.push_back(halfEdges[j]);
							disableCounter++;
						}
						else {
							// Mark for reusal on later iteration step
							m_mesh.disableHalfEdge(halfEdges[j]);
						}
					}
				}
				// Disable the face, but retain pointer to the points that were on the positive side of it. We need to assign those points
				// to the new faces we create shortly.
				auto t = m_mesh.disableFace(faceIndex);
				if (t) {
					assert(t->size()); // Because we should not assign point vectors to faces unless needed...
					m_disabledFacePointVectors.push_back(std::move(t));
				}
			}
			if (disableCounter < horizonEdgeCount*2) {
				const size_t newHalfEdgesNeeded = horizonEdgeCount*2-disableCounter;
				for (size_t i=0;i<newHalfEdgesNeeded;i++) {
					m_newHalfEdgeIndices.push_back(m_mesh.addHalfEdge());
				}
			}
			
			// Create new faces using the edgeloop
			for (size_t i = 0; i < horizonEdgeCount; i++) {
				const size_t AB = m_horizonEdges[i];

				auto horizonEdgeVertexIndices = m_mesh.getVertexIndicesOfHalfEdge(m_mesh.m_halfEdges[AB]);
				size_t A,B,C;
				A = horizonEdgeVertexIndices[0];
				B = horizonEdgeVertexIndices[1];
				C = activePointIndex;

				const size_t newFaceIndex = m_mesh.addFace();
				m_newFaceIndices.push_back(newFaceIndex);

				const size_t CA = m_newHalfEdgeIndices[2*i+0];
				const size_t BC = m_newHalfEdgeIndices[2*i+1];

				m_mesh.m_halfEdges[AB].m_next = BC;
				m_mesh.m_halfEdges[BC].m_next = CA;
				m_mesh.m_halfEdges[CA].m_next = AB;

				m_mesh.m_halfEdges[BC].m_face = newFaceIndex;
				m_mesh.m_halfEdges[CA].m_face = newFaceIndex;
				m_mesh.m_halfEdges[AB].m_face = newFaceIndex;

				m_mesh.m_halfEdges[CA].m_endVertex = A;
				m_mesh.m_halfEdges[BC].m_endVertex = C;

				auto& newFace = m_mesh.m_faces[newFaceIndex];

				const Vector3<T> planeNormal = mathutils::getTriangleNormal(m_vertexData[A],m_vertexData[B],activePoint);
				newFace.m_P = Plane<T>(planeNormal,activePoint);
				newFace.m_he = AB;

				m_mesh.m_halfEdges[CA].m_opp = m_newHalfEdgeIndices[i>0 ? i*2-1 : 2*horizonEdgeCount-1];
				m_mesh.m_halfEdges[BC].m_opp = m_newHalfEdgeIndices[((i+1)*2) % (horizonEdgeCount*2)];
			}

			// Assign points that were on the positive side of the disabled faces to the new faces.
			for (auto& disabledPoints : m_disabledFacePointVectors) {
				assert(disabledPoints);
				for (const auto& point : *(disabledPoints)) {
					if (point == activePointIndex) {
						continue;
					}
					for (size_t j=0;j<horizonEdgeCount;j++) {
						if (addPointToFace(m_mesh.m_faces[m_newFaceIndices[j]], point)) {
							break;
						}
					}
				}
				// The points are no longer needed: we can move them to the vector pool for reuse.
				reclaimToIndexVectorPool(disabledPoints);
			}

			// Increase face stack size if needed
			for (const auto newFaceIndex : m_newFaceIndices) {
				auto& newFace = m_mesh.m_faces[newFaceIndex];
				if (newFace.m_pointsOnPositiveSide) {
					assert(newFace.m_pointsOnPositiveSide->size()>0);
					if (!newFace.m_inFaceStack) {
						m_faceList.push_back(newFaceIndex);
						newFace.m_inFaceStack = 1;
					}
				}
			}
		}
		
		// Cleanup
		m_indexVectorPool.clear();
	}
	
	/*
	 * Private helper functions
	 */

	template <typename T>
	std::array<size_t, 6> QuickHull<T>::getExtremeValues() {
		std::array<size_t,6> outIndices{0, 0, 0, 0, 0, 0};
		T extremeVals[6] = { m_vertexData[0].x, m_vertexData[0].x, m_vertexData[0].y,
							 m_vertexData[0].y, m_vertexData[0].z, m_vertexData[0].z };
		const size_t vCount = m_vertexData.size();
		for (size_t i = 1; i < vCount; i++) {
			const Vector3<T>& pos = m_vertexData[i];
			if (pos.x>extremeVals[0]) {
				extremeVals[0]=pos.x;
				outIndices[0]=i;
			}
			else if (pos.x<extremeVals[1]) {
				extremeVals[1]=pos.x;
				outIndices[1]=i;
			}
			if (pos.y>extremeVals[2]) {
				extremeVals[2]=pos.y;
				outIndices[2]=i;
			}
			else if (pos.y<extremeVals[3]) {
				extremeVals[3]=pos.y;
				outIndices[3]=i;
			}
			if (pos.z>extremeVals[4]) {
				extremeVals[4]=pos.z;
				outIndices[4]=i;
			}
			else if (pos.z<extremeVals[5]) {
				extremeVals[5]=pos.z;
				outIndices[5]=i;
			}
		}
		return outIndices;
	}

	template<typename T>
	bool QuickHull<T>::reorderHorizonEdges(std::vector<size_t>& horizonEdges) {
		const size_t horizonEdgeCount = horizonEdges.size();
		for (size_t i=0;i<horizonEdgeCount-1;i++) {
			const size_t endVertex = m_mesh.m_halfEdges[ horizonEdges[i] ].m_endVertex;
			bool foundNext = false;
			for (size_t j=i+1;j<horizonEdgeCount;j++) {
				const size_t beginVertex = m_mesh.m_halfEdges[ m_mesh.m_halfEdges[horizonEdges[j]].m_opp ].m_endVertex;
				if (beginVertex == endVertex) {
					std::swap(horizonEdges[i+1],horizonEdges[j]);
					foundNext = true;
					break;
				}
			}
			if (!foundNext) {
				return false;
			}
		}
		assert(m_mesh.m_halfEdges[ horizonEdges[horizonEdges.size()-1] ].m_endVertex == m_mesh.m_halfEdges[ m_mesh.m_halfEdges[horizonEdges[0]].m_opp ].m_endVertex);
		return true;
	}
	
	template <typename T>
	T QuickHull<T>::getScale(const std::array<size_t,6>& extremeValues) {
		T s = 0;
		for (size_t i=0;i<6;i++) {
			const T* v = (const T*)(&m_vertexData[extremeValues[i]]);
			v += i/2;
			auto a = std::abs(*v);
			if (a>s) {
				s = a;
			}
		}
		return s;
	}

	template<typename T>
	void QuickHull<T>::setupInitialTetrahedron() {
		const size_t vertexCount = m_vertexData.size();
		
		// If we have at most 4 points, just return a degenerate tetrahedron:
		if (vertexCount <= 4) {
			size_t v[4] = {0,std::min((size_t)1,vertexCount-1),std::min((size_t)2,vertexCount-1),std::min((size_t)3,vertexCount-1)};
			const Vector3<T> N = mathutils::getTriangleNormal(m_vertexData[v[0]],m_vertexData[v[1]],m_vertexData[v[2]]);
			const Plane<T> trianglePlane(N,m_vertexData[v[0]]);
			if (trianglePlane.isPointOnPositiveSide(m_vertexData[v[3]])) {
				std::swap(v[0],v[1]);
			}
			return m_mesh.setup(v[0],v[1],v[2],v[3]);
		}
		
		// Find two most distant extreme points.
		T maxD = m_epsilonSquared;
		std::pair<size_t,size_t> selectedPoints;
		for (size_t i=0;i<6;i++) {
			for (size_t j=i+1;j<6;j++) {
				const T d = m_vertexData[ m_extremeValues[i] ].getSquaredDistanceTo( m_vertexData[ m_extremeValues[j] ] );
				if (d > maxD) {
					maxD=d;
					selectedPoints={m_extremeValues[i],m_extremeValues[j]};
				}
			}
		}
		if (maxD == m_epsilonSquared) {
			// A degenerate case: the point cloud seems to consists of a single point
			return m_mesh.setup(0,std::min((size_t)1,vertexCount-1),std::min((size_t)2,vertexCount-1),std::min((size_t)3,vertexCount-1));
		}
		assert(selectedPoints.first != selectedPoints.second);
		
		// Find the most distant point to the line between the two chosen extreme points.
		const Ray<T> r(m_vertexData[selectedPoints.first], (m_vertexData[selectedPoints.second] - m_vertexData[selectedPoints.first]));
		maxD = m_epsilonSquared;
		size_t maxI=std::numeric_limits<size_t>::max();
		const size_t vCount = m_vertexData.size();
		for (size_t i=0;i<vCount;i++) {
			const T distToRay = mathutils::getSquaredDistanceBetweenPointAndRay(m_vertexData[i],r);
			if (distToRay > maxD) {
				maxD=distToRay;
				maxI=i;
			}
		}
		if (maxD == m_epsilonSquared) {
			// It appears that the point cloud belongs to a 1 dimensional subspace of R^3: convex hull has no volume => return a thin triangle
			// Pick any point other than selectedPoints.first and selectedPoints.second as the third point of the triangle
			auto it = std::find_if(m_vertexData.begin(),m_vertexData.end(),[&](const vec3& ve) {
				return ve != m_vertexData[selectedPoints.first] && ve != m_vertexData[selectedPoints.second];
			});
			const size_t thirdPoint = (it == m_vertexData.end()) ? selectedPoints.first : std::distance(m_vertexData.begin(),it);
			it = std::find_if(m_vertexData.begin(),m_vertexData.end(),[&](const vec3& ve) {
				return ve != m_vertexData[selectedPoints.first] && ve != m_vertexData[selectedPoints.second] && ve != m_vertexData[thirdPoint];
			});
			const size_t fourthPoint = (it == m_vertexData.end()) ? selectedPoints.first : std::distance(m_vertexData.begin(),it);
			return m_mesh.setup(selectedPoints.first,selectedPoints.second,thirdPoint,fourthPoint);
		}

		// These three points form the base triangle for our tetrahedron.
		assert(selectedPoints.first != maxI && selectedPoints.second != maxI);
		std::array<size_t,3> baseTriangle{selectedPoints.first, selectedPoints.second, maxI};
		const Vector3<T> baseTriangleVertices[]={ m_vertexData[baseTriangle[0]], m_vertexData[baseTriangle[1]],  m_vertexData[baseTriangle[2]] };
		
		// Next step is to find the 4th vertex of the tetrahedron. We naturally choose the point farthest away from the triangle plane.
		maxD=m_epsilon;
		maxI=0;
		const Vector3<T> N = mathutils::getTriangleNormal(baseTriangleVertices[0],baseTriangleVertices[1],baseTriangleVertices[2]);
		Plane<T> trianglePlane(N,baseTriangleVertices[0]);
		for (size_t i=0;i<vCount;i++) {
			const T d = std::abs(mathutils::getSignedDistanceToPlane(m_vertexData[i],trianglePlane));
			if (d>maxD) {
				maxD=d;
				maxI=i;
			}
		}
		if (maxD == m_epsilon) {
			// All the points seem to lie on a 2D subspace of R^3. How to handle this? Well, let's add one extra point to the point cloud so that the convex hull will have volume.
			m_planar = true;
			const vec3 N1 = mathutils::getTriangleNormal(baseTriangleVertices[1],baseTriangleVertices[2],baseTriangleVertices[0]);
			m_planarPointCloudTemp.clear();
			m_planarPointCloudTemp.insert(m_planarPointCloudTemp.begin(),m_vertexData.begin(),m_vertexData.end());
			const vec3 extraPoint = N1 + m_vertexData[0];
			m_planarPointCloudTemp.push_back(extraPoint);
			maxI = m_planarPointCloudTemp.size()-1;
			m_vertexData = VertexDataSource<T>(m_planarPointCloudTemp);
		}

		// Enforce CCW orientation (if user prefers clockwise orientation, swap two vertices in each triangle when final mesh is created)
		const Plane<T> triPlane(N,baseTriangleVertices[0]);
		if (triPlane.isPointOnPositiveSide(m_vertexData[maxI])) {
			std::swap(baseTriangle[0],baseTriangle[1]);
		}

		// Create a tetrahedron half edge mesh and compute planes defined by each triangle
		m_mesh.setup(baseTriangle[0],baseTriangle[1],baseTriangle[2],maxI);
		for (auto& f : m_mesh.m_faces) {
			auto v = m_mesh.getVertexIndicesOfFace(f);
			const Vector3<T>& va = m_vertexData[v[0]];
			const Vector3<T>& vb = m_vertexData[v[1]];
			const Vector3<T>& vc = m_vertexData[v[2]];
			const Vector3<T> N1 = mathutils::getTriangleNormal(va, vb, vc);
			const Plane<T> plane(N1,va);
			f.m_P = plane;
		}

		// Finally we assign a face for each vertex outside the tetrahedron (vertices inside the tetrahedron have no role anymore)
		for (size_t i=0;i<vCount;i++) {
			for (auto& face : m_mesh.m_faces) {
				if (addPointToFace(face, i)) {
					break;
				}
			}
		}
	}
	
	/*
	 * Explicit template specifications for float and double
	 */

	template class QuickHull<float>;
	template class QuickHull<double>;
}