/************************************************************************/
/*  Demo program for simple policy (controller) for compass biped
*/
/************************************************************************/

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

#define COMPASS1

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

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

#define N_START_PARAMETERS 4

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

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

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

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

  init_policy1( &(x[1]) );
  init_dynamics_state( the_state );
  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; )
    {
      if ( count < MAX_N_STEPS - 1 )
	policy1( time, the_state, the_action );
      else
	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 );

  printf( "cost %g\n", cost );

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

/***************************************************************************/
/***************************************************************************/
/************************************************************************/
/************************************************************************/
/* Main: initialize and then call simulated annealing optimizer */

int main(int argc, char **argv)
{
  float cost;

  init_parameters[0] =     0.561854898930;
  init_parameters[1] =    -0.875659048557;
  init_parameters[2] =    -0.952670693398;
  init_parameters[3] =     2.244433641434;
  init_parameters[4] =    -2.077058315277;
  init_parameters[5] =    -0.467465430498;
  init_parameters[6] =     0.040317133069;
  init_parameters[7] =     0.280753523111;
  init_parameters[8] =    -1.161283016205;

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

  init_policy1( init_parameters );
  init_dynamics( init_state );
  init_my_graphics();

  write_stuff = 1;
  cost += func_one_traj( &(init_parameters[-1]) );
}

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