/*  runge-kutta.c
    
    This is an example of fourth order Runge-Kutta numerical integration
    using adaptive step sizing.
    
    J. Watlington, 10/16/95

    Usage: runge-kutta [ <starting_step_size> ]
*/

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

#undef  DEBUG_ADAPTIVE_STEP

#define DEFAULT_FINAL_TIME       ( 100.0 * 3.1415926535 )
#define NUM_STATE_VARIABLES      2

#define MAX_LOCAL_ERROR          0.000000001
#define MIN_LOCAL_ERROR          (MAX_LOCAL_ERROR / 20.0)

#define DEFAULT_STEP_SIZE        0.1
#define STEP_SIZE_SCALING_FACTOR 1.2
#define MAX_STEP_SIZE            10.0    /*  just some sanity checks */
#define MIN_STEP_SIZE            0.000001

#define MONITOR_SIMULATION
#define MONITOR_START_TIME       0.0
#define MONITOR_END_TIME         6.5

typedef double   sim_type;

sim_type   initial_state[ NUM_STATE_VARIABLES ] = { 1.0, 0.0 };

void take_step( sim_type *start_state, sim_type *end_state, double step );


void main( int argc, char **argv )
{
  int       i;
  double    step_size = DEFAULT_STEP_SIZE;
  double    final_time = DEFAULT_FINAL_TIME;
  double    t = 0.0;
  sim_type  local_error;
  sim_type  state[ NUM_STATE_VARIABLES ];
  sim_type  midpoint_state[ NUM_STATE_VARIABLES ];
  sim_type  twostep_state[ NUM_STATE_VARIABLES ];
  sim_type  onestep_state[ NUM_STATE_VARIABLES ];

  if( argc > 2 )
    {
      printf( "usage: %s [<step_size>]\n", argv[0] );
      exit( -1 );
    }
  else if( argc > 1 )
    {
      sscanf( argv[1], "%lf", &step_size );
      if( (step_size < MIN_STEP_SIZE) || (step_size > MAX_STEP_SIZE) )
	{
	  printf("%s: step_size (%f) out of range !\n", argv[0],
		 step_size );
	  exit( -1 );
	}
    }

  printf( "running till final time %f\n", final_time );
  printf( "limiting local error to %f\n", MAX_LOCAL_ERROR );
  printf( "starting with step size %f\n", step_size );
  printf( "time >    x        dx/dt\n" ); /*  problem dependent */

  for( i = 0; i < NUM_STATE_VARIABLES; i++ )
    state[ i ] = initial_state[ i ];


  while( t <= final_time )
    {
      take_step( state, onestep_state, step_size );
      take_step( state, midpoint_state, step_size / 2.0 );
      take_step( midpoint_state, twostep_state, step_size / 2.0 );

      local_error = fabs( onestep_state[ 0 ] - twostep_state[ 0 ] );
				
#ifdef USE_EXTRA_CALCS
      /*  The following statement means that this code now implements
	  something closer to a fifth order runge-kutta algorithm...  */
				
      for( i = 0; i < NUM_STATE_VARIABLES; i++ )
	state[ i ] = twostep_state[ i ] +
	  (twostep_state[ i ] - onestep_state[ i ])/15;
#else
      for( i = 0; i < NUM_STATE_VARIABLES; i++ )
	state[ i ] = twostep_state[ i ];
#endif
      t += step_size;

      /*  Now determine the step size of the next step, based on the
	  error bound indicated by this step !   */

      if( local_error > MAX_LOCAL_ERROR )
	{
	  step_size = step_size / STEP_SIZE_SCALING_FACTOR;
#ifdef DEBUG_ADAPTIVE_STEP
	  printf( "decreasing step size to: %f\n", step_size );
#endif
	}
      else if( local_error < MIN_LOCAL_ERROR )
	{
	  step_size *= STEP_SIZE_SCALING_FACTOR;
#ifdef DEBUG_ADAPTIVE_STEP
	  printf( "increasing step size to: %f\n", step_size );
#endif
	}

#ifdef MONITOR_SIMULATION
      /*  Print out information about the simulation   */

      if( (t >= MONITOR_START_TIME) && (t <= MONITOR_END_TIME) )
	{
	  printf( "%3.6f> ", t );
	  for( i = 0; i < NUM_STATE_VARIABLES; i++)
	    printf( "%2.6f ", state[ i ] );
	  printf( "\n" );
	}
#endif MONITOR_SIMULATION
    }

  /*  Print final result   */

  t -= step_size;
  printf( "%3.6f> ", t );
  for( i = 0; i < NUM_STATE_VARIABLES; i++)
    printf( "%2.6f ", state[ i ] );
  printf( "\n\nknown correct output at time %3.6f: %2.6f\n\n", t, cos( t ) );
}


#define NUM_INTERMEDIATES    4    /* for fourth order Runge-Kutta */

void take_step( sim_type *in, sim_type *out, double step )
{
  register double  step_size = step;
  sim_type         k[ NUM_STATE_VARIABLES ][ NUM_INTERMEDIATES ];

  /*  The following, along with the initial conditions specified
      above, determine the diff. equation set being solved...   */

  k[ 0 ][ 0 ] = step_size *   in[ 1 ];
  k[ 1 ][ 0 ] = step_size * - in[ 0 ];
  k[ 0 ][ 1 ] = step_size *  (in[ 1 ] + ( k[ 1 ][ 0 ] / 2.0 ));
  k[ 1 ][ 1 ] = step_size * -(in[ 0 ] + ( k[ 0 ][ 0 ] / 2.0 ));
  k[ 0 ][ 2 ] = step_size *  (in[ 1 ] + ( k[ 1 ][ 1 ] / 2.0 ));
  k[ 1 ][ 2 ] = step_size * -(in[ 0 ] + ( k[ 0 ][ 1 ] / 2.0 ));
  k[ 0 ][ 3 ] = step_size *  (in[ 1 ] +   k[ 1 ][ 2 ]);
  k[ 1 ][ 3 ] = step_size * -(in[ 0 ] +   k[ 0 ][ 2 ]);

  out[ 0 ] = in[ 0 ] + (k[ 0 ][ 0 ] + (2.0 * k[ 0 ][ 1 ]) +
			(2.0 * k[ 0 ][ 2 ]) + k[ 0 ][ 3 ]) / 6.0;
							
  out[ 1 ] = in[ 1 ] + (k[ 1 ][ 0 ] + (2.0 * k[ 1 ][ 1 ]) +
			(2.0 * k[ 1 ][ 2 ]) + k[ 1 ][ 3 ]) / 6.0;
}