%EA3-Fall
% 
%%---------------------------------------------------------
%%Example: spring-mass-spring-mass system
%%---------------------------------------------------------


clear all;
close all;

global A;				%share this with function rate_fn which computes Xdot
%%
%%System parameters
K1	=	3;					%element 1 - spring
m2	=	1;					%element 2 - mass
K3	=	2;					%element 3 - spring
m4	=	1;					%element 4 - mass


%
%State Variables:
%	X(1,:) = stretch of spring element 1
%	X(2,:) = velocity of mass element 2
%	X(3,:) = stretch of spring element 3
%	X(4,:) = velocity of mass element 4

% Initial conditions
i	=	1;					%time index
%X(1,1) = 0.5; X(2,1) = 0; X(3,1) = -0.2; X(4,1) = 0;
%
%normal mode
%X(1,1) = 0.7071; X(2,1) = 0; X(3,1) = 0.7071; X(4,1) = 0;
%%normal mode
X(1,1) = -0.5547; X(2,1) = 0; X(3,1) = 0.8321; X(4,1) = 0;
%
%normal mode
%X(1,1) = 0; X(2,1) = 0.5; X(3,1) = 0; X(4,1) = 1;
%

%Energy considerations
PotEnergy(i)=0.5*(K1*X(1,i)*X(1,i)+K3*X(3,i)*X(3,i));	%potential energy in the springs
KinEnergy(i)=0.5*(m2*X(2,i)*X(2,i)+m4*X(4,i)*X(4,i));	%kinetic energy in the masses
TotEnergy(i)=PotEnergy(i)+KinEnergy(i);					%total energy in the system
			

%time parameters
start_time = 0;
dt	=	0.01;				%time step chosen
final_time	=	20;			%time to end calculation 
time(i)	=	start_time;		%let us keep a vector of times for plotting

%State matrix
A = [0,			1,	 0, 	0;
	 -K1/m2,	0,	K3/m2,	0;
	 0,			-1,	0,		1;
	 0,			0,	-K3/m4,	0];   % for use in equation Xdot = A X


% Integrate equations of motion using Runge-Kutta algorithm

for t = start_time+dt:dt:final_time

        k1 = rate_fn(X(:,i));			% current rate

        k2 = rate_fn(X(:,i) + dt*k1/2);	% estimated mid-point rate

        k3 = rate_fn(X(:,i) + dt*k2/2);	% even better mid-point rate

        k4 = rate_fn(X(:,i) + dt*k3);	% excellent end-point rate

        X(:,i+1) = X(:,i) + dt*(k1/6 + k2/3 + k3/3 + k4/6);	% use weighted average of rates
		
      	time(i+1) = time(i) + dt;
		
		i = i+1;
		
		%energy
		PotEnergy(i)=0.5*(K1*X(1,i)*X(1,i)+K3*X(3,i)*X(3,i));
		KinEnergy(i)=0.5*(m2*X(2,i)*X(2,i)+m4*X(4,i)*X(4,i));
		TotEnergy(i)=PotEnergy(i)+KinEnergy(i);	
			
  
end


%%Plot the stretches of the springs and energy
comet(time(:),X(1,:))
pause
subplot(2,1,1), plot(time(:),X(1,:)), title('Spring 1'), grid
subplot(2,1,2), plot(time(:),X(3,:)), title('Spring 3'), xlabel('Time'), grid
%
%figure
%plot(time(:),PotEnergy(:),time(:),KinEnergy(:),time(:),TotEnergy(:)), title('Energy'),grid