#include "btBulletCollisionCommon.h"
#include "btBulletDynamicsCommon.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "CollisionInterface.h"




const int maxNumObjects = 4;
const int numObjects = 2;

#define NUM_DYNAMIC_BOXES_X 30
#define NUM_DYNAMIC_BOXES_Y 30

static btVector3*	gVertices=0;
static int*	gIndices=0;
static btBvhTriangleMeshShape* trimeshShape =0;
static btRigidBody* staticBody = 0;
static float waveheight = 5.f;

const float TRIANGLE_SIZE=8.f;

const int NUM_VERTS_X = 30;
const int NUM_VERTS_Y = 30;
const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;

void CollisionInterface::setVertexPositions(float waveheight, float offset)
{
	int i;
	int j;

	for ( i=0;i<NUM_VERTS_X;i++)
	{
		for (j=0;j<NUM_VERTS_Y;j++)
		{
			gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
				//0.f,
				waveheight*sinf((float)i+offset)*cosf((float)j+offset),
				(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
		}
	}
}

void CollisionInterface::cast (btCollisionWorld* cw)
	{
		btVector3 boxShapeHalfExtents = btVector3(2.0, 2.0, 2.0);
		btBoxShape boxShape = btBoxShape(boxShapeHalfExtents);
		btVector3 source = btVector3(0,0,0);
		btVector3 dest = btVector3(1,1,1);
		btCollisionWorld::ClosestConvexResultCallback cb(source, dest);
		btQuaternion qFrom;
		btQuaternion qTo;
		qFrom.setRotation (btVector3(1.0, 0.0, 0.0), 0.0);
		qTo.setRotation (btVector3(1.0, 0.0, 0.0), 0.0);
		btTransform from(qFrom, source);
		btTransform to(qTo, dest);
		cw->convexSweepTest (&boxShape, from, to, cb);
		if (cb.hasHit ())
		{
			bool bCollisionFound = true;
			
		} 
		else 
		{
			bool bCollisionFound = false;
			
		}
}
		
struct btDrawingResult : public btCollisionWorld::ContactResultCallback
{
	virtual	btScalar	addSingleResult(btManifoldPoint& cp,	const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
	{

		
		return 0;
	}
};

void CollisionInterface::InitPhysics1()
{
	
	#define TRISIZE 10.f   

	int vertStride = sizeof(btVector3);
	int indexStride = 3*sizeof(int);

	
	const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);

	gVertices = new btVector3[totalVerts];
	gIndices = new int[totalTriangles*3];

	int i;


	setVertexPositions(waveheight,0.f);

	int index=0;
	for ( i=0;i<NUM_VERTS_X-1;i++)
	{
		for (int j=0;j<NUM_VERTS_Y-1;j++)
		{
			gIndices[index++] = j*NUM_VERTS_X+i;
			gIndices[index++] = j*NUM_VERTS_X+i+1;
			gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;

			gIndices[index++] = j*NUM_VERTS_X+i;
			gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
			gIndices[index++] = (j+1)*NUM_VERTS_X+i;
		}
	}

	m_indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
		gIndices,
		indexStride,
		totalVerts,(btScalar*) &gVertices[0].x(),vertStride);

	bool useQuantizedAabbCompression = true;

	trimeshShape  = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression);

	//m_collisionShapes.push_back(trimeshShape);

	btCollisionShape* groundShape = trimeshShape;
	

	m_collisionConfiguration = new btDefaultCollisionConfiguration();

	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);

	btVector3 worldMin(-1000,-1000,-1000);
	btVector3 worldMax(1000,1000,1000);
	m_broadphase = new btAxisSweep3(worldMin,worldMax);
	m_solver = new btSequentialImpulseConstraintSolver();
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
	
	float mass = 0.f;
	btTransform	startTransform;
	startTransform.setIdentity();
	startTransform.setOrigin(btVector3(0,-2,0));

	btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
	m_collisionShapes.push_back(colShape);
	cast(m_dynamicsWorld);




}
void CollisionInterface::InitPhysics()
{
	
	//btCollisionObject	objects[maxNumObjects];
	//btCollisionWorld*	collisionWorld = 0;
	//btMatrix3x3 basisA;
	//basisA.setIdentity();

	//btMatrix3x3 basisB;
	//basisB.setIdentity();
	//btVector3 A(0,1,0);

	//objects[0].getWorldTransform().setBasis(basisA);
	//objects[1].getWorldTransform().setBasis(basisB);

	//btBoxShape* boxA = new btBoxShape(btVector3(52.5,105.0,104.5));
	//boxA->setMargin(0.f);

	//btBoxShape* boxB = new btBoxShape(btVector3(117.0,245.0,108.25));
	//boxB->setMargin(0.f);
	//objects[0].setCollisionShape(boxA);//&hullA;
	//objects[1].setCollisionShape(boxB);//&hullB;

	//btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
	//btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
	//btVector3	worldAabbMin(-1000,-1000,-1000);
	//btVector3	worldAabbMax(1000,1000,1000);

	//btAxisSweep3*	broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
	//
	//
	//collisionWorld = new btCollisionWorld(dispatcher,broadphase,collisionConfiguration);
	//
	//objects[0].getWorldTransform().setOrigin(btVector3(-369.5,0,0));
	//
	//btQuaternion rotA(0,0,0,1);
	//rotA.normalize();

	//objects[0].getWorldTransform().setRotation(rotA);

	//objects[1].getWorldTransform().setOrigin(btVector3(-441.8,452.0,0));

	//btQuaternion rotB(0,0.01685,0,0.99985);
	//rotB.normalize();
	//objects[1].getWorldTransform().setRotation(rotB);

	//collisionWorld->addCollisionObject(&objects[0]);
	//collisionWorld->addCollisionObject(&objects[1]);
	//collisionWorld->performDiscreteCollisionDetection();

	//btDrawingResult renderCallback;

	//collisionWorld->contactPairTest(&objects[0],&objects[1],renderCallback);

	////btCollisionAlgorithm* algo = collisionWorld->getDispatcher()->findAlgorithm(&objects[0],&objects[1]);
	////btManifoldResult contactPointResult(&objects[0],&objects[1]);
	////algo->processCollision(&objects[0],&objects[1],collisionWorld->getDispatchInfo(),&contactPointResult);
	////
	////btManifoldArray manifoldArray;
	////algo->getAllContactManifolds(manifoldArray);

	////int numManifolds = manifoldArray.size();
	////int numContacts =0;
	////for (int i=0;i<numManifolds;i++)
	////{
	////	btPersistentManifold* contactManifold = manifoldArray[i];
	////	btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
	//////	btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
	////
	////	
	////	numContacts = contactManifold->getNumContacts();
	////	bool swap = obA == &objects[0];

	////	for (int j=0;j<numContacts;j++)
	////	{
	////		btManifoldPoint& pt = contactManifold->getContactPoint(j);	
	////		

	////		btVector3 ptA = swap ?pt.getPositionWorldOnA():pt.getPositionWorldOnB();
	////		btVector3 ptB = swap ? pt.getPositionWorldOnB():pt.getPositionWorldOnA();
	////		int c=0;
	////	}
	////}
	
	

}