//ODEBox.cpp

#include "ODEBox.h"
#include <math.h>

ODEBox::ODEBox(ODEWorld* world, bool body, bool model, std::string dir, std::string file, 
			   dReal posx, dReal posy, dReal posz, dReal sizex, dReal sizey, dReal sizez)
: ODEObject(body, model, dir, file, posx, posy, posz)
{
	if (has3DModel)
	{
		//create OBJ object
		thisModel = new COBJModel();
		thisModel->LoadCOBJModel2(dirname, filename);
		//thisModel->ReadMTL();

		//setup bounding extremes, dimensions
		bboxMin.x = thisModel->Box[0];
		bboxMax.x = thisModel->Box[1];
		bboxMin.y = thisModel->Box[2];
		bboxMax.y = thisModel->Box[3];
		bboxMin.z = thisModel->Box[4];
		bboxMax.z = thisModel->Box[5];

		////setup bbox center
		//bboxCenter.x = (bboxMin.x + bboxMax.x)/2;
		//bboxCenter.y = (bboxMin.y + bboxMax.y)/2;
		//bboxCenter.z = (bboxMin.z + bboxMax.z)/2;

		modelDimensions.x = bboxMax.x - bboxMin.x;
		modelDimensions.y = bboxMax.y - bboxMin.y;
		modelDimensions.z = bboxMax.z - bboxMin.z;

		//sizex, sizey, sizez aren't used if we're loading a 3d model
		myDimensions[0] = modelDimensions.x;
		myDimensions[1] = modelDimensions.y;
		myDimensions[2] = modelDimensions.z;

		////if model center is off, we can adjust it here
		//posx += bboxCenter.x;
		//posy += bboxCenter.y;
		//posz += bboxCenter.z;
	}
	else
	{
		myDimensions[0] = sizex;
		myDimensions[1] = sizey;
		myDimensions[2] = sizez;
	}

	// ODE stuff
	thisGeomID = dCreateBox(world->GetSpaceID(), myDimensions[0], myDimensions[1], myDimensions[2]);

	if (hasBody)
	{
		thisBodyID = dBodyCreate(world->GetWorldID());
		dGeomSetBody(thisGeomID, thisBodyID);

		////calculating mass proportional to volume
		//dReal newMass = myDimensions[0]*myDimensions[1]*myDimensions[2]/MAX_VOLUME;
		//newMass = 5/(1 + exp(-newMass)); //yields values between 0.0 and 5.0
		//dMass mass;
		//dBodyGetMass(thisBodyID, &mass);
		//dMassAdjust(&mass, newMass);
		//dBodySetMass(thisBodyID, &mass);
	}
	else
	{
		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.
	//if (has3DModel)
	//{
	//	//do this last because it will destroy all the vertex/data
	//	thisModel->BuildList();
	//}
}

ODEBox::~ODEBox()
{
	if (hasBody)
	{
		dBodyDestroy(thisBodyID);
	}

	dGeomDestroy(thisGeomID);
}

void ODEBox::Draw()
{
	if (hasBody)
	{
		// 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(position.x, position.y, position.z);
		glMultMatrixd(m);

		if (has3DModel)
		{
			thisModel->DrawModel();
		}
		else
		{
			DrawBox();
		}

		glPopMatrix();
	}
	else
	{
		float matAmbientAndDiffuse[4] = {.5, .5, .5, 1};
		glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, matAmbientAndDiffuse);

		// getting geom position
		const dReal* geomPosition;
		geomPosition = dGeomGetPosition(thisGeomID);
		position.x = geomPosition[0];
		position.y = geomPosition[1];
		position.z = geomPosition[2];

///////
		// getting orientation
		const dReal* R;
		R = dGeomGetRotation(thisGeomID);

		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);

		if (has3DModel)
		{
			thisModel->DrawModel();
		}
		else
		{
			DrawBox();
		}

		glPopMatrix();
	}
}

void ODEBox::DrawBox()
{
	dReal halfx = myDimensions[0]/2;
	dReal halfy = myDimensions[1]/2;
	dReal halfz = myDimensions[2]/2;

	glBegin(GL_QUADS);
		//left
		glNormal3f(-1, 0, 0);
		glVertex3f(-halfx, -halfy, halfz);
		glVertex3f(-halfx, halfy, halfz);
		glVertex3f(-halfx, halfy, -halfz);
		glVertex3f(-halfx, -halfy, -halfz);
		//back
		glNormal3f(0, 0, -1);
		glVertex3f(-halfx, -halfy, -halfz);
		glVertex3f(-halfx, halfy, -halfz);
		glVertex3f(halfx, halfy, -halfz);
		glVertex3f(halfx, -halfy, -halfz);
		//right
		glNormal3f(1, 0, 0);
		glVertex3f(halfx, -halfy, -halfz);
		glVertex3f(halfx, halfy, -halfz);
		glVertex3f(halfx, halfy, halfz);
		glVertex3f(halfx, -halfy, halfz);
		//front
		glNormal3f(0, 0, 1);
		glVertex3f(halfx, -halfy, halfz);
		glVertex3f(halfx, halfy, halfz);
		glVertex3f(-halfx, halfy, halfz);
		glVertex3f(-halfx, -halfy, halfz);
		//top
		glNormal3f(0, 1, 0);
		glVertex3f(halfx, halfy, halfz);
		glVertex3f(halfx, halfy, -halfz);
		glVertex3f(-halfx, halfy, -halfz);
		glVertex3f(-halfx, halfy, halfz);
		//bottom
		glNormal3f(0, -1, 0);
		glVertex3f(-halfx, -halfy, halfz);
		glVertex3f(-halfx, -halfy, -halfz);
		glVertex3f(halfx, -halfy, -halfz);
		glVertex3f(halfx, -halfy, halfz);
	glEnd();
}

dReal ODEBox::GetXDimension()
{
	return myDimensions[0];
}

dReal ODEBox::GetYDimension()
{
	return myDimensions[1];
}

dReal ODEBox::GetZDimension()
{
	return myDimensions[2];
}
