clear all; close all; clc;

% Accompanying code for the DAFx-16 Paper: Rounding Corners with BLAMP
% by F. Esqueda, V. Välimäki and S. Bilbao
%
% Code used to generate Fig. 11 - Full-wave rectified string
%
% Code written by F. Esqueda
% Last modified: 06-09-2016
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %

% DISCLAIMER: This code is provided "as is", without warranty of any kind.


% Load and normalize audio file
f0 = 220;
[x, Fs] = audioread('KS-string-one-second.wav');
x = x(:,1);
x = x(1:Fs);
x = x/max(abs(x));
x = detrend(x);

% Output Signal
y = zeros(size(x));

% Control variables used to implemnt algorithm inside a loop

corner = 1;
corner_n1 = 0;

flag = 0;

 xn = 0;
xn1 = 0;
xn2 = 0;
xn3 = 0;

 yn = 0;
yn1 = 0;
yn2 = 0;
yn3 = 0;

% Main loop
for n=1:length(x)
    
    xn = x(n);
    
    % Implement rectification and detect corners
    if sign(xn) == -1
        corner = 1;
        yn = abs(xn);
    else
        corner = 0;
        yn = xn;
    end
    
    % Trivially-rectified signal can be extracted from here
    x_rectified(n) = yn;
    
    % If corner then implement correction
    if (corner-corner_n1) ~= 0
        flag = 1; 
    elseif flag==1
        flag = 0;
        
        % Perform polynomial interpolation around corner boundaries
        p_a =  (-1/6)*xn3  +    0.5*xn2  -  0.5*xn1  +  (1/6)*xn;
        p_b =         xn3  -  (5/2)*xn2  +    2*xn1  -    0.5*xn;
        p_c = (-11/6)*xn3  +      3*xn2  -  1.5*xn1  +  (1/3)*xn;
        p_e =         xn3;
        
        % Perform Newton-Raphson to estimate exact location of corner
        % (zero-crossings)
        x_d = 1.5;
        for m=1:100
        	err = (p_a*x_d^3 + p_b*x_d^2 + p_c*x_d + p_e)/(3*p_a*x_d^2 + 2*p_b*x_d + p_c);
            if abs(err) > 1e-6
            x_d = x_d - err;
            else
                break
            end
        end
        
        % Compute twice the value of slope at zero-crossings (where corners occur)
        mu = 2*abs(3*p_a*x_d^2 + 2*p_b*x_d + p_c);
        
        % Fractional delay required to center polyBLAMP
        d = x_d - 1;

        % Compute polyBLAMP coefficients
        h0 = -d^5/120 + d^4/24 - d^3/12 + d^2/12 - d/24 + 1/120;
        h2 = -d^5/40 + d^4/24 + d^3/12 + d^2/12 + d/24 + 1/120;
        h1 = d^5/40 - d^4/12 + d^2/3 - d/2 + 7/30;
        h3 = d^5/120;

        % Superimpose polyBLAMP at zero-crossings
        yn3 = yn3 + h0*mu;
        yn2 = yn2 + h1*mu;
        yn1 = yn1 + h2*mu;
         yn =  yn + h3*mu;
        
    end
    
    % Output sample
    y(n) = yn3;
    
    % Update ALL control variables
    corner_n1 = corner;
    
    xn3 = xn2; xn2 = xn1; xn1 = xn;
    
    yn3 = yn2; yn2 = yn1; yn1 = yn;
    
end

y = [y(4:end)' 0 0 0];

%% PLOT
figure(1)
solidLineWidth = 2;
dottedLineWidth = 1;
fontName = 'Times';
set(gcf,'PaperUnits','inches','PaperPosition',[0 0 8.5 9])
fontSize = 16;
dbrange = -80;
w = chebwin(length(x_rectified),200);

subplot(3,2,1)
plot(x,'k','LineWidth',solidLineWidth), hold off;
set(gca,'fontsize',fontSize,'fontname',fontName);
axis([1 1.5*Fs/f0 -1.1 1.1])
set(gca,'Xtick',[1 100:100:500])
set(gca,'XtickLabel',[0 10:10:50])
set(gca,'YTick',[-1 0 1])
xlabel({'Time (samples)','(a)'})

subplot(3,2,2)
XF = abs(fft(w.*x,Fs*1));
XF = db(XF/max(XF));
plot(0:Fs-1,XF,'k','LineWidth',dottedLineWidth);
axis([0 Fs/4 dbrange 5])
set(gca,'fontsize',fontSize,'fontname',fontName);
set(gca,'XTick',[0 5e3 10e3 15e3 20e3])
set(gca,'XTickLabel',[{'0'},'5', '10' '15' '20'])
set(gca,'YTick',-100:20:0)
xlabel({'Frequency (kHz)','(b)'})
ylabel('Magnitude (dB)')

subplot(3,2,3)
plot(x_rectified,'k','LineWidth',solidLineWidth), hold on;
plot(x,'k--','LineWidth',dottedLineWidth), hold off;
set(gca,'fontsize',fontSize,'fontname',fontName);
axis([1 1.5*Fs/f0 -1.1 1.1])
set(gca,'Xtick',[1 100:100:500])
set(gca,'XtickLabel',[0 10:10:50])
set(gca,'YTick',[-1 0 1])
xlabel({'Time (samples)','(c)'})

x_rectified = detrend(x_rectified);

subplot(3,2,4)
XF = abs(fft(w.*x_rectified',Fs*1));
XF = db(XF/max(XF));
plot(0:Fs-1,XF,'k','LineWidth',dottedLineWidth);
axis([0 Fs/4 dbrange 5])
set(gca,'fontsize',fontSize,'fontname',fontName);
set(gca,'XTick',[0 5e3 10e3 15e3 20e3])
set(gca,'XTickLabel',[{'0'},'5', '10' '15' '20'])
set(gca,'YTick',-100:20:0)
xlabel({'Frequency (kHz)','(d)'})
ylabel('Magnitude (dB)')

subplot(3,2,5)
plot(y,'k','LineWidth',solidLineWidth), hold on;
plot(x,'k--','LineWidth',dottedLineWidth), hold off;
set(gca,'fontsize',fontSize,'fontname',fontName);
axis([1 1.5*Fs/f0 -1.1 1.1])
set(gca,'Xtick',[1 100:100:500])
set(gca,'XtickLabel',[0 10:10:50])
set(gca,'YTick',[-1 0 1])
xlabel({'Time (samples)','(e)'})

y = detrend(y);

subplot(3,2,6)
YF = abs(fft(w.*y',Fs*1));
YF = db(YF/max(YF));
plot(0:Fs-1,YF,'k','LineWidth',dottedLineWidth);
axis([0 Fs/4 dbrange 5])
set(gca,'fontsize',fontSize,'fontname',fontName);
set(gca,'YTick',-100:20:0)
set(gca,'XTick',[0 5e3 10e3 15e3 20e3])
set(gca,'XTickLabel',[{'0'},'5', '10' '15' '20'])
xlabel({'Frequency (kHz)','(f)'})
ylabel('Magnitude (dB)')

% EOF