void nearCallback(void *data, dGeomID o1, dGeomID o2) {
    State* state = (State*)data;

    if(dGeomIsSpace(o1) || dGeomIsSpace(o2)) {
        dSpaceCollide2(o1, o2, data, &nearCallback);

        if(dGeomIsSpace(o1))
            dSpaceCollide((dSpaceID)o1, data, &nearCallback);
        if(dGeomIsSpace(o2))
            dSpaceCollide((dSpaceID)o2, data, &nearCallback);
    } else {
        dBodyID b1 = dGeomGetBody(o1);
        dBodyID b2 = dGeomGetBody(o2);

        const int MAX_CONTACTS = 18;
        dContact contact[MAX_CONTACTS];

        for(int i = 0; i < MAX_CONTACTS; i++) {
            contact[i].surface.mode = dContactBounce;
            contact[i].surface.mu = 2000;
            contact[i].surface.bounce = 0.1;
            contact[i].surface.bounce_vel = 0.15;
        }

        if(int numc = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact))) {
            for(int i = 0; i < numc; i++) {
                dJointID c = dJointCreateContact(state->world, state->physicsContactgroup, &contact[i]);
                dJointAttach(c, b1, b2);
            }
        }
    }
}

void simLoop (int pause)
{
    if (!pause) {

        const dReal step = 0.02;
        const unsigned nsteps = 1;

        for (unsigned i=0; i<nsteps; ++i) {
            dSpaceCollide(space, 0, nearCallback);
            dWorldQuickStep(world, step);
            dJointGroupEmpty(contact_group);
        }
    } else {
        dSpaceCollide(space, 0, nearCallback);
        dJointGroupEmpty(contact_group);
    }
    
    // now we draw everything
    unsigned ngeoms = dSpaceGetNumGeoms(space);
    for (unsigned i=0; i<ngeoms; ++i) {
        dGeomID g = dSpaceGetGeom(space, i);

        if (g == ground)
            continue; // drawstuff is already drawing it for us

        drawGeom(g);
    }
    
    if (dBodyGetPosition(ball1_body)[0] < -track_len)
        resetSim();
}

void CPhysicManager::Update(float tms){

        calc_time+=tms;

//		printf("update %f\n",calc_time);

//		float full_time = tms;
//		float dt=1.0/30.0f;
        //if ( dt > tms ) dt = tms;

if (calc_time>10) {
        //fast!!
		dSpaceCollide (space,0,&nearCallback);
		dWorldQuickStep(world, tms );
		dJointGroupEmpty(contactgroup);
        calc_time=0.0f;
}
else
		while (calc_time>1.0/30.0f) {

		dSpaceCollide (space,0,&nearCallback);
		dWorldQuickStep(world, (1.0/30.0f) );
		dJointGroupEmpty(contactgroup);
        calc_time-=1.0/30.0f;

		};

};

static void simLoop (int pause)
{
  dSpaceCollide (space,0,&nearCallback);
  if (!pause)
  {
    dWorldQuickStep (world, 0.01); // 100 Hz
  }
  dJointGroupEmpty (contactgroup);

  dsSetColorAlpha (1,1,0,0.5);

  const dReal *CPos = dBodyGetPosition(cylbody);
  const dReal *CRot = dBodyGetRotation(cylbody);
  float cpos[3] = {CPos[0], CPos[1], CPos[2]};
  float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
  dsDrawCylinder
  (
    cpos,
    crot,
    CYLLENGTH,
    CYLRADIUS
  ); // single precision

  const dReal *SPos = dBodyGetPosition(sphbody);
  const dReal *SRot = dBodyGetRotation(sphbody);
  float spos[3] = {SPos[0], SPos[1], SPos[2]};
  float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
  dsDrawSphere
  (
    spos,
    srot,
    SPHERERADIUS
  ); // single precision
}

static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldQuickStep (world,0.05);

  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }
  
  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0.8,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}

static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldQuickStep (world,0.02);

  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }


  if (doFeedback)
  {
    if (fbnum>MAX_FEEDBACKNUM)
      printf("joint feedback buffer overflow!\n");
    else
    {
      dVector3 sum = {0, 0, 0};
      printf("\n");
      for (int i=0; i<fbnum; i++) {
        dReal* f = feedbacks[i].first?feedbacks[i].fb.f1:feedbacks[i].fb.f2;
        printf("%f %f %f\n", f[0], f[1], f[2]);
        sum[0] += f[0];
        sum[1] += f[1];
        sum[2] += f[2];
      }
      printf("Sum: %f %f %f\n", sum[0], sum[1], sum[2]);
      dMass m;
      dBodyGetMass(obj[selected].body, &m);
      printf("Object G=%f\n", GRAVITY*m.mass);
    }
    doFeedback = 0;
    fbnum = 0;
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0.8,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}

static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  //if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldStepFast (world,0.05, 1);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}

// simulation loop
static void simLoop (int pause)
{
    const dReal *pos;
    const dReal *R;
    // force for the spheres
  
    // find collisions and add contact joints
    dSpaceCollide (space,0,&nearCallback);
    // step the simulation
    dWorldQuickStep (world,0.01);  
    // remove all contact joints
    dJointGroupEmpty (contactgroup);
    // redraw sphere at new location
    pos = dGeomGetPosition (sphere0_geom);
    R = dGeomGetRotation (sphere0_geom);
    dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere0_geom));
     
    pos = dGeomGetPosition (sphere1_geom);
    R = dGeomGetRotation (sphere1_geom);
    dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere1_geom));

    pos = dGeomGetPosition (sphere2_geom);
    R = dGeomGetRotation (sphere2_geom);
    dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere2_geom));
}

//"Pause" key pauses this loop, and any other key resumes it
static void simLoop (int pause)
{
    static int numSimSteps = 0;                 // Number of steps
    static int jointAngleStep = 0;              //

    /* control(): Assign the angle to the joint: target_angleR/L[i] to jointR/L[i]     */
    control();

    if (!pause)					                // If “pause” key is not pressed
    {
        dSpaceCollide(space,0,&nearCallback);	// Detect colision: nearCallback
        dWorldStep(world,SIM_STEP);             // Step of simulation
        dJointGroupEmpty(contactgroup);	        // Clear container of collision points
        numSimSteps++;                                // Increases number of steps
        //feedback = dJointGetFeedback(joint1); // Get joint1 feedback

        // Assign the precomputed angles to the joints
        if(numSimSteps%50 == 0 && jointAngleStep < TLENG)
        {
            for(int i=0;i<MAX_JOINTS;i++)
            {
                target_angleR[i] = PreComputedAngleR[jointAngleStep][i];
                target_angleL[i] = PreComputedAngleL[jointAngleStep][i];
            }
            jointAngleStep++;
        }
    }

    Model::rDrawRobot();				                // Draw robot with previously specified joint angles (in control() )
}

/***********************************************************
	Process function
***********************************************************/
void ODEPhysicHandler::Process()
{
	double currentime = SynchronizedTimeHandler::getInstance()->GetCurrentTimeDouble();
	float tdiff = (float)(currentime - _lasttime);


	static float nbSecondsByStep = 0.001f;

	// Find the time elapsed between last time
	float nbSecsElapsed = tdiff/1000.0f;

	// Find the corresponding number of steps that must be taken
	int nbStepsToPerform = static_cast<int>(nbSecsElapsed/nbSecondsByStep);

	// Make these steps to advance world time
	for (int i=0;i<nbStepsToPerform;++i)
	{
		// Detect collision
		dSpaceCollide(_space, this, &nearCallback);

		// Step world
		dWorldQuickStep(_world, nbSecondsByStep);

		// Remove all temporary collision joints now that the world has been stepped
		dJointGroupEmpty(_contactgroup); 
	}

	_lasttime = currentime;
}

void ompl::control::OpenDEStatePropagator::propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
{
    env_->mutex_.lock();

    // place the OpenDE world at the start state
    si_->getStateSpace()->as<OpenDEStateSpace>()->writeState(state);

    // apply the controls
    env_->applyControl(control->as<RealVectorControlSpace::ControlType>()->values);

    // created contacts as needed
    CallbackParam cp = { env_.get(), false };
    for (unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
        dSpaceCollide(env_->collisionSpaces_[i],  &cp, &nearCallback);

    // propagate one step forward
    dWorldQuickStep(env_->world_, (const dReal)duration);

    // remove created contacts
    dJointGroupEmpty(env_->contactGroup_);

    // read the final state from the OpenDE world
    si_->getStateSpace()->as<OpenDEStateSpace>()->readState(result);

    env_->mutex_.unlock();

    // update the collision flag for the start state, if needed
    if (!(state->as<OpenDEStateSpace::StateType>()->collision & (1 << OpenDEStateSpace::STATE_COLLISION_KNOWN_BIT)))
    {
        if (cp.collision)
            state->as<OpenDEStateSpace::StateType>()->collision &= (1 << OpenDEStateSpace::STATE_COLLISION_VALUE_BIT);
        state->as<OpenDEStateSpace::StateType>()->collision &= (1 << OpenDEStateSpace::STATE_COLLISION_KNOWN_BIT);
    }
}

bool Simulator::SimulateOneStep(const double speed, const double steer)
{
//set desired speed    
    dJointSetHinge2Param(m_robotJoints[0], dParamVel2, -speed); // hinge-2 velocity = -speed
    dJointSetHinge2Param(m_robotJoints[0], dParamFMax2, 10);   // maximum torque = 0.1
    
//set desired steering angle
    dReal v = steer - dJointGetHinge2Angle1 (m_robotJoints[0]);
    //if (v > 0.1) v = 0.1;
    //if (v < -0.1) v = -0.1;
    v *= 10.0;
    dJointSetHinge2Param(m_robotJoints[0], dParamVel, v);
    dJointSetHinge2Param(m_robotJoints[0], dParamFMax, 0.2);
    dJointSetHinge2Param(m_robotJoints[0], dParamLoStop, -0.75);
    dJointSetHinge2Param(m_robotJoints[0], dParamHiStop, 0.75);
    dJointSetHinge2Param(m_robotJoints[0], dParamFudgeFactor, 0.1);
    
//simulate the dynamics for one time step (set to 0.05)
    m_collision = false;

    dSpaceCollide(m_space, reinterpret_cast<void *>(this), &CollisionCheckingCallbackFn);
    dWorldStep(m_world, 0.2); // setting time step for integral
    dJointGroupEmpty(m_contacts);

    return !m_collision;    
}

// シミュレーションループ
static void simLoop(int pause) {
  control();
  dSpaceCollide(space,0,&nearCallback);  // 衝突検出計算
  dWorldStep(world,0.0001);                // 1ステップ進める
  dJointGroupEmpty(contactgroup);        // 衝突変数をリセット
  draw();  // ロボットの描画
}

void avancarPasso(dReal dt)
{
    dWorldSetQuickStepNumIterations (world, 20);
    dSpaceCollide(space,0,&nearCallback);
    dWorldQuickStep(world, dt);		// Esse valor deve ser sincronizado com as chamadas da função.
    dJointGroupEmpty(contactgroup);
}

void Simulate(int pause) {

	if ( showGraphics )
		envs->Draw();

	if ( envs->recordingVideo )

		envs->Video_Record();

	if ( !pause ) {

		dSpaceCollide (space,0,&nearCallback);
		dWorldStep (world,STEP_SIZE);
		dJointGroupEmpty(contactgroup);

		if ( envs->In_Evolution_Mode() )

			envs->Evolve(		world,
						space);

		else if ( envs->In_Champ_Mode() )

			envs->Show_Champ(	world, 
						space);

	}
}

static void simLoop (int pause)
{
  int i,j;

  for (i=0; i < NUM; i++) {
    for (j=0; j < NUM; j++) test_matrix[i][j] = 0;
  }
  dSpaceCollide (space,0,&nearCallback);
  for (i=0; i < NUM; i++) {
    for (j=i+1; j < NUM; j++) {
      if (good_matrix[i][j] && !test_matrix[i][j]) {
	printf ("failed to report collision (%d,%d) (seed=%ld)\n",i,j,seed);
      }
    }
  }

  seed++;
  init_test();

  for (i=0; i<NUM; i++) {
    dVector3 pos,side;
    dMatrix3 R;
    dRSetIdentity (R);
    for (j=0; j<3; j++) pos[j] = (bounds[i][j*2+1] + bounds[i][j*2]) * 0.5;
    for (j=0; j<3; j++) side[j] = bounds[i][j*2+1] - bounds[i][j*2];
    if (hits[i] > 0) dsSetColor (1,0,0);
    else dsSetColor (1,1,0);
    dsDrawBox (pos,R,side);
  }
}

int physics_step(float dt)
{
    dSpaceCollide(space, 0, callback);
    dWorldQuickStep(world, dt);
    dJointGroupEmpty(group);

    return 1;
}

static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldStepFast (world,0.05, 1);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }

  /*{
    for (int i = 1; i < IndexCount; i++) {
      dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
    }
  }*/

  {const dReal* Pos = dGeomGetPosition(TriMesh);
  const dReal* Rot = dGeomGetRotation(TriMesh);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
	const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
	const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
	dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
  }}}

  if (Ray){
	  dVector3 Origin, Direction;
	  dGeomRayGet(Ray, Origin, Direction);
	  
	  dReal Length = dGeomRayGetLength(Ray);
	  
	  dVector3 End;
	  End[0] = Origin[0] + (Direction[0] * Length);
	  End[1] = Origin[1] + (Direction[1] * Length);
	  End[2] = Origin[2] + (Direction[2] * Length);
	  End[3] = Origin[3] + (Direction[3] * Length);
	  
	  dsDrawLine(Origin, End);
  }
}

void PhysicsSpace::Collide( PhysicsWorld* const w )
{
    _CollideWorldID = w->getWorldID();

    
    //free contact Joints
    dJointGroupEmpty(_ColJointGroupId);

	dSpaceCollide(_SpaceID, reinterpret_cast<void *>(this), &PhysicsSpace::collisionCallback);
}

void simStep()
{
    if (loading == 0)
    { 
        dSpaceCollide (space,0,&nearCallback);
        dWorldStepFast1 (ode_world,0.05, 5);
        for (int j = 0; j < dSpaceGetNumGeoms(space); j++){
            dSpaceGetGeom(space, j);
        }
        dJointGroupEmpty (contactgroup);
    }
}