#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