// 1. Create a dynamics world.
World = dWorldCreate();
dWorldSetGravity (World, 0.0, 0.0, -9.8);
dWorldSetERP (World, ERP);

// 2. Create bodies in the dynamics world.
Box = dBodyCreate (World);
for (int i = 0; i < NUM_SEGMENTS; ++i)
  Snake[i] = dBodyCreate (World);

// 3. Set the state (position etc) of all bodies.
dMass mass;

dBodySetPosition (Box, 0.0, 0.0, BOX_SIDE/2.0);
dMassSetBoxTotal (&mass, BOX_MASS, BOX_SIDE, BOX_SIDE, BOX_SIDE);
dBodySetMass (Box, &mass);

dMassSetSphereTotal (&mass, SPHERE_MASS, SNAKE_RADIUS);
for (int i = 0; i < NUM_SEGMENTS; ++i)
{
  dBodySetPosition (Snake[i], SmallRand(), SmallRand(), 
		    5.0 + 2*SNAKE_RADIUS * i);
  dBodySetMass (Snake[i], &mass);
}

// 4. Create joints in the dynamics world.
for (int i = 0; i < NUM_SEGMENTS - 1; ++i)
  Joints[i] = dJointCreateUniversal (World, 0); // 0 is "joint group"

// 5. Attach the joints to the bodies.
for (int i = 0; i < NUM_SEGMENTS - 1; ++i)
  dJointAttach (Joints[i], Snake[i], Snake[i+1]);

// 6. Set the parameters of all joints.
for (int i = 0; i < NUM_SEGMENTS - 1; ++i)
{
  dJointSetUniversalAnchor (Joints[i], 0.0, 0.0, 5.0 + SNAKE_RADIUS + 2*i*SNAKE_RADIUS);
  dJointSetUniversalAxis1 (Joints[i], 0.0, 1.0, 0.0);
  dJointSetUniversalAxis2 (Joints[i], 1.0, 0.0, 0.0);
  dJointSetUniversalParam (Joints[i], dParamCFM, CFM);
}

// 7. Create a collision world and collision geometry objects, as necessary.
Space = dSimpleSpaceCreate(0);

BoxGeom = dCreateBox (Space, 2.0, 2.0, 2.0);
dGeomSetPosition (BoxGeom, 0.0, 0.0, 1.0);
dGeomSetBody (BoxGeom, Box);

for (int i = 0; i < NUM_SEGMENTS; ++i)
{
  SnakeGeom[i] = dCreateSphere (Space, SNAKE_RADIUS);
  dGeomSetPosition (SnakeGeom[i], 0.0, 0.0, 5.0 + 1.2*i);
  dGeomSetBody (SnakeGeom[i], Snake[i]);
}

GroundGeom = dCreatePlane (Space, 0.0, 0.0, 1.0, 0.0);

// 8. Create a joint group to hold the contact joints.
ContactGroup = dJointGroupCreate(0); // param unused; kept for backward compatibility

// Create a Producer-based viewer
osg::ref_ptr<ODEController> eh (osg::ref_ptr<ODEController>(new ODEController));
osgProducer::Viewer viewer;
viewer.setUpViewer (osgProducer::Viewer::STANDARD_SETTINGS);

viewer.getEventHandlerList().push_front (eh.get());

CreateOSGWorld();
viewer.setSceneData (SGRoot.get());
UpdateOSGWorld();

osg::Timer_t prevTime = osg::Timer::instance()->tick();
viewer.realize();

// 9. Loop:
while (!viewer.done())
{
  viewer.sync();
  viewer.update();
  viewer.frame();

  const double MAX_STEP = 0.01;
  const osg::Timer_t now = osg::Timer::instance()->tick();
  double deltaSecs = osg::Timer::instance()->delta_s (prevTime, now);
  prevTime = now;

  while (deltaSecs > 0.0)
    {
      // 9.1. Apply forces to the bodies as necessary.
      // (forces are valid for one time step only, so this must be done inside
      // this inner loop that keeps the time steps short)
      if (eh->isForceOn())
	dBodyAddForce (Box, 0.0, 0.0, 800.0);

      // 9.2. Adjust the joint parameters as necessary.
      // (nothing necessary here)

      // 9.3. Call collision detection.
      dSpaceCollide (Space, 0, NearCallback);

      // 9.4. Create a contact joint for every collision point, and put it in
      //      the contact joint group.
      // See 'NearCallback()', please.

      // 9.5. Take a simulation step
      const double step = std::min (MAX_STEP, deltaSecs);
      deltaSecs -= MAX_STEP;

      dWorldStep (World, step);
      // Alternatively, you may wish to try this:
      // dWorldQuickStep (World, step);

      // 9.6. Remove all joints in the contact joint group.
      dJointGroupEmpty (ContactGroup);
    }

  UpdateOSGWorld();
}

// 10. Destroy the dynamics and collision worlds.
dSpaceDestroy (Space);
dWorldDestroy (World);

viewer.sync();

