# (c) Rory Clune 3/4/2011
# 4th order Runge Kutta method with adaptive step size to solve pendulum with periodic base motion

from numpy import *
from pylab import *
#Physical properties
g = 10; l=1; A = 6; w = 1

h = 0.01
tmax = 10*pi

npoints = (int)(tmax/h+1)
yplot = tplot = array([0])

y = [1,0]#rotation angle & its slope
y1 = [1,0]
y2 = [1,0]
y3 = [1,0]
allowable_error = 0.0001
h_average = 0
steps = 0

def RK_timestep(yLower, yUpper, step):
	global y1, y2, y3
	k = zeros((2,4))
	k[0,0] = step*yLower[1]
	k[1,0] = -step * (((g+A*sin(w*t)) / l) * sin(yLower[0]))
	k[0,1] = step*(yLower[1]+k[1,0]/2)
	k[1,1] = -step * (((g+A*sin(w*(t+step/2.0))) / l) * sin(yLower[0]+k[0,0]/2))	
	k[0,2] = step*(yLower[1]+k[1,1]/2)
	k[1,2] = -step * (((g+A*sin(w*(t+step/2.0))) / l) * sin(yLower[0]+k[0,1]/2))	
	k[0,3] = step*(yLower[1]+k[1,2])
	k[1,3] = -step * (((g+A*sin(w*(t+step))) / l) * sin(yLower[0]+k[0,2]))	
	yUpper[0] = yLower[0] + k[0,0]/6 + k[0,1]/3 + k[0,2]/3 + k[0,3]/6
	yUpper[1] = yLower[1] + k[1,0]/6 + k[1,1]/3 + k[1,2]/3 + k[1,3]/6	
	return

t = 0
while (t<tmax):    
	RK_timestep(y, y1, h/2.0)
	RK_timestep(y1, y2, h/2.0)
	RK_timestep(y, y3, h)
	local_error = fabs(y3[0]-y2[0])
	if(local_error > allowable_error):
		h = h/2
	elif(local_error < 0.2*allowable_error):
		h = 1.1*h
	y[0] = y2[0]+(y2[0]-y3[0])/15
	y[1] = y2[1]+(y2[0]-y3[0])/15
	h_average += h
	t+=h
	steps+=1
	yplot = append(yplot, y[0])
	tplot = append(tplot, t)	
	
print "Average step size = " + str(t/steps)
plot(tplot,yplot)
axis([0,t,-2,2])
show()