clear all
close all
clc

%--------------------------------------------------------
%              Geometrically Exact
%            Nonlinear Wave Equation:
%        Linearly-Implicit, Conservative Scheme
%               Mixed FD / Modal
%               Raised Cosine Source
%                Dr M Ducceschi
%            University of Bologna
%                  22 Dec 2021
%--------------------------------------------------------

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

fs0         = 48e3 ;           % base sample rate     [Hz]
T           = 0.01  ;           % time of simulation   [s]
OF          = 10 ;             % oversampling factor

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


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


%--raised cosine conditions
Amp         = 5e-3 ;             % maximum force         [N]
sig         = 0.1 ;

%-- flags
flagLin     = 0 ;             % 1 = linear string dynamics
flagPlot    = 0 ;             % 1 = liveplotting
plotRefresh = 5*OF ;          % plot refresh rate    [samples]


typeFlag    = 0 ;             % eta param [0 = explicit D4, 1 = implicit D4] ;

NITER       = 5 ;
% 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          = 0 * E * I ;
EA          = E * A ;
rA          = rho * A;

fs          = OF * fs0 ;                    %-- sample rate
k           = 1 / fs;                       %-- time step
Ts          = floor(fs * T);                %-- number of samples
tv          = (0:Ts-1)*k ;                  %-- time array


hsq         = ( T0*k^2  ) + sqrt( ( T0*k^2 )^2 + 16*rA*EI*k^2 ) ;
hsq         = hsq / 2 / rA ;
h           = 1.5 * sqrt(hsq) ;            %-- grid spacing (from stability condition)
h           = sqrt(E/rho) * k ;
M           = floor(L / h)  ;               %-- number of grid points
h           = L / M;
rp          = round(rp*M) ;



%-- some constants
rAhhLksq    = 0.25*rA/k/k*L ;
rAksq       = rA/k/k ;

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


%-- number of modes
om          = 0;
Nmodes      = 0;
MatCos      = [];
MatSin      = [];
omv         = [] ;
while om <  fs * 2
    Nmodes      = Nmodes + 1 ;
    om          = sqrt(EA / rA) * Nmodes * pi / L ;
    omv         = [omv;om] ;
    vec         = sin(Nmodes*pi*h*((1:M-1).')/L);
    MatSin      = [MatSin,vec];
end


MatCos      = Dxm*MatSin ;
omv         = omv(1:end-1) ;
IdMod       = speye(Nmodes,Nmodes) ;
%Lambda      = diag((1:Nmodes)*pi/L) ;
dd          = sqrt(diag(-2*h/L*MatSin.'*Dxx*MatSin)) ;
Lambda      = diag(dd) ;            %-- reflects reviewer's comment
M0Mod       = L/2*(rA*IdMod/k^2 ) ;
MatSig0L    = IdMod ;
Mat00Mod    = M0Mod  ;
vecMod      = T0*L/2*(dd.^2) ;
vecMatMod   = sparse(diag(vecMod)) ;
MatCosP     = MatCos.' ;



%--initial conditions
x           = 1 : M - 1;
fRW         = zeros(1,M - 1);
xR          = floor((0.5-sig) * (M - 1)) : ceil((0.5+sig) * (M - 1));
x0          = 0.5 * (M - 1);
xW          = 2*(M - 1)*sig;
fRW(xR)     =  0.5 * Amp * (1 + cos( 2 * pi * ( xR - x0 ) / xW ) );

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


%--------------------------------------------------------
% Initialise

%--initial conditions

ovec        = ones(M,1) ;
u0          = fRW.' ;
s0          = zeros(Nmodes,1) ;
um          = u0;
sm          = s0;

q0          = Dxm*u0;
p0          = MatCos*s0;
qm          = Dxm*um;
pm          = MatCos*sm;

avec        = sqrt( (ovec + 0.5*(p0+pm)).^2 + (0.5*q0+0.5*qm).^2 ) ;
phi         = 0.5 * (EA-T0) * ((avec-ovec).^2 ) ;
chim        = sqrt(2*abs(phi)) ;

Et          = zeros(Ts,1);

outu        = zeros(Ts,1);
outz        = zeros(Ts,1);

time_elapsed = zeros(NITER,1) ;

% End Initialise
%--------------------------------------------------------


%--------------------------------------------------------
% Main Loop
for ncount = 1 : NITER
    ncount
    tic
    for n = 1 : Ts
        
        %-- build g's
        
        %--(nonlinear WE)
        q0          = Dxm*(u0) ;
        p0          = MatCos*s0 ;
        avec        = sqrt( (ovec+p0).^2 + q0.^2 ) ;
        
        g           = sqrt(EA - T0) * (  q0 ./ avec )  ;
        f           = sqrt(EA - T0) * ( (ovec + p0) ./ avec )   ;
        gsq         = g.*g ;
        fsq         = f.*f ;
        
        if flagLin == 1
            g = 0*g ;
            f = 0*f ;
        end
        
        
        %-- Build Loop Matrices
        Lg          = sparse(diag(g)) ;
        Lf          = sparse(diag(f)) ;
        
        B1          = Dxp*Lg ;
        B2          = -MatCosP*Lf ;
        
        Mat1s       = 0.25*h*B1*(B1.')    ;
        Mat2s       = 0.25*h*B2*(B2.') ;
        Mat11       = Mat00 + Mat1s ;
        Mat12       = 0.25*h*B1*(B2.') ;
        Mat21       = Mat12.' ;
        Mat22       = Mat00Mod + Mat2s ;
        
        %-- Block LU decomposition
        z10         = (rA/k^2)*h*(Id)*(2*u0 - um) + h*Dxx*(T0*u0) - EI*h*Dxxxx*u0  + Mat12*sm + h*Dxp*(chim.*g) ;
        b2          = M0Mod*(2*s0 - sm) - vecMod.*s0  + Mat21*um + Mat2s*sm  - h*(MatCosP)*(chim.*f ) ;
        z1          = z10 + Mat1s*um  ;
        y1          = Mat11\z1 ;
        z2          = -Mat21*y1 + b2 ;
        dt          = (Mat11\Mat12) ;
        
        %-- solutions
        sp          = (Mat22-Mat21*dt)\z2 ;
        up          = y1 - dt*sp ;
        
        chip        = 0.5*g.*( Dxm*(up-um) ) + 0.5*f.*( MatCos*(sp-sm) ) + chim ;
        
        
        %-- record output
        outu(n)    = u0(rp) ;
        %  outuR(n)    = u0(rpR) ;
        % outu(n,:)   = [outuL(n),outuR(n)] ; %-- this is the output (displacement)
        tt =  MatSin*sp ; outz(n) = tt(rp)  ;
        
        %-- update
        um          = u0 ;
        u0          = up ;
        sm          = s0 ;
        s0          = sp ;
        chim        = chip ;
        
        
        %-- live plotting
        if flagPlot == 1
            if mod(n,plotRefresh) == 0 || n == 1
                
                subplot(2,1,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]); grid on;
                xlabel('x [m]'); ylabel('u(x) [m]');  drawnow;
                
                subplot(2,1,2)
                plot((0:M)*h,[0;MatSin*sm;0],'linewidth',1.5,'color','k');
                ylim([-0.001,0.001]); xlim([0,M*h]);
                grid on; xlabel('x [m]'); ylabel('z(x) [m]'); drawnow;
            end
        end
    end
    time_elapsed(ncount)  = toc ;
end


mean(time_elapsed)


