clear all
close all
clc

%--------------------------------------------------------
%               Linear Stiff String :
%     Compare Implicit (CFL) vs Explicit (proposed)
%              Input Number of Grid Points
%           Raised Cosine Initial Conditions
%                  Dr M Ducceschi
%              University of Bologna
%                  22 Dec 2021
%--------------------------------------------------------

% Custom Parameters (user can modify these)
%--------------------------------------------------------

M          = 100 ;            % number of grid points 
T          = 0.1  ;           % time of simulation   [s]

%-- string parameters
rho         = 8000 ;          % string density       [Kg/m^3]
L           = 1.0 ;           % string length        [m]
E           = 2e11 ;          % Young's modulus      [Pa]
rd          = 3e-4 ;          % radius               [m]
T0          = 40 ;            % tension              [N]
Amp         = 0.01 ;          % initial raised cosine amplitude [m]

%-- output parameters
rp          = 0.32 ;          % readout point        [frac of L]


schemeType  = 2 ;             % 1 = implicit D4 (CFL condition) ; 2 = explicit D4 ;

%-- flags
EnCheck     = 0 ;             % 1 = check eneregy conservation
flagPlot    = 0 ;             % 1 = liveplotting
plotRefresh = 5 ;             % plot refresh rate    [samples]
% End Custom Parameters
%--------------------------------------------------------


% Derived Parameters (do not modify below)
%--------------------------------------------------------
A           = pi * rd * rd ;                    %-- area
I           = pi / 4 * rd * rd * rd * rd ;      %-- moment of inertia
EI          = E * I ;
rA          = rho * A;
h           = L / M ;

if schemeType == 1
    k = h * sqrt(rA/T0) ;
end

if schemeType == 2
    k       = h^2 * sqrt(rA/(T0*h^2+4*EI)) ;
end

Ts          = floor( T / k);                %-- number of samples
tv          = (0:Ts-1)*k ;                  %-- time array
rp          = round(rp*L/h) ;




%-- matrices
Id          = speye(M-1,M-1);
Dv          = ones(M,1);
D2          = 1/h^2*spdiags([Dv, -2*Dv, Dv],-1:1,M-1,M-1);
D4          = D2^2;
Dxp         = 1/h*spdiags([-Dv, Dv],0:1,M-1,M);
Dxm         = -Dxp.';

if schemeType == 1
    Cp = rA*Id/k^2 + 0.5*EI*D4  ;
    C0 = 2*rA/k^2*Id + T0*D2 ;
    Cm = -rA*Id/k^2 - 0.5*EI*D4 ;
end

if schemeType == 2
    Cp = rA/k^2*Id   ;
    C0 = 2*rA/k^2*Id + T0*D2 - EI*D4 ;
    Cm = -rA/k^2*Id  ;
end


% End Derived Parameters
%--------------------------------------------------------


%--------------------------------------------------------
% Initialise
ovec        = ones(M,1) ;
u0          = zeros(M-1,1) ;
%-- raised cosine initial conditions
sigRaised   = 0.1 ;
for m = 1 : M - 1
    if m*h >= 0.5*L-sigRaised && m*h <= 0.5*L + sigRaised
        u0(m) = 0.5*Amp * (1 + cos(2*pi*(m*h-0.5*L)/2/sigRaised/L)) ;
    end
end
um          = u0;


outu        = zeros(Ts,1) ;
H           = zeros(Ts,1) ;
% End Initialise
%--------------------------------------------------------


%--------------------------------------------------------
% Main Loop
tic
for n = 1 : Ts
    
    %-- build g's
    
    up = Cp \ (C0*u0 + Cm*um) ;
    
    outu(n) = up(rp) ;
    
    
    %-- Energy Calc
    if EnCheck == 1
        if schemeType == 1
            H(n) = 0.5 * rA * (up-u0).' * (up-u0) / k^2 + 0.25 * EI * (D2*(up-u0)).' * (D2*(up-u0)) + ...
                0.5 * T0 * (Dxm * up).' * (Dxm * u0) + 0.5 * EI * (D2 * up).' * (D2 * u0) ;
        else
            H(n) = 0.5 * rA * (up-u0).' * (up-u0) / k^2  + ...
                0.5 * T0 * (Dxm * up).' * (Dxm * u0) + 0.5 * EI * (D2 * up).' * (D2 * u0) ;
        end
    end
    
    
    %-- update
    um          = u0 ;
    u0          = up ;
    
    
    %-- live plotting
    if flagPlot == 1
        if mod(n,plotRefresh) == 0 || n == 1
            
            cla ;
            hold on ;
            plot((0:M)*h,[0;um;0],'linewidth',1.5,'color','k')
            hold off;
            ylim([-0.01,0.01]); xlim([0,M*h]);
            xlabel('x [m]'); ylabel('u(x) [m]');  drawnow;
        end
    end
end
toc


if EnCheck == 1
    subplot(2,1,1)
    plot(tv,outu/1e-3,'k') ;
    xlabel('t (s)'); ylabel('u (mm)') ;
    title('output displacement') ;
    subplot(2,1,2)
    plot(tv,1-H/H(1),'k') ;
    xlabel('t (s)'); ylabel('\Delta H') ;
    title('Energy Error') ;
else
    plot(tv,outu/1e-3,'k') ;
    xlabel('t (s)'); ylabel('u (mm)') ;
    title('output displacement') ;
end