%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%       Hammer-String 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.25 ;                %-- time of sim [s]
tol0            = eps ;
MaxIter         = 300 ;

%-- string parameters
rho             = 8000 ;
r               = 0.0005 ;
T0              = 10 ;
L               = 0.7 ;
xo              = 0.68 ;

%-- hammer parameters
mh              = 0.01 ;               %-- mass [Kg]
B               = 1e7 ;                %-- stiffness            [N/m^al]
al              = 1.3 ;                %-- nonlinear exponent   [-]
x0              = -0.001 ;             %-- hammer initial pos   [m] -- CHOOSE NEGATIVE
v0              = 0.2 ;                %-- hammer initial vel   [m/s]
xb              = 0.23 ;               %-- hammer strike location [0-1]

%-- Flags
flagPlot        = 1 ;
refreshrate     = 10*OF ;
EnCheck         = 1 ;
%++++++++++++++++++++++++++++++++++++++++++++++++++++++++


%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%-- 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) ;
mxx     = 1/4*spdiags([vy 2*vy vy],-1:1,M-1,M-1) ;

%-- hammer spreading op
Jb          = sparse(M-1,1) ;
pb          = xb * L / h ;
lbInt       = floor(pb) ;
fracb       = pb - lbInt ;
if lbInt < M - 1
    Jb(lbInt)   = (1-fracb) / h;
    Jb(lbInt+1) = fracb / h;
else
    Jb(M-1) = 1 ;
end
JbT       = Jb.' ;
J0        = sparse(M-1,1) ;
J0(lbInt) = 1 / h ;
J0T       = J0.' ;
Jl        = 0.5*(Jb + Jb) ;
JlT       = Jl.' ;


s       = k^2 * h/rA * JbT * Jb + k^2 / mh ;
%-- FD Output
Mo              = xo * L / h ;
Jo              = sparse(M-1,1) ;
alo             = Mo - floor(Mo) ;
Jo(floor(Mo))   = (1-alo)/h ;
Jo(floor(Mo)+1) = alo/h ;
JoT             = Jo.' ;
%++++++++++++++++++++++++++++++++++++++++++++++++++++++++


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

%-- FD
wm      = zeros(M-1,1) ;
w0      = zeros(M-1,1) ;
um      = zeros(M-1,1) ;
u0      = zeros(M-1,1) ;
chim    = 0 ;

%-- hammer
Um      = x0 ;
U0      = Um + k*v0 ;
Wm      = x0 ;
W0      = Wm + k*v0 ;

zetam   = Um - h*JbT*wm ;
zeta0   = U0 - h*JbT*w0 ;

etam    = Wm - h*JbT*um ;


%-- 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) ;
iterVec   = zeros(Ts,1) ;

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



%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%-- Main Loop
tic
for n = 1 : Ts
    
    %-- IT-2
    ws      = 2*w0 - wm + T0/rA*k^2*Dxx*w0 ;
    Us      = 2*U0 - Um ;
    zetast  = Us - h*JbT*ws ;
    if zetast < 0 && zetam < 0 || B == 0
        wp = ws ;
        Up = Us ;
    else
        r       = zetast - zetam ;
        tol     = 1 ;
        b       = T0*k^2*h/rA * JbT * (Dxx*w0) ;
        phia    = 0 ;
        if zetam > 0
            phia = B / (al+1) * zetam^(al+1) ;
        end
        iter    = 0 ;
        while tol > tol0 && iter < MaxIter
            iter    = iter + 1 ;
            phira   = 0 ;
            phipra  = 0 ;
            if r + zetam > 0
                phira   = B / (al+1) * (r+zetam)^(al+1) ;
                phipra  = B * (r+zetam)^(al) ;
            end
            f      = r + 2*zetam - 2*zeta0 + b + s * (phira - phia) / r ;
            fp     = 1 + s / r^2 * (phipra * r - phira + phia) ;
            rn     = r - f / fp ;
            tol    = abs(1-r/rn) ;
            r      = rn ;
        end
        iterVec(n) = iter ;
        
        fh   = (phira - phia) / r ;
        wp   = ws + Jb*fh/rA*k^2 ;
        Up   = Us - fh/mh*k^2 ;
    end
    
    zetap   = Up - h*JbT*wp ;
    
    %-- N-IT 
    
    us      = 2*u0 - um + T0/rA*k^2*Dxx*u0  ;
    Ws      = 2*W0 - Wm  ;
    eta0    = W0 - h*JbT*u0 ;
    etast   = Ws - h*JbT*us ;
    z       = 0 ;

    
    if eta0 >= 0 && B ~= 0
        z       = sqrt(0.5*B)*sqrt(al+1)*(eta0)^(al/2-1/2) ;
        if chim < 0
            z   =  -z ;
        end
    elseif eta0 < 0 && B ~= 0 && chim ~= 0 && etast ~= etam
        z    = -2*chim/(etast-etam)  ;
    end
    
    p1      = h*JbT*um ;
    p2      = 0.25*k^2*z^2*(p1 - Wm) + chim*z*k^2  ;
    
    
    A0     = speye(M-1) + 0.25*k^2*h*z^2/rA * Jb * JbT ;
    A1     = -0.25*k^2*z^2/rA*Jb ;
    A2     = -0.25*k^2*z^2/mh*h*JbT ;
    A3     = 1 + 0.25*k^2*z^2/mh ;
    tr     = [A0,A1;A2,A3] \ [us + Jb*p2/rA ; Ws - p2/mh] ;
    up     = tr(1:end-1) ;
    Wp     = tr(end) ;
    chip   = 0.5 * z * ( Wp - Wm - h*JbT*(up-um) ) + chim ;
    
    
    if flagPlot == 1
        if mod(n,refreshrate) == 0
            cla ;
            plot((1:M-1)*h, wp+0.001,'*','linestyle','-','color','k','markersize',1) ;  hold on ; grid on ;
            plot(xb*L, Up+0.001,'^','linestyle','none','color','r','markersize',5,'markerfacecolor','r') ;
            plot((1:M-1)*h, up,'+','linestyle','-','color','k','markersize',1) ;
            plot(xb*L, Wp,'o','linestyle','none','color','r','markersize',5,'markerfacecolor','r') ; hold off ;
            ylim([-4e-3,4e-3]) ; xlim([0,L]) ;
            set(gca,'Color',[1 1 1],'XColor',[0 0 0],'YColor',...
                [0 0 0]) ;
            ylabel('u [m]') ;
            xlabel('x [m]') ;
            str = sprintf('t = %d [ms]', round(n*k*1000)) ;
            title(str) ;
            drawnow ;
        end
    end
    
    if EnCheck == 1
        Ek(n) = 0.5*h*rA/k^2*((up-u0).')*(up-u0) + 0.5*mh/k^2*(Wp-W0)^2 ;
        Ep(n) = -0.5*h*T0*(up.')*(Dxx*u0) ;
        Ec(n) = 0.5*(chip.')*chip ;
        Et(n) = Ek(n) + Ep(n) + Ec(n) ;
        %
        EkFD(n) = 0.5*h*rA/k^2*((wp-w0).')*(wp-w0) + 0.5*mh/k^2*(Up-U0)^2 ;
        EpFD(n) = -0.5*h*T0*(wp.')*(Dxx*w0) ;
        EcFD(n) = 0.5 * B / (al+1) * ( max(0,zeta0)^(al+1) + max(0,zetap)^(al+1) );
        EtFD(n) = EkFD(n) + EpFD(n) + EcFD(n) ;
    end
    %
    
    %-- update
    wm      = w0 ;
    w0      = wp ;
    Um      = U0 ;
    U0      = Up ;
    zetam   = zeta0 ;
    zeta0   = zetap ;
    
    Wm      = W0 ;
    W0      = Wp ;
    um      = u0 ;
    u0      = up ;
    etam    = eta0 ;
    chim    = chip ;
    
    
    %-- output
    outFD(n) = 0.5*h*JoT*(wp+wm) ;
    out(n)   = 0.5*h*JoT*(up+um) ;
    
end
toc
tvec = (0:Ts-1)*k ;
