% Introduction of Finite Elements in Engineering Mechanics 
% Dr. X. Frank Xu, Stevens Institute of Technology
% femem_bardyn_cd1.m 


%----------------------
%Mesh Information in space and time
%----------------------
clear
NumEL = 4; % number of elements
NumNP = NumEL+1; %number of nodal points
NP=zeros(NumEL,2);
for i=1:NumEL
   NP(i,:)=[i i+1]; %Nodes in elements from 1 to NumEL
end
XORD(1:NumNP,1)=[0:20/NumEL:20]'; % X-coordinate of nodal points

%dt=2.4e-6;
dt=40/NumEL*2.483e-6;
T=0.24e-3;
nstep=T/dt;
t=[1:nstep]*dt;
%----------------------
%Specify Boundary Conditions
%----------------------

NPBC=zeros(NumNP,nstep);
NPBC(1)=1; %Displacement of Node 1 is constrained
UBC(1)=0; % Specified displacement for Node 1 (unit: in)


%----------------------
%Specify Material Properties
%----------------------
E(1:NumEL,1)=30e6; % Young's modulus (unit: psi)
A(1:NumEL,1)=1; % Cross section area(unit: in2)
rho(1:NumEL,1)=7.4e-4; %density
L=zeros(NumEL,1);

%--------------------------------------
% IMPLEMENTATION OF BOUNDARY CONDITIONS
%--------------------------------------

 NumEQ=0;
 for I=1:NumNP
      if NPBC(I)==0
         NumEQ=NumEQ+1;
         bc(I)=NumEQ;
      else
         bc(I)=UBC(I);
      end
 end

U=zeros(NumEQ,nstep);
V=zeros(NumEQ,nstep);
AC=zeros(NumEQ,nstep);
F=zeros(NumEQ,nstep);

F(bc(NumNP),:)=-100; % Specified load for Node 41 (unit: bl)
def=zeros(NumNP,nstep);
stress=zeros(NumEL,nstep);

%----------------------
% Form Stiffness Matrix
%----------------------
K=zeros(NumEQ, NumEQ);
M=zeros(NumEQ, NumEQ);
for I=1:NumEL
    Ke=zeros(2,2);
    L(I,1)=XORD(NP(I,2))-XORD(NP(I,1)); % Length (unit: m)
    k=E(I)*A(I)/L(I); % Bar stiffness
    Ke=[k -k;-k k];  %Element stiffness matrix
    Me=rho(I)*A(I)*L(I)/2*[1 0;0 1]; %Lumped mass matrix
     for J1=1:2
            for J2=1:2 
                 if NPBC(NP(I,J1))==0
                   if NPBC(NP(I,J2))==0
                     K(bc(NP(I,J1)), bc(NP(I,J2)))=K(bc(NP(I,J1)), bc(NP(I,J2)))+Ke(J1,J2);
                     M(bc(NP(I,J1)), bc(NP(I,J2)))=M(bc(NP(I,J1)), bc(NP(I,J2)))+Me(J1,J2);
                   else
                     F(bc(NP(I,J1)),1)=F(bc(NP(I,J1)),1)-Ke(J1,J2)*UBC(NP(I,J2));
                   end
                 end
               end
     end
  end

%----------------------
% Solve equations in multiple steps of time 
%----------------------
    
   AC(:,1)=M\(F(:,1)-K*U(:,1));
   UP=U(:,1)-dt*V(:,1)+dt^2/2*AC(:,1);
   U(:,2)=M\(dt^2*F(:,1)+(2*M-dt^2*K)*U(:,1)-M*UP);
    for I=1:NumNP
       if NPBC(I)==0
         def(I,2)=U(bc(I));
       else def(I,2)=bc(I);
       end
    end
   stress(1:NumEL,2)=E(1:NumEL).*(def(NP(1:NumEL,2),2)-def(NP(1:NumEL,1),2))./L(1:NumEL); 

  for n=3:nstep
      n
      U(:,n)=M\(dt^2*F(:,n-1)+(2*M-dt^2*K)*U(:,n-1)-M*U(:,n-2));
      AC(:,n-1)=M\(F(:,n-1)-K*U(:,n-1));
      V(:,n-1)=(U(:,n)-U(:,n-2))/2/dt;
      for I=1:NumNP
       if NPBC(I)==0
         def(I,n)=U(bc(I),n);
       else def(I,n)=bc(I);
       end
      end
     stress(1:NumEL,n)=E(1:NumEL).*(def(NP(1:NumEL,2),n)-def(NP(1:NumEL,1),n))./L(1:NumEL);
  end
  % Frequency
  [vector f]=eigs(K,M);
  figure,plot(t,stress(NumEL/2,:))