/************************************************************************/
/*
Example of linearizing dynamics.
*/
/************************************************************************/

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

/************************************************************************/
/************************************************************************/
/************************************************************************/
/************************************************************************/
/************************************************************************/
/************************************************************************/
/************************************************************************/
/************************************************************************/
/* main */

int main(int argc, char **argv)
{
  int i, j;
  double state[N_STATE_DIMENSIONS];
  double save_state[N_STATE_DIMENSIONS];
  double next_state_0[N_STATE_DIMENSIONS];
  double next_state_1[N_STATE_DIMENSIONS];
  double next_state_2[N_STATE_DIMENSIONS];
  double action[N_ACTION_DIMENSIONS];
  double save_action[N_ACTION_DIMENSIONS];
  double delta = 1e-4; // often need to play with this value to see where
                      // you get a linearization that is insensitive to it.
  double a[N_STATE_DIMENSIONS][N_STATE_DIMENSIONS];
  double b[N_STATE_DIMENSIONS][N_ACTION_DIMENSIONS];
  Dynamics *d;
	
  for( i = 0; i < N_STATE_DIMENSIONS; i++ )
    {
      state[i] = the_desired_state[i];
      save_state[i] = state[i];
    }
  for( i = 0; i < N_ACTION_DIMENSIONS; i++ )
    {
      action[i] = 0;
      save_action[i] = action[i];
    }
  init_dynamics();
  d = create_dynamics();
  set_state( d, 0.0, state );
  integrate_one_step( d, action, next_state_0 );

  for( i = 0; i < N_STATE_DIMENSIONS; i++ )
    {
      state[i] += delta;
      set_state( d, 0.0, state );
      integrate_one_step( d, action, next_state_1 );
      state[i] = save_state[i] - delta;
      set_state( d, 0.0, state );
      integrate_one_step( d, action, next_state_2 );
      for( j = 0; j < N_STATE_DIMENSIONS; j++ )
	{
	  a[j][i] = (next_state_1[j] - next_state_2[j])/(2*delta);
	}
      state[i] = save_state[i];
    }

  for( i = 0; i < N_ACTION_DIMENSIONS; i++ )
    {
      action[i] += delta;
      set_state( d, 0.0, state );
      integrate_one_step( d, action, next_state_1 );
      action[i] = save_action[i] - delta;
      set_state( d, 0.0, state );
      integrate_one_step( d, action, next_state_2 );
      for( j = 0; j < N_STATE_DIMENSIONS; j++ )
	{
	  b[j][i] = (next_state_1[j] - next_state_2[j])/(2*delta);
	}
      action[i] = save_action[i];
    }

  printf( "A:\n" );
  for( i = 0; i < N_STATE_DIMENSIONS; i++ )
    {
      for( j = 0; j < N_STATE_DIMENSIONS; j++ )
	{
	  printf( "%20.15g ", a[i][j] );
	}
      printf( "\n" );
    }

  printf( "B:\n" );
  for( i = 0; i < N_STATE_DIMENSIONS; i++ )
    {
      for( j = 0; j < N_ACTION_DIMENSIONS; j++ )
	{
	  printf( "%20.15g ", b[i][j] );
	}
      printf( "\n" );
    }

  return 0;
}

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