/************************************************************************/
/*
compass1-policy1.c: Implement a policy to control the biped.
*/
/************************************************************************/

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

#include "compass1.h"
#include "compass1-policy1.h"

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

/* policy1 parameters */
#define K_CONSTANT_POLICY1 0
#define K_SWING_MINUS_STANCE_LEG_POLICY1 1
#define K_SWING_MINUS_STANCE_LEG_D_POLICY1 2
#define K_STANCE_LEG_POLICY1 3
#define K_STANCE_LEG_D_POLICY1 4

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

static float p_policy1[N_POLICY1_PARAMETERS];

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

int init_policy1( float *parameters ) 
{
  int i;

  for( i = 0; i < N_POLICY1_PARAMETERS; i++ )
    p_policy1[i] = parameters[i];
}

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

int policy1( double time, double *full_state, double *action ) 
{
  int i;
  double x[MAX_N_X];
  int n_x;

  full_x_to_state_x( full_state, x, &n_x );

  action[HIP_TORQUE] = p_policy1[K_CONSTANT_POLICY1]
    + p_policy1[K_SWING_MINUS_STANCE_LEG_POLICY1]*x[S1_SWING_MINUS_STANCE_LEG]
    + p_policy1[K_SWING_MINUS_STANCE_LEG_D_POLICY1]
    *x[S1_SWING_MINUS_STANCE_LEG_D]
    + p_policy1[K_STANCE_LEG_POLICY1]*x[S1_STANCE_LEG]
    + p_policy1[K_STANCE_LEG_D_POLICY1]*x[S1_STANCE_LEG_D];

  /*
  for( i = 0; i < N_POLICY1_PARAMETERS; i++ )
    printf( "%g ", p_policy1[i] );
  printf( "\n" );
  dv_print( stdout, "%g ", x, 4 );
  printf( "\n" );
  getchar();
  */

  if( full_state[L_FOOT_DOWN] < 0.5 )
    action[HIP_TORQUE] = - action[HIP_TORQUE];

  return 1;
}

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