clear all
close all
clc

%-------------------------------------------------------------------------
%                  Linear Stiff Piano String
%                    Numerical Dispersion 
%            Comparing Implicit CFL vs Explicit Scheme
%-------------------------------------------------------------------------


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

fs          = 48e3 ;
%-- 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]

%------------------------------------------------------------------
k           = 1 / fs ;
A           = pi * rd * rd ;                    %-- area
I           = pi / 4 * rd * rd * rd * rd ;      %-- moment of inertia
EI          = E * I ;
rA          = rho * A;


%-- compute number of modes below Nyquist
ffm         = 0 ;
m           = 1 ;
while ffm < 24e3
    ffm = 1/2/pi*sqrt(T0/rA*(m*pi/L)^2 + EI/rA*(m*pi/L)^4) ;
    m = m + 1 ;
end
Nmodes = m ;


%-- create array of wavenumbers
gm          = 0 : 0.1 : 400 ;


%-- continuous dispersion relation
omCnt       = sqrt(1/rA*(T0*gm.^2 + EI*gm.^4)) ;

for cases = 1 : 2
    
    %--- numerical dispersion relation for implicit scheme
    % [rA*I + 0.5*k^2*EI*D4] * dtt u = T0 * D2*u - EI * D4*u ;
    %-- for which a stability condition is obtained as a classic CFL
    %-- h >= sqrt(T0/rA) * k
    if cases == 1
        h        = sqrt(T0/rA) * k ;
        GridPointsImplicit_CFL = floor(L/h)
    else
        h        = sqrt(T0*k^2 + sqrt(T0^2*k^4 + 16*EI*rA*k^2)) / sqrt(2*rA) ;
        GridPointsImplicit_hexp = floor(L/h)
    end
    ssq      = (sin(gm*h/2)).^2 ;
    tp       = (T0*k^2/h^2*ssq + 4*EI*k^2/h^4*ssq.^2)./(rA + 8*EI*k^2/h^4*ssq.^2) ;
    omNumImp = 2/k*asin(tp.^(1/2)) ;
    
    
    %--- numerical dispersion relation for explicit scheme
    % rA * dtt u = T0 * D2*u - EI * D4*u ;
    %-- for which a stability condition is obtained as per the JSV paper
    %-- h >= sqrt(T0*k^2 + sqrt(T0^2*k^4 + 16*EI*rA*k^2)) / sqrt(2*rA) ;
    h        = sqrt(T0*k^2 + sqrt(T0^2*k^4 + 16*EI*rA*k^2)) / sqrt(2*rA) ;
    if cases == 1
        GridPointsExplicit = floor(L/h)
    end
    ssq      = (sin(gm*h/2)).^2 ;
    tp       = (T0*k^2/h^2*ssq + 4*EI*k^2/h^4*ssq.^2)/rA ;
    omNumExp = 2/k*asin(tp.^(1/2)) ;
    
    subplot(2,2,2*cases-1)
    plot(gm,omCnt/2/pi,'k'); hold on
    plot(gm,omNumImp/2/pi,'b') ;
    plot(gm,omNumExp/2/pi,'r') ;
    line([gm(1),gm(end)],[2e4,2e4],'linestyle','--','color','k') ;
    xlabel('\gamma (rad/m)') ; ylabel('f (Hz)') ;
    if cases == 1
        legend('continuous','implicit (CFL)','explicit (h-h_{exp})','audio limit') ;
    else
        legend('continuous','implicit (h-h_{exp})','explicit (h-h_{exp})','audio limit') ;
    end
    subplot(2,2,2*cases)
    plot(omCnt/2/pi,abs(omCnt-omNumImp),'b') ; hold on ;
    plot(omCnt/2/pi,(omCnt-omNumExp),'r') ;
    xlabel('f (Hz)') ; ylabel('|E|') ;
    xlim([0,2e4]) ;
    if cases == 1
        legend('implicit (CFL)','explicit (h-h_{exp})') ;
    else
        legend('implicit (h-h_{exp})','explicit (h-h_{exp})') ;
    end
end