#include "StdAfx.h"
#include "Havok.h"

#define STACK_SIZE		1024 * 256

Havok::Havok(void)
{
	// set up memory manager
    hkMemory* memoryManager = new hkPoolMemory();
    ThreadMemory = new hkThreadMemory(memoryManager); 
	hkBaseSystem::init(memoryManager, ThreadMemory, ReportError);
    char* stackBuffer = hkAllocate<char>(STACK_SIZE, HK_MEMORY_CLASS_BASE);
    ThreadMemory->setStackArea(stackBuffer, STACK_SIZE);
    hkThreadMemory::getInstance().setStackArea(stackBuffer, STACK_SIZE);
    memoryManager->removeReference();

	// create world
    hkpWorldCinfo info;
    //info.m_simulationType = hkpWorldCinfo::SIMULATION_TYPE_CONTINUOUS;
    info.m_simulationType = hkpWorldCinfo::SIMULATION_TYPE_DISCRETE;
    info.m_gravity.set(0.0f, -9.8f, 0.0f);
    info.setBroadPhaseWorldSize(10000.0f);
    info.m_broadPhaseBorderBehaviour = hkpWorldCinfo::BROADPHASE_BORDER_ASSERT;
    //info.m_enableDeactivation = false;	// use for small timesteps!!!
	info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_4ITERS_MEDIUM);
    World = new hkpWorld(info);

	// register all collision agents
    hkpAgentRegisterUtil::registerAllAgents(World->getCollisionDispatcher());
}
Havok::~Havok(void)
{
	World->removeReference();
    World = HK_NULL;
    hkBaseSystem::quit();
}

// update havok world and sync with our game objects
void Havok::Update(float timestep)
{
	hkVector4 pos, vel, avel;	// declare first so they get aligned properly
	hkQuaternion rot;

	World->stepDeltaTime(timestep);	// update havok state

	// simple test to update active objects and remove inactive ones
	for (int i=0; i<Objects.Count(); i++)
	{
		Object* obj = Objects[i];
		if (!obj->PhysicsModel->isAddedToWorld())
		{
			Objects.Remove(obj);	// remove if not part of world - how does this happen???
			i--;
		}
		else if (!obj->PhysicsModel->isActive())
		{
			RemoveObject(obj);		// remove when deactivated
			i--;
		}
		else
		{
			// relay those changes to our objects for rendering by the graphics engine
			pos = obj->PhysicsModel->getPosition();
			pos.store3(&obj->Position.x);
			rot = obj->PhysicsModel->getRotation();
			rot.m_vec.store4(&obj->Rotation.x);
			vel = obj->PhysicsModel->getLinearVelocity();	// extra stuff
			vel.store3(&obj->Velocity.x);
			avel = obj->PhysicsModel->getAngularVelocity();
			avel.store3(&obj->AngularVelocity.x);
		}
	}

}
void Havok::AddObject(Object* object)
{
	Objects.Add(object);
	World->addEntity(object->PhysicsModel);
	object->PhysicsModel->removeReference();	// give world full ownership
	assert(object->PhysicsModel->getReferenceCount() == 1);
}
void Havok::RemoveObject(Object* object)
{
	object->PhysicsModel->activate();	// re-activate everything in current island
	World->removeEntity(object->PhysicsModel);
	assert(object->PhysicsModel->getReferenceCount() == 0);
	Objects.Remove(object);
}