//************************************************************************
// ODE
//************************************************************************

// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void nearCallbackopl (void *data, dGeomID o1, dGeomID o2)
{
  int i,n;

  // only collide things with the ground
//  int g1 = (o1 == ground || o1 == wall);
//  int g2 = (o2 == ground || o2 == wall);
//  if (!(g1 ^ g2)) return;

  const int N = 10;
  dContact contact[N];

  n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));

  if (n > 0) {
    for (i=0; i<n; i++) 
	{
      contact[i].surface.mode = dContactBounce | dContactSoftCFM;
      contact[i].surface.mu = dInfinity;
      contact[i].surface.slip1 = 0.1;
      contact[i].surface.slip2 = 0.1;
      contact[i].surface.soft_erp = 0.5;
      contact[i].surface.soft_cfm = 0.3;
      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
      dJointAttach (c,dGeomGetBody(contact[i].geom.g1),dGeomGetBody(contact[i].geom.g2));
    }
  }
}

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i;
  // if (o1->body && o2->body) return;

  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;

  dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box
  for (i=0; i<MAX_CONTACTS; i++) {
    contact[i].surface.mode = dContactBounce | dContactSoftCFM;
    contact[i].surface.mu = dInfinity;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = 0.1;
    contact[i].surface.bounce_vel = 0.1;
    contact[i].surface.soft_cfm = 0.01;
  }
  if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,sizeof(dContact))) 
  {
    dMatrix3 RI;
    dRSetIdentity (RI);
    const dReal ss[3] = {0.02,0.02,0.02};
    for (i=0; i<numc; i++) {
      dJointID c = dJointCreateContact (world,contactgroup,contact+i);
      dJointAttach (c,b1,b2);
      //if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
    }
  }
}
//************************************************************************

void initOde()
{
	// Create world

	int i;
	dMass m;
	// create world

	world = dWorldCreate();
	space = dHashSpaceCreate (0);
	dWorldSetCFM (world,1e-5);
	dWorldSetContactMaxCorrectingVel (world,1.1);
	//dWorldSetContactSurfaceLayer (world,0.001);
	//dWorldSetQuickStepNumIterations (world,30);//20 is def.

	contactgroup = dJointGroupCreate (0);
	dWorldSetGravity (world,0,0,sGravity);
	ground = dCreatePlane (space,0,0,1,0);
	//dWorldSetAutoDisableFlag (world, 1);

	//  dWorldSetAutoDisableAngularThreshold (world, .101);
	//  dWorldSetAutoDisableLinearThreshold (world, 1.1);
	//  dWorldSetAutoDisableSteps (world, 10);

	//mi Box'es, pillars!
  
	double sides[3] = {2,2,2};
	cntBox=0;
	int rx=5,ry=3;

	while (cntBox<nPillars)
	{
		miBox[cntBox].body = dBodyCreate (world);
		dBodySetPosition (miBox[cntBox].body,rx,ry,STARTZ); //<--POSITION
		dMassSetBox(&m,.01,sides[0],sides[1], sides[2]);
		dMassAdjust (&m,0.01);
		dBodySetMass (miBox[cntBox].body,&m);
			miBox[cntBox].geom =  dCreateBox(space,sides[0],sides[1],sides[2]);
			dGeomSetBody (miBox[cntBox].geom,miBox[cntBox].body);
		dBodySetAutoDisableFlag(miBox[cntBox].body, 1);

		ry=ry+2;
		if (ry>2){ry=3;rx=rx+2;}
		cntBox++;
	}

	//The box! the controllabe box
	  sides[0] = 2;sides[1] = 2;sides[2] = 0.2;
  	  miChair.body = dBodyCreate (world);
	  dBodySetPosition (miChair.body,0,0,STARTZ+100);
	  dMassSetBox(&m,0.5,sides[0],sides[1], sides[2]);
	  dMassAdjust (&m,0.5);
	  dBodySetMass (miChair.body,&m);
		miChair.geom =  dCreateBox(space,sides[0],sides[1],sides[2]);
		dGeomSetBody (miChair.geom,miChair.body);

	  // A sphere ball
	  int mySphereRADIUS = 5;
  	  mySphere[0].body = dBodyCreate (world);
	  dBodySetPosition (mySphere[0].body,10,-20,15);
	  dMassSetSphere(&m,5.0,mySphereRADIUS);
	  dMassAdjust (&m,5.0);
	  dBodySetMass (mySphere[0].body,&m);
	  mySphere[0].geom = dCreateSphere(0, mySphereRADIUS);
	  dGeomSetBody (mySphere[0].geom,mySphere[0].body);
	  dSpaceAdd (space, mySphere[0].geom);

   //////////////////////////////

   // environment , the walls and roof, floor
/*	wall[0] = dCreateBox (space,50,1,12);
		dGeomSetPosition (wall[0],23,33,6);
	wall[1] = dCreateBox (space,50,1,12);
		dGeomSetPosition (wall[1],23,-13,6);
	wall[2] = dCreateBox (space,1,46,12);
		dGeomSetPosition (wall[2],48.2,10,6);
	wall[3] = dCreateBox (space,1,46,12);
		dGeomSetPosition (wall[3],-2,10,6);
	wall[4] = dCreateBox (space,50,46,1);//top
		dGeomSetPosition (wall[4],23,10,-.2);

	wall[5] = dCreateBox (space,50,46,1);//bottom
		dGeomSetPosition (wall[5],23,10,-1);
*/	


	//Billboards
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,0, 1.5, 10, 3.14);

	miBB[0].geom=dCreateBox(space,10,0.5,10);
	dGeomSetPosition (miBB[0].geom,0,-10,2);
	dGeomSetRotation (miBB[0].geom,Rtx);
}

//***********************************************************************

void endOde()
{
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
}
//**********************************************************************************


//**********************************************************************************
// simulation loop

static void simLoop (int pause)
{

    dSpaceCollide (space,0,&nearCallback);
//    dWorldStep (world,stepSz); //def .05
    dWorldQuickStep (world,stepSz); //def .05

    // remove all contact joints
    dJointGroupEmpty (contactgroup);
}
