/************************************************************************
Does not implement:
in_air_not_allowed
FOOT_SLIDING not implemented.

/************************************************************************/
/*
  dynamics.c: This is where the numerical integration and SDFAST stuff is done.
*/
/************************************************************************/
/*
In this version we are going to use the following conventions:
x (right: the direction of roll)
y forward
z up

roll is the torso angle relative to vertical
+ leans the torso to positive X (right) 

hip is the joint angle between the torso and the thigh
hip +
moving the torso in postive X (right) for a fixed vertical thigh, 
moving the thigh in positve X (right) for a fixed vertical torso.

knee is the length between the thigh and the calf
knee +
lengthens leg.

ankle is the angle between the calf and vertical
ankle +
moving the calf in positive X for a fixed horizontal foot,
moving the right side of the foot upward for a fixed vertical calf.
*/
/************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "main.h"
#include "main2.h"
#include "sdfast/sra.h"

/************************************************************************/
/* DEFINES */

#define FEET_TOGETHER (1e-3) // Feet too close together to measure tilt

/************************************************************************/
/* GLOBAL VARIABLES */

/* Simulation structure */
SIM sim;

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

static void forward_kinematics( SIM *s )
{
  int i;
  double zero_offset[3] = { 0.0, 0.0, 0.0 };

  srapos( AIR_TORSO_BODY, s->head_offset, s->head );    
  srapos( AIR_TORSO_BODY, s->hip_offset[LEFT], s->hip[LEFT] );    
  srapos( AIR_TORSO_BODY, s->hip_offset[RIGHT], s->hip[RIGHT] );    
  srapos( AIR_L_CALF_BODY, s->knee_offset, &(s->knee[LEFT][0]) );    
  srapos( AIR_R_CALF_BODY, s->knee_offset, &(s->knee[RIGHT][0]) );    
  /*
  srapos( AIR_L_THIGH_BODY, s->knee_offset, &(s->knee[LEFT][0]) );    
  srapos( AIR_R_THIGH_BODY, s->knee_offset, &(s->knee[RIGHT][0]) );    
  */
  srapos( AIR_L_CALF_BODY, s->foot_offset, &(s->foot[LEFT][0]) );    
  srapos( AIR_R_CALF_BODY, s->foot_offset, &(s->foot[RIGHT][0]) );    

  srapos( AIR_TORSO_BODY, zero_offset, s->torso );
  srapos( AIR_L_THIGH_BODY, zero_offset, &(s->thigh[LEFT][0]) );
  srapos( AIR_R_THIGH_BODY, zero_offset, &(s->thigh[RIGHT][0]) );
  srapos( AIR_L_CALF_BODY, zero_offset, &(s->calf[LEFT][0]) );
  srapos( AIR_R_CALF_BODY, zero_offset, &(s->calf[RIGHT][0]) );

  sravel( AIR_TORSO_BODY, s->head_offset, s->headd );    
  sravel( AIR_TORSO_BODY, s->hip_offset[LEFT], s->hipd[LEFT] );    
  sravel( AIR_TORSO_BODY, s->hip_offset[RIGHT], s->hipd[RIGHT] );    
  sravel( AIR_L_CALF_BODY, s->knee_offset, &(s->kneed[LEFT][0]) );    
  sravel( AIR_R_CALF_BODY, s->knee_offset, &(s->kneed[RIGHT][0]) );    
  /*
  sravel( AIR_L_THIGH_BODY, s->knee_offset, &(s->kneed[LEFT][0]) );    
  sravel( AIR_R_THIGH_BODY, s->knee_offset, &(s->kneed[RIGHT][0]) );    
  */
  sravel( AIR_L_CALF_BODY, s->foot_offset, &(s->footd[LEFT][0]) );    
  sravel( AIR_R_CALF_BODY, s->foot_offset, &(s->footd[RIGHT][0]) );    

  sravel( AIR_TORSO_BODY, zero_offset, s->torsod );
  sravel( AIR_L_THIGH_BODY, zero_offset, &(s->thighd[LEFT][0]) );
  sravel( AIR_R_THIGH_BODY, zero_offset, &(s->thighd[RIGHT][0]) );
  sravel( AIR_L_CALF_BODY, zero_offset, &(s->calfd[LEFT][0]) );
  sravel( AIR_R_CALF_BODY, zero_offset, &(s->calfd[RIGHT][0]) );

  sraprinterr( stderr );
  sraclearerr();

  for ( i = 0; i < 3; i++ )
    {
      s->groin[i] = (s->hip[LEFT][i] + s->hip[RIGHT][i])/2;
      s->groind[i] = (s->hipd[LEFT][i] + s->hipd[RIGHT][i])/2;
      s->com[i] = s->torso[i]*s->torso_mass
	+ s->thigh[LEFT][i]*s->thigh_mass
	+ s->thigh[RIGHT][i]*s->thigh_mass
	+ s->calf[LEFT][i]*s->calf_mass
	+ s->calf[RIGHT][i]*s->calf_mass;
      s->com[i] /= s->total_mass;
      s->comd[i] = s->torsod[i]*s->torso_mass
	+ s->thighd[LEFT][i]*s->thigh_mass
	+ s->thighd[RIGHT][i]*s->thigh_mass
	+ s->calfd[LEFT][i]*s->calf_mass
	+ s->calfd[RIGHT][i]*s->calf_mass;
      s->comd[i] /= s->total_mass;
    }

  s->roll = -s->sdfast_state[AIR_ROLL];
  s->hip_angle[LEFT] = s->sdfast_state[AIR_L_HIP];
  s->hip_angle[RIGHT] = s->sdfast_state[AIR_R_HIP];
  s->knee_length[LEFT] = s->sdfast_state[AIR_L_KNEE];
  s->knee_length[RIGHT] = s->sdfast_state[AIR_R_KNEE];

  s->rolld = -s->sdfast_state[AIR_ROLLD];
  s->hip_angled[LEFT] = s->sdfast_state[AIR_L_HIPD];
  s->hip_angled[RIGHT] = s->sdfast_state[AIR_R_HIPD];
  s->knee_lengthd[LEFT] = s->sdfast_state[AIR_L_KNEED];
  s->knee_lengthd[RIGHT] = s->sdfast_state[AIR_R_KNEED];

  // Since the ankle angles don't exist in the sdfast model, we have to
  // compute them.
  s->ankle_angle[LEFT] = s->roll - s->hip_angle[LEFT];
  s->ankle_angle[RIGHT] = s->roll - s->hip_angle[RIGHT];
  s->ankle_angled[LEFT] = s->rolld - s->hip_angled[LEFT];
  s->ankle_angled[RIGHT] = s->rolld - s->hip_angled[RIGHT];

  s->torso_abs_angle = 
    -atan2( s->head[XX] - s->groin[XX], s->head[ZZ] - s->groin[ZZ] );
  for( i = 0; i < 2; i++ )
    {
      s->thigh_abs_angle[i] = 
	-atan2( s->hip[i][XX] - s->knee[i][XX],
		s->hip[i][ZZ] - s->knee[i][ZZ] );
      s->calf_abs_angle[i] = 
	-atan2( s->knee[i][XX] - s->foot[i][XX],
		s->knee[i][ZZ] - s->foot[i][ZZ] );
    }

  for ( i = LEFT; i <= RIGHT; i++ )
    {
      if ( s->foot[i][ZZ] >= s->ground_level 
	   || s->ground_force[i][ZZ] <= 0 )
	s->foot_status[i] = FOOT_IN_AIR;
      else
	s->foot_status[i] = FOOT_ON_GROUND;
    }
  if ( s->foot_status[LEFT] == FOOT_ON_GROUND 
       && s->foot_status[RIGHT] == FOOT_ON_GROUND )
    s->dynamics_type = DOUBLE_SUPPORT;
  else if ( s->foot_status[LEFT] == FOOT_IN_AIR 
       && s->foot_status[RIGHT] == FOOT_IN_AIR )
    s->dynamics_type = IN_AIR;
  else
    s->dynamics_type = SINGLE_SUPPORT;
}

/************************************************************************/
/* Initialize the sdfast state vector */

static void set_sdfast_state( SIM *s, double *sdfast_state )
{
  int i, j;

  for( i = 0; i < AIR_N_STATES; i++ )
    {
      s->sdfast_state[i] = sdfast_state[i];
      s->sdfast_stated[i] = 0;
    }

  for( i = 0, j = AIR_N_Q; i < AIR_N_U; i++, j++ )
    { 
      s->sdfast_stated[i] = s->sdfast_state[j];
    }

  srastate( 0.0, s->sdfast_state, s->sdfast_stated );

  sraprinterr( stderr );
  sraclearerr();

  forward_kinematics( s );

  s->status = STATUS_OK;
}

/************************************************************************/
// For debugging

sradump()
{
  double value;
  double vector[3];
  double inertia[3][3];

  sragetmass( AIR_TORSO_BODY, &value );
  printf( "Torso mass: %g\n", value );
  sragetmass( AIR_L_THIGH_BODY, &value );
  printf( "L Thigh mass: %g\n", value );
  sragetmass( AIR_R_THIGH_BODY, &value );
  printf( "R Thigh mass: %g\n", value );
  sragetmass( AIR_L_CALF_BODY, &value );
  printf( "L Calf mass: %g\n", value );
  sragetmass( AIR_R_CALF_BODY, &value );
  printf( "R Calf mass: %g\n", value );

  sragetiner( AIR_TORSO_BODY, inertia );
  printf( "Torso I: %g\n", inertia[YY][YY] );
  sragetiner( AIR_L_THIGH_BODY, inertia );
  printf( "L Thigh I: %g\n", inertia[YY][YY] );
  sragetiner( AIR_R_THIGH_BODY, inertia );
  printf( "R Thigh I: %g\n", inertia[YY][YY] );
  sragetiner( AIR_L_CALF_BODY, inertia );
  printf( "L Calf I: %g\n", inertia[YY][YY] );
  sragetiner( AIR_R_CALF_BODY, inertia );
  printf( "R Calf I: %g\n", inertia[YY][YY] );

  sragetbtj( AIR_L_THIGH_BODY, vector );
  printf( "L_THIGH BTJ: %g %g %g\n", vector[0],
	  vector[1], vector[2] );
  sragetbtj( AIR_R_THIGH_BODY, vector );
  printf( "R_THIGH BTJ: %g %g %g\n", vector[0],
	  vector[1], vector[2] );
  sragetbtj( AIR_L_CALF_BODY, vector );
  printf( "L_Calf BTJ: %g %g %g\n", vector[0],
	  vector[1], vector[2] );
  sragetbtj( AIR_R_CALF_BODY, vector );
  printf( "R_Calf BTJ: %g %g %g\n", vector[0],
	  vector[1], vector[2] );

  sragetitj( AIR_L_THIGH_BODY, vector );
  printf( "L_THIGH ITJ: %g %g %g\n", vector[0],
	  vector[1], vector[2] );
  sragetitj( AIR_R_THIGH_BODY, vector );
  printf( "R_THIGH ITJ: %g %g %g\n", vector[0],
	  vector[1], vector[2] );
  sragetitj( AIR_L_CALF_BODY, vector );
  printf( "L_Calf ITJ: %g %g %g\n", vector[0],
	  vector[1], vector[2] );
  sragetitj( AIR_R_CALF_BODY, vector );
  printf( "R_Calf ITJ: %g %g %g\n", vector[0],
	  vector[1], vector[2] );

  sraprinterr( stderr );
  sraclearerr();
}

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

void debug_positions( SIM *s )
{

  printf( "head: %g %g\n", s->head[XX], s->head[ZZ] );
  printf( "torso: %g %g\n", s->torso[XX], s->torso[ZZ] );
  printf( "lhip: %g %g\n", s->hip[LEFT][XX], s->hip[LEFT][ZZ] );
  printf( "rhip: %g %g\n", s->hip[RIGHT][XX], s->hip[RIGHT][ZZ] );
  printf( "lthigh: %g %g\n", s->thigh[LEFT][XX], s->thigh[LEFT][ZZ] );
  printf( "lknee: %g %g\n", s->knee[LEFT][XX], s->knee[LEFT][ZZ] );
  printf( "lcalf: %g %g\n", s->calf[LEFT][XX], s->calf[LEFT][ZZ] );
  printf( "lfoot: %g %g\n", s->foot[LEFT][XX], s->foot[LEFT][ZZ] );
  printf( "rthigh: %g %g\n", s->thigh[RIGHT][XX], s->thigh[RIGHT][ZZ] );
  printf( "rknee: %g %g\n", s->knee[RIGHT][XX], s->knee[RIGHT][ZZ] );
  printf( "rcalf: %g %g\n", s->calf[RIGHT][XX], s->calf[RIGHT][ZZ] );
  printf( "rfoot: %g %g\n", s->foot[RIGHT][XX], s->foot[RIGHT][ZZ] );

  printf( "roll: %g\n", s->roll );
  printf( "hips: %g %g\n", s->hip_angle[LEFT], s->hip_angle[RIGHT] );
  printf( "knees: %g %g\n", s->knee_length[LEFT], s->knee_length[RIGHT] );
  printf( "ankles: %g %g\n", s->ankle_angle[LEFT], s->ankle_angle[RIGHT] );

  printf( "\n" );
  printf( "head: %g %g\n", s->head[XX] - s->foot[LEFT][XX], s->head[ZZ] );
  printf( "torso: %g %g\n", s->torso[XX] - s->foot[LEFT][XX], s->torso[ZZ] );
  // printf( "hip: %g %g\n", s->hip[XX] - s->foot[LEFT][XX], s->hip[ZZ] );
  printf( "lthigh: %g %g\n", s->thigh[LEFT][XX] - s->foot[LEFT][XX],
	  s->thigh[LEFT][ZZ] - s->foot[LEFT][ZZ] );
  printf( "lknee: %g %g\n", s->knee[LEFT][XX] - s->foot[LEFT][XX],
	  s->knee[LEFT][ZZ] - s->foot[LEFT][ZZ] );
  printf( "lcalf: %g %g\n", s->calf[LEFT][XX] - s->foot[LEFT][XX],
	  s->calf[LEFT][ZZ] - s->foot[LEFT][ZZ] );
  printf( "lfoot: %g %g\n", s->foot[LEFT][XX] - s->foot[LEFT][XX],
	  s->foot[LEFT][ZZ] - s->foot[LEFT][ZZ] );
  printf( "rthigh: %g %g\n", s->thigh[RIGHT][XX] - s->foot[LEFT][XX],
	  s->thigh[RIGHT][ZZ] - s->foot[LEFT][ZZ] );
  printf( "rknee: %g %g\n", s->knee[RIGHT][XX] - s->foot[LEFT][XX],
	  s->knee[RIGHT][ZZ] - s->foot[LEFT][ZZ] );
  printf( "rcalf: %g %g\n", s->calf[RIGHT][XX] - s->foot[LEFT][XX],
	  s->calf[RIGHT][ZZ] - s->foot[LEFT][ZZ] );
  printf( "rfoot: %g %g\n", s->foot[RIGHT][XX] - s->foot[LEFT][XX],
	  s->foot[RIGHT][ZZ] - s->foot[LEFT][ZZ] );

  // exit( 0 );
}

/************************************************************************/
/* Initialize sdfast  */

void init_dynamics( SIM *s )
{
  int i, j;
  double vector[3];
  double inertia[3][3];

  if ( AIR_N_STATES > MAX_N_SDFAST_STATE )
    {
      fprintf( stderr, 
	       "Need to increast MAX_N_SDFAST_STATE (%d) to be at least %d\n",
	       MAX_N_SDFAST_STATE, AIR_N_STATES );
      exit( -1 );
    }

  s->status = STATUS_OK;

  // sradump();

#ifdef COMMENT
  sramass( AIR_TORSO_BODY, s->torso_mass );
  sramass( AIR_L_THIGH_BODY, s->thigh_mass );
  sramass( AIR_R_THIGH_BODY, s->thigh_mass );
  sramass( AIR_L_CALF_BODY, s->calf_mass );
  sramass( AIR_R_CALF_BODY, s->calf_mass );

  for ( i = 0; i < 3; i++ )
    {
      vector[i] = 0;
      for ( j = 0; j < 3; j++ )
	{
	  if ( i == j )
	    inertia[i][i] = 1.0;
	  else
	    inertia[i][j] = 0;
	}
    }
  inertia[YY][YY] = s->torso_I;
  srainer( AIR_TORSO_BODY, inertia );
  inertia[YY][YY] = s->thigh_I;
  srainer( AIR_L_THIGH_BODY, inertia );
  srainer( AIR_R_THIGH_BODY, inertia );
  inertia[YY][YY] = s->calf_I;
  srainer( AIR_L_CALF_BODY, inertia );
  srainer( AIR_R_CALF_BODY, inertia );

  vector[ZZ] = -s->torso_cm;
  srabtj( AIR_TORSO_BODY, vector );
  vector[ZZ] = s->thigh_cm;
  srabtj( AIR_L_THIGH_BODY, vector );
  srabtj( AIR_R_THIGH_BODY, vector );
  vector[ZZ] = s->calf_cm;
  srabtj( AIR_L_CALF_BODY, vector );
  srabtj( AIR_R_CALF_BODY, vector );

  vector[ZZ] = -s->torso_cm;
  sraitj( AIR_L_THIGH_BODY, vector );
  sraitj( AIR_R_THIGH_BODY, vector );

  vector[ZZ] = -(s->thigh_length - s->thigh_cm);
  sraitj( AIR_L_CALF_BODY, vector );
  sraitj( AIR_R_CALF_BODY, vector );

  // sradump();
#endif

  srainit(); /* initialize SDFAST model */

  s->simulation_type = SDFAST_SPRINGS;

  sraprinterr( stderr );
  sraclearerr();

  srastab( 2.0*s->sdfast_baumgarte, s->sdfast_baumgarte*s->sdfast_baumgarte ); 

  for( i = 0; i < AIR_N_STATES; i++ )
    s->sdfast_state[i] = 0;

  sraprinterr( stderr );
  sraclearerr();
}

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

void init_state_one_foot_on_ground( SIM *s )
{
  double min;

  s->status = STATUS_OK;

  s->sdfast_state[AIR_X] = s->torso[XX];
  s->sdfast_state[AIR_Z] = s->torso[ZZ];
  s->sdfast_state[AIR_ROLL] = -s->roll;
  s->sdfast_state[AIR_L_HIP] = s->hip_angle[LEFT];
  s->sdfast_state[AIR_R_HIP] = s->hip_angle[RIGHT];
  s->sdfast_state[AIR_L_KNEE] = -s->knee_length[LEFT];
  s->sdfast_state[AIR_R_KNEE] = -s->knee_length[RIGHT];

  s->sdfast_state[AIR_XD] = s->torsod[XX];
  s->sdfast_state[AIR_ZD] = s->torsod[ZZ];
  s->sdfast_state[AIR_ROLLD] = -s->rolld;
  s->sdfast_state[AIR_L_HIPD] = s->hip_angled[LEFT];
  s->sdfast_state[AIR_R_HIPD] = s->hip_angled[RIGHT];
  s->sdfast_state[AIR_L_KNEED] = -s->knee_lengthd[LEFT];
  s->sdfast_state[AIR_R_KNEED] = -s->knee_lengthd[RIGHT];

  set_sdfast_state( s, s->sdfast_state );

  min = s->foot[LEFT][ZZ];
  if ( min < s->foot[RIGHT][ZZ] )
    min = s->foot[RIGHT][ZZ];
  s->sdfast_state[AIR_Z] = s->sdfast_state[AIR_Z] - (min - s->ground_level);

  set_sdfast_state( s, s->sdfast_state );
}

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

void init_state_two_feet_on_ground( SIM *s )
{
  double min;
  double offset;

  s->status = STATUS_OK;

  s->sdfast_state[AIR_X] = s->torso[XX];
  s->sdfast_state[AIR_Z] = s->torso[ZZ];
  s->sdfast_state[AIR_ROLL] = -s->roll;
  s->sdfast_state[AIR_L_HIP] = s->hip_angle[LEFT];
  s->sdfast_state[AIR_R_HIP] = s->hip_angle[RIGHT];
  s->sdfast_state[AIR_L_KNEE] = -s->knee_length[LEFT];
  s->sdfast_state[AIR_R_KNEE] = -s->knee_length[RIGHT];

  s->sdfast_state[AIR_XD] = s->torsod[XX];
  s->sdfast_state[AIR_ZD] = s->torsod[ZZ];
  s->sdfast_state[AIR_ROLLD] = -s->rolld;
  s->sdfast_state[AIR_L_HIPD] = s->hip_angled[LEFT];
  s->sdfast_state[AIR_R_HIPD] = s->hip_angled[RIGHT];
  s->sdfast_state[AIR_L_KNEED] = -s->knee_lengthd[LEFT];
  s->sdfast_state[AIR_R_KNEED] = -s->knee_lengthd[RIGHT];

  set_sdfast_state( s, s->sdfast_state );

  /* Level the two feet */
  /* Need to handle case where two feet together */
  if ( fabs( s->foot[RIGHT][XX] - s->foot[LEFT][XX] ) > FEET_TOGETHER )
    {
      if ( s->foot[RIGHT][XX] - s->foot[LEFT][XX] > 0 )
	offset = atan2( s->foot[RIGHT][ZZ] - s->foot[LEFT][ZZ],
			s->foot[RIGHT][XX] - s->foot[LEFT][XX] );
      else
	offset = atan2( s->foot[LEFT][ZZ] - s->foot[RIGHT][ZZ],
			s->foot[LEFT][XX] - s->foot[RIGHT][XX] );

      /*
	printf( "two feet on ground: %g %g %g %g: %g\n", 
	s->foot[RIGHT][ZZ], s->foot[LEFT][ZZ],
	s->foot[RIGHT][XX], s->foot[LEFT][XX], offset );
      */

      s->sdfast_state[AIR_ROLL] += offset;
      
      set_sdfast_state( s, s->sdfast_state );
    }

  min = s->foot[LEFT][ZZ];
  if ( min < s->foot[RIGHT][ZZ] )
    min = s->foot[RIGHT][ZZ];
  s->sdfast_state[AIR_Z] = s->sdfast_state[AIR_Z] - (min - s->ground_level);

  set_sdfast_state( s, s->sdfast_state );

  /*
  printf( "%g %g\n", s->foot[RIGHT][ZZ], s->foot[LEFT][ZZ] );
  */
}

/************************************************************************/
/* This is what is called on each integration step */

void integrate_one_time_step( SIM *s )
{ 
  int i, step;
  int err; 
    /* { OK, DERIVATIVE_DISCONTINUITY, SYSTEM_LOCKED, CONSTRAINTS_ERR } */
  double errest;
  int flag = 1;
  int is_bad_number = 0;

  if( s->status == CRASHED )
    {
      s->time += s->time_step;
      return;
    }

  /* Give up if hip is too high or too low */
  if ( s->groin[ZZ] < s->hip_min || s->groin[ZZ] > s->hip_max )
    {
      s->status = CRASHED;
      s->time += s->time_step;
      return;
    }

  // clear outstanding error flags
  sraprinterr( stderr );
  sraclearerr();

  for( step = 0; step < s->sdfast_integration_steps_per_control_step; step++ )
    {
      srafmotion( &(s->time), s->sdfast_state, s->sdfast_stated,
		  s->time_step/s->sdfast_integration_steps_per_control_step,
		  s->sdfast_ctol, flag, &errest, &err );
      sraprinterr( stderr );
      sraclearerr();

      for( i = 0; i < AIR_N_STATES; i++ )
	{ // isfinite() should have worked.
	  if ( isnan( s->sdfast_state[i] ) 
	       || isinf( s->sdfast_state[i])
	       || isnan( s->sdfast_stated[i] ) 
	       || isinf( s->sdfast_stated[i]) ) 
	    {
	      is_bad_number = 1;
	      fprintf( stderr, "Nan detected.\n" );
	      break;
	    }
	}

      if ( err > 0 )
	{
	  fprintf( stderr, "sdfast error %d\n", err );
	  break;
	}
    }

  if ( is_bad_number || (err > 0) )
    {
      s->status = CRASHED;
      return;
    }

  forward_kinematics( s );

  /*
  printf( "Press return to continue.\n" );
  getchar();
  */
}

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

// Utility function to compute the ground force function.
static 
void compute_ground_force(double contactpos[3],  // contact position wrt world
			  double contact_zero[3], // original contact point.
			  double contactvel[3],  // contact velocity wrt world
			  double bodypos[3],     // contact point wrt body
			  int body,            
			  double force[3]) // resultant force vector wrt world
			  
{
  int i;
  double fvec[3];  // ground force vector in body coordintes

  // force only exists if foot is below ground level
  if ( contactpos[2] >= sim.ground_level )
    {
      for( i = 0; i < 3; i++ )
	{
	  force[i] = 0;
	  contact_zero[i] = contactpos[i];
	}
      return;
    }

  // Compute the desired ground force vector in the world coordinates.

  // Initialize original contact point. */
  if ( contact_zero[ZZ] > sim.ground_level )
    {
      for( i = 0; i < 2; i++ )
	contact_zero[i] = contactpos[i];
      contact_zero[ZZ] = sim.ground_level;
    }

  force[XX] = (contact_zero[XX] - contactpos[XX])*sim.ground_k[XX]
    - contactvel[XX] * sim.ground_b[XX];
  force[YY] = 0.0;   // not relevant
  force[ZZ] = (sim.ground_level - contactpos[ZZ]) * sim.ground_k[ZZ] 
    - contactvel[ZZ] * sim.ground_b[ZZ];

  // ground can't pull down
  if ( force[ZZ] <= 0 ) 
    {
      force[XX] = 0.0;
      force[YY] = 0.0;
      force[ZZ] = 0.0;
      /* Simulate a slip: reset contact point. */
      for( i = 0; i < 3; i++ )
	contact_zero[i] = contactpos[i];
    }
  // check for slipping caused by friction cone violation.
  else if ( fabs( force[XX]/force[ZZ] ) > sim.friction_cone_limit )
    {
      // This slip is probably overly strong. There would be some
      // force and only partial motion in reality.
      force[XX] = 0.0;
      /* Simulate a slip: reset contact point. */
      for( i = 0; i < 3; i++ )
	contact_zero[i] = contactpos[i];
    }

  // transform the vector into body coordinates
  sratrans(AIR_WORLD_BODY, force, body, fvec);
  
  sraprinterr( stderr );
  sraclearerr();

  // apply to the model
  srapointf(body, bodypos, fvec);

  sraprinterr( stderr );
  sraclearerr();
}

/************************************************************************/
/* SDFAST stuff */
/************************************************************************/
/* This is where the control (joint torques) are applied. May be called many
times per integration step at any time or state. */

void srauforce( double t, double *q, double *u )
{
  int i;
  double force[3] = { 0.0, 0.0, 0.0 }; // force vector in world coordinates
  double fvec[3];  // force vector in body coordinates
  double bodypos[3] = { 0.0, 0.0, 0.0 };
  double torque[3] = { 0.0, 0.0, 0.0 };
  double foot[2][3];
  double footd[2][3];

  // find the position and velocity of each foot point in the world frame
  srapos( AIR_L_CALF_BODY, sim.foot_offset, foot[LEFT] );    
  srapos( AIR_R_CALF_BODY, sim.foot_offset, foot[RIGHT] );    

  sravel( AIR_L_CALF_BODY, sim.foot_offset, footd[LEFT] );    
  sravel( AIR_R_CALF_BODY, sim.foot_offset, footd[RIGHT] );    

  // Apply ground forces to each foot as point forces.
  compute_ground_force( foot[LEFT], sim.foot_zero[LEFT], footd[LEFT],
			sim.foot_offset, AIR_L_CALF_BODY,
			sim.ground_force[LEFT] );
  compute_ground_force( foot[RIGHT], sim.foot_zero[RIGHT], footd[RIGHT],
			sim.foot_offset, AIR_R_CALF_BODY,
			sim.ground_force[RIGHT] );

  // Apply horizontal perturbation to torso
  force[XX] = sim.torso_perturbation;
  // transform the vector into body coordinates
  sratrans( AIR_WORLD_BODY, force, AIR_TORSO_BODY, fvec );
  // apply to the model
  srapointf( AIR_TORSO_BODY, bodypos, fvec );

  sraprinterr( stderr );
  sraclearerr();

  /* Apply joint torques */
  srahinget( AIR_L_HIP_JOINT, 0, sim.hip_torque[LEFT] );
  srahinget( AIR_L_KNEE_JOINT, 0, sim.knee_force[LEFT] );
  srahinget( AIR_R_HIP_JOINT, 0, sim.hip_torque[RIGHT] );
  srahinget( AIR_R_KNEE_JOINT, 0, sim.knee_force[RIGHT] );

  /* Apply ankle torques */
  torque[YY] = sim.ankle_torque[LEFT];
  srabodyt( AIR_L_CALF_BODY, torque );
  torque[YY] = sim.ankle_torque[RIGHT];
  srabodyt( AIR_R_CALF_BODY, torque );

  sraprinterr( stderr );
  sraclearerr();
}

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