//ODEObject.cpp

#include "ODEObject.h"

ODEObject::ODEObject(bool body, bool model, std::string dir, std::string file, 
					 dReal posx, dReal posy, dReal posz)
: Base3DObject(true, model, dir, file, posx, posy, posz)
{
	//velocity.x = 0;
	//velocity.y = 0;
	//velocity.z = 0;

	lastPosition.x = 0;
	lastPosition.y = 0;
	lastPosition.z = 0;

	hasBody = body;
	floats = false;

	//Not implemented yet...
	//dBodySetAutoDisableSF1(thisBodyID, true);
}

ODEObject::~ODEObject()
{

}

void ODEObject::SetPosition(point3d newposition)
{
	position = newposition;

	//if (hasBody)
	//{
	//	dBodySetPosition(thisBodyID, position.x, position.y, position.z);
	//}
	//else // this object is only a geom
	//{
		dGeomSetPosition(thisGeomID, position.x, position.y, position.z);
	//}
}

point3d ODEObject::GetPosition()
{
	//if (dGeomGetBody(thisGeomID) != 0)
	//{
	//	const dReal* bodyPosition;
	//	bodyPosition = dBodyGetPosition(thisBodyID);
	//	position.x = bodyPosition[0];
	//	position.y = bodyPosition[1];
	//	position.z = bodyPosition[2];
	//}
	//else // this object is only a geom
	//{
		const dReal* geomPosition;
		geomPosition = dGeomGetPosition(thisGeomID);
		position.x = geomPosition[0];
		position.y = geomPosition[1];
		position.z = geomPosition[2];
	//}
	return position;
}

point3d ODEObject::GetLastPosition()
{
	return lastPosition;
}

void ODEObject::SetOrientation(int axis, dReal angle)
{
	//orientation = neworientation;

	dQuaternion q;

	switch (axis)
	{
		case 0:
			dQFromAxisAndAngle(q, 1, 0, 0, angle);
			break;
		case 1:
			dQFromAxisAndAngle(q, 0, 1, 0, angle);
			break;
		case 2:
			dQFromAxisAndAngle(q, 0, 0, 1, angle);
			break;
		default:
			break;
	}

	dGeomSetQuaternion(thisGeomID, q);
}

//point3d ODEObject::GetOrientation()
//{
//	const dReal* geomPosition;
//	geomPosition = dGeomGetPosition(thisGeomID);
//	position.x = geomPosition[0];
//	position.y = geomPosition[1];
//	position.z = geomPosition[2];
//
//	return orientation;
//}

void ODEObject::SetVelocity(vector3d velocity)
{
	if (hasBody)
	{
		dBodySetLinearVel(thisBodyID, velocity.x, velocity.y, velocity.z);
	}
	else // this object is only a geom
	{
		//geoms have no velocity, so this shouldn't be called
	}
}

vector3d ODEObject::GetVelocity()
{
	const dReal* velocity;
	vector3d newVelocity = {0, 0, 0};

	if (hasBody)
	{
		velocity = dBodyGetLinearVel(thisBodyID);

		newVelocity.x = velocity[0];
		newVelocity.y = velocity[1];
		newVelocity.z = velocity[2];

		return newVelocity;
	}
	else // this object is only a geom
	{
		//geoms have no velocity, so this shouldn't be called
		return newVelocity;
	}
}

dBodyID ODEObject::GetBodyID()
{
	return thisBodyID;
}

void ODEObject::SetBodyID(dBodyID newBodyID)
{
	thisBodyID = newBodyID;
}

dGeomID ODEObject::GetGeomID()
{
	return thisGeomID;
}

void ODEObject::SetGeomID(dGeomID GeomID)
{
	thisGeomID = GeomID;
}

void ODEObject::Draw()
{

}

bool ODEObject::HasBody()
{
	return hasBody;
}

void ODEObject::MakeDisplayList()
{
	//can re-implement this in derived classes
	Base3DObject::MakeDisplayList();
}

bool ODEObject::Update(dReal deltaTime)
{
	//give each object a chance to update things every frame
	//re-implement this in derived classes

	lastPosition = position;

	return true;
}

void ODEObject::SetFloats(bool doesFloat)
{
	//can re-implement this in derived classes
	floats = doesFloat;
}

bool ODEObject::Floats()
{
	return floats;
}

void ODEObject::AddForce(dReal x, dReal y, dReal z)
{
	//can re-implement this in derived classes
	if (hasBody)
	{
		dBodyAddForce(thisBodyID, x, y, z);
	}
}

void ODEObject::DampenVelocity(dReal dampingAmount)
{
	//can re-implement this in derived classes
	if (!hasBody)
	{
		return;
	}

	const dReal* linearVelocity = dBodyGetLinearVel(thisBodyID);
	const dReal* angularVelocity = dBodyGetAngularVel(thisBodyID);

	dReal lvx, lvy, lvz, avx, avy, avz;

	if (linearVelocity[0] > AUTO_DISABLE_THRESHOLD*2)
	{
		lvx = linearVelocity[0]/dampingAmount;
	}
	else
	{
		lvx = linearVelocity[0];
	}

	if (linearVelocity[1] > AUTO_DISABLE_THRESHOLD*2)
	{
		lvy = linearVelocity[1]/dampingAmount;
	}
	else
	{
		lvy = linearVelocity[1];
	}

	if (linearVelocity[2] > AUTO_DISABLE_THRESHOLD*2)
	{
		lvz = linearVelocity[2]/dampingAmount;
	}
	else
	{
		lvz = linearVelocity[2];
	}

	if (angularVelocity[0] > AUTO_DISABLE_THRESHOLD*2)
	{
		avx = angularVelocity[0]/(5*dampingAmount);
	}
	else
	{
		avx = angularVelocity[0];
	}

	if (angularVelocity[1] > AUTO_DISABLE_THRESHOLD*2)
	{
		avy = angularVelocity[1]/(5*dampingAmount);
	}
	else
	{
		avy = angularVelocity[1];
	}

	if (angularVelocity[2] > AUTO_DISABLE_THRESHOLD*2)
	{
		avz = angularVelocity[2]/(5*dampingAmount);
	}
	else
	{
		avz = angularVelocity[2];
	}

	dBodySetLinearVel(thisBodyID, lvx, lvy, lvz);
	dBodySetAngularVel(thisBodyID, avx, avy, avz);
}

void ODEObject::SetMass(dReal newMass)
{
	if (!hasBody)
	{
		return;
	}

	if (newMass > MAX_MASS)
	{
		newMass = MAX_MASS;
	}

	if (newMass < MIN_MASS)
	{
		newMass = MIN_MASS;
	}

	dMass mass;
	dBodyGetMass(thisBodyID, &mass);
	dMassAdjust(&mass, newMass);
	dBodySetMass(thisBodyID, &mass);
}