% 2pt_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.5;

%% 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
yn = 0;
xn1 = 0;
yn1 = 0;
clipping_flag = 0;
clipping_flag_n1 = 1;

% Critical loop
for n=1:length(x)
    
    if (abs(x(n)) >= L)
		clipping_flag = 1;
		yn = sign(x(n))*L;
    else
		clipping_flag = 0;
		yn = x(n);
    end
    
    if (clipping_flag-clipping_flag_n1) ~= 0
        
        mu = x(n) - xn1;
		d = (sign(xn1)*L - xn1)/mu;

		yn1 = yn1  - sign(xn1) * abs(mu) * (-d^3/6 + d^2/2 - d/2 + 1/6);
		yn = yn - sign(x(n)) * abs(mu) * (d^3/6);
        
    end
    
    y(n) = yn1;
	yn1 = yn;
	xn1 = x(n);
	clipping_flag_n1 = clipping_flag;

end

y = [y(2:end) 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;
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])