/*
* rkstep.c
* (c) Neil Gershenfeld  3//203
* integrate simple harmonic motion with
*    an adaptive stepper 4th order Runge-Kutta
*/

#include <math.h>
void step();

void step(ystart,yend,step)
	double ystart[2], yend[2], step; {
	double A[2],B[2],C[2],D[2];
	A[0] = step * ystart[1];
	A[1] = -step * ystart[0];
	B[0] = step * (ystart[1] + A[1]/2.0);
	B[1] = -step * (ystart[0] + A[0]/2.0);
	C[0] = step * (ystart[1] + B[1]/2.0);
	C[1] = -step * (ystart[0] + B[0]/2.0);
	D[0] = step * (ystart[1] + C[1]);
	D[1] = -step * (ystart[0] + C[0]);
	yend[0] = ystart[0] + (A[0]+2.0*B[0]+2.0*C[0]+D[0])/6.0;
	yend[1] = ystart[1] + (A[1]+2.0*B[1]+2.0*C[1]+D[1])/6.0;
	}

main() {
	double error,final_err,local_err,err_ave,t,h,
		h_ave,tmax,pi,target,y1[2],y2[2],y3[2],y[2];
	int nsteps;
	printf("Desired error?  ");
	scanf("%lf",&target);
	pi = 4.0 * atan(1.0);
	tmax = 100*pi;
	t = 0;
	nsteps = 0;
	y[0] = 1;
	y[1] = 0;
	error = 0;
	err_ave = 0;
	h = 0.1;
	h_ave = 0;
	while (t < tmax) {
		nsteps += 1;
		step(y,y1,(h/2.0));
		step(y1,y2,(h/2.0));
		step(y,y3,h);
		local_err = y3[0] - y2[0];
		if (fabs(local_err) > target) {
			h = h / 1.3;
			continue;
		}
		y[0] = y2[0];
		y[1] = y2[1];
		t += h;
		err_ave = err_ave + fabs(local_err);
		error = error + fabs(cos(t) - y[0]);
		h_ave = h_ave + h;
		if (fabs(local_err) < (target/10.0))
			h = 1.2 * h;
		}
	error = error / nsteps;
	h_ave = h_ave / nsteps;
	err_ave = err_ave / nsteps;
	final_err = fabs(cos(t) - y[0]);
	printf("step: %g  error: %g  end error %g  loc error: %g\n",
		h_ave, error, final_err, err_ave);
}