//ODETrimesh.cpp

#include "ODETrimesh.h"

ODETrimesh::ODETrimesh(ODEWorld* world, dReal posx, dReal posy, dReal posz, 
					   std::string dirname, std::string filename)
: ODEObject(false, true, dirname, filename, posx, posy, posz)
{
	//create OBJ object
	thisModel = new COBJModel();
	thisModel->LoadCOBJModel2(dirname, filename);
	//thisModel->ReadMTL();

	// ODE stuff
	thisTrimeshDataID = dGeomTriMeshDataCreate();

	//making deep copy of geometry...

	//vertices

	//numVertices = thisModel.GetNumVertices();
	//vertexArray = new StridedVertex[numVertices];
	//float* xCoords = NULL;
	//float* yCoords = NULL;
	//float* zCoords = NULL;
	//thisModel.GetVertexArrays(xCoords, yCoords, zCoords);
	//for (int i=0; i<numVertices; i++)
	//{
	//	vertexArray[i].Vertex[0] = xCoords[i];
	//	vertexArray[i].Vertex[1] = yCoords[i];
	//	vertexArray[i].Vertex[2] = zCoords[i];
	//}

	numVertices = thisModel->GetNumVertices();
	vertexArray = new dVector3[numVertices];
	float* xCoords = NULL;
	float* yCoords = NULL;
	float* zCoords = NULL;
	thisModel->GetVertexArrays(xCoords, yCoords, zCoords);
	for (int i=0; i<numVertices; i++)
	{
		vertexArray[i][0] = xCoords[i];
		vertexArray[i][1] = yCoords[i];
		vertexArray[i][2] = zCoords[i];
	}

	//faces
	//numFaces = thisModel.GetNumFaces();
	//faceArray = new StridedTri[numFaces];
	//int (*tempFaceArray)[9] = NULL;
	//thisModel.GetFaceArray(tempFaceArray);
	//for (int i=0; i<numFaces; i++)
	//{
	//	faceArray[i].Indices[0] = tempFaceArray[i][0];
	//	faceArray[i].Indices[1] = tempFaceArray[i][3];
	//	faceArray[i].Indices[2] = tempFaceArray[i][6];
	//}

	numFaces = thisModel->GetNumFaces();
	faceArray = new int[numFaces*3];
	int (*tempFaceArray)[9] = NULL;
	thisModel->GetFaceArray(tempFaceArray);
	for (int i=0; i<numFaces; i++)
	{
		//faceArray[i] = new int[3];
		//faceArray[i][0] = tempFaceArray[i][0];
		//faceArray[i][1] = tempFaceArray[i][3];
		//faceArray[i][2] = tempFaceArray[i][6];
		faceArray[i*3] = tempFaceArray[i][0];
		faceArray[i*3+1] = tempFaceArray[i][3];
		faceArray[i*3+2] = tempFaceArray[i][6];
	}

	dGeomTriMeshDataBuildSimple (thisTrimeshDataID, (dReal*)vertexArray, numVertices, faceArray, numFaces*3);

	//int VertexStride = sizeof (StridedVertex);
	//int TriStride = sizeof (StridedTri);

//#ifdef dSINGLE
//
//	dGeomTriMeshDataBuildSingle(thisTrimeshDataID, vertexArray, sizeof(dVector3), numVertices,
//									faceArray, numFaces*3, 3*sizeof(int));
//#endif
//#ifdef dDOUBLE
//
//	dGeomTriMeshDataBuildDouble(thisTrimeshDataID, vertexArray, 3*sizeof(dReal), numVertices,
//									(int*)faceArray, numFaces, 3*sizeof(int));
//#endif

	thisGeomID = dCreateTriMesh(world->GetSpaceID(), thisTrimeshDataID, NULL, NULL, NULL);

	// this geom has no body
	dGeomSetBody(thisGeomID, 0);

	point3d startPoint = {posx, posy, posz};
	SetPosition(startPoint);

	//Do this in a separate function so it can be done in VRJuggler's contextInit function.
	//thisModel->BuildList();
}

ODETrimesh::~ODETrimesh()
{
	//destroy vertex/face arrays here
	delete [] vertexArray;
	//for (int i=0; i<numFaces; i++)
	//{
	//	delete [] faceArray[i];
	//}
	delete [] faceArray;

	dGeomTriMeshDataDestroy(thisTrimeshDataID);
	dGeomDestroy(thisGeomID);
}

void ODETrimesh::Draw()
{
	thisModel->DrawModel();
}

//void ODETrimesh::Draw()
//{
//	// getting body position
//	const dReal* bodyPosition;
//	bodyPosition = dBodyGetPosition(thisBodyID);
//	position.x = bodyPosition[0];
//	position.y = bodyPosition[1];
//	position.z = bodyPosition[2];
//
//	// getting orientation
//	//const dReal* R;
//	//R = dBodyGetRotation(thisBodyID);
//
//	//double m[16];
//	//m[0] = R[0];
//	//m[1] = R[4];
//	//m[2] = R[8];
//	//m[3] = 0;
//	//m[4] = R[1];
//	//m[5] = R[5];
//	//m[6] = R[9];
//	//m[7] = 0;
//	//m[8] = R[2];
//	//m[9] = R[6];
//	//m[10] = R[10];
//	//m[11] = 0;
//	//m[12] = R[3];
//	//m[13] = R[7];
//	//m[14] = R[11];
//	//m[15] = 1;
//
//	glPushMatrix();
//	glTranslatef((GLfloat)position.x, (GLfloat)position.y, (GLfloat)position.z);
//	//glMultMatrixd(m);
//
//	//DrawBox();
//
//	glPopMatrix();
//}
