// rungekutta.h
#include <cassert>
#include <cmath>
using namespace std;

template<class TYPE>
float RungeKutta( TYPE& Object, double h, double xStart, double xEnd, double yStart,
                  double tolorance)
{
  const double  
    
  safety = 0.9, grow = -0.2, shrink = -0.25, 

  a2 = 1.0/4.0, a3 = 3.0/8.0, a4 = 12.0/13.0, a5 = 1.0, a6 = 1.0/2.0,

  b21 = 1.0/4.0,
  b31 = 3.0/32.0, b32 = 9.0/32.0,
  b41 = 1932.0/2197.0, b42 = -7200.0/2197.0, b43 = 7296.0/2197.0,
  b51 = 439.0/216.0, b52 = -8.0, b53 = 3680.0/513.0, b54 = -845.0/4104.0,
  b61 = -8.0/27.0, b62 = 2.0, b63 = -3544.0/2565.0, b64 = 1859.0/4104.0, b65 = -11.0/40.0,

  c1 = 25.0/216.0, c3 = 1408.0/2565.0, c4 = 2197.0/4104.0, c5 = -1.0/5.0,

  e1 =  1.0/360.0, e3 =  -128.0/4275.0, e4 = -2197.0/75240.0, e5 = 1.0/50.0, e6 = 2.0/55.0;

  double k1 = 0, k2 = 0, k3 = 0, k4 = 0, k5 = 0, k6 = 0;

  double x = 0, y = 0, error = 0, yDelta = 0, ratio = 0, hOld = 0;

  x = xStart;
  y = yStart;

  while( x < xEnd ){
    k1 = h * Object.calcDerivative( x, y );
    yDelta = b21*k1;
 
    k2 = h * Object.calcDerivative( x + a2*h, y + yDelta );
    yDelta = b31*k1 + b32*k2;

    k3 = h * Object.calcDerivative( x + a3*h, y + yDelta );
    yDelta = b41*k1 + b42*k2 + b43*k3;

    k4 = h * Object.calcDerivative( x + a4*h, y + yDelta );
    yDelta = b51*k1 + b52*k2 + b53*k3 + b54*k4;

    k5 = h * Object.calcDerivative( x + a5*h, y + yDelta );
    yDelta = b61*k1 + b62*k2 + b63*k3 + b64*k4 + b65*k5;

    k6 = h * Object.calcDerivative( x + a6*h, y + yDelta );
    error = e1*k1 + e3*k3 + e4*k4 + e5*k5 + e6*k6;

    if(error < 0 ) error *= -1.0;

    // check error against tolorance
    ratio = (tolorance * h) / error;
    hOld = h;

    if( ratio >= 1.0 ) {  // ok, increment and move on
      x += h;
      y += c1*k1 + c3*k3 + c4*k4 + c5*k5;
      //cout << "x: " << x << "  y: " << y << "\n";
      // increase stepsize - replace with something better
      //h *= safety * pow(ratio, grow);
     // if( h > 5*hOld) h = 5*hOld;

      h *= 2;
    }

    else {  // need to reduce stepsize
      //h *= safety * pow(ratio, shrink);
      h /= 2;
    }



    // don't want to overrun interval
    if( h > xEnd - x ) h = xEnd - x;

  } // end of while loop

  return y;

}

 





