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. 7 - Hard Clipped Sinewave
%
% Code written by F. Esqueda
% Last modified: 06-09-2016
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %

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

% NOTE FROM THE AUTHOR: To aid readability, this code has not been 
% optimized to work for high-frequency input sinewaves or low clipping 
% thresholds. The parts of the algorithm that require such optimizaiton are
% the estimation of the clipping point detection and polarity.

% 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));

% Clipping Point
L = 0.3;

% Control variables used to implemnt algorithm inside a loop

clipping = 1;
clipping_n1 = 1;

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 clipping and detect clipping points (corners)
    if abs(xn)>=L
        clipping = 1;
        yn = sign(xn)*L;
    else
        clipping = 0;
        yn = xn;
    end
    
    % Trivially-clipped signal can be extracted from here
    x_clipped(n) = yn;
    
    % If clipping point then implement correction
    if (clipping-clipping_n1) ~= 0
        flag = 1; 
    elseif flag==1
        flag = 0;
        
        %Determine Polarity of Clipping Point
        pol = sign(xn1);
        
        % Perform polynomial interpolation around clipping 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 clipping point
        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 - pol*L)/(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 clipping point
        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 clipping point
        yn3 = yn3 - pol*h0*mu;
        yn2 = yn2 - pol*h1*mu;
        yn1 = yn1 - pol*h2*mu;
         yn = yn  - pol*h3*mu;
        
    end
    
    % Output sample
    y(n) = yn3;
    
    % Update ALL control variables
    clipping_n1 = clipping;
    
    xn3 = xn2; xn2 = xn1; xn1 = xn;
    
    yn3 = yn2; yn2 = yn1; yn1 = yn;
    
end

% Trim output signal
y = [y(4:end) 0 0 0];

%% PLOT Results
figure(1)
solidLineWidth = 2;
dottedLineWidth = 1;
fontName = 'Times';
fontSize = 16;
markerSize = 2;
w = chebwin(length(t),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,'k--','LineWidth',dottedLineWidth), hold on;
plot(x_clipped,'k','LineWidth',dottedLineWidth,'MarkerSize',markerSize);
plot(x_clipped,'ok','LineWidth',solidLineWidth,'MarkerSize',markerSize,'MarkerFaceColor','k'), hold off;
set(gca,'fontsize',fontSize,'fontname',fontName);
axis([1 1.5*Fs/f0 -1.1 1.1])
set(gca,'Xtick',[1 20 40 60])
set(gca,'XtickLabel',[0 20 40 60])
set(gca,'YTick',[-1 0 1])
xlabel({'Time (samples)','(a)'})

subplot(2,2,2)
XF = abs(fft(w.*x_clipped,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,'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(2,2,3)
plot(x,'k--','LineWidth',dottedLineWidth), hold on;
plot(y,'k','LineWidth',dottedLineWidth,'MarkerSize',markerSize);
plot(y,'ok','LineWidth',solidLineWidth,'MarkerSize',markerSize,'MarkerFaceColor','k'), hold off;
set(gca,'fontsize',fontSize,'fontname',fontName);
axis([1 1.5*Fs/f0 -1.1 1.1])
set(gca,'Xtick',[1 20 40 60])
set(gca,'XtickLabel',[0 20 40 60])
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,'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)')

% print -deps2 Fig-clipped-sinewave