#define BLAAT
#include "RealTimeBullet3CollisionSdk.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"

//convert the opaque pointer to int
struct RTB3_ColliderOpaque2Int
{
    union
    {
        plCollisionObjectHandle m_ptrValue;
        int m_intValue;
    };
};
struct RTB3_ShapeOpaque2Int
{
    union
    {
        plCollisionShapeHandle m_ptrValue;
        int m_intValue;
    };
};

enum RTB3ShapeTypes
{
	RTB3_SHAPE_SPHERE=0,
	RTB3_SHAPE_PLANE,
	RTB3_SHAPE_CAPSULE,
	MAX_NUM_SINGLE_SHAPE_TYPES,
	RTB3_SHAPE_COMPOUND_INTERNAL,

	
};

//we start at 1, so that the 0 index is 'invalid' just like a nullptr
#define START_COLLIDABLE_INDEX 1
#define START_SHAPE_INDEX 1

struct RTB3CollisionWorld
{
	b3AlignedObjectArray<void*> m_collidableUserPointers;
	b3AlignedObjectArray<int> m_collidableUserIndices;
	b3AlignedObjectArray<b3Float4> m_collidablePositions;
	b3AlignedObjectArray<b3Quaternion> m_collidableOrientations;
	b3AlignedObjectArray<b3Transform> m_collidableTransforms;

	b3AlignedObjectArray<b3Collidable> m_collidables;
	
	b3AlignedObjectArray<b3GpuChildShape> m_childShapes;
	b3AlignedObjectArray<b3Aabb>	m_localSpaceAabbs;
	b3AlignedObjectArray<b3Aabb>	m_worldSpaceAabbs;
	b3AlignedObjectArray<b3GpuFace> m_planeFaces;
	b3AlignedObjectArray<b3CompoundOverlappingPair> m_compoundOverlappingPairs;
	int m_nextFreeShapeIndex;
	int m_nextFreeCollidableIndex;
	int m_nextFreePlaneFaceIndex;

	RTB3CollisionWorld()
	:m_nextFreeCollidableIndex(START_COLLIDABLE_INDEX),
	m_nextFreeShapeIndex(START_SHAPE_INDEX),
	m_nextFreePlaneFaceIndex(0)//this value is never exposed to the user, so we can start from 0
	{
	}
};

struct RealTimeBullet3CollisionSdkInternalData
{
	b3AlignedObjectArray<RTB3CollisionWorld*> m_collisionWorlds;
};

RealTimeBullet3CollisionSdk::RealTimeBullet3CollisionSdk()
{
//	int szCol = sizeof(b3Collidable);
//	int szShap = sizeof(b3GpuChildShape);
//	int szComPair = sizeof(b3CompoundOverlappingPair);
	m_internalData = new RealTimeBullet3CollisionSdkInternalData;
}
	
RealTimeBullet3CollisionSdk::~RealTimeBullet3CollisionSdk()
{
	delete m_internalData;
	m_internalData=0;
}
	
plCollisionWorldHandle RealTimeBullet3CollisionSdk::createCollisionWorld(int maxNumObjsCapacity, int maxNumShapesCapacity, int maxNumPairsCapacity)
{
	RTB3CollisionWorld* world = new RTB3CollisionWorld();
	world->m_collidables.resize(maxNumObjsCapacity+START_COLLIDABLE_INDEX);
	world->m_collidablePositions.resize(maxNumObjsCapacity+START_COLLIDABLE_INDEX);
	world->m_collidableOrientations.resize(maxNumObjsCapacity+START_COLLIDABLE_INDEX);
	world->m_collidableTransforms.resize(maxNumObjsCapacity+START_COLLIDABLE_INDEX);
	world->m_collidableUserPointers.resize(maxNumObjsCapacity+START_COLLIDABLE_INDEX);
	world->m_collidableUserIndices.resize(maxNumObjsCapacity+START_COLLIDABLE_INDEX);
	world->m_childShapes.resize(maxNumShapesCapacity+START_SHAPE_INDEX);
	world->m_planeFaces.resize(maxNumShapesCapacity);
	
	world->m_compoundOverlappingPairs.resize(maxNumPairsCapacity);

	m_internalData->m_collisionWorlds.push_back(world);
	return (plCollisionWorldHandle) world;
}
	
void RealTimeBullet3CollisionSdk::deleteCollisionWorld(plCollisionWorldHandle worldHandle)
{
	RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle;
	int loc = m_internalData->m_collisionWorlds.findLinearSearch(world);
	b3Assert(loc >=0 && loc<m_internalData->m_collisionWorlds.size());
	if (loc >=0 && loc<m_internalData->m_collisionWorlds.size())
	{
		m_internalData->m_collisionWorlds.remove(world);
		delete world;
	}
}

plCollisionShapeHandle RealTimeBullet3CollisionSdk::createSphereShape(plCollisionWorldHandle worldHandle, plReal radius)
{
	RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle;
	b3Assert(world->m_nextFreeShapeIndex<world->m_childShapes.size());
	if (world->m_nextFreeShapeIndex<world->m_childShapes.size())
	{
		b3GpuChildShape& shape = world->m_childShapes[world->m_nextFreeShapeIndex];
		shape.m_childPosition.setZero();
		shape.m_childOrientation.setValue(0,0,0,1);
		shape.m_radius = radius;
		shape.m_shapeType = RTB3_SHAPE_SPHERE;
		return (plCollisionShapeHandle) world->m_nextFreeShapeIndex++;
	}
	return 0;
}
	
plCollisionShapeHandle RealTimeBullet3CollisionSdk::createPlaneShape(plCollisionWorldHandle worldHandle, 
													plReal planeNormalX, 
													plReal planeNormalY, 
													plReal planeNormalZ, 
													plReal planeConstant)
{
	RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle;
	b3Assert(world->m_nextFreeShapeIndex < world->m_childShapes.size() && world->m_nextFreePlaneFaceIndex < world->m_planeFaces.size());

	if (world->m_nextFreeShapeIndex < world->m_childShapes.size() && world->m_nextFreePlaneFaceIndex < world->m_planeFaces.size())
	{
		b3GpuChildShape& shape = world->m_childShapes[world->m_nextFreeShapeIndex];
		shape.m_childPosition.setZero();
		shape.m_childOrientation.setValue(0,0,0,1);
		world->m_planeFaces[world->m_nextFreePlaneFaceIndex].m_plane = b3MakeVector4(planeNormalX,planeNormalY,planeNormalZ,planeConstant);
		shape.m_shapeIndex = world->m_nextFreePlaneFaceIndex++;
		shape.m_shapeType = RTB3_SHAPE_PLANE;
		return (plCollisionShapeHandle) world->m_nextFreeShapeIndex++;
	}
	return 0;
}

plCollisionShapeHandle RealTimeBullet3CollisionSdk::createCapsuleShape(plCollisionWorldHandle worldHandle, 
													plReal radius,
													plReal height,
													int capsuleAxis)
{
	RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle;
	b3Assert(world->m_nextFreeShapeIndex < world->m_childShapes.size() && world->m_nextFreePlaneFaceIndex < world->m_planeFaces.size());

	if (world->m_nextFreeShapeIndex < world->m_childShapes.size() && world->m_nextFreePlaneFaceIndex < world->m_planeFaces.size())
	{
		b3GpuChildShape& shape = world->m_childShapes[world->m_nextFreeShapeIndex];
		shape.m_childPosition.setZero();
		shape.m_childOrientation.setValue(0,0,0,1);
		shape.m_radius = radius;
		shape.m_height = height;
		shape.m_shapeIndex = capsuleAxis;
		shape.m_shapeType = RTB3_SHAPE_CAPSULE;
		return (plCollisionShapeHandle) world->m_nextFreeShapeIndex++;
	}
	return 0;
}

plCollisionShapeHandle RealTimeBullet3CollisionSdk::createCompoundShape(plCollisionWorldHandle worldHandle)
{
	RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle;
	b3Assert(world->m_nextFreeShapeIndex < world->m_childShapes.size() && world->m_nextFreePlaneFaceIndex < world->m_planeFaces.size());

	if (world->m_nextFreeShapeIndex < world->m_childShapes.size() && world->m_nextFreePlaneFaceIndex < world->m_planeFaces.size())
	{
		b3GpuChildShape& shape = world->m_childShapes[world->m_nextFreeShapeIndex];
		shape.m_childPosition.setZero();
		shape.m_childOrientation.setValue(0,0,0,1);
		shape.m_numChildShapes = 0;
		shape.m_shapeType = RTB3_SHAPE_COMPOUND_INTERNAL;
		return (plCollisionShapeHandle) world->m_nextFreeShapeIndex++;
	}
	return 0;
}

void RealTimeBullet3CollisionSdk::addChildShape(plCollisionWorldHandle worldHandle,plCollisionShapeHandle compoundShape, plCollisionShapeHandle childShape,plVector3 childPos,plQuaternion childOrn)
{

}
void RealTimeBullet3CollisionSdk::deleteShape(plCollisionWorldHandle worldHandle, plCollisionShapeHandle shape)
{
	///todo
	//deleting shapes would involve a garbage collection phase, and mess up all user indices
	//this would be solved by one more in-direction, at some performance penalty for certain operations
	//for now, we don't delete and eventually run out-of-shapes
}
	
void RealTimeBullet3CollisionSdk::addCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object)
{
	///createCollisionObject already adds it to the world
}

void RealTimeBullet3CollisionSdk::removeCollisionObject(plCollisionWorldHandle world, plCollisionObjectHandle object)
{
	///todo, see deleteShape
}
	
plCollisionObjectHandle RealTimeBullet3CollisionSdk::createCollisionObject(  plCollisionWorldHandle worldHandle, void* userPointer, 
														int userIndex,  plCollisionShapeHandle shapeHandle ,
                                                        plVector3 startPosition,plQuaternion startOrientation )
{
	RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle;
	b3Assert(world->m_nextFreeCollidableIndex < world->m_collidables.size());
	if (world->m_nextFreeCollidableIndex < world->m_collidables.size())
	{
		b3Collidable& collidable = world->m_collidables[world->m_nextFreeCollidableIndex];
		world->m_collidablePositions[world->m_nextFreeCollidableIndex].setValue(startPosition[0],startPosition[1],startPosition[2]);
		world->m_collidableOrientations[world->m_nextFreeCollidableIndex].setValue(startOrientation[0],startOrientation[1],startOrientation[2],startOrientation[3]);
		world->m_collidableTransforms[world->m_nextFreeCollidableIndex].setOrigin(world->m_collidablePositions[world->m_nextFreeCollidableIndex]);
		world->m_collidableTransforms[world->m_nextFreeCollidableIndex].setRotation(world->m_collidableOrientations[world->m_nextFreeCollidableIndex]);
		world->m_collidableUserPointers[world->m_nextFreeCollidableIndex] = userPointer;
		world->m_collidableUserIndices[world->m_nextFreeCollidableIndex] = userIndex;
        RTB3_ShapeOpaque2Int caster;
        caster.m_ptrValue = shapeHandle;
        int shapeIndex = caster.m_intValue;
        collidable.m_shapeIndex = shapeIndex;
		b3GpuChildShape& shape = world->m_childShapes[shapeIndex];
		collidable.m_shapeType = shape.m_shapeType;
		collidable.m_numChildShapes = 1;

		switch (collidable.m_shapeType)
		{
		case RTB3_SHAPE_SPHERE:
			{
				break;
			}
		case RTB3_SHAPE_PLANE:
			{
				break;
			}
		case RTB3_SHAPE_COMPOUND_INTERNAL:
			{
				
				break;
			}
		default:
			{
				b3Assert(0);
			}
		}

		/*case SHAPE_COMPOUND_OF_CONVEX_HULLS:
		case SHAPE_COMPOUND_OF_SPHERES:
		case SHAPE_COMPOUND_OF_CAPSULES:
			{
				collidable.m_numChildShapes = shape.m_numChildShapes;
				collidable.m_shapeIndex = shape.m_shapeIndex;
				break;
		*/
		return (plCollisionObjectHandle)world->m_nextFreeCollidableIndex++;
	}
	return 0;
}
  
void RealTimeBullet3CollisionSdk::deleteCollisionObject(plCollisionObjectHandle body)
{
	///todo, see deleteShape
}
	
void RealTimeBullet3CollisionSdk::setCollisionObjectTransform(plCollisionWorldHandle world, plCollisionObjectHandle body,
											plVector3 position,plQuaternion orientation )
{
}

struct plContactCache
{
	lwContactPoint* pointsOut;
	int pointCapacity;
	int numAddedPoints;
};

typedef void (*plDetectCollisionFunc)(RTB3CollisionWorld* world,int colA, int shapeIndexA, int colB, int shapeIndexB,
									plContactCache* contactCache);

void detectCollisionDummy(RTB3CollisionWorld* world,int colA, int shapeIndexA, int colB, int shapeIndexB,
									plContactCache* contactCache)
{
	(void)world;
	(void)colA,(void)colB;
	(void)contactCache;
}

void plVecCopy(float* dst,const b3Vector3& src)
{
	dst[0] = src.x;
	dst[1] = src.y;
	dst[2] = src.z;
}
void plVecCopy(double* dst,const b3Vector3& src)
{
    dst[0] = src.x;
    dst[1] = src.y;
    dst[2] = src.z;
}

void ComputeClosestPointsPlaneSphere(const b3Vector3& planeNormalWorld, b3Scalar planeConstant, const b3Vector3& spherePosWorld,b3Scalar sphereRadius,  plContactCache* contactCache) 
{
	if (contactCache->numAddedPoints < contactCache->pointCapacity)
	{
		lwContactPoint& pointOut = contactCache->pointsOut[contactCache->numAddedPoints];
		b3Scalar t = -(spherePosWorld.dot(-planeNormalWorld)+planeConstant);
		b3Vector3 intersectionPoint = spherePosWorld+t*-planeNormalWorld;
		b3Scalar distance = t-sphereRadius;
		if (distance<=0)
		{
			pointOut.m_distance = distance;
			plVecCopy(pointOut.m_ptOnBWorld,intersectionPoint);
			plVecCopy(pointOut.m_ptOnAWorld,spherePosWorld+sphereRadius*-planeNormalWorld);
			plVecCopy(pointOut.m_normalOnB,planeNormalWorld);
			contactCache->numAddedPoints++;
		}
	}
}

void ComputeClosestPointsSphereSphere(b3Scalar sphereARadius, const b3Vector3& sphereAPosWorld, b3Scalar sphereBRadius, const b3Vector3& sphereBPosWorld, plContactCache* contactCache) {

	if (contactCache->numAddedPoints < contactCache->pointCapacity)
	{
		lwContactPoint& pointOut = contactCache->pointsOut[contactCache->numAddedPoints];
		b3Vector3 diff = sphereAPosWorld-sphereBPosWorld;

		b3Scalar len = diff.length();
		pointOut.m_distance = len - (sphereARadius+sphereBRadius);
		if (pointOut.m_distance<=0)
		{
			b3Vector3 normOnB = b3MakeVector3(1,0,0);
			if (len > B3_EPSILON) {
				normOnB = diff / len;
			}
			
			plVecCopy(pointOut.m_normalOnB,normOnB);
			b3Vector3 ptAWorld = sphereAPosWorld - sphereARadius*normOnB;
			plVecCopy(pointOut.m_ptOnAWorld,ptAWorld);
			plVecCopy(pointOut.m_ptOnBWorld,ptAWorld-normOnB*pointOut.m_distance);
			
			contactCache->numAddedPoints++;
		}
	}
}

B3_FORCE_INLINE void detectCollisionSphereSphere(RTB3CollisionWorld* world,int colA, int shapeIndexA, int colB, int shapeIndexB,
									plContactCache* contactCache)
{

	const b3Scalar radiusA = world->m_childShapes[shapeIndexA].m_radius;
	const b3Scalar radiusB = world->m_childShapes[shapeIndexB].m_radius;
	
	const b3Transform& trA = world->m_collidableTransforms[colA];
	const b3Vector3& sphereALocalPos = world->m_childShapes[shapeIndexA].m_childPosition;
	b3Vector3 spherePosAWorld = trA(sphereALocalPos);
	//b3Vector3 spherePosAWorld = b3QuatRotate( world->m_collidableOrientations[colA], sphereALocalPos ) + (world->m_collidablePositions[colA]);
	
	const b3Transform& trB = world->m_collidableTransforms[colB];
	const b3Vector3& sphereBLocalPos = world->m_childShapes[shapeIndexB].m_childPosition;
	b3Vector3 spherePosBWorld = trB(sphereBLocalPos);
	//b3Vector3 spherePosBWorld = b3QuatRotate( world->m_collidableOrientations[colB], sphereBLocalPos ) + (world->m_collidablePositions[colB]);

	ComputeClosestPointsSphereSphere(radiusA,spherePosAWorld,radiusB,spherePosBWorld,contactCache);
}

void detectCollisionSpherePlane(RTB3CollisionWorld* world,int colA, int shapeIndexA, int colB, int shapeIndexB,
									plContactCache* contactCache)
{
	const b3Transform& trA = world->m_collidableTransforms[colA];
	const b3Vector3& sphereALocalPos = world->m_childShapes[shapeIndexA].m_childPosition;
	b3Vector3 spherePosAWorld = trA(sphereALocalPos);

	
	int planeFaceIndex = world->m_childShapes[shapeIndexB].m_shapeIndex;
	b3Vector3 planeNormal = world->m_planeFaces[planeFaceIndex].m_plane;
	b3Scalar planeConstant = planeNormal[3];
	planeNormal[3] = 0.f;
	
	ComputeClosestPointsPlaneSphere(planeNormal, planeConstant, spherePosAWorld,world->m_childShapes[shapeIndexA].m_radius, contactCache);

}

void detectCollisionPlaneSphere(RTB3CollisionWorld* world,int colA, int shapeIndexA, int colB, int shapeIndexB,
									plContactCache* contactCache)
{
	(void)world;
	(void)colA,(void)shapeIndexA,(void)colB,(void)shapeIndexB;
	(void)contactCache;
}



#ifdef RTB3_SHAPE_CAPSULE
plDetectCollisionFunc funcTbl_detectCollision[MAX_NUM_SINGLE_SHAPE_TYPES,][MAX_NUM_SINGLE_SHAPE_TYPES,] = {
	{detectCollisionSphereSphere	,detectCollisionSpherePlane	,detectCollisionSphereCapsule},
	{detectCollisionPlaneSphere		,detectCollisionDummy		,detectCollisionPlaneCapsule},
	{detectCollisionCapsuleSphere	,detectCollisionCapsulePlane	,detectCollisionCapsuleCapsule},
};
#else
plDetectCollisionFunc funcTbl_detectCollision[MAX_NUM_SINGLE_SHAPE_TYPES][MAX_NUM_SINGLE_SHAPE_TYPES] = {
	{detectCollisionSphereSphere	,detectCollisionSpherePlane},
	{detectCollisionPlaneSphere		,detectCollisionDummy		},
};

#endif

int RealTimeBullet3CollisionSdk::collide(plCollisionWorldHandle worldHandle,plCollisionObjectHandle colAHandle, plCollisionObjectHandle colBHandle,
                    lwContactPoint* pointsOutOrg, int pointCapacity)
{
	
	RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle;
    RTB3_ColliderOpaque2Int caster;
    caster.m_ptrValue =colAHandle;
    int colAIndex = caster.m_intValue;
    caster.m_ptrValue = colBHandle;
    int colBIndex = caster.m_intValue;
    const b3Collidable& colA = world->m_collidables[colAIndex];
	const b3Collidable& colB = world->m_collidables[colBIndex];

	plContactCache contactCache;
	contactCache.pointCapacity = pointCapacity;
	contactCache.pointsOut = pointsOutOrg;
	contactCache.numAddedPoints = 0;
	
	for (int i=0;i<colA.m_numChildShapes;i++)
	{
		for (int j=0;j<colB.m_numChildShapes;j++)
		{
			if (contactCache.numAddedPoints<pointCapacity)
			{
				//funcTbl_detectCollision[world->m_childShapes[colA.m_shapeIndex+i].m_shapeType]
				//					   [world->m_childShapes[colB.m_shapeIndex+j].m_shapeType](world,colAIndex,colA.m_shapeIndex+i,colBIndex,colB.m_shapeIndex+j,&contactCache);
			}
		}
		return contactCache.numAddedPoints;
	}

	return 0;
}

void RealTimeBullet3CollisionSdk::collideWorld( plCollisionWorldHandle worldHandle,
                            plNearCallback filter, void* userData)
{
	RTB3CollisionWorld* world = (RTB3CollisionWorld*) worldHandle;
	if (filter)
	{
		RTB3_ColliderOpaque2Int caster;
		plCollisionObjectHandle colA;
		plCollisionObjectHandle colB;
		for (int i=START_COLLIDABLE_INDEX;i<world->m_nextFreeCollidableIndex;i++)
		{
			
			for (int j=i+1;j<world->m_nextFreeCollidableIndex;j++)
			{
				caster.m_intValue = i;
                colA = caster.m_ptrValue;
                caster.m_intValue = j;
				colB = caster.m_ptrValue;
                filter((plCollisionSdkHandle)this,worldHandle,userData,colA,colB);
			}
		}
	}
}

plCollisionSdkHandle RealTimeBullet3CollisionSdk::createRealTimeBullet3CollisionSdkHandle()
{
	return (plCollisionSdkHandle) new RealTimeBullet3CollisionSdk();
}