/* Note: all floats have been changed to doubles in this function, and
 * all memory allocations using the function vector() have been changed
 * to dvector() which allocates doubles instead of floats. */

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

#define MIN_STEP_SIZE 1.0e-5
#define PGROW -0.20
#define PSHRNK -0.25
#define FCOR 0.06666666		/* 1.0/15.0 */
#define SAFETY 0.9
#define ERRCON 6.0e-4

long int display_count = 0;     /* counter used to show run-time progress  */

extern void rk4();

void rkqc(y,dydx,n,x,htry,eps,yscal,hdid,hnext,ptr_derivs)
double y[],dydx[],*x,htry,eps,yscal[],*hdid,*hnext;
void (*ptr_derivs)(double,double *,double *);
int n;
{
	int i;
	double xsav,hh,h,temp,errmax;
	double *dysav,*ysav,*ytemp,*dvector();
	void nrerror(),free_dvector();

	dysav=dvector(1,n);
	ysav=dvector(1,n);
	ytemp=dvector(1,n);
	xsav=(x[0]);
	for (i=1;i<=n;i++) {
		ysav[i]=y[i];
		dysav[i]=dydx[i];
	}
	h=htry;
	for (;;) {
	        /* Display current simulation time on screen. */
	        if((display_count % 100) == 0)
		  {
		    printf("%11.8lf", x[0]);
		    fflush(stdout);
		    printf("\b\b\b\b\b\b\b\b\b\b\b");
		  }
		display_count++;

		hh=0.5*h;
		rk4(ysav,dysav,n,xsav,hh,ytemp,ptr_derivs);
		x[0]=xsav+hh;
		(*ptr_derivs)(x[0],ytemp,dydx);
		rk4(ytemp,dydx,n,x[0],hh,y,ptr_derivs);
		x[0]=xsav+h;
		if (x[0] == xsav) nrerror("Step size too small in routine RKQC");
		rk4(ysav,dysav,n,xsav,h,ytemp,ptr_derivs);
		errmax=0.0;
		for (i=1;i<=n;i++) {
			ytemp[i]=y[i]-ytemp[i];
			temp=fabs(ytemp[i]/yscal[i]);
			if (errmax < temp) errmax=temp;
		}
		errmax /= eps;

		if (errmax <= 1.0) 
		  {
		    hdid[0]=h;
		    hnext[0]=(errmax > ERRCON ?
			      SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
		    break;
		  }
		
		/* If the current step size is less than or equal to the minimum
		 * step size allowed, do not reduce the step size any further.
		 * This precaution is necessary due to the discontinuities in 
		 * the non-linear model (e.g. stiction, amp current, etc.). */
		if (h <= MIN_STEP_SIZE)
		  {
		    fprintf(stderr, "\n WARNING: minimum step size reached!\n");
		    printf("\n Current Time: ");
		    hdid[0] = h;
		    hnext[0] = h;
		    break;
		  }

		h=SAFETY*h*exp(PSHRNK*log(errmax));
		
	}
	for (i=1;i<=n;i++) y[i] += ytemp[i]*FCOR;
	free_dvector(ytemp,1,n);
	free_dvector(dysav,1,n);
	free_dvector(ysav,1,n);
}

#undef MIN_STEP_SIZE
#undef PGROW
#undef PSHRNK
#undef FCOR
#undef SAFETY
#undef ERRCON