#include "GpuConvexScene.h"

#include "GpuRigidBodyDemo.h"
#include "../OpenGLWindow/ShapeData.h"

#include "../OpenGLWindow/GLInstancingRenderer.h"
#include "Bullet3Common/b3Quaternion.h"
#include "../CommonInterfaces/CommonWindowInterface.h"
#include "Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h"
#include "../CommonOpenCL/GpuDemoInternalData.h"
#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
#include "../OpenGLWindow/OpenGLInclude.h"
#include "../OpenGLWindow/GLInstanceRendererInternalData.h"
#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
#include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
#include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
#include "GpuRigidBodyDemoInternalData.h"

#include "Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h"
#include "../OpenGLWindow/GLPrimitiveRenderer.h"
#include "Bullet3OpenCL/Raycast/b3GpuRaycast.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
#include "Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h"

#include "../OpenGLWindow/GLRenderToTexture.h"

static bool gUseInstancedCollisionShapes = true;
extern int gGpuArraySizeX;
extern int gGpuArraySizeY;
extern int gGpuArraySizeZ;


#include "GpuRigidBodyDemo.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h"

class GpuConvexScene : public GpuRigidBodyDemo
{
protected:
	
	class b3GpuRaycast*	m_raycaster;

public:

	GpuConvexScene(GUIHelperInterface* helper) 
		:GpuRigidBodyDemo(helper), m_raycaster(0)
	{
	}
	virtual ~GpuConvexScene(){}
	virtual const char* getName()
	{
		return "Tetrahedra";
	}

	

	virtual void setupScene();

	virtual void	destroyScene();

	virtual int	createDynamicsObjects();

	virtual int	createDynamicsObjects2(const float* vertices, int numVertices, const int* indices,int numIndices);

	virtual void createStaticEnvironment();

};


class GpuConvexPlaneScene : public GpuConvexScene
{
public:

	GpuConvexPlaneScene(GUIHelperInterface* helper)
	:GpuConvexScene(helper){}
	virtual ~GpuConvexPlaneScene(){}
	virtual const char* getName()
	{
		return "ConvexOnPlane";
	}

	

	virtual void createStaticEnvironment();

};


class GpuBoxPlaneScene : public GpuConvexPlaneScene
{
public:

	GpuBoxPlaneScene(GUIHelperInterface* helper):GpuConvexPlaneScene(helper){}
	virtual ~GpuBoxPlaneScene(){}
	virtual const char* getName()
	{
		return "BoxBox";
	}


	virtual int	createDynamicsObjects();


};

class GpuTetraScene : public GpuConvexScene
{

protected:
void createFromTetGenData(const char* ele,	const char* node);

public:
	virtual const char* getName()
	{
		return "TetraBreakable";
	}

	

	virtual int	createDynamicsObjects();

};


b3Vector4 colors[4] =
{
	b3MakeVector4(1,0,0,1),
	b3MakeVector4(0,1,0,1),
	b3MakeVector4(0,1,1,1),
	b3MakeVector4(1,1,0,1),
};

void GpuConvexScene::setupScene()
{
	

	m_raycaster = new b3GpuRaycast(m_clData->m_clContext,m_clData->m_clDevice,m_clData->m_clQueue);

	int index=0;
	createStaticEnvironment();

	index+=createDynamicsObjects();

	m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();

	float camPos[4]={0,0,0,0};//ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0};
	//float camPos[4]={1,12.5,1.5,0};
	
	m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraTargetPosition(camPos[0],camPos[1],camPos[2]);
	m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(150);
	//m_instancingRenderer->setCameraYaw(85);
	m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraYaw(30);
	m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(225);
	

	m_guiHelper->getRenderInterface()->updateCamera(1);//>updateCamera();

	char msg[1024];
	int numInstances = index;
	sprintf(msg,"Num objects = %d",numInstances);
	b3Printf(msg);

	//if (ci.m_gui)
	//	ci.m_gui->setStatusBarMessage(msg,true);
}

void	GpuConvexScene::destroyScene()
{
	delete m_raycaster;
	m_raycaster = 0;
}

int	GpuConvexScene::createDynamicsObjects()
{
	int strideInBytes = 9*sizeof(float);
	/*int numVertices = sizeof(barrel_vertices)/strideInBytes;
	int numIndices = sizeof(barrel_indices)/sizeof(int);
	return createDynamicsObjects2(ci,barrel_vertices,numVertices,barrel_indices,numIndices);
	*/

	int numVertices = sizeof(tetra_vertices)/strideInBytes;
	int numIndices = sizeof(tetra_indices)/sizeof(int);
	return createDynamicsObjects2(tetra_vertices,numVertices,tetra_indices,numIndices);
	
}

int	GpuBoxPlaneScene::createDynamicsObjects()
{
	int strideInBytes = 9*sizeof(float);
	int numVertices = sizeof(cube_vertices)/strideInBytes;
	int numIndices = sizeof(cube_indices)/sizeof(int);
	return createDynamicsObjects2(cube_vertices_textured,numVertices,cube_indices,numIndices);
}


int	GpuConvexScene::createDynamicsObjects2( const float* vertices, int numVertices, const int* indices, int numIndices)
{
	int strideInBytes = 9*sizeof(float);
	int textureIndex  = -1;
	if (0)
	{
		int width,height,n;
		
		const char* filename = "data/cube.png";
		const unsigned char* image=0;
		
		const char* prefix[]={"./","../","../../","../../../","../../../../"};
		int numprefix = sizeof(prefix)/sizeof(const char*);
		
		for (int i=0;!image && i<numprefix;i++)
		{
			char relativeFileName[1024];
			sprintf(relativeFileName,"%s%s",prefix[i],filename);
			image = loadImage(relativeFileName,width,height,n);
		}
		
		b3Assert(image);
		if (image)
		{
			textureIndex = m_instancingRenderer->registerTexture(image,width,height);
		}
	}

	int shapeId = m_guiHelper->getRenderInterface()->registerShape(&vertices[0],numVertices,indices,numIndices,B3_GL_TRIANGLES,textureIndex);
	int group=1;
	int mask=1;
	int index=0;





	{
		

		int curColor = 0;
		float scaling[4] = {1,1,1,1};
		int prevBody = -1;
		int insta = 0;

		b3ConvexUtility* utilPtr = new b3ConvexUtility();

		{
			b3AlignedObjectArray<b3Vector3> verts;

			unsigned char* vts = (unsigned char*) vertices;
			for (int i=0;i<numVertices;i++)
			{
				float* vertex = (float*) &vts[i*strideInBytes];
				verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2]));
			}

			bool merge = true;
			if (numVertices)
			{
				utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge);
			}
		}

		int colIndex=-1;
		if (gUseInstancedCollisionShapes)
			colIndex = m_data->m_np->registerConvexHullShape(utilPtr);

		//int colIndex = m_data->m_np->registerSphereShape(1);
		for (int i=0;i<gGpuArraySizeX;i++)
		{


			//printf("%d of %d\n", i, ci.arraySizeX);
			for (int j=0;j<gGpuArraySizeY;j++)
			{

				for (int k=0;k<gGpuArraySizeZ;k++)
				{
					//int colIndex = m_data->m_np->registerConvexHullShape(&vertices[0],strideInBytes,numVertices, scaling);
					if (!gUseInstancedCollisionShapes)
						colIndex = m_data->m_np->registerConvexHullShape(utilPtr);

					float mass = 1.f;
					if (j==0)//ci.arraySizeY-1)
					{
						//mass=0.f;
					}
					b3Vector3 position = b3MakeVector3(((j+1)&1)+i*2.2,1+j*2.,((j+1)&1)+k*2.2);
					//b3Vector3 position = b3MakeVector3(i*2,1+j*2,k*2);
					//b3Vector3 position=b3MakeVector3(1,0.9,1);
					b3Quaternion orn(0,0,0,1);

					b3Vector4 color = colors[curColor];
					curColor++;
					curColor&=3;
					b3Vector4 scalin=b3MakeVector4(1,1,1,1);
					int id = m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
					int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false);


					if (prevBody>=0)
					{
						//b3Point2PointConstraint* p2p = new b3Point2PointConstraint(pid,prevBody,b3Vector3(0,-1.1,0),b3Vector3(0,1.1,0));
//						 m_data->m_rigidBodyPipeline->addConstraint(p2p);//,false);
					}
					prevBody = pid;

					index++;
				}
			}
		}
		delete utilPtr;
	}
	return index;
}


void GpuConvexScene::createStaticEnvironment()
{
	int strideInBytes = 9*sizeof(float);
	int numVertices = sizeof(cube_vertices)/strideInBytes;
	int numIndices = sizeof(cube_indices)/sizeof(int);
	//int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
	int shapeId = m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
	int group=1;
	int mask=1;
	int index=0;


	{
		b3Vector4 scaling=b3MakeVector4(400,400,400,1);
		int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
		b3Vector3 position=b3MakeVector3(0,-400,0);
		b3Quaternion orn(0,0,0,1);

		b3Vector4 color=b3MakeVector4(0,0,1,1);

		int id = m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
		int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false);

	}
}

void GpuConvexPlaneScene::createStaticEnvironment()
{
	int strideInBytes = 9*sizeof(float);
	int numVertices = sizeof(cube_vertices)/strideInBytes;
	int numIndices = sizeof(cube_indices)/sizeof(int);
	//int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
	int shapeId = m_guiHelper->getRenderInterface()->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
	int group=1;
	int mask=1;
	int index=0;


	{
		b3Vector4 scaling=b3MakeVector4(400,400,400,1);
		int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
		b3Vector3 position=b3MakeVector3(0,-400,0);
		b3Quaternion orn(0,0,0,1);

		b3Vector4 color=b3MakeVector4(0,0,1,1);

		int id = m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
		int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false);

	}

}

/*
void GpuConvexPlaneScene::createStaticEnvironment(const ConstructionInfo& ci)
{
	int strideInBytes = 9*sizeof(float);
	int numVertices = sizeof(cube_vertices)/strideInBytes;
	int numIndices = sizeof(cube_indices)/sizeof(int);
	//int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
	int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
	int group=1;
	int mask=1;
	int index=0;

	
	{
		b3Vector4 scaling=b3MakeVector4(100,0.001,100,1);
		
	
		//int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
		b3Vector3 normal=b3MakeVector3(0,1,0);
		float constant=0.f;
		int colIndex = m_data->m_np->registerPlaneShape(normal,constant);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
		b3Vector3 position=b3MakeVector3(0,0,0);
		

		
		b3Quaternion orn(0,0,0,1);

		b3Vector4 color=b3MakeVector4(0,0,1,1);

		int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
		int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false);

	}

}
*/




struct	TetraBunny
{
#include "bunny.inl"
};

struct	TetraCube
{
#include "cube.inl"
};




static int nextLine(const char* buffer)
{
	int numBytesRead=0;

	while (*buffer != '\n')
	{
		buffer++;
		numBytesRead++;
	}

	
	if (buffer[0]==0x0a)
	{
		buffer++;
		numBytesRead++;
	}
	return numBytesRead;
}

static float mytetra_vertices[] =
{
	-1.f,	0,  -1.f,  0.5f,	0,  1,0,	0,0,
	-1.f,	0,	1.f,   0.5f,	0,  1,0,	1,0,
	1.f,	0,  1.f,   0.5f,	0,	1,0,	1,1,
	1.f,	0,  -1.f,  0.5f,	0,	1,0,	0,1
};

static  int mytetra_indices[]=
{
	0,1,2,
	3,1,2,3,2,0,
	3,0,1
};


/* Create from TetGen .ele, .face, .node data */ 
void GpuTetraScene::createFromTetGenData(const char* ele,
	const char* node)
{
	b3Scalar scaling(10);

	b3AlignedObjectArray<b3Vector3>	pos;
	int								nnode=0;
	int								ndims=0;
	int								nattrb=0;
	int								hasbounds=0;
	int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
	result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);
	node += nextLine(node);

	//b3AlignedObjectArray<b3Vector3> rigidBodyPositions;
	//b3AlignedObjectArray<int> rigidBodyIds;

	pos.resize(nnode);
	for(int i=0;i<pos.size();++i)
	{
		int			index=0;
		//int			bound=0;
		float	x,y,z;
		sscanf(node,"%d %f %f %f",&index,&x,&y,&z);

		//	sn>>index;
		//	sn>>x;sn>>y;sn>>z;
		node += nextLine(node);

		//for(int j=0;j<nattrb;++j) 
		//	sn>>a;

		//if(hasbounds) 
		//	sn>>bound;

		pos[index].setX(b3Scalar(x)*scaling);
		pos[index].setY(b3Scalar(y)*scaling);
		pos[index].setZ(b3Scalar(z)*scaling);
	}


	if(ele&&ele[0])
	{
		int								ntetra=0;
		int								ncorner=0;
		int								neattrb=0;
		sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb);
		ele += nextLine(ele);

		//se>>ntetra;se>>ncorner;se>>neattrb;
		for(int i=0;i<ntetra;++i)
		{
			int			index=0;
			int			ni[4];

			//se>>index;
			//se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
			sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]);
			ele+=nextLine(ele);

			b3Vector3 average=b3MakeVector3(0,0,0);

			for (int v=0;v<4;v++)
			{
				average+=pos[ni[v]];
			}
			average/=4;

			for (int v=0;v<4;v++)
			{
				b3Vector3 shiftedPos = pos[ni[v]]-average;
				mytetra_vertices[0+v*9] = shiftedPos.getX();
				mytetra_vertices[1+v*9] = shiftedPos.getY();
				mytetra_vertices[2+v*9] = shiftedPos.getZ();
			}
			//todo: subtract average

			int strideInBytes = 9*sizeof(float);
			int numVertices = sizeof(mytetra_vertices)/strideInBytes;
			int numIndices = sizeof(mytetra_indices)/sizeof(int);
			int shapeId = m_instancingRenderer->registerShape(&mytetra_vertices[0],numVertices,mytetra_indices,numIndices);
			int group=1;
			int mask=1;
			


			{
				b3Vector4 scaling=b3MakeVector4(1,1,1,1);
				int colIndex = m_data->m_np->registerConvexHullShape(&mytetra_vertices[0],strideInBytes,numVertices, scaling);
				b3Vector3 position=b3MakeVector3(0,150,0);
//				position+=average;//*1.2;//*2;
				position+=average*1.2;//*2;
				//rigidBodyPositions.push_back(position);
				b3Quaternion orn(0,0,0,1);

				static int curColor=0;
				b3Vector4 color = colors[curColor++];
				curColor&=3;
				
				int id = m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
				int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(1.f,position,orn,colIndex,0,false);
				//rigidBodyIds.push_back(pid);

			}



			//for(int j=0;j<neattrb;++j) 
			//	se>>a;
			//psb->appendTetra(ni[0],ni[1],ni[2],ni[3]);

		}
		//	printf("Nodes:  %u\r\n",psb->m_nodes.size());
		//	printf("Links:  %u\r\n",psb->m_links.size());
		//	printf("Faces:  %u\r\n",psb->m_faces.size());
		//	printf("Tetras: %u\r\n",psb->m_tetras.size());

	}

	m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
	m_data->m_np->writeAllBodiesToGpu();
	m_data->m_bp->writeAabbsToGpu();
	m_data->m_rigidBodyPipeline->setupGpuAabbsFull();
	m_data->m_bp->calculateOverlappingPairs(m_data->m_config.m_maxBroadphasePairs);

	int numPairs = m_data->m_bp->getNumOverlap();
	cl_mem pairs = m_data->m_bp->getOverlappingPairBuffer();
	b3OpenCLArray<b3Int2> clPairs(m_clData->m_clContext,m_clData->m_clQueue);
	clPairs.setFromOpenCLBuffer(pairs,numPairs);
	b3AlignedObjectArray<b3Int2> allPairs;
	clPairs.copyToHost(allPairs);

	for (int p=0;p<allPairs.size();p++)
	{
		b3Vector3 posA,posB;
		b3Quaternion ornA,ornB;
		int bodyIndexA = allPairs[p].x;
		int bodyIndexB = allPairs[p].y;
		
		m_data->m_np->getObjectTransformFromCpu(posA,ornA,bodyIndexA);
		m_data->m_np->getObjectTransformFromCpu(posB,ornB,bodyIndexB);

		b3Vector3 pivotWorld = (posA+posB)*0.5f;
		b3Transform transA,transB;
		transA.setIdentity();
		transA.setOrigin(posA);
		transA.setRotation(ornA);
		transB.setIdentity();
		transB.setOrigin(posB);
		transB.setRotation(ornB);
		b3Vector3 pivotInA = transA.inverse()*pivotWorld;
		b3Vector3 pivotInB = transB.inverse()*pivotWorld;

		b3Transform frameInA,frameInB;
		frameInA.setIdentity();
		frameInB.setIdentity();
		frameInA.setOrigin(pivotInA);
		frameInB.setOrigin(pivotInB);
		b3Quaternion relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
							
		//c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB);
		float breakingThreshold = 45;//37.f;
		//c->setBreakingImpulseThreshold(37.1);
		bool useGPU = true;
		if (useGPU)
		{
			int cid = m_data->m_rigidBodyPipeline->createFixedConstraint(bodyIndexA,bodyIndexB,pivotInA,pivotInB,relTargetAB,breakingThreshold);
		} else
		{
			b3FixedConstraint* c = new b3FixedConstraint(bodyIndexA,bodyIndexB,frameInA,frameInB);
			c->setBreakingImpulseThreshold(breakingThreshold);
			m_data->m_rigidBodyPipeline->addConstraint(c);
		}


	}

	printf("numPairs = %d\n",numPairs);

}


int	GpuTetraScene::createDynamicsObjects()
{

	//createFromTetGenData(TetraCube::getElements(),TetraCube::getNodes());
	createFromTetGenData(TetraBunny::getElements(),TetraBunny::getNodes());

	return 0;
}

class CommonExampleInterface*    OpenCLBoxBoxCreateFunc(struct CommonExampleOptions& options)
{
	return new GpuBoxPlaneScene(options.m_guiHelper);
}