/********************************************************************/
/*
Modified from drop.cpp CGA test program.
Balance a single link.
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 TIMESTEP 0.01
#define INTEGRATOR_RATIO 10 // Number of integrator steps for each control step

// 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 S_SIZE 2 // just angles and angular velocities
#define S_SIZE_ALL 4 // includes foot values
#define S_ANKLE_L 0
#define S_ANKLED_L 1
#define S_FOOTX_L 2
#define S_FOOTZ_L 3

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

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

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

dReal ankle_torque = 0.0;

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

static void start()
{
  dMass calf_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

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

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

/********************************************************************/
/* 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 = 1000.0*(0 - contact_pos[XX]) + 10.0*(-contact_vel[XX]);
      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;
}

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

int get_state( dReal *state )
{
  int i;
  dVector3 foot, knee;
  dVector3 footd, kneed;
  dReal calf_x, calf_z;

  for( i = 0; i < S_SIZE_ALL; i++ )
    state[i] = 0;
  dBodyGetRelPointPos ( body[CALF_L], 0, 0, -CALF_L_LENGTH/2, foot );
  dBodyGetRelPointPos ( body[CALF_L], 0, 0, CALF_L_LENGTH/2, knee );
  dBodyGetRelPointVel ( body[CALF_L], 0, 0, -CALF_L_LENGTH/2, footd );
  dBodyGetRelPointVel ( body[CALF_L], 0, 0, CALF_L_LENGTH/2, kneed );
  state[S_FOOTX_L] = foot[XX];
  state[S_FOOTZ_L] = foot[ZZ];
  calf_x = knee[XX] - foot[XX];
  calf_z = knee[ZZ] - foot[ZZ];
  state[S_ANKLE_L] = atan2( (double) calf_x, (double) calf_z );
  state[S_ANKLED_L] = ((kneed[XX] - footd[XX])*calf_z 
		       - (kneed[ZZ] - footd[ZZ])*calf_x)/CALF_L_LENGTH;
  // printf( "state: %g %g %g %g\n", state[0], state[1], state[2], state[3] );
  return 1;
}

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

int control()
{
  dReal state[S_SIZE_ALL];

  get_state( state );

  ankle_torque = 2.2253*(0.0 - state[S_ANKLE_L]) + 0.6515*(- state[S_ANKLED_L]);

  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)
{
  int i;
  static int count = 0;
  dVector3 foot;
  dReal state[S_SIZE_ALL];

  if (!pause)
    {
      collision_detection_and_floor_force_generation();
  
      if ( time < 1.0 )
	{ /* provide initial perturbation */
	  ankle_torque = 0.01;
	}
      else
	{
	  /* Your control routine goes here. */
	  control();
	  get_state( state );
	  printf( "%g %g %g %g\n", time, state[S_ANKLE_L], state[S_ANKLED_L], ankle_torque );
	}

      for( i = 0; i < INTEGRATOR_RATIO; i++ )
	{
	  if ( i != 0 )
	    collision_detection_and_floor_force_generation();
	  dBodyAddTorque( body[CALF_L], 0.0, ankle_torque, 0.0 );
	  dWorldStep (world,TIMESTEP/INTEGRATOR_RATIO);  // integrate forward
	}
      /*
      collision_detection_and_floor_force_generation();
      dWorldStep (world,TIMESTEP);  // integrate forward
      */
      time += TIMESTEP;
      count++;
      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] );
	}
      if ( time > 3.0 )
	exit( 0 );
    }

  /* 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);
}

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

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;
}

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