/* Dynamics types */
#define N_DYNAMICS 2
/* leg indices */
#define LEFT 0
#define RIGHT 1

/* state variables in full (hidden) state */
#define N_STATE_VARIABLES 10
#define HIP_X 0
#define HIP_Y 1
#define L_LEG 2
#define LR_LEG 3
#define HIP_X_D 4
#define HIP_Y_D 5
#define L_LEG_D 6
#define LR_LEG_D 7
#define L_FOOT_DOWN 8
#define R_FOOT_DOWN 9

/* state variables in minimal (exposed) state S1 */
#define N_EXPOSED_STATE_VARIABLES 4
#define S1_SWING_MINUS_STANCE_LEG 0
#define S1_SWING_MINUS_STANCE_LEG_D 1
#define S1_STANCE_LEG 2
#define S1_STANCE_LEG_D 3

/* actions */
#define N_ACTION_VARIABLES 1
#define HIP_TORQUE 0

/* Other useful definitions */
#define MAX_N_X 4
#define MAX_N_U 1
#define N_X_FULL 10
#define N_U_FULL 1

/* some useful quantities */
#define N_LEGS 2
#define LEG_LENGTH 1.0
#define SDFAST_LEFT_LEG 0
#define SDFAST_RIGHT_LEG 1

/* Is foot down or not */
#define FREE 0
#define DOWN 1

extern int switch_dynamics_enabled;
extern double time_step;
extern int traj_too_long;

double one_step_cost( double *x_full, double *u_full );
int dynamics_type( double *x_full, double *u_full );
void full_x_to_state_x( double *x_full, double *x, int *n_x );
void full_u_to_state_u( double *u_full, double *u, int *n_u );
void state_u_to_full_u( double *u, double *u_full );
void state_x_to_full_x( double *x, double *x_full_context, double *x_full_result );