/********************************************************************/
/*
Modified from drop.cpp CGA test program.
Identify two link system.
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

// 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.

/********************************************************************/
// 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.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 = 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*CALF_L_LENGTH);
  printf( "vel: %g %g; %g %g\n", footd[XX], footd[ZZ], kneed[XX], kneed[ZZ] );
  printf( "state: %g %g %g %g\n", state[0], state[1], state[2], state[3] );
  return 1;
}

/********************************************************************/
/* Now set state. ODE makes this complicated. */

int set_state( dReal *state )
{
  dVector3 knee;
  dMatrix3 R;
  double sa, ca;
  dVector3 footd;

  sa = sin( (double) state[S_ANKLE_L] );
  ca = cos( (double) state[S_ANKLE_L] );

  // Need to set position taking into account angles at angle and knee
  knee[XX] = CALF_L_LENGTH*sa + state[S_FOOTX_L];
  knee[ZZ] = CALF_L_LENGTH*ca + state[S_FOOTZ_L];
  dBodySetPosition( body[CALF_L], (knee[XX] + state[S_FOOTX_L])/2,
		    0, (knee[ZZ] + state[S_FOOTZ_L])/2 );

  // Now set angles
  dRFromAxisAndAngle( R, 0.0, 1.0, 0.0, state[S_ANKLE_L] );
  dBodySetRotation( body[CALF_L], R );      

  // Now set velocities
  dBodySetAngularVel( body[CALF_L], 0.0, state[S_ANKLED_L], 0.0 );
  dBodySetLinearVel( body[CALF_L], ca*state[S_ANKLED_L]*CALF_L_LENGTH/2,
		     0.0, sa*state[S_ANKLED_L]*CALF_L_LENGTH/2 );

  dBodyGetRelPointVel ( body[CALF_L], 0, 0, -CALF_L_LENGTH/2, footd );
  printf( "Foot vel: %g %g %g\n", footd[0], footd[1], footd[2] );

  return 1;
}

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

int integrate_one_step( dReal *state )
{

  printf( "integrate: state: %g %g %g %g\n", state[0], state[1], state[2], state[3] );
  set_state( state );
  get_state( state );
  collision_detection_and_floor_force_generation();
  dWorldStep (world,TIMESTEP);  /* integrate forward 10 milliseconds */
  time += TIMESTEP;
  get_state( state );
  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;
  int i, j;
  dReal state[S_SIZE_ALL];
  dReal initial_state[S_SIZE_ALL];
  dReal A[S_SIZE][S_SIZE];
  dReal B[S_SIZE];
  dVector3 foot;
  dReal delta = 1e-4;

  /* Run simulation for a while to get floor to reach steady state. */
  if ( count < 1000 )
    {
      collision_detection_and_floor_force_generation();
  
      dWorldStep (world,TIMESTEP);  /* integrate forward 10 milliseconds */
      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 ( count == 1000 )
    { /* Set state or force for identifying row of A or B */
      /* first compute current state */
      get_state( initial_state );
      
      for( i = 0; i < S_SIZE_ALL; i++ )
	state[i] = initial_state[i];
      state[S_ANKLE_L] += delta;
      integrate_one_step( state );
      for( i = 0; i < S_SIZE; i++ )
	A[i][0] = (state[i] - initial_state[i])/delta;


      for( i = 0; i < S_SIZE_ALL; i++ )
	state[i] = initial_state[i];
      state[S_ANKLED_L] += delta;
      integrate_one_step( state );
      for( i = 0; i < S_SIZE; i++ )
	A[i][1] = (state[i] - initial_state[i])/delta;

      printf( "A = [ " );
      for( i = 0; i < S_SIZE; i++ )
	{
	  for( j = 0; j < S_SIZE; j++ )
	    {
	      printf( "%20.15f ", A[i][j] );
	    }
	  printf( "\n" );
	}
      printf( "]\n" );

      for( i = 0; i < S_SIZE_ALL; i++ )
	state[i] = initial_state[i];
      dBodyAddTorque( body[CALF_L], 0.0, delta, 0.0 );
      integrate_one_step( state );
      for( i = 0; i < S_SIZE; i++ )
	{
	  B[i] = (state[i] - initial_state[i])/delta;
	}

      printf( "B = [ " );
      for( i = 0; i < S_SIZE; i++ )
	printf( "%20.15f\n", B[i] );
      printf( "]\n" );
  
      exit( -1 );
    }

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

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