/************************************************************************/
/*
Run a trajectory.
*/
/************************************************************************/

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

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

#define MAX_TIME 5.0

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

double k_matrix[N_ACTION_DIMENSIONS][N_STATE_DIMENSIONS];

/************************************************************************/
/************************************************************************/
/************************************************************************/
/************************************************************************/
/****************************************************************************/
/* Generate random numbers with a Gaussian distribution
Not sure where this comes from, but it is similar to the
Numerical recipes routine gasdev(). See Numerical Recipes
for an explanation of how it works.
*/

double gaussiand( double mean, double stddev )
{
  static int have_n_numbers = 0;
  static double number = 0.0;
  double n1, n2;
  double rsq, fac;

  switch( have_n_numbers )
    {
    case 1:
      have_n_numbers = 0;
      return number*stddev + mean;
      break;
    case 0:
      do
	{
	  n1 = 2.0*random()/2.147483647000000e+09 - 1.0;
	  n2 = 2.0*random()/2.147483647000000e+09 - 1.0;
	  rsq = n1*n1 + n2*n2;
	}
      while ( rsq >= 1.0 || rsq == 0.0 );
      fac = sqrt( -2.0*log(rsq)/rsq );
      number = n1*fac;
      have_n_numbers = 1;
      // printf( "%g %g %g %g %g %g\n", n1, n2, rsq, fac, n1*fac, n2*fac );
      return n2*fac*stddev + mean;
    }
}

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

int init_control( Dynamics *d )
{
  int i, j;
  FILE *stream;

  stream = fopen( "gains", "r" );
  if ( stream == NULL )
    {
      fprintf( stderr, "Can't open gains file.\n" );
      exit( -1 );
    }
  for ( i = 0; i < N_ACTION_DIMENSIONS; i++ )
    {
      for( j = 0; j < N_STATE_DIMENSIONS; j++ )
	{
	  fscanf( stream, "%lf", &(k_matrix[i][j]) );
	}
    }
  fclose( stream );

  /* Print it out
  for ( i = 0; i < N_ACTION_DIMENSIONS; i++ )
    {
      for( j = 0; j < N_STATE_DIMENSIONS; j++ )
	{
	  printf( "%g ", k_matrix[i][j] );
	}
      printf( "\n" );
    }
  */
}

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

init_kalman_filter()
{
  /* You read in your steady state filter design here. See init_control() */
}

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

kalman_filter( double *measurement, double *last_action, double *state_estimate )
{
  /* Your steady state Kalman filter goes here */
}

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

int control( Dynamics *d, double *action )
{
  int i, j;
  double torque;
  double measurement[N_STATE_DIMENSIONS];
  static last_action[N_ACTION_DIMENSIONS];

  for( j = 0; j < N_STATE_DIMENSIONS; j++ )
    measurement[j] = d->state[j];

  // simulate measurement noise: perturb action
  measurement[0] += gaussiand( 0.0, 0.1 );
  measurement[1] += gaussiand( 0.0, 0.1 );
  measurement[2] += gaussiand( 0.0, 1.0 );
  measurement[3] += gaussiand( 0.0, 1.0 );
  measurement[4] += gaussiand( 0.0, 1.0 );
  measurement[5] += gaussiand( 0.0, 1.0 );

  kalman_filter( measurement, last_action, state_estimate );

  for ( i = 0; i < N_ACTION_DIMENSIONS; i++ )
    {
      torque = 0;
      for( j = 0; j < N_STATE_DIMENSIONS; j++ )
	{
	  torque -= k_matrix[i][j]*(measurement[j] - the_desired_state[j]);
	}
      last_action[i] = action[i] = torque;
    }

  /* Need to obey ankle torque limit */

  return 0;
}

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

int main(int argc, char **argv)
{
  int i, j;
  int n_points = 0;
  double initial_state[N_STATE_DIMENSIONS];
  double action[N_ACTION_DIMENSIONS];
  Dynamics *d;
  double sums[N_STATE_DIMENSIONS];
	
  init_dynamics();
  d = create_dynamics();
  init_control( d );
  init_kalman_filter();

  for( i = 0; i < N_STATE_DIMENSIONS; i++ )
    {
      initial_state[i] = the_desired_state[i];
      sums[i] = 0;
    }
  /* Put in initial condition perturbation */
  // initial_state[0] += 0.1;
  // initial_state[5] += 1;

  set_state( d, 0.0, initial_state );

  for( ; ; n_points++ )
    {
      control( d, action );
      // simulate process noise: perturb action
      action[0] += gaussiand( 0.0, 100.0 );
      action[1] += gaussiand( 0.0, 100.0 );
      action[2] += gaussiand( 0.0, 100.0 );

      for( j = 0; j < N_STATE_DIMENSIONS; j++ )
	sums[j] += (d->state[j] - the_desired_state[j])*(d->state[j] - the_desired_state[j]);

      printf( "%7.2f ", d->time );
      for( j = 0; j < N_STATE_DIMENSIONS; j++ )
	printf( "%8.3f ", d->state[j] );
      for( j = 0; j < N_ACTION_DIMENSIONS; j++ )
	printf( "%8.3f ", action[j] );
      printf( "\n" );

      integrate_one_step( d, action, NULL );
      if ( d->time > MAX_TIME )
	break;
    }

  for( j = 0; j < N_STATE_DIMENSIONS; j++ )
    printf( "%g ", sqrt(sums[j]/n_points) );
  printf( "\n" );

  return 0;
}

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