#!-/usr/bin/python
# Runge-Kutta (4th order) method of Solving the following System
# Harmonic Oscillator
# y''(t) + y(t) = 0
# y(0)=1, 	y'(0)=0

import matplotlib.pyplot as plt
import numpy as np

global y, error, t, ynew, yprev, values
y = np.array([0,0])
values = np.array([[],[]])
val_t = np.array([])
val_calc = np.array([])
error = 0	# initialize error
error_max = 0.01
error_min = 0.00001
errorEval = True

h = .1		# step size
hStep = 1.1	# step size adaptive constant
tmax = 100*np.pi	# max time


t = 0
ynew = 0
yprev = 1
k1 = 0
k2 = 0
k3 = 0
k4 = 0

def rk4_values(step, time, preVal):
	k1 = step*slope(time,preVal)
	k2 = step*slope(time+step/2, preVal+k1/2)	#looks like there will be no difference for R-K
	k3 = step*slope(time+step/2, preVal+k2/2)
	k4 = step*slope(time+step, preVal+k3)
	
	ynew = preVal + k1/6 + k2/3 + k3/3 + k4/6
	return ynew


def slope(x,y):
	return -np.sin(x)

while t < tmax :
	#check step size
	while (errorEval):
		#nominal Step
		ynom_new = rk4_values(h,t,yprev)

		#half step 1
		y_half1 = rk4_values(h/2, t, yprev)

		#half step 2
		y_half2 = rk4_values(h/2+h/2, t, y_half1)

		#check error
		errorCheck = np.absolute(ynom_new - y_half2)
		if errorCheck < error_min:
			h = h*hStep
		elif errorCheck > error_max:
			h = h*1/hStep
		else :
			h = h
			errorEval = False
		print errorCheck

	errorEval = True	#need to reset for next time step

	#now actually calculate 
	ynew = rk4_values(h, t, yprev)
	yprev = ynew


	val_t = np.append(val_t, t)
	val_calc = np.append(val_calc, ynew)
	error = error + np.absolute(np.cos(t) - ynew)
	
	t+= h

error_avg = error/len(val_t)
error_final = np.absolute(np.cos(val_t[len(val_t)-1]) - val_calc[len(val_calc)-1])

print "Step Size: %f" % h
print "average error: %f" % error_avg
print "final error: %f" % error_final
print "number of steps %d" % len(val_t) 	

print "final slope: %f" % slope(t-h, 0)
plt.plot(val_t, val_calc)
plt.show()