/********************************************************************/
/*
Modified from drop.cpp CGA test program.
Two links hit the floor.
We are going to implement our own collision detection and 
   spring-damper forces.
*/
/********************************************************************/
/* Include ODE and graphics stuff */

#include "ode/ode.h" /* Dynamics stuff */
#include "drawstuff/drawstuff.h" /* Graphics stuff */
#include "ode-mumble.h" /* CGA stuff to make things clearer */

/********************************************************************/
/********************************************************************/
/* Defines (constants) */

#define MAX_N_BODIES 100 // maximum number of bodies 
#define MAX_N_JOINTS 100 // maximum number of joints

// some constants
#define CALF_L_LENGTH (0.5f)	// length of link
#define CALF_L_WIDTH (0.1f)    // width of link
#define CALF_L_MASS (0.5f)     // mass of link

#define THIGH_L_LENGTH (0.5f)	// length of link
#define THIGH_L_WIDTH (0.1f)    // width of link
#define THIGH_L_MASS (0.5f)     // mass of link

#define TIMESTEP 0.01

// add delay so simulation runs at correct speed.
// adjust this for your computer.
#define DELAY_AMOUNT 10000000.0

#define XX 0
#define YY 1
#define ZZ 2

#define CALF_L 0
#define THIGH_L 1

#define KNEE_L 0

/********************************************************************/
/* Globals */

// ODE objects.
static dWorldID world;         // the simulation object
static dBodyID body[MAX_N_BODIES]; // body IDs get stashed here
static dJointID joint[MAX_N_JOINTS]; // joint IDs get stashed here

dReal time = 0.0; // keep track of how much time has passed.

/********************************************************************/
// start(): set things up

static void start()
{
  dMass calf_l_m, thigh_l_m;

  // create world
  world = dWorldCreate();
  dWorldSetGravity ( world, 0, 0, -9.81 ); // gravity is down along Z axis

  /* set up mass, cm location, and moment of inertia for body 0 */
  dMassSetBox (&calf_l_m,1,CALF_L_WIDTH,CALF_L_WIDTH,CALF_L_LENGTH); // make it a box
  dMassAdjust (&calf_l_m,CALF_L_MASS); // set total mass
  dMassSetBox (&thigh_l_m,1,THIGH_L_WIDTH,THIGH_L_WIDTH,THIGH_L_LENGTH); // make it a box
  dMassAdjust (&thigh_l_m,THIGH_L_MASS); // set total mass

  // set up bodies
  body[CALF_L] = dBodyCreate (world);
  body[THIGH_L] = dBodyCreate (world);
  dBodySetMass (body[CALF_L],&calf_l_m);
  dBodySetMass (body[THIGH_L],&thigh_l_m);
  dBodySetPosition (body[CALF_L],0,0,CALF_L_LENGTH/2);
  dBodySetPosition (body[THIGH_L],0,0,THIGH_L_LENGTH/2 + CALF_L_LENGTH);

  // create joint: knee_l
  joint[KNEE_L] = dJointCreateHinge( world, 0 );
  dJointAttach( joint[KNEE_L], body[THIGH_L], body[CALF_L] );
  dJointSetHingeAnchor( joint[KNEE_L], 0.0, 0.0, CALF_L_LENGTH );
  dJointSetHingeAxis( joint[KNEE_L], 0.0, 1.0, 0.0 );

  // set up view point
  static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
  static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
  dsSetViewpoint (xyz,hpr);

  printf ("Simulating ...\n");
}

/********************************************************************/
/* Do collision detection and spring damper force generation */

int collision_detection_and_floor_force_generation()
{
  dVector3 contact_pos;
  dVector3 contact_vel;
  dReal fx, fy, fz;

  dBodyGetRelPointPos ( body[CALF_L], 0, 0, -CALF_L_LENGTH/2, contact_pos );
  dBodyGetRelPointVel ( body[CALF_L], 0, 0, -CALF_L_LENGTH/2, contact_vel );

  if ( contact_pos[ZZ] < 0 )
    {
      fx = 0.0;
      fy = 0.0;
      fz = 1000.0*(0 - contact_pos[ZZ]) + 10.0*(-contact_vel[ZZ]);
    }
  else
    {
      fx = 0.0;
      fy = 0.0;
      fz = 0.0;
    }

  dBodyAddForceAtPos( body[CALF_L], fx, fy, fz, 
		      contact_pos[XX], contact_pos[YY], contact_pos[ZZ] );

  return 1;
}

/********************************************************************/
// called when a key pressed

static void command (int cmd)
{
  // don't handle user input yet.
}

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

// simulation loop

static void simLoop (int pause)
{
  static int count = 0;
  float delay;
  dVector3 foot;

  if (!pause)
    {
      if ( time < 1.0 )
	{ /* provide initial perturbation */
	  dJointAddHingeTorque( joint[KNEE_L], 0.001 );
	}
      else
	{
	  /* Your control goes here. */
	  dJointAddHingeTorque( joint[KNEE_L], 0.0 );
	}

      collision_detection_and_floor_force_generation();

      dWorldStep (world,TIMESTEP);  /* integrate forward 10 milliseconds */
      time += TIMESTEP;
      count++;
      for ( delay = 0; delay < DELAY_AMOUNT; delay += 1.0 )
	;
      if ( (count % 100) == 0 )
	{
	  dBodyGetRelPointPos ( body[CALF_L], 0, 0, -CALF_L_LENGTH/2, foot );
	  printf( "%g: %g %g %g\n", time, foot[XX], foot[YY], foot[ZZ] );
	}
    }

  /* Graphics */
  dsSetTexture (DS_WOOD);
  dsSetColor (1,1,0);
  dReal calf_l_sides[3] = {CALF_L_WIDTH,CALF_L_WIDTH,CALF_L_LENGTH};
  dsDrawBox (dBodyGetPosition(body[CALF_L]),dBodyGetRotation(body[CALF_L]),calf_l_sides);

  dsSetTexture (DS_WOOD);
  dsSetColor (0,0,1);
  dReal thigh_l_sides[3] = {THIGH_L_WIDTH,THIGH_L_WIDTH,THIGH_L_LENGTH};
  dsDrawBox (dBodyGetPosition(body[THIGH_L]),dBodyGetRotation(body[THIGH_L]),thigh_l_sides);
}

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

int main (int argc, char **argv)
{

  CALLBACK_STUFF;

  // run simulation
  dsSimulationLoop( argc, argv, /* command line arguments */
		    352, 288, /* window size? */
		    &fn ); /* callback info */

  // clean up stuff (not critical, we are killing process anyway)
  dWorldDestroy( world );

  return 0;
}

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