#define LQRD_BAD_VALUE 1e300
#define STEADY_STATE_INDEX -1

typedef struct lqrd
{
  /* Inputs: These need to be set by user. */
  /* Set by initialize_lqrd() or by set_xxx() */ 
  int n_states; /* dimensionality of state */
  int n_controls; /* dimensionality of action */
  int max_n_points; /* length of arrays below */
  int n_points; /* actual number of valid points in trajectory */
  // double *X0;         /* The fixed initial state. */
  int use_U_limits;    /* Do we use U limits. */
  double *U_min;       /* minimum commands */
  double *U_max;       /* maximum commands */
  double **X;         /* The current trajectory. */
  double **X_d;       /* The desired trajectory. */
  double *X_d_final;    /* The desired terminal state. */

  double **U;         /* The current command trajectory. */
  double **U_d;       /* The desired command trajectory. */

#ifdef NOT_USED
  int cost_time_varying; /* Is cost function time varying? */
  int cost_linear_terms;   /* Does cost have linear terms? */
  int cost_cross_terms;    /* Does cost have cross terms? */
#endif

  /* Other adjustable parameters */
  double epsilon_gradient;
  double epsilon_second_order;
  double delta;
  double discount;
  int ss_printout_interval;
  int ss_min_iterations;
  int ss_max_iterations;
  double ss_convergence_threshold;

  /* Outputs: These are set by program. */
  /* X[][] and U[][] are set by program */
  double **P_ss;          /* steady state P matrix */
  double **K_ss;          /* steady state gains */
  double **delta_U_array; /* The changes to the commands versus time */
  double ***K_array;      /* The gains versus time. */
  double *V_array;        /* V versus time */
  double **Vx_array;      /* The gradient of V wrt x versus time */
  double ***Vxx_array;    /* The Hessian of V wrt x versus time */
  double **X_best;        /* A place to stash the best trajectory so far */
  double **U_best;      /* It is up to the user to call save_best_trajectory */
  
  /* temporary variables */
  double *X_new;
  double *U_new;
  double **A;
  double **B;
  double *Lx;           /* linear state cost terms */
  double *Lu;           /* linear action cost terms */  
  double **Lxx;         /* state penalty matrix */
  double **Lxu;         /* cost cross terms matrix */  
  double **Lux;         /* cost cross terms matrix */  
  double **Luu;         /* control penalty matrix */  
  double **P;
  double **P_new;
  double **K;
  double *Vx;
  double **Vxx;
  double *temps1;
  double *temps2;
  double *temps3;
  double *tempc1;
  double *tempc2;
  double *tempc3;
  double **tempss1;
  double **tempss2;
  double **tempss3;
  double **tempsc1;
  double **tempsc2;
  double **tempcc1;
  double **tempcs1;
  double **tempcs2;
  double ***Fxx;
  double ***Fxu;
  double ***Fux;
  double ***Fuu;
  double *Zx;
  double *Zu;
  double **Zxu;
  double **Zux;
  double **Zuu;
  double **Zuu_inv;
  double **Zuu_L;
  double *Zuu_D;
  double **Zxx;

  int (*one_step_dynamics_function)();
  double (*cost_function)();
  double (*terminal_cost_function)();
  int (*compute_linear_model_function)();
  void *argument;
}
  LQRD;

LQRD *initialize_lqrd( int nstates, /* Dimensionality of state */
		       int ncontrols, /* Dimensionality of controls */
		       int max_npoints, /* Maximum number of points in trajectory */
		       int (*one_step_dynamics)(),
		       double (*cost_function)(),
		       void *argument /* argument to dynamics and cost functions */
		       );

double total_cost( LQRD *z );


