//ODESphere.cpp

#include "ODESphere.h"

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

	radius = rad;

	// ODE stuff
	thisGeomID = dCreateSphere(world->GetSpaceID(), radius);

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

		////calculating mass proportional to volume
		//dReal newMass = (4.2*pow(radius, 3))/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();
	//}
}

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

	dGeomDestroy(thisGeomID);
}

void ODESphere::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
		{
			glutSolidSphere(radius, 20, 20);
		}
		
		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];

		glPushMatrix();
		glTranslatef((GLfloat)position.x, (GLfloat)position.y, (GLfloat)position.z);

		if (has3DModel)
		{
			thisModel->DrawModel();
		}
		else
		{
			glutSolidSphere(radius, 20, 20);
		}

		glPopMatrix();
	}

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

	//glutSolidSphere(radius, 20, 20);

	//glPopMatrix();
}

dReal ODESphere::GetRadius()
{
	return radius;
}
