%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%       String-Fretboard Collision
%        Dr Michele Ducceschi
%       University of Bologna
%            18 Apr 2021
%++++++++++++++++++++++++++++++++++++++++++++++++++++++++

clear all
close all
clc

%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%-- Custom Parameters
fs0             = 44100 ;               %-- base sample rate [Hz]
OF              = 1 ;                   %-- oversampling factor
T               = 0.035 ;               %-- time of sim [s]

%-- string parameters
rho             = 8000 ;
r               = 0.0003 ;
T0              = 30 ;
L               = 0.7 ;
xo              = 0.68 ;

%-- barrier parameters
B               = 1e13 ;              %-- stiffness            [N/m^al]
al              = 1.2 ;               %-- nonlinear exponent   [-]


%-- barrier type. Choose either :
% 1 = continuous b = hb0 + hb1*x + hb2*x^2 ; 
% 2 = 12 frets in semitones ;
% 3 = xb (choose locations as per xb below) ;
flagBarr        = 2 ;                  
xb              = [0.23,0.12] ;
hb0             = -0.001 ;           
hb1             = -0.001 ;
hb2             = -0.02 ;

%-- Flags
flagPlot        = 1 ;
refreshrate     = 10 ;
EnCheck         = 0 ;

%++++++++++++++++++++++++++++++++++++++++++++++++++++++++


%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%-- Derived Parameters
fs      = fs0*OF ;
k       = 1 / fs ;
Ts      = floor(T*fs) ;

%-- FD string grid spacing
A       = pi*r^2 ;
rA      = rho*A ;
c       = sqrt(T0/rA) ;
h       = 1.00 * c * k ;
M       = floor(L/h) ;
h       = 1.00 * L / M ;

%-- build string FD matrix
vy      = ones(M,1) ;
Dxx     = 1/h^2*spdiags([vy -2*vy vy],-1:1,M-1,M-1) ;


%-- barrier
if flagBarr == 1
    xb      = (1:M-1)*h ;
    hb      = xb.^2*hb2 + xb*hb1  + hb0 ;
    hb      = hb.' ;
    Bp      = length(xb) ;
    hbplot  = hb ;
    xbplot  = xb ;
elseif flagBarr == 2
    xb      = L-L*2.^(-(1:12)/12) ;
    Bp      = length(xb) ;
    hb      = ones(Bp,1)*hb0 ;
    hbplot  = -0.0035*ones(200,1) ;
    fretloc = xb*200/L ;
    xbplot  = (1:200)/200*L;
    hbplot(floor(fretloc)) = hb0 ;
elseif flagBarr == 3
    xb      = xb*L ;
    Bp      = length(xb) ;
    hb      = ones(Bp,1)*hb0 ;
    hbplot  = -0.005*ones(200,1) ;
    fretloc = xb*200/L ;
    xbplot  = (1:200)/200*L;
    hbplot(floor(fretloc)) = hb0 ;
end

Jb          = zeros(M-1,Bp) ;
for m = 1 : Bp
    pb          = xb(m) / h ;
    lbInt       = floor(pb) ;
    fracb       = pb - lbInt ;
    if lbInt < M - 1
        Jb(lbInt,m)   = (1-fracb) ;
        Jb(lbInt+1,m) = fracb ;
    else
        Jb(M-1,m) = 1 ;
    end
end
Jb      = Jb/h ;
JbT     = Jb.' ;

%-- FD Output
Mo              = xo*L/h ;
Jo              = zeros(M-1,1) ;
alo             = Mo - floor(Mo) ;
Jo(floor(Mo))   = (1-alo)/h ;
Jo(floor(Mo)+1) = alo/h ;
JoT             = Jo.' ;


%-- Modes
omsq      = 0 ;
Nm        = 0 ;
Lm        = [] ;

while omsq < 4
    Nm      = Nm + 1 ;
    omsq    = k^2*T0/rA*Nm^2*pi^2/L^2 ;
    Lm(Nm)  = omsq ;
end

Nm      = Nm - 1 ;
Lm      = diag(Lm(1:Nm)) ;
Lm      = sparse(Lm) ;
C       = (2*speye(Nm) - Lm) ;

Xi      = zeros(Nm,Bp) ;
Xo      = zeros(Nm,1) ;

%-- Output - Barrier Points
for m = 1 : Nm
    Xo(m) = sin(m*pi*xo) ;
    for n = 1 : Bp
        Xi(m,n) = sin(m*pi*xb(n)/L) ;
    end
end

%-- Modal Plot Projection
Xplot = zeros(M-1,Nm) ;
for m = 1 : M-1
    for n = 1 : Nm
        Xplot(m,n) = sin(n*pi*m*h/L) ;
    end
end

XiT = Xi.' ;
XoT = Xo.' ;
%++++++++++++++++++++++++++++++++++++++++++++++++++++++++


%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%-- Init

%-- Modes
q0          = zeros(Nm,1) ;
qm          = zeros(Nm,1) ;
q0(1)       = 0.003 ;
qm(1)       = 0.003 ;
psim        = zeros(Bp,1) ;
etam        = hb - XiT*qm ;

%-- FD
wm      = zeros(M-1,1) ;
w0      = zeros(M-1,1) ;
wm      = 0.003*sin(pi*h*(1:M-1).'/L) ;
w0      = wm ;
chim    = zeros(Bp,1) ;
zetam   = hb - h*JbT*wm ;

%-- Energies and Output
Ek        = zeros(Ts,1) ;
Ep        = zeros(Ts,1) ;
Ec        = zeros(Ts,1) ;
Et        = zeros(Ts,1) ;
EkFD      = zeros(Ts,1) ;
EpFD      = zeros(Ts,1) ;
EcFD      = zeros(Ts,1) ;
EtFD      = zeros(Ts,1) ;
out       = zeros(Ts,1) ;
outFD     = zeros(Ts,1) ;


if flagPlot == 1
    figure1     = figure('Color',[1 1 1]);
    colormap(bone);
end
%++++++++++++++++++++++++++++++++++++++++++++++++++++++++


%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%-- Main Loop
tic
for n = 1 : Ts
    
    %-- FD
    ws      = 2*w0 - wm + T0/rA*k^2*Dxx*w0 ;
    zeta0   = hb - h*JbT*w0 ;
    zetast  = hb - h*JbT*ws ;
    z       = zeros(Bp,1) ;
    p2      = zeros(M-1,1) ;
    Ac      = zeros(M-1,M-1) ;
    
    for m = 1 : Bp
        if zeta0(m) > 0 && B ~= 0
            z(m)       = sqrt(0.5*B)*sqrt(al+1)*(zeta0(m))^(al/2-1/2) ;
            if chim(m) < 0
                z(m)   =  -z(m) ;
            end
        elseif zeta0(m) < 0 && B ~= 0 && chim(m) ~= 0 && zetast(m) ~= zetam(m)
            z(m)    = -2*(chim(m))/(zetast(m)-zetam(m))  ;
        end
        p1      = JbT(m,:)*wm ;
        p2      = p2 + k^2/rA*z(m)*(0.25*z(m)*p1*h + chim(m)) * Jb(:,m) ;
        Ac      = Ac + 0.25*k^2*h*z(m)^2/rA * Jb(:,m) * JbT(m,:) ;
    end
    
    A0     = speye(M-1) + Ac ;
    wp     = A0 \ (ws + p2) ;
    chip   = 0.5 * z .* ( - h*JbT*(wp-wm) ) + chim ;
    
    %-- modal
    qs      = C*q0 - qm ;
    eta0    = hb - XiT*q0 ;
    etast   = hb - XiT*qs ;
    g       = zeros(Bp,1) ;
    
    t2      = zeros(Nm,1) ;
    Mc      = zeros(Nm,Nm) ;
    
    for m = 1 : Bp
        if eta0(m) > 0 && B ~= 0
            g(m)       = sqrt(0.5*B)*sqrt(al+1)*(eta0(m))^(al/2-1/2) ;
            if psim(m) < 0
                g(m)   =  -g(m) ;
            end
        elseif eta0(m) < 0 && B ~= 0 && psim(m) ~= 0 && etast(m) ~= etam(m)
            g(m)    = -2*(psim(m))/(etast(m)-etam(m))  ;
        end
        t1      = XiT(m,:)*qm ;
        t2      = t2 + k^2/rA*g(m)/(L/2)*(0.25*g(m)*t1 + psim(m)) * Xi(:,m) ;
        Mc      = Mc + 0.25*k^2*g(m)^2/rA/(L/2) * Xi(:,m) * XiT(m,:) ;
    end
    
    M0      = speye(Nm) + Mc ;
    qp      = M0 \ (qs + t2) ;
    psip    = 0.5 * g .* ( - XiT*(qp-qm) ) + psim ;
    
    if flagPlot == 1
        if mod(n,refreshrate) == 0
            %subplot(3,3,ind) ;
            uplot  = 0.25*Xplot*(2*q0+qp+qp) ;
            wplot  = 0.25*(2*w0+wp+wp) ;
            plot((1:M-1)*h, uplot*1e3,'linestyle','-','color','k','linewidth',1.5) ;  hold on ; grid on ;
            plot((1:M-1)*h, wplot*1e3,'linestyle','--','color',[0.5 0.5 0.5],'linewidth',1.5) ;  hold on ;
            plot(xbplot, hbplot*1e3,'k','linewidth',0.5) ; hold off ;
            ylim([-4,4]) ; xlim([0,L]) ;
            set(gca,'Color',[1 1 1],'XColor',[0 0 0],'YColor',...
                [0 0 0]) ;
            set(gca,'Fontsize',14) ;
            ylabel('u [mm]') ;
            xlabel('x [m]') ;
            str = sprintf('t = %d [ms]',round(n*k*1000)) ;
            title(str) ;
            drawnow ;
        end
    end
    
    if EnCheck == 1
        Ek(n) = 0.5*rA*L/2/k^2*((qp-q0).')*(qp-q0) ;
        Ep(n) = 0.5*T0*L/2*((1:Nm).'.*qp).'*((1:Nm).'.*q0) * pi^2/L^2 ;
        Ec(n) = 0.5*(psip.')*psip ;
        Et(n) = Ek(n) + Ep(n) + Ec(n) ;
        
        EkFD(n) = 0.5*h*rA/k^2*((wp-w0).')*(wp-w0) ;
        EpFD(n) = -0.5*h*T0*(wp.')*(Dxx*w0) ;
        EcFD(n) = 0.5*(chip.')*chip;
        EtFD(n) = EkFD(n) + EpFD(n) + EcFD(n) ;
    end
    
    %-- output
    out(n) = 0.25*XoT*(qp+qm+2*q0) ;
    outFD(n) = 0.25*h*JoT*(wp+wm+2*w0) ;
    
    %-- update
    qm      = q0 ;
    q0      = qp ;
    psim    = psip ;
    etam    = eta0 ;
    
    wm      = w0 ;
    w0      = wp ;
    chim    = chip ;
    zetam   = zeta0 ;
    
end
toc