%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%                   Particle-Barrier Collision
%
%  NIT (JASA 2021) - vs IT-2 (AAUA 2015) - vs IT-1 (JSV 2015)
%
%                       Dr M Ducceschi
%                   University of Bologna
%                       18 Apr 2021
%++++++++++++++++++++++++++++++++++++++++++++++++++++++++

clear all
close all
clc



%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% Custom Parameters
fs0     = 44100  ;     %-- base sample rate [Hz]
OF      = 1 ;          %-- oversampling factor
M       = 1e-2 ;       %-- Mass of particle [Kg]
B       = 1e13 ;       %-- Stiffness of Barrier [N/m^al]
al      = 1.0 ;        %-- Barrier Exponent
T       = 0.02 ;       %-- Time of Simulation [s]
f0      = 0 ;          %-- Linear Frequency of Oscillator [Hz]
disp0   = -0.01 ;      %-- inital displacement [m] (must be < 0);
vel0    = 1.5 ;        %-- intial velocity
tol0    = eps ;        %-- tolerance for Newt-Raph
MaxIter = 20 ;         %-- Max Newt-Raph Iterations

%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% Derived Parameters

fs      = fs0*OF ;
k       = 1/fs;                      %-- timestep [s]
Ts      = floor(T*fs);               %-- length of sim [samples]
Omsq    = (2*pi*f0)^2;               %-- radian freq of linear osc [rad/s]^2

ksqM    = k^2/M ;
ksqOmsq = k^2*Omsq ;

bt      = B*k^2/M;
Ant     = bt/(2*(al+1));
kk      = M*Omsq;


%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% Initialise

n0          = floor(-disp0/vel0*fs) ;
disp0       = -vel0*(n0-0.5)*k ;

um          = disp0;
u0          = vel0*k + um - 0.5*k^2*Omsq*um;


wm          = um;
w0          = u0;
phi         = 0 ;
chim        = sqrt(2*phi) ;

out         = zeros(Ts,1) ;
outJSV      = zeros(Ts,1) ;
outchi      = zeros(Ts,1) ;
outforce    = zeros(Ts,1) ;
outNR       = zeros(Ts,1) ;
Et          = zeros(Ts,1) ;
H           = zeros(Ts,1) ;
Eit         = zeros(Ts,1) ;

indv        = zeros(Ts,1) ;
itervecJSV  = zeros(Ts,1) ;
itervecDAFx = zeros(Ts,1) ;

q           = zeros(Ts,1);
y           = zeros(Ts,1);
p           = zeros(Ts,1);

y(1)        = disp0;
p(1)        = M*vel0;
q(1)        = k*p(1)/(2*M);
x           = q(1);

tv          = (0:Ts-1)*k;

%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% NIT (JASA 2021) 
tic
for n = 1 : Ts
    
    up          = 2*u0-um - ksqOmsq*u0  ;
    up0         = up ;
    g           = 2*(-chim)/(up-um) ;
    
    
    if u0 > 0
        phi         = B / (al+1) * (u0^(al+1) );
        g           = B*u0^al/sqrt(2*phi) ;
        if chim < 0
            g = - g;
        end
        if 0.25*g*um > chim
            g = 0;
        end
    end
    
    
    fac         = 1 + 0.25*g^2*ksqM ;
    up          = (up0 + 0.25*g^2*um*ksqM - (chim*g)*ksqM)/fac ;
    chip        = chim + 0.5*g*(up-um) ;
    out(n)      = u0 ;
    outchi(n)   = chim ;
    outforce(n)   = 0.5 * (chim + chip) * g;
    
    
    Et(n)  = 0.5*M/k^2*(up-u0)^2 + 0.5*M*Omsq*u0*up + 0.5*chip^2;
    
    um = u0; u0 = up; chim = chip;
    
end
time_NIT = toc;
% End NIT


%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% Newton-Raphson (Bilbao et al AAUA 2015) IT-2
tic
for n = 1 : Ts
    
    wp = (2*w0-wm - k^2*Omsq*w0) ;
    if wp > 0 || wm > 0
        r0 = w0-wm;
        a = wm;
        iter = 0;
        tol = 10 ;
        phia = 0 ;
        if a > 0
            phia = B/(al+1)*(a)^(al+1);
        end
        while tol > tol0 && iter < MaxIter
            iter = iter+1;
            phir0a = 0 ;
            phipr0a = 0 ;
            if r0+a > 0
                phir0a = B/(al+1)*(r0+a)^(al+1);
                phipr0a = B*(r0+a)^al;
            end
            f = r0+(phir0a-phia)/r0*k^2/M-2*(w0-wm)+k^2*Omsq*w0;
            dfdr = 1+(phipr0a*r0-phir0a+phia)/r0^2*k^2/M;
            r = r0-f/dfdr;
            tol = abs(r-r0);
            r0 = r;
        end
        itervecDAFx(n) = iter ;
        wp = r0+wm;
    end
    
    Eit(n) = (wp-w0)^2 + k^2*Omsq*w0*wp + k^2*B/(al+1)/M*( (max(0,wp))^(al+1) + (max(0,w0))^(al+1) ) ;
    
    outNR(n) = w0;
    wm = w0; w0 = wp;
    
end
% End Newton-Raphson

time_NR = toc;
%timeGainNR = time_NR/time_NIT

%++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% Newton-Raphson (Chatziioannou and van Walstijn JSV 2015) IT-1
yprev = y(1) ;
qprev  = q(1) ;
tic
for n = 1:Ts
    xdiff = 10;
    iter = 0 ;
    while xdiff > tol0 && iter < MaxIter
        iter   = iter + 1 ;
        m1 = max(0,yprev)^(al+1) ;
        m2 = max(0,yprev+x)^(al+1) ;
        F = Ant*(m2-m1)/x + x - 2*qprev + k^2*kk*(x+2*yprev)/(4*M) ;
        derF = Ant*((al+1)*x*m2/(yprev+x) - m2 + m1)/(x^2)...
            + 1 + k^2*kk/(4*M);
        xnew = x - F/derF;
        xdiff = abs(xnew-x);
        x = xnew;
    end
    itervecJSV(n) = iter ;
    qcur = x - qprev;
    ycur = yprev + x;
    
    p      = (2*M/k)*qcur;
    H(n)   = p^2/(2*M) + 0.5*M*Omsq*ycur^2 + (B/(al+1))*max([0,ycur])^(al+1) ;
    
    
    qprev = qcur ;
    outJSV(n) = ycur ;
    yprev = ycur ;
    
end


time_JSV = toc;
%timeGainJSV = time_JSV/time_NIT

%-----------------------------------
% Plot Stuff


subplot(2,2,1) ;
hold on ;
rectangle('Position',[tv(1),0,tv(end),0.0005],'FaceColor',[0.8 0.8 0.8]) ;
plot(tv,out,'-','color','k','linewidth',1) ;
plot(tv,outJSV,'--','color','k','linewidth',1) ;
plot(tv,outNR,'--','color',[0.5,0.5,0.5],'linewidth',1) ;
hold off ;
grid on ;
ylim([1.1*disp0,0.005]); xlim([0,tv(end)]) ;
ylabel('u [m]'); xlabel('t [s]') ;
set(gca,'Fontsize',12) ;
legend('N-IT','IT-1','IT-2')
title('Displacement')

subplot(2,2,2)
plot(tv,1-Et/Et(1),'-','color','k','linewidth',1) ; hold on ;
plot(tv,1-H/H(1),'--','color','k','linewidth',1) ;
plot(tv,1-Eit/Eit(1),'--','color',[0.5,0.5,0.5],'linewidth',1) ;
grid on ;
ylabel('\epsilon'); xlabel('t [s]') ;
ylim([-120e-15,120e-15]); xlim([0,tv(end)]) ;
set(gca,'Fontsize',12) ;
legend('N-IT','IT-1','IT-2')
title('Energy Variation')


subplot(2,2,3)
itervecNIT = ones(length(tv),1) ;
hold on ;
%stem(tv,itervecNIT,'-','color','k','linewidth',1) ;
plot(tv,itervecJSV,'--','color','k','linewidth',1) ;
plot(tv,itervecDAFx,'--','color',[0.5,0.5,0.5],'linewidth',1) ;
grid on ;
xlabel('t [s]') ; ylabel('iter') ; ylim([-1,12]);  xlim([0,tv(end)]) ;
set(gca,'Fontsize',12) ;
legend('IT-1','IT-2') ;
title('Iterations Newton-Raphson')

subplot(2,2,4)
plot(tv,outforce,'-','color','k','linewidth',1) ;
grid on
title('Collision Force N-IT') ;
xlabel('t [s]') ; ylabel('Force [N]')