// Called by Physics::Server when the Level is removed from the server.
void CLevel::Deactivate()
{
	n_assert(ODEWorldID);
	n_assert(ODEDynamicSpaceID);
	n_assert(ODEStaticSpaceID);
	n_assert(ODECommonSpaceID);

	for (int i = 0; i < Shapes.Size(); i++) Shapes[i]->Detach();
	Shapes.Clear();

	for (int i = 0; i < Entities.Size(); i++) Entities[i]->OnRemovedFromLevel();
	Entities.Clear();

	// delete the Contact group for joints
	dJointGroupDestroy(ContactJointGroup);

	// shutdown ode
	dSpaceDestroy(ODEDynamicSpaceID);
	dSpaceDestroy(ODEStaticSpaceID);
	dSpaceDestroy(ODECommonSpaceID);
	dWorldDestroy(ODEWorldID);
	dCloseODE();
	ODECommonSpaceID = NULL;
	ODEDynamicSpaceID = NULL;
	ODEStaticSpaceID = NULL;
	ODEWorldID = NULL;
}

/* ------------------------
* メイン関数
------------------------ */
int main(int argc, char *argv[])
{
    /* txtデータ読み込み */
    LoadTxt("route.txt", routeX, routeY, routeZ, &lineRoute);
    LoadTxt("obstacle.txt", obstX, obstY, obstZ, &lineObst);
    /* ODEの初期化 */
    dInitODE();
    /* 描画関数の設定 */
    setDrawStuff();
    /* ワールド, スペース, 接触点グループの生成 */
    world = dWorldCreate();
    space = dHashSpaceCreate(0);
    contactgroup = dJointGroupCreate(0);
    /* 地面, 重力の生成 */
    ground = dCreatePlane(space,0,0,1,0);
    dWorldSetGravity(world, 0.0, 0.0, -9.8);
    /* CFM, ERPの設定 */
    dWorldSetCFM(world,1e-3);
    dWorldSetERP(world,0.8);
    /* 全方向移動ロボットの生成 */
    t1 = clock();
    MakeBox();
    MakeOmni();
    /* シミュレーションループ */
    dsSimulationLoop(argc,argv,640,480,&fn);
    /* 接触点グループ, スペース, ワールドの破壊, ODEの終了 */
    dJointGroupDestroy(contactgroup);
    dSpaceDestroy(space);
    dWorldDestroy(world);
    dCloseODE();
    return 0;
}

int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = "../../drawstuff/textures";

  // create world

  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dWorldSetCFM (world,1e-5);
  dWorldSetAutoDisableFlag (world,1);
  dWorldSetContactMaxCorrectingVel (world,0.1);
  dWorldSetContactSurfaceLayer (world,0.001);
  dCreatePlane (space,0,0,1,0);
  memset (obj,0,sizeof(obj));

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);

  return 0;
}

int main( int argc, char **argv )
{
	// setup pointers to drawstuff callback functions
	dsFunctions fn;
	fn.version = DS_VERSION;
	fn.start = &start;
	fn.step = &simLoop;
	fn.command = &command;
	fn.stop = 0;
	fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

	// create world
	dInitODE2( 0 );
	world = dWorldCreate();

	space = dSimpleSpaceCreate( 0 );
	contactgroup = dJointGroupCreate( 0 );
	dWorldSetGravity( world,0,0,-0.5 );
	dWorldSetCFM( world,1e-5 );
	dCreatePlane( space,0,0,1,0 );
	memset( obj,0,sizeof( obj ) );

	// run simulation
	dsSimulationLoop( argc,argv,352,288,&fn );

	dJointGroupDestroy( contactgroup );
	dSpaceDestroy( space );
	dWorldDestroy( world );
	dCloseODE();
	return 0;
}

/*******************************************************************************
Function to clean the ODE system.
*******************************************************************************/
void closeODE()
{
	dJointGroupDestroy(jointgroup);
	dJointGroupDestroy( ContactGroup );		//Remove the contact joints.
	dSpaceDestroy( Space );					//Remove the space and all of its geoms.
	dWorldDestroy( World );					//Destroy all bodies and joints (not in a group).
}

Simulation::~Simulation()
{
  for(std::list<Element*>::const_iterator iter = elements.begin(), end = elements.end(); iter != end; ++iter)
    delete *iter;

  if(contactGroup)
    dJointGroupDestroy(contactGroup);
  if(rootSpace)
    dSpaceDestroy(rootSpace);
  if(physicalWorld)
  {
#ifdef MULTI_THREADING
    dThreadingImplementationShutdownProcessing(threading);
    dThreadingThreadPoolWaitIdleState(pool);
    dThreadingFreeThreadPool(pool);
    dWorldSetStepThreadingImplementation(physicalWorld, nullptr, nullptr);
    dThreadingFreeImplementation(threading);
#endif
    dWorldDestroy(physicalWorld);
    dCloseODE();
  }

  ASSERT(simulation == this);
  simulation = 0;
}

ODEDomain::~ODEDomain()
{
    dJointGroupEmpty (contactgroup);
    dJointGroupDestroy (contactgroup);        
    
    //deleting Heightfields starting from the end
    for (int i=heightfields.size()-1; i>=0; i--)
        DeleteHeightfield(i);
    heightfields.clear();            
            
    
    //deleting trimeshes starting from the end
    for (int i=trimeshes.size()-1; i>=0; i--)
        DeleteTriMesh(i);
    trimeshes.clear();
    
    //deleting bodies starting from the end
    for (int i=bodies.size()-1; i>=0; i--)
        DeleteBody(i);
    bodies.clear();

    //deleting Kinematic_bodies starting from the end
    for (int i=kinematic_bodies.size()-1; i>=0; i--)
        DeleteKinematicBody(i);
    kinematic_bodies.clear();
    
    dSpaceDestroy (space);        
    dWorldDestroy (world);    
    dCloseODE();        
    
    printf("ODEDomain destructor\n");   
}

void destruirMundo()
{
    for(int i=0; i < 6; i++)//destruir Robos
    {
        dGeomDestroy (robot[i].box[0]);
        dGeomDestroy (robot[i].box[1]);
        dGeomDestroy (robot[i].cylinder[0]);
        dGeomDestroy (robot[i].cylinder[1]);
    }

    bola.destruir();

    for(int i=0; i < 6; i++) //destruir Campo
        dGeomDestroy(wall[i]);

    for(int i=0; i < 3; i++) //destruir Gols
    {
        dGeomDestroy(goalR[i]);
        dGeomDestroy(goalL[i]);
    }

    for(int i=0; i < 4; i++)//destruir Quinas
    {
        dGeomDestroy(triangle[i]);
    }

    dJointGroupDestroy(contactgroup);		// Destrói o grupo de juntas de contato
    dSpaceDestroy (space);					// Destrói o espaço
    dWorldDestroy (world);					// Destrói o mundo
}

	void ODESimulator::destroy()
	{
		// These temporary copies are necessary because the
		// ODESimulator::~ODESimulator call (due to "delete this") will
		// invalidate the data members.
		dSpaceID rootSpaceID = mRootSpaceID;
		dWorldID worldID = mWorldID;
		dJointGroupID contactJointGroupID = mContactJointGroupID;

		delete this;

		// The following must occur after Simulator::~Simulator() is called;
		// otherwise, Simulator::~Simulator() will try to destroy Solids after
		// ODE has closed.
		dSpaceDestroy(rootSpaceID);
		dWorldDestroy(worldID);
		dJointGroupDestroy(contactJointGroupID);

		// We can only close ODE once.
		--mInitCounter;
		if (0 == mInitCounter)
		{
			dCloseODE();
		}
	}

void PhysWorld::DeInitialize()
{
    dSpaceDestroy(mSpace);
    dWorldDestroy(mWorld);
    dCloseODE();
	isInitialized = false;
}

int main (int argc, char **argv)
{
	doFast = true;
	
	// setup pointers to drawstuff callback functions
	dsFunctions fn;
	fn.version = DS_VERSION;
	fn.start = &start;
	fn.step = &simLoop;
	fn.command = &command;
	fn.stop = 0;
	fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
	
	dInitODE2(0);

	bodies = 0;
	joints = 0;
	boxes = 0;
	spheres = 0;
	
	resetSimulation();
	
	// run simulation
	dsSimulationLoop (argc,argv,352,288,&fn);
	
	dJointGroupDestroy (contactgroup);
	dSpaceDestroy (space);
	dWorldDestroy (world);
	dCloseODE();
	return 0;
}

PWorld::~PWorld()
{
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
}

void physics_quit (void)
{
	printlog(1, "Quit physics");
	dJointGroupDestroy (contactgroup);
	dSpaceDestroy (space);
	dWorldDestroy (world);
	dCloseODE();
}

void Simulator_Destroy(void) {

	dGeomDestroy(ground);
	dJointGroupDestroy (contactgroup);
	dSpaceDestroy (space);
	dWorldDestroy (world);
	dCloseODE();
}

void CPHShell::Deactivate(){

#ifdef ANIMATED_PHYSICS_OBJECT_SUPPORT
	if (m_pPhysicsShellAnimatorC)
	{
		xr_delete<CPhysicsShellAnimator>(m_pPhysicsShellAnimatorC); 
	}
#endif

	if(!isActive())return;
	R_ASSERT2(!ph_world->Processing(),"can not deactivate physics shell during physics processing!!!");
	R_ASSERT2(!ph_world->IsFreezed(),"can not deactivate physics shell when ph world is freezed!!!");
	R_ASSERT2(!CPHObject::IsFreezed(),"can not deactivate freezed !!!");
	ZeroCallbacks();
	VERIFY(ph_world&&ph_world->Exist());
	if(isFullActive())
	{
		vis_update_deactivate();
		CPHObject::activate();
		ph_world->Freeze();
		CPHObject::UnFreeze();
		ph_world->StepTouch();
		ph_world->UnFreeze();
		//Fmatrix m;
		//InterpolateGlobalTransform(&m);
	}
	spatial_unregister();
	
	vis_update_activate();
	//if(ref_object && !CPHObject::is_active() && m_active_count == 0)
	//{
	//	ref_object->processing_activate();
	//}
	DisableObject();
	CPHObject::remove_from_recently_deactivated();
	

	ELEMENT_I i;
	for(i=elements.begin();elements.end() != i;++i)
		(*i)->Deactivate();

	JOINT_I j;
	for(j=joints.begin();joints.end() != j;++j)
		(*j)->Deactivate();

	

	if(m_space) {
		dSpaceDestroy(m_space);
		m_space=NULL;
	}
	//bActive=false;
	//bActivating=false;
	m_flags.set(flActivating,FALSE);
	m_flags.set(flActive,FALSE);
	m_traced_geoms.clear();
	CPHObject::UnsetRayMotions();
}

WorldManagerServer::~WorldManagerServer()
{
	dSpaceDestroy(mStaticSpace);

	dJointGroupEmpty(mContactGroup);
	dJointGroupDestroy(mContactGroup);
	dWorldDestroy(mWorld);
	dCloseODE();
}

HarrierSim::~HarrierSim()
{
  dSpaceDestroy(space);
  dWorldDestroy(world);

  delete vp;
  delete itsWorldDisp;
  pthread_mutex_destroy(&itsDispLock);
}

int main (int argc, char **argv)
{
    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.command = &command;
    fn.stop = 0;
    fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

    // create world
    dInitODE();
    world = dWorldCreate();

#if 1
    space = dHashSpaceCreate (0);
#elif 0
    dVector3 center = {0,0,0}, extents = { 100, 100, 100};
    space = dQuadTreeSpaceCreate(0, center, extents, 5);
#elif 0
    space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
#else
    space = dSimpleSpaceCreate(0);
#endif

    contactgroup = dJointGroupCreate (0);
    dWorldSetGravity (world,0,0,-0.5);
    dWorldSetCFM (world,1e-5);
    
    dWorldSetLinearDamping(world, 0.00001);
    dWorldSetAngularDamping(world, 0.005);
    dWorldSetMaxAngularSpeed(world, 200);

    dWorldSetContactSurfaceLayer (world,0.001);
    ground = dCreatePlane (space,0,0,1,0);
    
    memset (obj,0,sizeof(obj));

    // create lift platform
    platform = dCreateBox(space, 4, 4, 1);

    dGeomSetCategoryBits(ground, 1ul);
    dGeomSetCategoryBits(platform, 2ul);
    dGeomSetCollideBits(ground, ~2ul);
    dGeomSetCollideBits(platform, ~1ul);

    // run simulation
    dsSimulationLoop (argc,argv,352,288,&fn);

    dJointGroupDestroy (contactgroup);
    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();
    return 0;
}

int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
  if(argc==2)
    {
        fn.path_to_textures = argv[1];
    }

  // create world
  dInitODE2(0);
  world = dWorldCreate();
 
  space = dSimpleSpaceCreate(0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dWorldSetCFM (world,1e-5);
  dCreatePlane (space,0,0,1,0);
  memset (obj,0,sizeof(obj));

  // note: can't share tridata if intending to trimesh-trimesh collide
  TriData1 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  TriData2 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  
  TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0);
  TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0);
  dGeomSetData(TriMesh1, TriData1);
  dGeomSetData(TriMesh2, TriData2);
  
  {dGeomSetPosition(TriMesh1, 0, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh1, Rotation);}

  {dGeomSetPosition(TriMesh2, 1, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh2, Rotation);}
  
  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}

void endWorldModelling()
{
    dJointGroupDestroy (contactgroup);
    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();

    printf("God knows his rules and he has determined that this world must be terminated.\n");
    printf("The World has been terminated.\n");
}