% 4pt_polyBLAMP.m
% 
% Aalto University, Dept. of Signal Processing and Acoustics
%
% Algorithm to implement the 2-point polyBLAMP algorithm on a clipped
% sinewave.
% 
% Authors: Fabian F Esqueda, Stefan Bilbao & Vesa V?lim?ki. October 2014
% Updated: February 19, 2016
%
% Fs = Sampling Rate
% f0 = Fundamental frequency of the test signal
% x - Input Signal
% L - Clipping Threshold.
% 
% y - Output Signal
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

%% User-defined parameters
Fs = 44100;
f0 = 1245;
L = 0.2;

%% Processing Section

% Input Signal
t = (1/Fs)*(0:round(Fs/1)-1);
x = cos(2*pi*f0*t);

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

% Trivial Clipping
x_clipped = sign(x).*min(abs(x),L);

% Control Variables
xn1 = 0;
xn2 = 0;
xn3 = 0;

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

clipping = 0;
clipping_n1 = 1;

flag = 0;

% # Main loop
for n=1:length(x)

	if (abs(x(n)) > L)
		clipping = 1;
		yn = sign(x(n))*L;
    else
		clipping = 0;
		yn = x(n);
    end
    
	if (clipping-clipping_n1) ~=  0
		flag = 1;
    elseif (flag == 1)
		flag = 0;

		a = (-1/6)*xn3 + (1/2)*xn2 - (1/2)*xn1 + (1/6)*x(n);
        b = xn3 - (5/2)*xn2 + 2*xn1 - (1/2)*x(n);
        c = (-11/6)*xn3 + 3*xn2 - (3/2)*xn1 + (1/3)*x(n);
        e = xn3;

        % Newton-Raphson
		x_n = 1.5;
        for m=1:100
			err = (a*x_n^3 + b*x_n^2 + c*x_n + e - sign(xn2)*L) / (3*a*x_n^2 + 2*b*x_n + c);
            if abs(err) > 0.00001
                x_n = x_n - err;
            else
                break
            end
        end
        
		% Slope at clipping point
		mu = 3*a*x_n^2 + 2*b*x_n + c;

		% Fractional part of clippiing point
		d = x_n - 1;

        h0 = -d^5/120 + 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;
        h2 = -d^5/40 + d^4/24 + d^3/12 + d^2/12 + d/24 + 1/120;
        h3 = d^5/120;

		mu = abs(mu);
		pol = sign(xn2);

		yn3 = yn3 - pol*h0*mu;
		yn2 = yn2 - pol*h1*mu;
		yn1 = yn1 - pol*h2*mu;
		yn  = yn  - pol*h3*mu;
    
    end
    
	clipping_n1 = clipping;

	y(n) = yn3;
	yn3 = yn2;
	yn2 = yn1;
	yn1 = yn;

	xn3 = xn2;
	xn2 = xn1;
	xn1 = x(n);
    
end

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

%%
% % % % %
% PLOT

% Find odd harmonics
Ny = round(Fs*0.5); % Nyquist Frequency
max_harm = floor(Ny/f0); % Maximum harmonic to synthesize
odd_harm = 1:2:max_harm;
odd_harm = f0.*odd_harm;

x_clipped = sign(x).*min(abs(x),L);

% Plot Parameters
figure(1)
set(1, 'Color', 'w');
fontsize = 16;
fontname = 'Times';
solidLineWidth = 2;
dottedLineWidth = 1;
markersize = 5;
f_vector = 0:Fs-1;
dbrange = -120; % (-160 for plots in the paper)
w = chebwin(length(x),180)';

% % % % % % % % % % % % 
subplot(2,2,1), plot((0:length(x)-1),x,'k--','LineWidth',dottedLineWidth);
hold on, plot((0:length(x)-1),x_clipped,'k','LineWidth',solidLineWidth);
set(gca,'fontsize',fontsize,'fontname',fontname);
xlabel({'Time (ms)'}');
set(gca,'XTick', [0, 0.5*Fs/f0, Fs/f0, 1.5*Fs/f0, 2*Fs/f0]); 
set(gca,'XTicklabel',[{0}, 0.4, 0.8, 1.2, 1.6]);
set(gca,'YTick', [-1, 0, 1]); 
axis([0 2*Fs/f0 -1.2 1.2])
% % % % % % % % % % % % 
data_fft = abs(fft(w.*x_clipped,Fs));
data_fft = db(data_fft/max(data_fft));
subplot(2,2,2), plot(data_fft,'k','LineWidth',1), hold on;
plot(odd_harm+1,data_fft(odd_harm+1),'ko','LineWidth',1);
set(gca,'fontsize',fontsize,'fontname',fontname);
xlabel({'Frequency (kHz)'}');
ylabel('Magnitude (dB)');
set(gca,'XTick', [1, 5000, 10000, 15000, 20000]); 
set(gca,'XTicklabel',{0, 5, 10, 15, 20});
set(gca,'YTick', [-200, -160, -120, -80, -40, 0]); 
set(gca,'YTicklabel',{-200, -160, -120, -80, -40, 0});
axis([1 Fs/2 dbrange 20])
% % % % % % % % % % % % 

subplot(2,2,3), plot((0:length(x)-1),x,'k--','LineWidth',dottedLineWidth);
hold on, plot((0:length(x)-1),y,'k','LineWidth',solidLineWidth);
set(gca,'fontsize',fontsize,'fontname',fontname);
xlabel({'Time (ms)','(c)'}');
set(gca,'XTick', [0, 0.5*Fs/f0, Fs/f0, 1.5*Fs/f0, 2*Fs/f0]); 
set(gca,'XTicklabel',[{0}, 0.4, 0.8, 1.2, 1.6]);

set(gca,'YTick', [-1, 0, 1]); 
axis([0 2*Fs/f0 -1.2 1.2])
% % % % % % % % % % % % 
data_fft = abs(fft(w.*y,Fs*1));
data_fft = db(data_fft/max(data_fft));
bl_freq(odd_harm+1) = data_fft(odd_harm+1);
subplot(2,2,4), plot(data_fft,'k','LineWidth',1), hold on;
plot(odd_harm+1,data_fft(odd_harm+1),'ko','LineWidth',1);
set(gca,'fontsize',fontsize,'fontname',fontname);
xlabel({'Frequency (kHz)','(d)'}');
ylabel('Magnitude (dB)');
set(gca,'XTick', [1, 5000, 10000, 15000, 20000]); 
set(gca,'XTicklabel',{0, 5, 10, 15, 20});
set(gca,'YTick', [-200, -160, -120, -80, -40, 0]); 
set(gca,'YTicklabel',{-200, -160, -120, -80, -40, 0});
axis([1 Fs/2 dbrange 20])