#pragma once
#include "../../stdafx.h"

#include "glLibMathLA.h"
#include "glLibMathVec.h"
#include "glLibMathMisc.h"

#pragma region Defines
#define GLLIBINTERNAL_CLASSIFY_TRI_BY_FACE(v1,v2,v3,faceplane,out_of_face) {\
	_distances[0] = DISTANCE_PLANE_POINT(faceplane,v1);\
	_distances[1] = _distances[0] * DISTANCE_PLANE_POINT(faceplane,v2);\
	_distances[2] = _distances[0] * DISTANCE_PLANE_POINT(faceplane,v3);\
	if(_distances[1]>0.0f && _distances[2]>0.0f)\
	{\
		out_of_face = 1;\
	}\
	else\
	{\
		out_of_face = 0;\
	}\
}\

#define GLLIBINTERNAL_CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri_points,tri_edge_planes, clipped_points, clipped_point_count) {\
	clipped_point_count = 0;\
	_temp_clip_count = 0;\
	PLANE_CLIP_POLYGON(tri_edge_planes[0],tri_points,3,_temp_clip,_temp_clip_count,MAX_TRI_CLIPPING);\
	if (_temp_clip_count>0)\
	{\
		_temp_clip_count2 = 0;\
		PLANE_CLIP_POLYGON(tri_edge_planes[1],_temp_clip,_temp_clip_count,_temp_clip2,_temp_clip_count2,MAX_TRI_CLIPPING);\
		if (_temp_clip_count2>0)\
		{\
			PLANE_CLIP_POLYGON(tri_edge_planes[2],_temp_clip2,_temp_clip_count2,clipped_points,clipped_point_count,MAX_TRI_CLIPPING);\
		}\
	}\
}\

#define GLLIBINTERNAL_MOST_DEEP_POINTS(plane,points,point_count,deep_points,deep_points_count,maxdeep) {\
	maxdeep=-1000.0f;\
	GUINT32 _k;\
	GREAL _dist;\
	deep_points_count = 0;\
	for(_k=0;_k<point_count;_k++) {\
		_dist = -DISTANCE_PLANE_POINT(plane,points[_k]);\
		if(_dist>maxdeep) {\
			maxdeep = _dist;\
			_max_candidates[0] = _k;\
			deep_points_count=1;\
		}\
		else if((_dist+G_EPSILON)>=maxdeep) {\
			_max_candidates[deep_points_count] = _k;\
			deep_points_count++;\
		}\
	}\
	if(maxdeep<0.0f) {\
		deep_points_count = 0;\
	}\
	else {\
		for(_k=0;_k<deep_points_count;_k++) {\
			VEC_COPY(deep_points[_k],points[_max_candidates[_k]]);\
		}\
	}\
}\

#pragma endregion

namespace glLibMathIntersect {
	template <typename type_float> bool ray_triangle(type_float ray_pos[3], type_float ray_dir[3], type_float triangle[3][3], type_float intersection[3]) {
		//adapted from http://geometryalgorithms.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
		type_float u[3], v[3], temp[3], n[3], v1[3], I[3], w[3];
		glLibMathVec::subtract(triangle[1],triangle[0],u,3);
		glLibMathVec::subtract(triangle[2],triangle[0],v,3);
		glLibMathVec::cross(u,v,temp);
		glLibMathVec::normalize(temp,n,3);

		glLibMathVec::subtract(triangle[0],ray_pos,v1,3);

		glLibMathVec::negate(v1,temp,3);
		type_float a = -glLibMathVec::dot(n,temp,3);
		type_float b = glLibMathVec::dot(n,ray_dir,3);
		if (fabs(b)<=0) { //ray is parallel to triangle plane
			return false; //=> no intersect
		}

		//get intersect point of ray with triangle plane
		type_float r = a / b;
		if (r<0) { //ray goes away from triangle
			return false; //=> no intersect
		}

		//for a segment, also test if (r > 1.0) => no intersect
		glLibMathVec::scale(r,ray_dir,I,3); //intersect point of ray and plane

		//is I inside the triangle?
		type_float uu = glLibMathVec::dot(u,u,3);
		type_float uv = glLibMathVec::dot(u,v,3);
		type_float vv = glLibMathVec::dot(v,v,3);
		glLibMathVec::subtract(I,v1,w,3);
		type_float wu = glLibMathVec::dot(w,u,3);
		type_float wv = glLibMathVec::dot(w,v,3);
		type_float D = uv * uv - uu * vv;

		//get and test parametric coords
		type_float s = (uv * wv - vv * wu) / D;
		if ( (s<0) || (s>1) ) { //I is outside the triangle
			return false; //=> no intersect
		}
		type_float t = (uv * wu - uu * wv) / D;
		if ( (t<0) || (s+t>1) ) { //I is outside the triangle
			return false; //=> no intersect
		}

		glLibMathVec::add(I,ray_pos,intersection,3); //I is in the triangle
		return true;
	}

	template <typename type_float> char line_line(
		type_float p1[3], type_float p2[3],
		type_float p3[3], type_float p4[3],
		type_float pa[3], type_float pb[3], type_float eps)
	{
		//http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/

		type_float len;

		type_float p21[3], p21n[3];
		glLibMathVec::subtract(p2,p1,p21,3);
		len = length(p21);
		if (len<eps) return 0;
		glLibMathVec::scale(1/len,p21,p21n,3);

		type_float p43[3], p43n[3];
		glLibMathVec::subtract(p4,p3,p43,3);
		len = length(p43);
		if (len<eps) return 0;
		glLibMathVec::scale(1/len,p43,p43n,3);

		type_float p13[3], p24[3];
		glLibMathVec::subtract(p1,p3,p13,3);
		glLibMathVec::subtract(p2,p4,p24,3);

		type_float temp[3];

		if ( fabs(fabs(glLibMathVec::dot(p43n,p21n))-1.0) < eps ) {
			//lines are parallel
			glLibMathVec::normalize(p13,temp,3);
			if ( fabs(fabs(glLibMathVec::dot(temp,p21n))-1.0) < eps ) {
				//lines overlap
				return 2;
			}
			return 0;
		}

		type_float d1343 = glLibMathVec::dot(p13,p43);
		type_float d4321 = glLibMathVec::dot(p43,p21);
		type_float d1321 = glLibMathVec::dot(p13,p21);
		type_float d4343 = glLibMathVec::dot(p43,p43);
		type_float d2121 = glLibMathVec::dot(p21,p21);

		type_float denom = d2121 * d4343 - d4321 * d4321;
		if (denom==0.0) return 0;
		type_float numer = d1343 * d4321 - d1321 * d4343;

		type_float mua = numer / denom;
		type_float mub = (d1343 + d4321*mua) / d4343;

		glLibMathVec::scale(mua,p21,temp,3); glLibMathVec::add(p1,temp,pa,3);
		glLibMathVec::scale(mub,p43,temp,3); glLibMathVec::add(p3,temp,pb,3);
		return 1;
	}

	#pragma region OLD
	/*template <typename type_float> bool point_triangle(type_float point[3],U0,U1,U2) {
		float a,b,c,d0,d1,d2;
		//Is T1 completly inside T2?
		//Check if V0 is inside tri(U0,U1,U2)

		a=U1[i1]-U0[i1];
		b=-(U1[i0]-U0[i0]);
		c=-a*U0[i0]-b*U0[i1];
		d0 = a*point[i0]+b*point[i1]+c
	
		a=U2[i1]-U1[i1];
		b=-(U2[i0]-U1[i0]);
		c=-a*U1[i0]-b*U1[i1];
		d1 = a*point[i0]+b*point[i1]+c;
	
		a=U0[i1]-U2[i1];
		b=-(U0[i0]-U2[i0]);
		c=-a*U2[i0]-b*U2[i1];
		d2 = a*point[i0]+b*point[i1]+c;

		if ((d0*d1>0.0)&&(d0*d2>0.0)) return true;
		return false;
	}

	//TODO
	template <typename type_float> bool triangle_triangle(
		type_float triangle_v[3][3], type_float triangle_u[3][3],
		type_float intersection1[3], type_float intersection2[3], type_float intersection3[3])
	{
		type_float E1[3],E2[3];
		type_float N1[3],N2[3],d1,d2;
		type_float du0,du1,du2,dv0,dv1,dv2;
		type_float D[3];
		type_float isect1[2], isect2[2];
		type_float isectpointA1[3],isectpointA2[3];
		type_float isectpointB1[3],isectpointB2[3];
		type_float du0du1,du0du2,dv0dv1,dv0dv2;
		int index;
		type_float vp0,vp1,vp2;
		type_float up0,up1,up2;
		type_float b,c,max;
		type_float tmp,diff[3];
		int smallest1,smallest2;

		//Compute plane equation of triangle_v
		glLibMathVec::subtract(triangle_v[1],triangle_v[0],E1,3);
		glLibMathVec::subtract(triangle_v[2],triangle_v[0],E2,3);
		glLibMathVec::cross(E1,E2,N1);
		d1 = -glLibMathVec::dot(N1,triangle_v[0],3);
		//Plane equation 1: N1.X+d1=0

		//Put U0,U1,U2 into plane equation 1 to compute signed distances to the plane
		du0 = glLibMathVec::dot(N1,triangle_u[0],3) + d1;
		du1 = glLibMathVec::dot(N1,triangle_u[1],3) + d1;
		du2 = glLibMathVec::dot(N1,triangle_u[2],3) + d1;

		//Coplanarity robustness check
#if USE_EPSILON_TEST==TRUE
		if (fabs(du0)<EPSILON) du0=0.0;
		if (fabs(du1)<EPSILON) du1=0.0;
		if (fabs(du2)<EPSILON) du2=0.0;
#endif
		du0du1=du0*du1;
		du0du2=du0*du2;

		if (du0du1>0.0f && du0du2>0.0f) {//Same sign on all of them + not equal 0 ?
			return 0;                    //No intersection occurs
		}

		//Compute plane of triangle_u
		glLibMathVec::subtract(triangle_u[1],triangle_u[0],E1,3);
		glLibMathVec::subtract(triangle_u[2],triangle_u[0],E2,3);
		glLibMathVec::cross(E1,E2,N2);
		d2 = -glLibMathVec::dot(N2,triangle_u[0],3);
		//Plane equation 2: N2.X+d2=0

		//Put triangle_v into plane equation 2
		dv0 = glLibMathVec::dot(N2,triangle_v[0],3) + d2;
		dv1 = glLibMathVec::dot(N2,triangle_v[1],3) + d2;
		dv2 = glLibMathVec::dot(N2,triangle_v[2],3) + d2;

#if USE_EPSILON_TEST==TRUE
		if (fabs(dv0)<EPSILON) dv0=0.0;
		if (fabs(dv1)<EPSILON) dv1=0.0;
		if (fabs(dv2)<EPSILON) dv2=0.0;
#endif

		dv0dv1 = dv0 * dv1;
		dv0dv2 = dv0 * dv2;

		if (dv0dv1>0.0f && dv0dv2>0.0f) { //Same sign on all of them + not equal 0?
			return false;                 //No intersection occurs
		}

		//Compute direction of intersection line
		glLibMathVec::cross(N1,N2,D);

		//Compute and index to the largest component of D
		max = fabs(D[0]);
		index = 0;
		b = fabs(D[1]);
		c = fabs(D[2]);
		if (b>max) { max = b; index = 1; }
		if (c>max) { max = c; index = 2; }

		//This is the simplified projection onto L
		vp0 = triangle_v[0][index];
		vp1 = triangle_v[1][index];
		vp2 = triangle_v[2][index];

		up0 = triangle_u[0][index];
		up1 = triangle_u[1][index];
		up2 = triangle_u[2][index];

		//Compute interval for triangle 1
		/**coplanar = compute_intervals_isectline(
			V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2,
			dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2);
		if (*coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);


		//Compute interval for triangle 2
		compute_intervals_isectline(
			U0,U1,U2,up0,up1,up2,du0,du1,du2,
			du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2);

		SORT2(isect1[0],isect1[1],smallest1);
		SORT2(isect2[0],isect2[1],smallest2);

		if (isect1[1]<isect2[0] || isect2[1]<isect1[0]) return false;

		//At this point, we know that the triangles intersect
		if (isect2[0]<isect1[0]) {
			if (smallest1==0) { glLibMathVec::copy(isectpointA1,intersection1,3); }
			else { glLibMathVec::copy(isectpointA2,intersection1,3); }

			if (isect2[1]<isect1[1]) {
				if (smallest2==0) { glLibMathVec::copy(isectpointB2,intersection2,3); }
				else { glLibMathVec::copy(isectpointB1,intersection2,3); }
			}
			else {
				if (smallest1==0) { glLibMathVec::copy(isectpointA2,intersection2,3); }
				else { glLibMathVec::copy(isectpointA1,intersection2,3); }
			}
		}
		else {
			if (smallest2==0) { glLibMathVec::copy(isectpointB1,intersection1,3); }
			else { glLibMathVec::copy(isectpointB2,intersection1,3); }

			if (isect2[1]>isect1[1]) {
				if (smallest1==0) { glLibMathVec::copy(isectpointA2,intersection2,3); }
				else { glLibMathVec::copy(isectpointA1,intersection2,3); }      
			}
			else {
				if (smallest2==0) { glLibMathVec::copy(isectpointB2,intersection2,3); }
				else { glLibMathVec::copy(isectpointB1,intersection2,3); } 
			}
		}
		return true;
	}*/

	//Returns the number of points of intersection (0, 1, 2, or 3) and sets the appropriate
	//intersection points
	/*template <typename type_float> char triangle_triangle(
		type_float triangle1[3][3], type_float triangle2[3][3],
		type_float intersection1[3], type_float intersection2[3], type_float intersection3[3])
	{
		type_float vec1[3], vec2[3], n2[3], d2, dv0, dv1, dv2;

		//Get "N" of containing plane for triangle 2
		glLibMathVec::subtract(triangle2[1],triangle2[0],vec1);
		glLibMathVec::subtract(triangle2[2],triangle2[0],vec2);
		glLibMathVec::cross(vec1,vec2,n2);
		d2 = -glLibMathVec::dot(n2,triangle2[0]);

		//Signed distances multiplied by n2*n2 (n2 isn't normalized)
		dv0 = glLibMathVec::dot(n2,triangle1[0]) + d2;
		dv1 = glLibMathVec::dot(n2,triangle1[1]) + d2;
		dv2 = glLibMathVec::dot(n2,triangle1[2]) + d2;

		if ((dv0>0)&&(dv1>0)&&(dv2>0)) return 0; //All points of triangle 1 on same side; no intersection
		if ((dv0<0)&&(dv1<0)&&(dv2<0)) return 0; //All points of triangle 1 on same side; no intersection
		if ((dv0==0)&&(dv1==0)&&(dv2==0)) {
			//Conatining planes are coplanar
		}

		type_float n1[3], d1, D[3];

		//Get "N" of containing plane for triangle 1
		glLibMathVec::subtract(triangle1[1],triangle1[0],vec1);
		glLibMathVec::subtract(triangle1[2],triangle1[0],vec2);
		glLibMathVec::cross(vec1,vec2,n1);
		//d1 = -glLibMathVec::dot(n1,triangle2[0]);

		//Parallel to planes' intersection
		glLibMathVec::cross(n1,n2,D);

		//Find O such that O + t*D is the line of intersection of the triangles' planes
		//This wouldn't be necessary if we weren't going to compute the intersections later
		type_float a1=n1[0], b1=n1[1], c1=n1[2];
		type_float a2=n2[0], b2=n2[1], c2=n2[2];
		if (D[0]!=0) {
			type_float denom = a1*b2 - a2*b1;
			O[0] = (b1*d2 - b2*d1)  /  denom;
			O[1] = (a2*d1 - a1*d2)  /  denom;
			O[2] = 0;
		}
		else if (D[1]!=0) {
			type_float denom = a1*c2 - a2*c1;
			O[0] = (c1*d2 - c2*d1)  /  denom;
			O[1] = 0;
			O[2] = (a2*d1 - a1*d2)  /  denom;
		}
		else {
			type_float denom = b1*c2 - b2*c1;
			O[0] = 0;
			O[1] = (c1*d2 - c2*d1)  /  denom;
			O[2] = (b2*d1 - b1*d2)  /  denom;
		}

		//Project points of triangle 1 onto the intersection-line of the triangles' planes
		type_float Tri1P[3][3];
		type_float Tri2P[3][3];
		glLibMathVec::subtract(triangle1[0],O,Tri1P[0],3); glLibMathVec::scale(D,Tri1P[0],3);
		glLibMathVec::subtract(triangle1[1],O,Tri1P[1],3); glLibMathVec::scale(D,Tri1P[1],3);
		glLibMathVec::subtract(triangle1[2],O,Tri1P[2],3); glLibMathVec::scale(D,Tri1P[2],3);

		//Compute intervals
		type_float tri1_t1, tri1_t2;
		type_float tri2_t1, tri2_t2;
		tri1_t1 = 
	}*/
	#pragma endregion

	/*char triangle_triangle(
		GIM_TRIANGLE_DATA* tri1,
		GIM_TRIANGLE_DATA* tri2,
		GIM_TRIANGLE_CONTACT_DATA* contact_data)
	{
		typedef type_float float;

		type_float _distances[3];
		char out_of_face = 0;

		GLLIBINTERNAL_CLASSIFY_TRI_BY_FACE(tri1->m_vertices[0],tri1->m_vertices[1],tri1->m_vertices[2],tri2->m_planes.m_planes[0],out_of_face);
		if (out_of_face==1) return 0;

		GLLIBINTERNAL_CLASSIFY_TRI_BY_FACE(tri2->m_vertices[0],tri2->m_vertices[1],tri2->m_vertices[2],tri1->m_planes.m_planes[0],out_of_face);
		if (out_of_face==1) return 0;

		//Cache variables for triangle intersection
		unsigned int _max_candidates[MAX_TRI_CLIPPING];
		vec3f _temp_clip[MAX_TRI_CLIPPING];
		unsigned int _temp_clip_count = 0;
		vec3f _temp_clip2[MAX_TRI_CLIPPING];
		unsigned int _temp_clip_count2 = 0;
		vec3f clipped_points2[MAX_TRI_CLIPPING];
		vec3f deep_points2[MAX_TRI_CLIPPING];
		vec3f clipped_points1[MAX_TRI_CLIPPING];
		vec3f deep_points1[MAX_TRI_CLIPPING];

		//State variabnles
		unsigned int mostdir = 0;
		unsigned int clipped2_count = 0;

		//Clip tri2 by tri1 edges
		GLLIBINTERNAL_CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri2->m_vertices,(&tri1->m_planes.m_planes[1]), clipped_points2, clipped2_count);

		if (clipped2_count==0) {
			return 0;
		}

		//find most deep interval face1
		GUINT32 deep2_count = 0;

		type_float maxdeep;

		GLLIBINTERNAL_MOST_DEEP_POINTS((tri1->m_planes.m_planes[0]), clipped_points2, clipped2_count, deep_points2, deep2_count, maxdeep);
		if (deep2_count==0) {
			//*perror = 0.0f;
			return 0;
		}

		//Normal pointing to triangle1
		VEC_SCALE(contact_data->m_separating_normal,-1.0f,(tri1->m_planes.m_planes[0]));

		//Clip tri1 by tri2 edges
		unsigned int clipped1_count = 0;

		GLLIBINTERNAL_CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri1->m_vertices,(&tri2->m_planes.m_planes[1]), clipped_points1, clipped1_count);

		if (clipped2_count == 0 ) {
			//*perror = 0.0f;
			return 0;
		}


		//find interval face2
		GUINT32 deep1_count=0;

		type_float dist;

		GLLIBINTERNAL_MOST_DEEP_POINTS((tri2->m_planes.m_planes[0]), clipped_points1, clipped1_count, deep_points1, deep1_count, dist);

		if (deep1_count==0) {
			//*perror = 0.0f;
			return 0;
		}

		if (dist<maxdeep) {
			maxdeep = dist;
			mostdir = 1;
			VEC_COPY(contact_data->m_separating_normal,(tri2->m_planes.m_planes[0]));
		}
		//set deep
		contact_data->m_penetration_depth = maxdeep;

		//check most dir for contacts
		if (mostdir==0) {
			contact_data->m_point_count = deep2_count;
			for (mostdir=0;mostdir<deep2_count;mostdir++) {
				VEC_COPY(contact_data->m_points[mostdir],deep_points2[mostdir]);
			}
		}
		else {
			contact_data->m_point_count = deep1_count;
			for (mostdir=0;mostdir<deep1_count;mostdir++) {
				VEC_COPY(contact_data->m_points[mostdir],deep_points1[mostdir]);
			}
		}
		return 1;
	}*/

	template <typename type_float> type_float ray_aabb(type_float aabb_min[3], type_float aabb_max[3], type_float origin[3], type_float direction[3], type_float eps=0.0001) {
		//Adapted from http://www.cs.utah.edu/~awilliam/box/

		float tmin, tmax, tymin, tymax, tzmin, tzmax;

		if (direction[0]==0) direction[0] = eps;
		if (direction[1]==0) direction[1] = eps;
		if (direction[2]==0) direction[2] = eps;

		tmin = (((direction[0]>0)?aabb_min[0]:aabb_max[0])-origin[0]) / direction[0];
		tmax = (((direction[0]<0)?aabb_min[0]:aabb_max[0])-origin[0]) / direction[0];
		tymin = (((direction[1]>0)?aabb_min[1]:aabb_max[1])-origin[1]) / direction[1];
		tymax = (((direction[1]<0)?aabb_min[1]:aabb_max[1])-origin[1]) / direction[1];
		if ( (tmin > tymax) || (tymin > tmax) ) return -1.0;
		if (tymin > tmin) tmin = tymin;
		if (tymax < tmax) tmax = tymax;
		tzmin = (((direction[2]>0)?aabb_min[2]:aabb_max[2])-origin[2]) / direction[2];
		tzmax = (((direction[2]<0)?aabb_min[2]:aabb_max[2])-origin[2]) / direction[2];
		if ( (tmin > tzmax) || (tzmin > tmax) ) return -1.0;
		//if (tzmin > tmin) tmin = tzmin;
		//if (tzmax < tmax) tmax = tzmax;
		//return ( (tmin < t1) && (tmax > t0) ) - 0.5;
		return 1.0;
	}
};