%Spring-block system with no damping or friction
%%---------------------------------------------------------
%%Matrix implementation of the spring-mass system
%%Compares Forward Euler, Midpoint Rate, Average Rate, and Runge-Kutta algorithms
%%also includes analytical solution
%%---------------------------------------------------------

clear all;
close all;
global A;					%share this with function rate_fn which computes Xdot
%%
%%System parameters
m	=	2;					%mass of block
K	=	20;					%spring constant of spring
w=sqrt(K/m);				%calculate angular frequency of oscillation
period = 2*pi/w;			%calculate period of oscillation
%
% Initial conditions
i	=	1;					%time index
rx0 = 0.1;					%initial position
vx0 = 0;					%initial velocity

%%store calculated positions in **_x(1,i) and calculated velocities in **_x(2,i) 
%%for different times pointed to by index 'i'
rk_x(1,i)	=	rx0; rk_x(2,i)	=	vx0;		%stores runge kutta solution
fe_x(1,i)	=	rx0; fe_x(2,i) = vx0;			%stores forward euler solution
avg_x(1,i) = rx0; avg_x(2,i) = vx0;				%stores average rate solution
mp_x(1,i) = rx0; mp_x(2,i) = vx0;				%stores midpoint rate solution
an_x(1,i) = rx0; an_x(2,i) = vx0;				%stores analytical solution values for comparison

%error calculations
amplitude_E=sqrt((vx0/w)^2+rx0^2); 					%amplitude of oscillation
error_rk(i) = (rk_x(1,i)-an_x(1,i))/amplitude_E;	% stores normalized error using Runge Kutta
error_avg(i) = (avg_x(1,i)-an_x(1,i))/amplitude_E;	% stores normalized error using Average rate
error_mp(i) = (mp_x(1,i)-an_x(1,i))/amplitude_E;	% stores normalized error using Midpoint rate
error_fe(i) = (fe_x(1,i)-an_x(1,i))/amplitude_E;	% stores normalized error using Forward Euler

%
%time parameters
start_time = 0;
dt	=	period/100;				%time step chosen as a fraction of one period
final_time	=	20*period;		%end calculation after twenty periods
time(i)	=	start_time;			%let us keep a vector of times for plotting

%
A = [0, 1; -K/m, 0];            % for use in equation Xdot = A X

%

% Integrate equations of motion using the various methods

for t = start_time+dt:dt:final_time

		%%Analytical solution
		an_x(1,i+1) = (vx0/w)*sin(w*t)+rx0*cos(w*t);					%analytical position
		an_x(2,i+1) = vx0*cos(w*t)-w*rx0*sin(w*t);						%analytical velocity

		%%Forward Euler method
		k1 = rate_fn(fe_x(:,i));					%current rate
		fe_x(:,i+1) = fe_x(:,i) + dt*k1;			%next value using current rate
		
		%%Mid-point Rate method
		k1 = rate_fn(mp_x(:,i));					%current rate
		k2 = rate_fn(mp_x(:,i)+dt*k1/2);			%estimate for mid-point rate
		mp_x(:,i+1) = mp_x(:,i) + dt*k2;			%next value using midpoint rate rate
		
		%%Average Rate method
		k1 = rate_fn(avg_x(:,i));					%current rate
		k2 = rate_fn(avg_x(:,i)+dt*k1);				%estimate for next rate
		avg_x(:,i+1) = avg_x(:,i) + dt*(k1+k2)/2;	%next value using average rate
				
		%%Runge Kutta method
        k1 = rate_fn(rk_x(:,i));					% current rate
        k2 = rate_fn(rk_x(:,i) + dt*k1/2);			% estimated mid-point rate
        k3 = rate_fn(rk_x(:,i) + dt*k2/2);			% better mid-point rate
        k4 = rate_fn(rk_x(:,i) + dt*k3);			% excellent end-point rate
        rk_x(:,i+1) = rk_x(:,i) + dt*(k1/6 + k2/3 + k3/3 + k4/6);	% next value using weighted average of rates

		%%Error in position using various algorithms
		error_rk(i+1) = (rk_x(1,i+1)-an_x(1,i+1))/amplitude_E;		%error using Runge Kutta     
		error_avg(i+1) = (avg_x(1,i+1)-an_x(1,i+1))/amplitude_E;	%error using Average Rate	     
		error_mp(i+1) = (mp_x(1,i+1)-an_x(1,i+1))/amplitude_E;		%error using Midpoint Rate	     
		error_fe(i+1) = (fe_x(1,i+1)-an_x(1,i+1))/amplitude_E;		%error using Forward Euler	     

		%%increment time and do next step
        time(i+1) = time(i) + dt;
		i = i+1;
end

%Plot the position and error
subplot(4,1,1), plot(time(:),rk_x(1,:),time(:),an_x(1,:)), title('Runge Kutta'), grid
%subplot(4,1,2), plot(time(:),rk_x(2,:),time(:),an_x(2,:)), title('Runge Kutta'), grid
subplot(4,1,2), plot(time(:),avg_x(1,:),time(:),an_x(1,:)), title('Average Rate'), grid
subplot(4,1,3), plot(time(:),mp_x(1,:),time(:),an_x(1,:)), title('Midpoint Rate'), grid
subplot(4,1,4), plot(time(:),fe_x(1,:),time(:),an_x(1,:)), title('Forward Euler'),xlabel('Time'), grid
pause
figure
subplot(4,1,1),plot(time(:),error_rk(:)), title('Runge-Kutta Error'), grid
subplot(4,1,2),plot(time(:),error_avg(:)), title('Average Rate Error'), grid
subplot(4,1,3),plot(time(:),error_mp(:)), title('Midpoint Rate Error'), grid
subplot(4,1,4),plot(time(:),error_fe(:)), title('Forward Euler Error'), xlabel ('Time'), grid