clear all
close all
clc

%--------------------------------------------------------
%              Geometrically Exact
%            Nonlinear Wave Equation:
%        Linearly-Implicit, Conservative Scheme
%                Full FD (ICA_NIT)
%               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          = 4 ;              % oversampling factor

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


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


%--raised cosine conditions
Amp         = 2e-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]

NITER       = 5 ;
% End Custom Parameters
%--------------------------------------------------------

%--------------------------------------------------------
% Derived Parameters
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          = fs0*OF ;
k           = 1 / fs;             %% time step
Ts          = floor(fs * T);      %% number of samples
hsq         = ( T0*k^2  ) + sqrt( ( T0*k^2 )^2 + 16*rA*EI*k^2 ) ;
hsq         = hsq / 2 / rA ;
h           = 1.05 * 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);

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


%--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 ) );


mf          = max(abs(fRW));  %% this is used in plot ranges
mfq         = max(abs(Dxm*fRW.'));

if Amp == 0
    mf = fAmp;
    mfq = mf;
end

%--------------------------------------------------------
% Initialise
%--------------------------------------------------------
ovec        = ones(M,1);
z0          = zeros(M - 1, 1).*rand(M - 1,1)*1e-15;
u0          = fRW.';

um          = u0;
zm          = z0;

q0          = Dxm*u0;
p0          = Dxm*z0;
qm          = Dxm*um;
pm          = Dxm*zm;

avec        = sqrt( (ovec+p0).^2 + q0.^2 );
phiq        = 0.5*(EA - T0) * (avec  -  1).^2 ;
phip        = 0.5*(EA - T0) * (avec  -  1).^2  + 0.0 * (T0-EA) * p0.^2;
chimq       = sqrt(2*abs(phiq));
chimp       = sqrt(2*abs(phip));

chim        = chimq;

Et          = zeros(Ts,1);



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


time_elapsed = zeros(NITER,1);


%--------------------------------------------------------
%--------------------------------------------------------
% Main Loop
tic
for ncount = 1 : NITER
    ncount
    tic
    for n = 1 : Ts
        %%--- get current
        q0          =  Dxm*u0 ;
        p0          =  Dxm*z0 ;
        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 ;
        
        
        fg          = f .* g ;
        
        Lgsq        = sparse(diag(gsq)) ;
        Lfsq        = sparse(diag(fsq)) ;
        Lfg         = sparse(diag(fg)) ;
        
        %%--- Build Loop Matrices
        Mat11       = rA*Id/k^2 - 0.25*Dxp*Lgsq*Dxm ;
        Mat12       = - 0.25*Dxp*Lfg*Dxm ;
        Mat21       = Mat12 ;
        Mat22       = rA*Id/k^2 - 0.25*Dxp*Lfsq*Dxm ;
        Mat         = [Mat11,Mat12;Mat21,Mat22 ] ;
        bvec        = [
            rA/k^2*(2*u0 - um) + Dxp*(chim.*g)  - 0.25*Dxp*Lgsq*Dxm*um + T0*D2*u0 - EI*D4*u0 - 0.25*Dxp*Lfg*Dxm*zm  ;
            rA/k^2*(2*z0 - zm) + Dxp*(chim.*f)  - 0.25*Dxp*Lfsq*Dxm*zm + T0*D2*z0 - 0.25*Dxp*Lfg*Dxm*um  ] ;
        
        %%--- get solution
        solvec      = Mat\bvec ;
        up          = solvec(1:M-1) ;
        zp          = solvec(M:end) ;
        dudt        = (up-um)/2/k ;
        dzdt        = (zp-zm)/2/k ;
        chip        = k*g.*( Dxm*dudt ) + k*f.*( Dxm*dzdt ) + chim ;
        
        
        %%--- record output
        outu(n)     = up(rp) ;
        outz(n)     = zp(rp) ;
        
        
        %%--- update
        um          = u0 ;
        u0          = up ;
        zm          = z0 ;
        z0          = zp ;
        chim        = chip;
        
        
    end
    
    time_elapsed(ncount)  = toc ;
end

mean(time_elapsed)