PhysicsHk::PhysicsHk()
{
	instance = this ;
	oneSecond = 0 ;
	matrixTransformHk = new hkReal() ;
	for( int i = 0; i < NUM_BODIES; i++ )
	{
		m_bodies = HK_NULL;		
	}	
	m_currentBody = 0 ;

	hkpWorldCinfo info;
	info.m_gravity = hkVector4(0.0f, -9.8f, 0.0f);
	info.setBroadPhaseWorldSize(1000.0f);
	info.setupSolverInfo(hkpWorldCinfo::SOLVER_TYPE_8ITERS_MEDIUM);
	info.m_collisionTolerance = 0.01f; 
	info.m_simulationType = hkpWorldCinfo::SIMULATION_TYPE_CONTINUOUS;

	memoryManager = new hkPoolMemory();
	threadMemory = new hkThreadMemory (memoryManager);
	hkBaseSystem::init( memoryManager, threadMemory, errorReportFunction );
	int stackSize = 0x40000; // ensure 16 byte aligned
	char* stackBuffer = hkAllocate<char>( stackSize, HK_MEMORY_CLASS_BASE);
	hkThreadMemory::getInstance().setStackArea( stackBuffer, stackSize);

	m_world = new hkpWorld( info );
	//m_world->lock();//Locks the world in a multithreaded environment. 

	// Register the single agent required (a hkpBoxBoxAgent)
	//{
	hkpAgentRegisterUtil::registerAllAgents(m_world->getCollisionDispatcher());
	//}

	// Create the rigid bodies
	createBodies();

	// Create the ground
	createGround();

	//m_world->unlock();
}

void PhysicsHk::createGround()
{
	const hkVector4 halfExtents(20.0f, 4.0f, 20.0f);
	hkpShape* groundShape = new hkpBoxShape(halfExtents, 0.05f );

	hkpRigidBodyCinfo bodyInfo;
	bodyInfo.m_mass = 0.0f;	
	bodyInfo.m_shape = groundShape;
	bodyInfo.m_motionType = hkpMotion::MOTION_FIXED;
	bodyInfo.m_position.set(0.0f, -10.0f, 0.0f);
// 	hkVector4 axis(0.0f, 0.0f, 1.0f);
// 	bodyInfo.m_rotation = hkQuaternion(axis, 0.0f);

	hkpRigidBody* groundBody = new hkpRigidBody(bodyInfo);
	groundShape->removeReference();

	m_world->addEntity(groundBody);
	groundBody->removeReference();	
}

void PhysicsHk::createBodies()
{	
	// Each body needs at least one shape
	const hkVector4 halfExtents(1.5f, 1.5f, 1.5f);
	hkpShape* shape = new hkpBoxShape(halfExtents);//, 0.05f );

	// Compute the inertia tensor from the shape
	hkpMassProperties massProperties;
	hkpInertiaTensorComputer::computeShapeVolumeMassProperties(shape, 1.0f, massProperties);

	// Assign the rigid body properties
	hkpRigidBodyCinfo bodyInfo;
	bodyInfo.m_mass = massProperties.m_mass;
	bodyInfo.m_centerOfMass = massProperties.m_centerOfMass;
	bodyInfo.m_inertiaTensor = massProperties.m_inertiaTensor;
	bodyInfo.m_shape = shape;
	bodyInfo.m_motionType = hkpMotion::MOTION_DYNAMIC;

	for(int i = 0; i < NUM_BODIES; i++)
	{
		hkpRigidBody *body = new hkpRigidBody(bodyInfo);
		m_bodies = body;		
	}
	shape->removeReference();

}

void PhysicsHk::addBody(hkpRigidBody* currentBody)
{
	//
	// Add the current body to the simulation
	//

	
	// Zero velocites (still preserved perhaps from previous simulation)
	currentBody->setLinearVelocity(hkVector4::getZero());
	currentBody->setAngularVelocity(hkVector4::getZero());

	// Place above ground, unrotated
	hkVector4 position(0.0f, 0.0f, 0.0f);
	currentBody->setPosition(position);
	/*hkQuaternion rot;
	rot.setIdentity();
	currentBody->setRotation(rot);*/

	// Add body
	m_world->addEntity(currentBody);

}

void PhysicsHk::removeBody(hkpRigidBody* body)
{	
	//
	// Remove the current body to the simulation
	//
	m_world->removeEntity(body);	
}

hkReal *PhysicsHk::synchronousStepping()//BUG : régler le timing
{
	//m_world->lock();

	// add/remove a rigid body
	if((oneSecond % 100000) == 0)
	{
		oneSecond = 0 ;
		// remove the body if it is already in the world
		if(m_bodies[m_currentBody]->isAddedToWorld())
		{
			removeBody(m_bodies[m_currentBody]);
		}

		// add the body
		addBody(m_bodies[m_currentBody]);

		// move onto the next body
		m_currentBody = (++m_currentBody % NUM_BODIES);
	}
// 	hkTransform transformation = m_bodies[m_currentBody]->getTransform();
// 	transformation.get4x4ColumnMajor(matrixTransformHk);
// 	transformation.setInverse(transformation);
// 	transformation.get4x4ColumnMajor(matrixTransformHk);
	//m_world->unlock();

	/*m_world->stepDeltaTime(0.01f);*/
	// Integrate the world forward in time - solves all constraints, and advances all transforms to the next timestep
	m_world->integrate(0.010f);

	// Perform collision detection 
	m_world->collide();

	// Perform continuous simulation
	m_world->advanceTime();
	hkTransform transformation = m_bodies[m_currentBody]->getTransform();
	transformation.get4x4ColumnMajor(matrixTransformHk);
	oneSecond += 10 ;
	return matrixTransformHk ;
}