#ifndef _RUNGEK_
#define _RUNGEK_
#include <iostream>

/////////////////////////////////////////////////////////////////////////////
/////////// METHOD OF RUNKGE-KUTTA 4-th ORDER, FIXED STEP   /////////////////
/////////////////////////////////////////////////////////////////////////////
template <class functor, class container>
void rungek4(double x, double h, container& y, functor& derivs)
{// Runke-Kutta of fourth order for fixed time-step h.
 // derivs is a function which evaluates RHS of the ODE system, y contains values of dependent variables
  static container yt(y.size()), fk1(y.size()), fk2(y.size()), fk3(y.size()), fk4(y.size()); // temporary arrays
  double h2=h*0.5;
  double xh = x + h2;
  int N = y.size();      
  derivs(x, y, fk1);                                  // First step : evaluating k1
  for (int i=0; i<N; i++) yt[i] = y[i] + h2*fk1[i];   // Preparing second step by  ty <- y + k1/2
  derivs(xh, yt, fk2);                                // Second step : evaluating k2
  for (int i=0; i<N; i++) yt[i] = y[i] + h2*fk2[i];   // Preparing third step by   yt <- y + k2/2
  derivs(xh, yt, fk3);                                // Third step : evaluating k3
  for (int i=0; i<N; i++) yt[i] = y[i] + h*fk3[i];    // Preparing fourth step  yt <- y + k3
  derivs(x+h, yt, fk4);                               // Final step : evaluating k4
  double h6=h/6.0;                                    // Accumulate increments with proper weights
  for (int i=0; i<N; i++) y[i] += h6*(fk1[i]+2.0*(fk2[i]+fk3[i])+fk4[i]);
}

////////////////////////////////////////////////////////////////////////
///////// METHOD OF RUNGE-KUTTA 5-th ORDER, ADAPTIVE STEP  /////////////
////////////////////////////////////////////////////////////////////////
template <class container>
class VRungeKutta
{
  static const double SAFETY = 0.9;
  static const double PGROW  = -0.2;
  static const double PSHRNK = -0.25;
  static const double ERRCON = 1.89e-4;
  container yscal;   // scaling factors
  double ht;         // current time-step
  double eps;        // Accuracy we check at each step
  double xt, t_stop; // start and stop time
  int nok, nbad;     // number of bad and good steps
public:
  VRungeKutta(double t_start_, double t_stop_, int size, double accuracy=1e-6, double hmin=1e-4, double h1=0.1) :
    yscal(size), ht(h1), eps(accuracy), xt(t_start_), t_stop(t_stop_), nok(0), nbad(0){};
public:
  // The main function which takes next RK step with predefined accuracy
  template <class functor>
  bool Step(double& x, container& y, functor& derivs){
    static container dydx(y.size()); // // current derivatives are local to this functions
    // Calculates derivatives when new step is taken
    derivs(xt,y,dydx);
    // good way of determining desired accuracy
    for (int i=0; i<y.size(); i++) yscal[i] = fabs(y[i]) + fabs(dydx[i]*ht) + 1e-3;
    double hnext, hdid;
    // Cals the Runge-Kutta rountine
    RKStep(y, dydx, xt, ht, hdid, hnext, derivs); // Make one RK step
    if (hdid == ht) ++nok; else ++nbad; // good or bad step
    ht = hnext;
    x = xt;
    return x<t_stop;
  }
  
  double h(){return ht;}
private:
  template <class functor>
  void RKTry(container& y, container& dydx, double& x, double h, container& yout, container& yerr, functor& derivs);
  template <class functor>
  void RKStep(container& y, container& dydx, double& x, double htry, double& hdid, double& hnext, functor& derivs);
};

template <class container>
template <class functor>
void VRungeKutta<container>::RKStep(container& y, container& dydx, double& x, double htry,
				    double& hdid, double& hnext, functor& derivs)
//Fifth-order Runge-Kutta step with monitoring of local truncation error to ensure accuracy and
//adjust stepsize. Input are the dependent variable vector y[...] and its derivative dydx[...]
//at the starting value of the independent variable x. Also input are the stepsize to be attempted
//htry, the required accuracy eps, and the vector yscal[...] against which the error is
//scaled. On output, y and x are replaced by their new values, hdid is the stepsize that was
//actually accomplished, and hnext is the estimated next stepsize. derivs is the user-supplied
//routine that computes the right-hand side derivatives.
{
  container yerr(y.size()), ytemp(y.size());
  double errmax;
  double h=htry; // Set stepsize to the initial trial value.
  for (;;) {
    RKTry(y, dydx, x, h, ytemp, yerr, derivs); // Take a step.
    errmax=0.0;                               // Evaluate accuracy.
    for (int i=0; i<y.size(); i++) errmax=std::max(errmax,fabs(yerr[i]/yscal[i]));
    errmax /= eps;                            // Scale relative to required tolerance.
    if (errmax <= 1.0) break;                 // Step succeeded. Compute size of next step.
    double htemp=SAFETY*h*pow(errmax,PSHRNK); // Truncation error too large, reduce stepsize.
    h=(h >= 0.0 ? std::max(htemp,0.1*h) : std::min(htemp,0.1*h)); // No more than a factor of 10.
    if ((x+h) == x) std::cerr<<"stepsize underflow in rkqs"<<std::endl;
  }
  if (errmax > ERRCON) hnext=SAFETY*h*pow(errmax,PGROW); // Step is too small, increase it next time
  else hnext=5.0*h;                                      // No more than a factor of 5 increase.
  x += (hdid=h);
  for (int i=0; i<y.size();i++) y[i] = ytemp[i];
}

template <class container>
template <class functor>
void VRungeKutta<container>::RKTry(container& y, container& dydx, double& x, double h, container& yout, container& yerr, functor& derivs)
  //Given values for variables y[...] and their derivatives dydx[...] known at x, use
  //the fifth-order Cash-Karp Runge-Kutta method to advance the solution over an interval h
  //and return the incremented variables as yout[..]. Also return an estimate of the local
  //truncation error in yout using the embedded fourth-order method. The user supplies the routine
  //derivs(x,y,dydx), which returns derivatives dydx at x.
{
  static double as[] = {0, 0.2, 0.3, 0.6, 1.0, 0.875};
  static double b10 = 0.2, b20=3.0/40.0, b30=0.3, b40 = -11.0/54.0, b50=1631.0/55296.0;
  static double b21 = 9.0/40.0, b31 = -0.9, b41=2.5, b51=175.0/512.0;
  static double b32 = 1.2, b42 = -70.0/27.0, b52=575.0/13824.0;
  static double b43 = 35.0/27.0, b53=44275.0/110592.0;
  static double b54 = 253.0/4096.0;
  static double c0=37.0/378.0,c2=250.0/621.0,c3=125.0/594.0,c5=512.0/1771.0;
  static double dc4 = -277.00/14336.0;
  static double dc0=c0-2825.0/27648.0,dc2=c2-18575.0/48384.0,dc3=c3-13525.0/55296.0,dc5=c5-0.25;
  int N=y.size();
  static container ak2(N),  ak3(N), ak4(N), ak5(N), ak6(N), yt(N);
  
  
  for (int i=0; i<N; i++) yt[i] = y[i] + b10*h*dydx[i]; // First step.
  derivs(x+as[1]*h, yt, ak2);                           // Second step.
  for (int i=0; i<N; i++) yt[i] = y[i] + h*(b20*dydx[i]+b21*ak2[i]);
  derivs(x+as[2]*h,yt,ak3);                             // Third step.
  for (int i=0; i<N; i++) yt[i] = y[i] + h*(b30*dydx[i]+b31*ak2[i]+b32*ak3[i]);
  derivs(x+as[3]*h,yt,ak4);                             // Fourth step.
  for (int i=0; i<N; i++) yt[i] = y[i] + h*(b40*dydx[i]+b41*ak2[i]+b42*ak3[i]+b43*ak4[i]);
  derivs(x+as[4]*h,yt,ak5);                             // Fifth step.
  for (int i=0;i<N;i++)   yt[i] = y[i] + h*(b50*dydx[i]+b51*ak2[i]+b52*ak3[i]+b53*ak4[i]+b54*ak5[i]);
  derivs(x+as[5]*h,yt,ak6);                             // Sixth step.
  // Accumulate increments with proper weights.
  for (int i=0;i<N;i++)   yout[i] = y[i] + h*(c0*dydx[i]+c2*ak3[i]+c3*ak4[i]+c5*ak6[i]);
  // Estimate error as difference between fourth and fifth order methods.
  for (int i=0; i<N; i++) yerr[i] = h*(dc0*dydx[i]+dc2*ak3[i]+dc3*ak4[i]+dc4*ak5[i]+dc5*ak6[i]);
}


#endif//_RUNGEK_