#ifndef B3_CLIP_FACES_H
#define B3_CLIP_FACES_H

#include "Bullet3Common/shared/b3Int4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"

inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
{
	return b3MakeFloat4(a.x + (b.x - a.x) * t,
						a.y + (b.y - a.y) * t,
						a.z + (b.z - a.z) * t,
						0.f);
}

// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut)
{
	int ve;
	float ds, de;
	int numVertsOut = 0;
	//double-check next test
	//	if (numVertsIn < 2)
	//		return 0;

	b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
	b3Float4 endVertex = pVtxIn[0];

	ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS;

	for (ve = 0; ve < numVertsIn; ve++)
	{
		endVertex = pVtxIn[ve];
		de = b3Dot(planeNormalWS, endVertex) + planeEqWS;
		if (ds < 0)
		{
			if (de < 0)
			{
				// Start < 0, end < 0, so output endVertex
				ppVtxOut[numVertsOut++] = endVertex;
			}
			else
			{
				// Start < 0, end >= 0, so output intersection
				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
			}
		}
		else
		{
			if (de < 0)
			{
				// Start >= 0, end < 0 so output intersection and end
				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
				ppVtxOut[numVertsOut++] = endVertex;
			}
		}
		firstVertex = endVertex;
		ds = de;
	}
	return numVertsOut;
}

__kernel void clipFacesAndFindContactsKernel(__global const b3Float4* separatingNormals,
											 __global const int* hasSeparatingAxis,
											 __global b3Int4* clippingFacesOut,
											 __global b3Float4* worldVertsA1,
											 __global b3Float4* worldNormalsA1,
											 __global b3Float4* worldVertsB1,
											 __global b3Float4* worldVertsB2,
											 int vertexFaceCapacity,
											 int pairIndex)
{
	//    int i = get_global_id(0);
	//int pairIndex = i;
	int i = pairIndex;

	float minDist = -1e30f;
	float maxDist = 0.02f;

	//	if (i<numPairs)
	{
		if (hasSeparatingAxis[i])
		{
			//			int bodyIndexA = pairs[i].x;
			//		int bodyIndexB = pairs[i].y;

			int numLocalContactsOut = 0;

			int capacityWorldVertsB2 = vertexFaceCapacity;

			__global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
			__global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];

			{
				__global b3Int4* clippingFaces = clippingFacesOut;

				int closestFaceA = clippingFaces[pairIndex].x;
				// int closestFaceB = clippingFaces[pairIndex].y;
				int numVertsInA = clippingFaces[pairIndex].z;
				int numVertsInB = clippingFaces[pairIndex].w;

				int numVertsOut = 0;

				if (closestFaceA >= 0)
				{
					// clip polygon to back of planes of all faces of hull A that are adjacent to witness face

					for (int e0 = 0; e0 < numVertsInA; e0++)
					{
						const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0];
						const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)];
						const b3Float4 WorldEdge0 = aw - bw;
						b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
						b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1);
						b3Float4 worldA1 = aw;
						float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1);
						b3Float4 planeNormalWS = planeNormalWS1;
						float planeEqWS = planeEqWS1;
						numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut);
						__global b3Float4* tmp = pVtxOut;
						pVtxOut = pVtxIn;
						pVtxIn = tmp;
						numVertsInB = numVertsOut;
						numVertsOut = 0;
					}

					b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
					float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]);

					for (int i = 0; i < numVertsInB; i++)
					{
						float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
						if (depth <= minDist)
						{
							depth = minDist;
						}
						/*
						static float maxDepth = 0.f;
						if (depth < maxDepth)
						{
							maxDepth = depth;
							if (maxDepth < -10)
							{
								printf("error at framecount %d?\n",myframecount);
							}
							printf("maxDepth = %f\n", maxDepth);

						}
*/
						if (depth <= maxDist)
						{
							b3Float4 pointInWorld = pVtxIn[i];
							pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
						}
					}
				}
				clippingFaces[pairIndex].w = numLocalContactsOut;
			}

			for (int i = 0; i < numLocalContactsOut; i++)
				pVtxIn[i] = pVtxOut[i];

		}  //		if (hasSeparatingAxis[i])
	}      //	if (i<numPairs)
}

#endif  //B3_CLIP_FACES_H