%EA3
% 
%%---------------------------------------------------------
%%Example: Weakly-coupled systems
%%---------------------------------------------------------


clear all;
close all;

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

%
%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
%	X(5,:) = stretch of spring element 5

%time parameters
i	=	1;					%time index
start_time = 0;
dt	=	.2;				%time step chosen
final_time	=	200;			%time to end calculation 
time(i)	=	start_time;		%let us keep a vector of times for plotting
% Initial conditions
X(1,1) = 0.1;
X(2,1) = 0;
X(3,1) = 0;
X(4,1) = 0;
X(5,1) = 0;

%Energy considerations
Energy(1,i)=0.5*(K1*X(1,i)*X(1,i));	%potential energy in the spring element 1
Energy(2,i)=0.5*(m2*X(2,i)*X(2,i));	%kinetic energy in the mass element 2
Energy(3,i)=0.5*(K3*X(3,i)*X(3,i));	%potential energy in the spring element 3
Energy(4,i)=0.5*(m4*X(4,i)*X(4,i));	%kinetic energy in the mass element 4
Energy(5,i)=0.5*(K5*X(5,i)*X(5,i));	%potential energy in the spring element 5
		

%State matrix
A = [0,			1,		 0, 	0,		0;
	 -K1/m2,	0,		K3/m2,	0,		0;
	 0,			-1,		0,		1,		0;
	 0,			0,		-K3/m4,	0,		K5/m4;
	 0,			0,		0,		-1,		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
		Energy(1,i)=0.5*(K1*X(1,i)*X(1,i));	%potential energy in the spring element 1
		Energy(2,i)=0.5*(m2*X(2,i)*X(2,i));	%kinetic energy in the mass element 2
		Energy(3,i)=0.5*(K3*X(3,i)*X(3,i));	%potential energy in the spring element 3
		Energy(4,i)=0.5*(m4*X(4,i)*X(4,i));	%kinetic energy in the mass element 4
		Energy(5,i)=0.5*(K5*X(5,i)*X(5,i));	%potential energy in the spring element 5
		  
end

%%Plot the stretches of the springs and the energy in various parts of the system
%comet(time(:),X(1,:))
%pause
figure
subplot(3,1,1), plot(time(:),X(1,:)), title('Spring 1'), grid
subplot(3,1,2), plot(time(:),X(3,:)), title('Spring 3'), grid
subplot(3,1,3), plot(time(:),X(5,:)), title('Spring 5'), xlabel('Time'), grid
pause
figure
subplot(2,1,1), plot(time(:),Energy(1,:)+Energy(2,:)),title('Energy in Left Oscillator'),grid
subplot(2,1,2), plot(time(:),Energy(4,:)+Energy(5,:)),title('Energy in Right Oscillator'),xlabel('Time'),grid
%subplot(3,1,3), plot(time(:),Energy(3,:)),title('Energy in the Coupling Spring'),grid