/* kutta.c:  source file for the differential equation solver.
             The solver will carry out one step integration with error
             control.  Driver is given in rkdriver.c.
             Written by Peng Zhang.  Nov. 9, 1995                     */
 


#include <stdio.h>
#include <math.h>
#include <time.h>
#include "/cs1/shared/robotics/toolkit/I_COLLIDE/newpro/kutta.h"
#define SAFETY 0.9
#define PGROW -0.2
#define PSHRNK -0.25
#define ERRCON 1.89e-4
#define MALLOCTYPE void

/* the value of ERRCON equals (5/SAFETY) raised to the power (1/PGROW) */

/* function to find the max precision of the working machine           */

double FindMachEps(void) {
 
  double temp=1.0;
 
  while ((1.0+temp)!=1.0)
    temp/=10.0;
  return temp;
} /* FindMachEps */

/* Fifth-order Runge-Kutta step with monitoring of local truncation
   error.  Input are the dependent variable vector y[] and derivative
   dydx[], x is independent variable at start.  htry is attempted step
   size. eps is the required acuracy, vector yscal[] is the error 
   weight.  On output, y and x are replaced by their new value.
   hdid is the stesize that was actually accomplished, and hnext is
   the estimated next stepsize.  f is the user-supplied
   routine that computes the right-hand side derivatives              */
 

void rkqs(double *y, double *dydx, int n, double *x, double htry,
          double eps, double *yscal, double *hdid, double *hnext,
          void (*f)(double, double *, double *))
{
     int i;
/*     double errmax, h, htemp, xnew, *yerr, *ytemp;   */
     double errmax, h, htemp, xnew, yerr[3], ytemp[3];

/*     yerr = VectorAlloc(n);
     ytemp = VectorAlloc(n);     */

     h = htry;

     for( ; ; ) {
         rkrk(y, dydx, n, *x, h, ytemp, yerr, f);
         errmax = 0.0;
         for(i=0; i<n; i++) errmax = MaxPair(errmax, fabs(yerr[i]/yscal[i]));
         errmax /= eps;
         if(errmax <= 1.0) break;
         htemp = SAFETY*h*pow(errmax, PSHRNK);
         h = (h >= 0.0? MaxPair(htemp, 0.1*h): MaxPair(htemp, 0.1*h));
         
         xnew = (*x) + h;
         if(xnew == *x) rkerror("stepsize underflow in rkqs");
      }
      if(errmax > ERRCON) *hnext = SAFETY*h*pow(errmax, PGROW);
          else *hnext = 5.0 * h;
      *x += (*hdid = h);
      for(i=0; i<n; i++) y[i] = ytemp[i];
    
/*      VectorFree(n, ytemp);
      VectorFree(n, yerr);     */
}



void rkerror(char *str) {
 
  fprintf(stderr, "Failure in numerical integration: %s.\n", str);
  fprintf(stderr, "Aborting the program - sorry.\n");
  exit(-1);
} /* rkerror */


double MaxPair(double x_, double y_) {
  if (x_>y_)
    return x_;
  else
    return y_;
} /* MaxPair */


double MinPair(double x_, double y_) {
  if (x_>y_)
    return y_;
  else
    return x_;
} /* MaxPair */


/****************************************************************************
  A fixed-fifth order Runge-Kutta method. The constants are due to Cash
  and Karp ([6] and [1, pp. 711-724]).
  Adaptive stepsize control: Estimate of local error due to embedded fourth
  order method.
  The output from RKFNK: xnew_ and xerr_ contain respectively the new values
  and the estimate of the local error of the dependent variables.
*****************************************************************************/

void rkrk(double *y_, double *dydx_, int N_, double x_, double h_,
          double *yout_, double *yerr_,  
	  void (*f_)(double, double *, double *)) {

/*  double  *k2_, *k3_, *k4_, *k5_, *k6_, *temp1_;    */

  double  k2_[3], k3_[3], k4_[3], k5_[3], k6_[3], temp1_[3];

  static const double    a2_ =      0.2;
  static const double    a3_ =      0.3;
  static const double    a4_ =      0.6;
  static const double    a5_ =      1.0;
  static const double    a6_ =      0.875;
  static const double   b21_ =      1.0/5.0;
  static const double   b31_ =      3.0/40.0;
  static const double   b32_ =      9.0/40.0;
  static const double   b41_ =      3.0/10.0;
  static const double   b42_ =     -9.0/10.0;
  static const double   b43_ =      6.0/5.0;
  static const double   b51_ =    -11.0/54.0;
  static const double   b52_ =      5.0/2.0;
  static const double   b53_ =    -70.0/27.0;
  static const double   b54_ =     35.0/27.0;
  static const double   b61_ =   1631.0/55296.0;
  static const double   b62_ =    175.0/512.0;
  static const double   b63_ =    575.0/13824.0;
  static const double   b64_ =  44275.0/110592.0;
  static const double   b65_ =    253.0/4096.0;
  static const double   c1_  =     37.0/378.0;
  static const double   dc1_ =     37.0/378.0 -  2825.0/27648.0;
  static const double   c3_  =    250.0/621.0;
  static const double   dc3_ =    250.0/621.0 - 18575.0/48384.0;
  static const double   c4_  =    125.0/594.0;
  static const double   dc4_ =    125.0/594.0 - 13525.0/55296.0;
  static const double   dc5_ =      0.0 - 277.0/14336.0;
  static const double   c6_  =    512.0/1771.0;
  static const double   dc6_ =    512.0/1771.0 - 1.0/4.0;

  int    i;

/*  k2_=VectorAlloc(N_);
  k3_=VectorAlloc(N_);
  k4_=VectorAlloc(N_);
  k5_=VectorAlloc(N_);
  k6_=VectorAlloc(N_);
  temp1_=VectorAlloc(N_);   */

  for(i=0; i<N_; i++) {
    temp1_[i]=y_[i]+b21_*h_*dydx_[i];
  } /* for i */
  f_(x_+a2_*h_, temp1_, k2_);
  for(i=0; i<N_; i++) {
    temp1_[i]=y_[i]+h_*(b31_*dydx_[i]+b32_*k2_[i]);
  } /* for i */
  f_(x_+a3_*h_, temp1_, k3_);
  for(i=0; i<N_; i++) {
    temp1_[i]=y_[i]+h_*(b41_*dydx_[i]+b42_*k2_[i]+b43_*k3_[i]);
  } /* for i */
  f_(x_+a4_*h_, temp1_, k4_);
  for(i=0; i<N_; i++) {
    temp1_[i]=y_[i]+h_*(b51_*dydx_[i]+b52_*k2_[i]+b53_*k3_[i]+b54_*k4_[i]);
  } /* for i */
  f_(x_+a5_*h_, temp1_, k5_);
  for(i=0; i<N_; i++) {
    temp1_[i]=y_[i]+h_*(b61_*dydx_[i]+b62_*k2_[i]+b63_*k3_[i]
      +b64_*k4_[i]+b65_*k5_[i]);
  } /* for i */
  f_(x_+a6_*h_, temp1_, k6_);
  for(i=0; i<N_; i++) {
    yout_[i]=y_[i]+h_*(c1_*dydx_[i]+c3_*k3_[i]+c4_*k4_[i]+c6_*k6_[i]);
    yerr_[i]=h_*(dc1_*dydx_[i]+dc3_*k3_[i]+dc4_*k4_[i]+dc5_*k5_[i]+dc6_*k6_[i]);
  } /* for i */

/*
  VectorFree(N_, k2_);
  VectorFree(N_, k3_);
  VectorFree(N_, k4_);
  VectorFree(N_, k5_);
  VectorFree(N_, k6_);
  VectorFree(N_, temp1_);
*/

} /* rkrk */


/****************************************************************************
  VectorAlloc allocated space for an n-dimensional vector of the type
  double *, [3]. It can be freed by VectorFree.
*****************************************************************************/
 
double *VectorAlloc(const int n) {
 
  double *temp;
 
  temp=(double *)calloc(n, sizeof(double));
  if (temp==NULL)
    fprintf(stderr, "In MatrixLib: Not able to allocate vector.\n");
  return temp;
} /* VectorAlloc */

void VectorFree(const int n, double *vector) {
 
  free((MALLOCTYPE *)vector);
} /* VectorFree */


/*****************************************************************************
  MatrixAlloc allocates storage for a square matrix with dimension
  n*n. An error message is printed, if it was impossible to allocate
  the neccesary space, [3].
*****************************************************************************/

double **MatrixAlloc(const int n, int m) {

  double **matrix;
  int    i;

  matrix=(double **)calloc(n, sizeof(double *));
  if (matrix==NULL)
    fprintf(stderr, "In MatrixLib: Not able to allocate matrix.\n");
  else
    for(i=0; i<n; i++) {
      matrix[i]=(double *)calloc(m, sizeof(double));
      if (matrix[i]==NULL)
        fprintf(stderr, "In MatrixLib: Not able to allocate matrix.\n");
    } /* for i */
  return matrix;
} /* MatrixAlloc */

void MatrixFree(const int n, double **matrix) {

  int i;

  for(i=0; i<n; i++)
    free((MALLOCTYPE *)matrix[i]);
  free((MALLOCTYPE *)matrix);
} /* MatrixFree */