% Demo 3: 
% ODE Solver using z as an array
% MAE 2030, Written in Lecture on Monday Jan 28, 2019
% Comments added after lecture, a few errors corrected after lecture
% Some other tidying of code.

% Assume consistent units, like meters, kg, s, and Newtons
clear  % A good habit to put this at the top.  Helps debugging. 

% Part 1: Setup and asking for solution
% Part 2: Euler's method of soln, for ANY set of ODEs
% Part 3: The right hand side file, (which ODE we are solving).

% NOTE: The state z is a _column_ list of numbers.
% The state at each time is a _row_ of the matrix zarray
% Why?  No good reason.  It just matches other Matlab built-in functions

% Demo3.m shows this same code written more compactly and without comments
% It is a _bad_ version.  But, because it doesn't have all of the
% intermediate variables, and it doesn't have all of the comments, it looks
% less imposing.  I wrote it to show that there really isn't much here. It
% just looks like a lot because of the attempt to make the code clear.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PART 1: General Setup
p.m  = 1;  p.k = 1;                    %  Parameters mass and stiffness
p.omegaf= 0; p.g = 0; p.c =0; p.delta =0; p.L0 = 0;
tend = 2 * pi;                          %  Ending time "t end"
npointspers = 50;                    %  number of h intervals per time unit
ntimes      = tend * npointspers + 1; %  total number of time points
tarray = linspace(0, tend, ntimes);   %  All of the times soln is output
x0 = 1; v0 = 0;     % Inititial conditions (ICs)
z0 = [x0;v0];       % ICs, packed into a column vector

% Next 3 lines gets the solution to the ODES, and plots it 
[tarray zarray]  = myMidpointSolver(@myrhs, tarray, z0, p);
%[tarray zarray]  = myEulerSolver(@myrhs, tarray, z0, p);

x = zarray(:,1); v = zarray(:,2);   % Extract the two columns of zarray
plot (x,v); axis 'equal'; shg       % "cross plot" or "phase plane"
x(end)-1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PART II: A. Numerical method of solution, Euler
function  [tarray, zarray]   = myEulerSolver(eqn, tarray, z0, p)
ntimes = length(tarray);     % Didn't pass it in, so need to find it
zarray = zeros(ntimes, length(z0));  % Memory allocation for solution
zarray(1,:) = z0';     % First row of soln is the initial conditions
 for i  = 2:ntimes     % Once through the loop for each time interval
   h    = tarray(i) - tarray(i-1);       % size of time step
   z    = zarray(i-1,:)';   t = tarray(i);  % set present values
   zdot = eqn(t,z,p);     % evaluate RSH (right hand side) at present time
     % The heart of this whole Script is the next line of code
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     znew = z + zdot * h;   % one Euler-method time step.
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 zarray(i,:) = znew'; % Add a row to the array of solutions. 
                      % Recall, z is a column, zarray is in rows.
 end % loop
end  % function

% PART II: B. Numerical method of midpoint
function  [tarray, zarray]   = myMidpointSolver(eqn, tarray, z0, p)
ntimes = length(tarray);     % Didn't pass it in, so need to find it
zarray = zeros(ntimes, length(z0));  % Memory allocation for solution
zarray(1,:) = z0';     % First row of soln is the initial conditions
 for i  = 2:ntimes     % Once through the loop for each time interval
   h    = tarray(i) - tarray(i-1);       % size of time step
   z    = zarray(i-1,:)';   t = tarray(i);  % set present values
   zdot = eqn(t,z,p);     % evaluate RSH (right hand side) at present time
    ztemp = z + zdot * h/2;   % one Euler-method time step.
   zdot = eqn(t+h/2,ztemp,p);     % evaluate RSH (right hand side) at present time
   znew = z + zdot * h;   % one Euler-method time step.
 
 zarray(i,:) = znew'; % Add a row to the array of solutions. 
                      % Recall, z is a column, zarray is in rows.
 end % loop
end  % function






%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PART III:  The differential equations
function zdot = myrhs(t,z,p)
m= p.m; k = p.k;  c = p.c;  omegaf = p.omegaf; g = p.g; L0= p.L0;
delta = p.delta;
x = z(1); v= z(2);   % Unpack z into understandable variables

% Tne next two lines are the ODEs, written with sensible variables
  xdot = v;            % Basic kinematics
  vdot = (1/m)*(k*(-x+L0+ delta* sin(omegaf*t) )-c*v +m*g);   % Spring force causes acceleration
  
zdot = [xdot;vdot];  % Pack zdot into a column vector
end %function