function twodof
% Two degrees of freedom with damping and forcing
% See TAM 2030 lectures from 2/7/13 and 2/12/13.
clear all
%Initial conditions and time span
 t0 = 0;    h = .2;  t1 = 100;  %      t1=6.4 is nice    
 x0 = [0 0];  v0 = [ 0 0];  % start from rest with [1 1] is nice.


%Parameters
p.m1 = 1; p.m2 = 1;  % value of masses,    m2  =.44     is nice
p.k1 = 1; p.k2 = 1; p.k3 =1; % springs,    k's = 1,0,1  is nice
p.w1 = 0; p.w2 =0; p.L=0;
p.L10 = 0; p.L20 = 0;p.L30 = 0;
p.c2 = 0;          % damping
p.omega= sqrt(3);  % forcing frequency
p.d     = .1;       % forcing amplitude

n = floor((t1-t0)/h);         % number of steps
tarray = linspace(t0,t1,n);   % set of all times

tnew=t0; znew = [x0 v0]; % the four initial conditions

zarray=zeros(n,4);  zarray(1,:) = znew; % initialize z

%%%%%%%%
%Midpoint method
for i = 2:n
 t    = tarray(i-1);  % the present 'old' time  
 z    = znew;         % the new old is the old new
 zdottemp =  rhs2(t,z,p); % Use equations of motion and kinematics
 znewtemp =  z + zdottemp*h/2;  % half step
 zdot     = rhs2(t+h/2,znewtemp,p);
 znew     =  z   + zdot*h;
 zarray(i,:) = znew;  % store the new values in an array
end
x1array = zarray(:,1); x2array = zarray(:,2);

figure(1)
plot(tarray, x1array, 'r',tarray, x2array, 'b--' )
xlabel('time'); ylabel('both positions');
title('Andy''s plot','fontsize', 20)

figure(2)
plot(x1array, x2array,'r-','linewidth',5 )       %linewidth=5 is nice
%xlabel('time'); ylabel('both positions');
title('Cross plot','fontsize', 10, 'color', 'b')  
title('Be mine!','fontsize', 80, 'color', 'm')  %change title is nice


end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%THIS IS THE RIGHT HAND SIDE FUNCTION FOR THE ODEs
function  zdot=rhs(t,z,p)
% Each line of code is readable.

x=z(1:2);  v=z(3:4);      % Unpack z into variables with good names
x0 = p.d* cos(p.omega*t); % The 'forcing function' 

L1    = x(1) - x0;            % Calculate lengths
L2    = x(2) -(x(1) + p.w1); 
L2dot = v(2) - v(1);
L3    = p.L  - (x(2) + p.w2);

T1  = p.k1 * (L1 - p.L10);    % Calculate tensions
T2s = p.k2 * (L2 - p.L20);
T3  = p.k3 * (L3 - p.L30);
T2d = p.c2 * L2dot;

%The next 3 lines are the 4 ODEs
xdot  = v;   % first two ODEs
v1dot = (T2s + T2d - T1)/p.m1; % These 2 ODEs are ...
v2dot = (T3  - T2d -T2s)/p.m2; % ... F=ma for the two masses

vdot  = [v1dot v2dot];         % Store the 2 v's together

zdot= [xdot vdot];             % four numbers: 2 x's and 2 v's
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%THIS IS THE RIGHT HAND SIDE IN ONE (stupid) LINE OF CODE
function  zdot=rhs2(t,z,p)
zdot = [ z(3), z(4),  ...
         -(p.k2*(p.L20 + p.w1 + z(1) - z(2)) + ...
           p.c2*(z(3) - z(4)) - p.k1*(p.L10 - z(1) + ... 
           p.d*cos(p.omega*t)))/p.m1, ...
         (p.k2*(p.L20 + p.w1 + z(1) - z(2)) - ...
          p.k3*(p.L30 - p.L + p.w2 + z(2))+ ...
          p.c2*(z(3) - z(4)))/p.m2];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


