// header file:
#include <DirectXMath.h>
#include <Vector>
using namespace DirectX;

// the return structure, with these values, you should be able to calculate the impulse
// the depth shouldn't be used in your impulse calculation, it is a redundant value
// if the normalWorld == XMVectorZero(), no collision
struct CollisionInfo{
	bool isValid;                          // whether there is a collision point, true for yes
	GamePhysics::Vec3 collisionPointWorld; // the position of the collision point in world space
	GamePhysics::Vec3 normalWorld;         // the direction of the impulse to A, negative of the collision face of A
	float depth;                           // the distance of the collision point to the surface, not necessary.
};

// tool data structures/functions called by the collision detection method, you can ignore the details here
namespace collisionTools{
	struct Projection{
		float min, max;
	};


	inline std::vector<XMVECTOR> discritizeObject(const XMMATRIX& obj2World)
	{
		const XMVECTOR centerWorld = XMVector3Transform(XMVectorZero(), obj2World);
		XMVECTOR edges[3];
		std::vector<XMVECTOR> results;
		for (int precession = 0.1; precession <= 0.5; precession += 0.1)
		{
			for (size_t i = 0; i < 3; ++i)
				edges[i] = XMVector3TransformNormal(XMVectorSetByIndex(XMVectorZero(), precession, i), obj2World);
			results.push_back(centerWorld - edges[0] - edges[1] - edges[2]);
			results.push_back(centerWorld + edges[0] - edges[1] - edges[2]);
			results.push_back(centerWorld - edges[0] + edges[1] - edges[2]);
			results.push_back(centerWorld + edges[0] + edges[1] - edges[2]);
			results.push_back(centerWorld - edges[0] - edges[1] + edges[2]);
			results.push_back(centerWorld + edges[0] - edges[1] + edges[2]);
			results.push_back(centerWorld - edges[0] + edges[1] + edges[2]);
			results.push_back(centerWorld + edges[0] + edges[1] + edges[2]);
		}
	}

	inline XMVECTOR getVectorConnnectingCenters(const XMMATRIX& obj2World_A, const XMMATRIX& obj2World_B)
	{
		const XMVECTOR centerWorld_A = XMVector3Transform(XMVectorZero(), obj2World_A);
		const XMVECTOR centerWorld_B = XMVector3Transform(XMVectorZero(), obj2World_B);
		return centerWorld_B - centerWorld_A;

	}

	// Get Corners
	inline std::vector<XMVECTOR> getCorners(const XMMATRIX& obj2World)
	{
		const XMVECTOR centerWorld = XMVector3Transform(XMVectorZero(), obj2World);
		XMVECTOR edges[3];
		for (size_t i = 0; i < 3; ++i)
			edges[i] = XMVector3TransformNormal(XMVectorSetByIndex(XMVectorZero(), 0.5f, i), obj2World);
		std::vector<XMVECTOR> results;
		results.push_back(centerWorld - edges[0] - edges[1] - edges[2]);
		results.push_back(centerWorld + edges[0] - edges[1] - edges[2]);
		results.push_back(centerWorld - edges[0] + edges[1] - edges[2]);
		results.push_back(centerWorld + edges[0] + edges[1] - edges[2]); // this +,+,-
		results.push_back(centerWorld - edges[0] - edges[1] + edges[2]);
		results.push_back(centerWorld + edges[0] - edges[1] + edges[2]); //this +,-,+
		results.push_back(centerWorld - edges[0] + edges[1] + edges[2]); //this -,+,+
		results.push_back(centerWorld + edges[0] + edges[1] + edges[2]);//this +,+,+
		return results;
	}

	// Get Rigid Box Size
	inline XMVECTOR getBoxSize(const XMMATRIX& obj2World)
	{
		XMVECTOR size = XMVectorZero();
		XMVECTOR edges[3];
		for (size_t i = 0; i < 3; ++i){
			edges[i] = XMVector3TransformNormal(XMVectorSetByIndex(XMVectorZero(), 0.5f, i), obj2World);
			XMVECTOR length = XMVector3Length(edges[i]);

			size = XMVectorSetByIndex(size, 2.0f*XMVectorGetByIndex(length, 0), i);
		}
		return size;
	}

	// Get important Edges
	inline std::vector<XMVECTOR> getImportantEdges(const XMMATRIX& obj2World)
	{
		XMVECTOR xaxis = XMVectorSet(1, 0, 0, 1);
		XMVECTOR yaxis = XMVectorSet(0, 1, 0, 1);
		XMVECTOR zaxis = XMVectorSet(0, 0, 1, 1);
		XMVECTOR edge1 = XMVector3TransformNormal(xaxis, obj2World);
		XMVECTOR edge2 = XMVector3TransformNormal(yaxis, obj2World);
		XMVECTOR edge3 = XMVector3TransformNormal(zaxis, obj2World);
		std::vector<XMVECTOR> results;
		results.push_back(edge1);
		results.push_back(edge2);
		results.push_back(edge3);
		return results;
	}

	// Get the Normal to the faces
	inline std::vector<XMVECTOR> getAxisNormalToFaces(const XMMATRIX& obj2World)
	{
		std::vector<XMVECTOR> edges;
		XMVECTOR xaxis = XMVectorSet(1, 0, 0, 1);
		XMVECTOR yaxis = XMVectorSet(0, 1, 0, 1);
		XMVECTOR zaxis = XMVectorSet(0, 0, 1, 1);
		XMVECTOR edge1 = XMVector3Normalize(XMVector3TransformNormal(xaxis, obj2World));
		XMVECTOR edge2 = XMVector3Normalize(XMVector3TransformNormal(yaxis, obj2World));
		XMVECTOR edge3 = XMVector3Normalize(XMVector3TransformNormal(zaxis, obj2World));
		std::vector<XMVECTOR> results;
		edges.push_back(edge1);
		edges.push_back(edge2);
		edges.push_back(edge3);
		return edges;
	}


	// Get the pair of edges
	inline std::vector<XMVECTOR> getPairOfEdges(const XMMATRIX& obj2World_A, const XMMATRIX& obj2World_B)
	{
		std::vector<XMVECTOR> edges1 = getAxisNormalToFaces(obj2World_A);
		std::vector<XMVECTOR> edges2 = getAxisNormalToFaces(obj2World_B);

		std::vector<XMVECTOR> results;
		for (int i = 0; i < edges1.size(); i++)
		{
			for (int j = 0; j<edges2.size(); j++)
			{
				XMVECTOR vector = XMVector3Cross(edges1[i], edges2[j]);
				if (XMVectorGetX(XMVector3Length(vector)) > 0)
					results.push_back(XMVector3Normalize(vector));
			}
		}
		return results;
	}

	// project a shape on an axis
	inline Projection project(const XMMATRIX& obj2World, XMVECTOR axis)
	{
		// Get corners
		std::vector<XMVECTOR> cornersWorld = getCorners(obj2World);
		float min = XMVectorGetX(XMVector3Dot(cornersWorld[0], axis));
		float max = min;
		for (int i = 1; i < cornersWorld.size(); i++)
		{
			float p = XMVectorGetX(XMVector3Dot(cornersWorld[i], axis));
			if (p < min) {
				min = p;
			}
			else if (p > max) {
				max = p;
			}
		}
		Projection projection;
		projection.max = max;
		projection.min = min;
		return projection;
	}

	inline bool overlap(Projection p1, Projection p2)
	{
		return !((p1.max > p2.max && p1.min > p2.max) || (p2.max > p1.max && p2.min > p1.max));
	}

	inline float getOverlap(Projection p1, Projection p2)
	{
		return XMMin(p1.max, p2.max) - XMMax(p1.min, p2.min);
	}

	static inline XMVECTOR contactPoint(
		const XMVECTOR &pOne,
		const XMVECTOR &dOne,
		float oneSize,
		const XMVECTOR &pTwo,
		const XMVECTOR &dTwo,
		float twoSize,

		// If this is true, and the contact point is outside
		// the edge (in the case of an edge-face contact) then
		// we use one's midpoint, otherwise we use two's.
		bool useOne)
	{
		XMVECTOR toSt, cOne, cTwo;
		float dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
		float denom, mua, mub;

		smOne = XMVectorGetX(XMVector3LengthSq(dOne));
		smTwo = XMVectorGetX(XMVector3LengthSq(dTwo));
		dpOneTwo = XMVectorGetX(XMVector3Dot(dTwo, dOne));

		toSt = pOne - pTwo;
		dpStaOne = XMVectorGetX(XMVector3Dot(dOne, toSt));
		dpStaTwo = XMVectorGetX(XMVector3Dot(dTwo, toSt));

		denom = smOne * smTwo - dpOneTwo * dpOneTwo;

		// Zero denominator indicates parrallel lines
		if (abs(denom) < 0.0001f) {
			return useOne ? pOne : pTwo;
		}

		mua = (dpOneTwo * dpStaTwo - smTwo * dpStaOne) / denom;
		mub = (smOne * dpStaTwo - dpOneTwo * dpStaOne) / denom;

		// If either of the edges has the nearest point out
		// of bounds, then the edges aren't crossed, we have
		// an edge-face contact. Our point is on the edge, which
		// we know from the useOne parameter.
		if (mua > oneSize ||
			mua < -oneSize ||
			mub > twoSize ||
			mub < -twoSize)
		{
			return useOne ? pOne : pTwo;
		}
		else
		{
			cOne = pOne + dOne * mua;
			cTwo = pTwo + dTwo * mub;

			return cOne * 0.5 + cTwo * 0.5;
		}
	}

	inline XMVECTOR handleVertexToface(const XMMATRIX& obj2World, const XMVECTOR& toCenter)
	{
		std::vector<XMVECTOR> corners = getCorners(obj2World);
		float min = 1000;
		XMVECTOR vertex;
		for (int i = 0; i < corners.size(); i++)
		{
			float value = XMVectorGetX(XMVector3Dot(corners[i], toCenter));
			if (value < min)
			{
				vertex = corners[i];
				min = value;
			}
		}

		return vertex;
	}


	inline CollisionInfo checkCollisionSATHelper(const XMMATRIX& obj2World_A, const XMMATRIX& obj2World_B, XMVECTOR size_A, XMVECTOR size_B)
	{
		CollisionInfo info;
		info.isValid = false;
		XMVECTOR collisionPoint = XMVectorZero();
		float smallOverlap = 10000.0f;
		XMVECTOR axis;
		int index;
		int fromWhere = -1;
		bool bestSingleAxis = false;
		XMVECTOR toCenter = getVectorConnnectingCenters(obj2World_A, obj2World_B);
		std::vector<XMVECTOR> axes1 = getAxisNormalToFaces(obj2World_A);
		std::vector<XMVECTOR> axes2 = getAxisNormalToFaces(obj2World_B);
		std::vector<XMVECTOR> axes3 = getPairOfEdges(obj2World_A, obj2World_B);
		// loop over the axes1
		for (int i = 0; i < axes1.size(); i++) {
			// project both shapes onto the axis
			Projection p1 = project(obj2World_A, axes1[i]);
			Projection p2 = project(obj2World_B, axes1[i]);
			// do the projections overlap?
			if (!overlap(p1, p2)) {
				// then we can guarantee that the shapes do not overlap
				return info;
			}
			else{
				// get the overlap
				float o = getOverlap(p1, p2);
				// check for minimum
				if (o < smallOverlap) {
					// then set this one as the smallest
					smallOverlap = o;
					axis = axes1[i];
					index = i;
					fromWhere = 0;
				}
			}
		}
		// loop over the axes2
		for (int i = 0; i < axes2.size(); i++) {
			// project both shapes onto the axis
			Projection p1 = project(obj2World_A, axes2[i]);
			Projection p2 = project(obj2World_B, axes2[i]);
			// do the projections overlap?
			if (!overlap(p1, p2)) {
				// then we can guarantee that the shapes do not overlap
				return  info;
			}
			else{
				// get the overlap
				float o = getOverlap(p1, p2);
				// check for minimum
				if (o < smallOverlap) {
					// then set this one as the smallest
					smallOverlap = o;
					axis = axes2[i];
					index = i;
					fromWhere = 1;
					bestSingleAxis = true;
				}
			}
		}
		int whichEdges = 0;
		// loop over the axes3
		for (int i = 0; i < axes3.size(); i++) {
			// project both shapes onto the axis
			Projection p1 = project(obj2World_A, axes3[i]);
			Projection p2 = project(obj2World_B, axes3[i]);
			// do the projections overlap?
			if (!overlap(p1, p2)) {
				// then we can guarantee that the shapes do not overlap
				return info;
			}
			else{
				// get the overlap
				float o = getOverlap(p1, p2);
				// check for minimum
				if (o < smallOverlap) {
					// then set this one as the smallest
					smallOverlap = o;
					axis = axes3[i];
					index = i;
					whichEdges = i;
					fromWhere = 2;
				}
			}
		}
		// if we get here then we know that every axis had overlap on it
		// so we can guarantee an intersection
		XMVECTOR normal;
		switch (fromWhere){
		case 0:{
				   normal = axis;
				   if (XMVectorGetX(XMVector3Dot(axis, toCenter)) <= 0)
				   {
					   normal = normal * -1.0f;
				   }
				   collisionPoint = handleVertexToface(obj2World_B, toCenter);
		}break;
		case 1:{
				   normal = axis;
				   if (XMVectorGetX(XMVector3Dot(axis, toCenter)) <= 0)
				   {
					   normal = normal * -1.0f;
				   }
				   collisionPoint = handleVertexToface(obj2World_A, toCenter*-1);
		}break;
		case 2:{
				   XMVECTOR axis = XMVector3Normalize(XMVector3Cross(axes1[whichEdges / 3], axes2[whichEdges % 3]));
				   normal = axis;
				   if (XMVectorGetX(XMVector3Dot(axis, toCenter)) <= 0)
				   {
					   normal = normal * -1.0f;
				   }
				   XMVECTOR ptOnOneEdge = XMVectorSet(0.5, 0.5, 0.5, 1);
				   XMVECTOR ptOnTwoEdge = XMVectorSet(0.5, 0.5, 0.5, 1);

				   for (int i = 0; i < 3; i++)
				   {
					   if (i == whichEdges / 3) ptOnOneEdge = XMVectorSetByIndex(ptOnOneEdge, 0, i);
					   else if (XMVectorGetX(XMVector3Dot(axes1[i], normal)) < 0) ptOnOneEdge = XMVectorSetByIndex(ptOnOneEdge, -XMVectorGetByIndex(ptOnOneEdge, i), i);

					   if (i == whichEdges % 3) ptOnTwoEdge = XMVectorSetByIndex(ptOnTwoEdge, 0, i);
					   else if (XMVectorGetX(XMVector3Dot(axes2[i], normal)) > 0) ptOnTwoEdge = XMVectorSetByIndex(ptOnTwoEdge, -XMVectorGetByIndex(ptOnTwoEdge, i), i);
				   }
				   ptOnOneEdge = XMVector3Transform(ptOnOneEdge, obj2World_A);
				   ptOnTwoEdge = XMVector3Transform(ptOnTwoEdge, obj2World_B);
				   collisionPoint = contactPoint(ptOnOneEdge,
					   axes1[whichEdges / 3],
					   (float)XMVectorGetByIndex(size_A, (whichEdges / 3)),
					   ptOnTwoEdge,
					   axes2[whichEdges % 3],
					   XMVectorGetByIndex(size_B, (whichEdges % 3)),
					   bestSingleAxis);
		}break;
		}


		info.isValid = true;
		info.collisionPointWorld = collisionPoint;
		info.depth = smallOverlap;
		info.normalWorld = normal*-1;
		return info;
	}
}

/* params:
obj2World_A, the transfer matrix from object space of A to the world space
obj2World_B, the transfer matrix from object space of B to the world space
*/
inline CollisionInfo checkCollisionSAT(GamePhysics::Mat4& obj2World_A, GamePhysics::Mat4& obj2World_B) {
	using namespace collisionTools;
	XMMATRIX MatA = obj2World_A.toDirectXMatrix(), MatB = obj2World_B.toDirectXMatrix();
	XMVECTOR calSizeA = getBoxSize(MatA);
	XMVECTOR calSizeB = getBoxSize(MatB);
	
	return checkCollisionSATHelper(MatA, MatB, calSizeA, calSizeB);
}

// example of using the checkCollisionSAT function
inline void testCheckCollision(int caseid){

	if (caseid == 1){// simple examples, suppose that boxes A and B are cubes and have no rotation
		GamePhysics::Mat4 AM; AM.initTranslation(1.0, 1.0, 1.0);// box A at (1.0,1.0,1.0)
		GamePhysics::Mat4 BM; BM.initTranslation(2.0, 2.0, 2.0);  //box B at (2.0,2.0,2.0)

		// check for collision
		CollisionInfo simpletest = checkCollisionSAT(AM, BM);// should find out a collision here
		if (!simpletest.isValid)
			std::printf("No Collision\n");
		else {
			std::printf("collision detected at normal: %f, %f, %f\n", simpletest.normalWorld.x, simpletest.normalWorld.y, simpletest.normalWorld.z);
			std::printf("collision point : %f, %f, %f\n", (simpletest.collisionPointWorld).x, (simpletest.collisionPointWorld).y, simpletest.collisionPointWorld.z);
		}
		// case 1 result:
		// collision detected at normal: -1.000000, -0.000000, -0.000000
		// collision point : 1.500000, 1.500000, 1.500000
		// Box A should be pushed to the left
	}
	else if (caseid == 2){// case 2, collide at a corner of Box B:
		GamePhysics::Mat4 AM, BM;
		AM.initTranslation(0.2f, 5.0f, 1.0f); // box A moves(0.2f, 5.0f, 1.0f) from origin
		BM.initRotationZ(45); // box B rotates 45 degree around axis z
		// box A size(9,2,3), box B size(5.656854f, 5.656854f, 2.0f)
		GamePhysics::Mat4 SizeMat;
		SizeMat.initScaling(9.0f, 2.0f, 3.0f);
		AM = SizeMat * AM;
		SizeMat.initScaling(5.656854f, 5.656854f, 2.0f);
		BM = SizeMat * BM;
		// check for collision
		CollisionInfo simpletest = checkCollisionSAT(AM, BM);// should find out a collision here

		if (!simpletest.isValid)
			std::printf("No Collision\n");
		else {
			std::printf("collision detected at normal: %f, %f, %f\n", simpletest.normalWorld.x, simpletest.normalWorld.y, simpletest.normalWorld.z);
			std::printf("collision point : %f, %f, %f\n", (simpletest.collisionPointWorld).x, (simpletest.collisionPointWorld).y, simpletest.collisionPointWorld.z);
		}
		// case 2 result:
		// collision detected at normal : 0.000000, 1.000000, 0.000000
		// collision point : 0.000000, 4.000000, 1.000000
	}
	else if (caseid == 3){// case 3, collide at a corner of Box A:
		// box A first rotates 45 degree around axis z
		// box A moves(-2.0f, 0.0f, 1.0f) from origin,(-2.0f,0.0f,1.0f) is the centre position of A in world space
		// box A size(2.829f, 2.829f, 2.0f)
		GamePhysics::Mat4 AM_rot; AM_rot.initRotationZ(45);
		GamePhysics::Mat4 AM_tra; AM_tra.initTranslation(-2.0f, 0.0f, 1.0f);
		GamePhysics::Mat4 AM_sca; AM_sca.initScaling(2.829f, 2.829f, 2.0f);
		// get the object 2 world matrix of A
		GamePhysics::Mat4 AM = AM_sca * AM_rot * AM_tra; // pay attention to the order! 
		// order, since we are working with the DirectX, we use left-handed matrixes!

		// box B first rotates 90 degree around axis z
		// box B then moves (1.0f,0.5f,0.0f) from origin, (1.0f,0.5f,0.0f) is also the centre position of B in world space
		// box B size(9.0f, 2.0f, 4.0f)
		GamePhysics::Mat4 BM_rot; BM_rot.initRotationZ(90);
		GamePhysics::Mat4 BM_tra; BM_tra.initTranslation(1.0f, 0.5f, 0.0f);
		GamePhysics::Mat4 BM_sca; BM_sca.initScaling(9.0f, 2.0f, 4.0f);
		GamePhysics::Mat4 BM = BM_sca * BM_rot * BM_tra; // pay attention to the order! 

		// check for collision
		CollisionInfo simpletest = checkCollisionSAT(AM, BM);// should find out a collision here

		if (!simpletest.isValid)
			std::printf("No Collision\n");
		else {
			std::printf("collision detected at normal: %f, %f, %f\n", simpletest.normalWorld.x, simpletest.normalWorld.y, simpletest.normalWorld.z);
			std::printf("collision point : %f, %f, %f\n", (simpletest.collisionPointWorld).x, (simpletest.collisionPointWorld).y, simpletest.collisionPointWorld.z);
		}
		// case 3 result:
		// collision detected at normal: -1.000000, 0.000000, -0.000000
		// collision point : 0.000405, 0.000000, 0.000000
	}
}