/************************************************************************/
/* Demo program for compass gait passive dynamic walker.
*/
/************************************************************************/

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

#define COMPASS1

#ifdef COMPASS1
#include "compass1.h"
#define MAX_N_STEPS 20000
#endif

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

#define N_POLICY1_PARAMETERS 5
#define N_START_PARAMETERS 4
#define G 9.81

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

static int write_stuff = 0;
static float init_parameters[N_POLICY1_PARAMETERS + N_START_PARAMETERS];
static double init_state[N_STATE_VARIABLES];
static double the_angle = 0.009;

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

float func_one_traj( float *x )
{
  int i;
  float cost = 0;
  extern double time;
  double the_state[N_STATE_VARIABLES];
  double the_action[N_ACTION_VARIABLES];
  double last_state[N_STATE_VARIABLES];
  FILE *stream = NULL;
  int count = 0;

  for( i = 0; i < N_STATE_VARIABLES; i++ )
    the_state[i] = last_state[i] = init_state[i];

  reinit_display();

  if ( write_stuff )
    {
      stream = fopen( "traj", "w" );
      if ( stream == NULL )
	{
	  fprintf( stderr, "Can't open file traj.\n" );
	  exit( -1 );
	}
    }

  for( ; count < MAX_N_STEPS; )
    {
      for( i = 0; i < N_ACTION_VARIABLES; i++ )
	the_action[i] = 0;
      cost += one_step_cost( the_state, the_action );
      if ( write_stuff )
	write_a_point_ascii( stream, time, the_state, the_action );
      integrate_one_time_step( the_action, the_state );
      redisplay_stuff();
      count++;
    }

  if ( write_stuff )
    fclose( stream );

  return cost;
}

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

void setup_state( double *full_state, float *p, int index, double value )
{
  full_state[LR_LEG] = p[N_POLICY1_PARAMETERS+0];
  full_state[LR_LEG_D] = p[N_POLICY1_PARAMETERS+1];
  full_state[L_LEG] = p[N_POLICY1_PARAMETERS+2];
  full_state[L_LEG_D] = p[N_POLICY1_PARAMETERS+3];

  if ( index >= 0 ) 
    full_state[index] += value;

  full_state[HIP_X] = 0.0;
  full_state[HIP_Y] = LEG_LENGTH*cos(full_state[L_LEG]);
  full_state[HIP_X_D] 
    = -LEG_LENGTH*cos(full_state[L_LEG])*full_state[L_LEG_D];
  full_state[HIP_Y_D] 
    = -LEG_LENGTH*sin(full_state[L_LEG])*full_state[L_LEG_D];
  full_state[L_FOOT_DOWN] = 1.0;
  full_state[R_FOOT_DOWN] = 0.0;
}

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

int main(int argc, char **argv)
{
  float dummy[100];
  float cost;
  double gravity[3];

  gravity[0] = G*sin(the_angle);
  gravity[1] = -G*cos(the_angle);
  gravity[2] = 0;
  sdgrav( gravity );

  init_parameters[0] = 0.0;
  init_parameters[1] = 0.0;
  init_parameters[2] = 0.0;
  init_parameters[3] = 0.0;
  init_parameters[4] = 0.0;
  init_parameters[5] =    -0.3830319759807516;
  init_parameters[6] =     0.2487301056052789;
  init_parameters[7] =     0.1847139618368037;
  init_parameters[8] =    -0.7638161648818906;

  setup_state( init_state, init_parameters, -1, 0.0 );
  dv_print( stdout, "%g ", init_state, 10 );
  printf( "\n" );

  init_dynamics( init_state );
  init_my_graphics();

  write_stuff = 1;
  cost += func_one_traj( dummy );
}

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