#include "CPhysicsEntity.h"


CPhysicsEntity::CPhysicsEntity()
{
}

CPhysicsEntity::~CPhysicsEntity()
{

/*	if (m_compoundArray.empty == false)
		for (int i=0;i<m_compoundArray.size();i++)
			NewtonReleaseCollision(m_world, m_compoundArray[i]);
*/
	if(m_modifier != NULL)
		NewtonReleaseCollision(m_world, m_modifier);

	if (getCollision()!=NULL)
		NewtonReleaseCollision(m_world, getCollision());

	if (m_body)
	{
		if (NewtonBodyGetUserData(m_body))
		{
			NewtonBodySetDestructorCallback( m_body, NULL );
			NewtonDestroyBody( m_world, m_body );
		}
	}

}

// destructor callback
void _cdecl CPhysicsEntity::destructor( const NewtonBody* body )
{
	//newton wants to destroy the body.. so first find it.
	CPhysicsEntity* me;

	me = (CPhysicsEntity*)NewtonBodyGetUserData( body );

	// remove destructor callback
	NewtonBodySetDestructorCallback( body, NULL );
	// remove the user data
	NewtonBodySetUserData( body, NULL );

	//now delete the object.
	delete me;
}

void CPhysicsEntity::init(NewtonWorld* world, NewtonBody *body, ISceneNode *node,CLog* logger)
{
	m_world = world;
	m_body = body;
	m_node = node;

	m_log = logger;

//	NewtonBodySetDestructorCallback(body, destructor);
}

void CPhysicsEntity::serialize(stringc file)
{
	FILE* handle = fopen(file.c_str(), "wb");

	if (handle == NULL)
	{
//		m_log->writeLog("Invalid path name");
		return;
	}

	NewtonTreeCollisionSerialize(
		NewtonBodyGetCollision(m_body),
		writeToFile,
		handle);

	fclose(handle);
}

void CPhysicsEntity::writeToFile(void* serializeHandle, const void* buffer, size_t size)
{
	fwrite (buffer, size, 1, (FILE*) serializeHandle);
}

void CPhysicsEntity::createConvexHullModifier()
{
	m_modifier = NewtonCreateConvexHullModifier(m_world, getCollision());
}

matrix4 CPhysicsEntity::getMatrix()
{
	matrix4 matrix;
	NewtonConvexHullModifierGetMatrix(m_modifier, matrix.pointer());
//	for (int i=0;i<16;i++)
//		matrix.pointer()[i] *= NewtonToIrr;
	matrix *= NewtonToIrr; // SiO2
	return matrix;
}

void CPhysicsEntity::setMatrix(matrix4 matrix)
{
	NewtonConvexHullModifierSetMatrix(m_modifier, matrix.pointer());
}

void CPhysicsEntity::setUserID(int id)
{
	NewtonConvexCollisionSetUserID(getCollision(), id);
}

int CPhysicsEntity::getUserID()
{
	return NewtonConvexCollisionGetUserID(getCollision());
}

f32 CPhysicsEntity::calculateVolume()
{
	return NewtonConvexCollisionCalculateVolume(getCollision());
}

vector3df CPhysicsEntity::calculateInertia()
{
	vector3df inertia;
	NewtonConvexCollisionCalculateInertialMatrix(getCollision(), &inertia.X, NULL);
	return inertia;
}

vector3df CPhysicsEntity::calculateCentreOfMass()
{
	vector3df centreOfMass;
	NewtonConvexCollisionCalculateInertialMatrix(getCollision(), NULL, &centreOfMass.X);
	return centreOfMass;
}


void CPhysicsEntity::setMaterial(int material)
{
	NewtonBodySetMaterialGroupID (m_body, material);
}

// set position for the physics body and graphic node
void CPhysicsEntity::setPosition(vector3df position)
{
	matrix4 matrix;
	NewtonBodyGetMatrix(m_body, matrix.pointer());
	matrix.setTranslation(position * IrrToNewton);
	NewtonBodySetMatrix(m_body, matrix.pointer());

	if (m_node)
		m_node->setPosition(position);
}

// set rotation for the physics body and graphic node
void CPhysicsEntity::setRotation(vector3df rotation)
{
	matrix4 matrix;
	NewtonBodyGetMatrix(m_body, matrix.pointer());
	matrix.setRotationDegrees(rotation * IrrToNewton);
	NewtonBodySetMatrix(m_body, matrix.pointer());

	if (m_node)
		m_node->setRotation(rotation);
}

void CPhysicsEntity::setScale(vector3df scale)
{
	m_node->setScale(scale);

	matrix4 temp_mat;
	NewtonBodyGetMatrix(m_body,temp_mat.pointer());
	temp_mat.setScale(scale);
	NewtonBodySetMatrix(m_body,temp_mat.pointer());
}

vector3df CPhysicsEntity::getPosition()
{
	matrix4 matrix;
	NewtonBodyGetMatrix(m_body, matrix.pointer());
	return matrix.getTranslation() * NewtonToIrr;
}

vector3df CPhysicsEntity::getRotation()
{
	matrix4 matrix;
	NewtonBodyGetMatrix(m_body, matrix.pointer());
	return matrix.getRotationDegrees() * NewtonToIrr;
}

vector3df CPhysicsEntity::getScale()
{
	return m_node->getScale();
}

vector3df CPhysicsEntity::getSize()
{
	return m_size;
}

NewtonBody* CPhysicsEntity::getBody()
{
	return m_body;
}

ISceneNode *CPhysicsEntity::getNode()
{
	if (m_node)
		return m_node;

	return NULL;
}

