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

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

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

  // only collide things with the ground
  //int g1 = (o1 == ground || o1 == ground_box);
  //int g2 = (o2 == ground || o2 == ground_box);
  //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 = dContactSlip1 | dContactSlip2 |
	dContactSoftERP | dContactSoftCFM | dContactApprox1;
      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));
    }
  }
}
//************************************************************************

void initBuggy()
{

  int i;
  dMass m;

	// chassis body
  body[0] = dBodyCreate (world);
  dBodySetPosition (body[0],0,0,STARTZ);
  dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
  dMassAdjust (&m,CMASS);
  dBodySetMass (body[0],&m);
  box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
  dGeomSetBody (box[0],body[0]);

  // wheel bodies
  for (i=1; i<=3; i++) {
    body[i] = dBodyCreate (world);
    dQuaternion q;
    dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
    dBodySetQuaternion (body[i],q);
    dMassSetSphere (&m,1,RADIUS);
    dMassAdjust (&m,WMASS);
    dBodySetMass (body[i],&m);
    sphere[i-1] = dCreateSphere (0,RADIUS);
    dGeomSetBody (sphere[i-1],body[i]);
  }
  dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5);
  dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5);
  dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5);

  // front wheel hinge
  /* */
  joint[0] = dJointCreateHinge2 (world,0);
  dJointAttach (joint[0],body[0],body[1]);
  const dReal *a = dBodyGetPosition (body[1]);
  dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
  dJointSetHinge2Axis1 (joint[0],0,0,1);
  dJointSetHinge2Axis2 (joint[0],0,1,0);
 

  // front and back wheel hinges
  for (i=0; i<3; i++) {
    joint[i] = dJointCreateHinge2 (world,0);
    dJointAttach (joint[i],body[0],body[i+1]);
    const dReal *a = dBodyGetPosition (body[i+1]);
    dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
    dJointSetHinge2Axis1 (joint[i],0,0,1);
    dJointSetHinge2Axis2 (joint[i],0,1,0);
  }

  // set joint suspension
  for (i=0; i<3; i++) {
    dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
    dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
  }

  // lock back wheels along the steering axis
  for (i=1; i<3; i++) {
    // set stops to make sure wheels always stay in alignment
    dJointSetHinge2Param (joint[i],dParamLoStop,0);
    dJointSetHinge2Param (joint[i],dParamHiStop,0);
    // the following alternative method is no good as the wheels may get out
    // of alignment:
    //   dJointSetHinge2Param (joint[i],dParamVel,0);
    //   dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
  }

  // create car space and add it to the top level space
  car_space = dSimpleSpaceCreate (space);
  dSpaceSetCleanup (car_space,0);
  dSpaceAdd (car_space,box[0]);
  dSpaceAdd (car_space,sphere[0]);
  dSpaceAdd (car_space,sphere[1]);
  dSpaceAdd (car_space,sphere[2]);

}

void initOde()
{
	// Create world

	int i;
	dMass m;
	// create world
	dInitODE();
	world = dWorldCreate();
	space = dHashSpaceCreate (0);
	//dWorldSetCFM (world,1e-5);
	//dWorldSetContactMaxCorrectingVel (world,1.1);
	//dWorldSetContactSurfaceLayer (world,0.001);
	//dWorldSetQuickStepNumIterations (world,30);//20 is def.
	sGravity=-.5;
	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!
  
	initBuggy();

	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,10,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 = 1;
  	  mySphere[0].body = dBodyCreate (world);
	  dBodySetPosition (mySphere[0].body,10,-20,15);
	  dMassSetSphere(&m,1.0,mySphereRADIUS);
	  dMassAdjust (&m,1.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,0,-0.15);

	miBB[0].geom=dCreateBox(space,2,1.5,1);
	dGeomSetPosition (miBB[0].geom,2,0,-0.34);
	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

    // motor
    dJointSetHinge2Param (joint[0],dParamVel2,-speed);
    dJointSetHinge2Param (joint[0],dParamFMax2,0.1);

    // steering
    dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
    if (v > 0.1) v = 0.1;
    if (v < -0.1) v = -0.1;
    v *= 10.0;
    dJointSetHinge2Param (joint[0],dParamVel,v);
    dJointSetHinge2Param (joint[0],dParamFMax,0.2);
    dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
    dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
    dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);

    dSpaceCollide (space,0,&nearCallback);
    dWorldStep (world,0.05);

    // remove all contact joints
    dJointGroupEmpty (contactgroup);

}
