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. 9 - Half-Wave Rectified Sinewave
%
% Code written by F. Esqueda
% Last modified: 06-09-2016
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %

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

% Sampling rate
Fs = 44100;
Ts = 1/Fs;

% Generate input sinewave
f0 = 1660;
t = Ts*(0:Fs-1);
x = cos(2*pi*f0*t);

% Allocate output vector
y = zeros(size(x));

% Rectifier Mode (1: Block positive samples; -1: Block negative samples)
L = -1;

% 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) == L
        corner = 1;
        yn = 0;
    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
        
        % Estimate slope at zero-crossings (where corners occur)
        mu = 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 - L*h0*mu;
        yn2 = yn2 - L*h1*mu;
        yn1 = yn1 - L*h2*mu;
         yn =  yn - L*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
toc

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

%% PLOT
figure(1)
solidLineWidth = 2;
dottedLineWidth = 1;
fontName = 'Times';
fontSize = 16;
w = chebwin(length(x_rectified),200)';

% Determine non-aliased components
Ny = round(Fs*0.5); % Nyquist Frequency
max_harm = floor(Ny/f0); % Highest harmonic
odd_harm = f0*(1:2:max_harm) + 1;

subplot(2,2,1)
plot(x_rectified,'k','LineWidth',solidLineWidth), hold on;
plot(x,'k--','LineWidth',dottedLineWidth), hold off;
set(gca,'fontsize',fontSize,'fontname',fontName);
axis([1 2*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(2,2,2)
XF = abs(fft(w.*x_rectified,Fs*1));
XF = db(XF/max(XF));
plot(0:Fs-1,XF,'k','LineWidth',dottedLineWidth), hold on;
plot(odd_harm,XF(odd_harm),'ok','LineWidth',dottedLineWidth), hold off;
axis([0 Fs/2 -100 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)','(b)'})
ylabel('Magnitude (dB)')

subplot(2,2,3)
plot(y,'k','LineWidth',solidLineWidth), hold on;
plot(x,'k--','LineWidth',dottedLineWidth), hold off;
set(gca,'fontsize',fontSize,'fontname',fontName);
axis([1 2*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)'})

subplot(2,2,4)
YF = abs(fft(w.*y,Fs*1));
YF = db(YF/max(YF));
plot(0:Fs-1,YF,'k','LineWidth',dottedLineWidth), hold on;
plot(odd_harm,YF(odd_harm),'ok','LineWidth',dottedLineWidth), hold off;
axis([0 Fs/2 -100 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)','(d)'})
ylabel('Magnitude (dB)')

% print -deps2 Fig-half-wave-rectifier-sinewave