clc; close all; clear all;

f1 = 10;            % start frequency
f2 = 6000;         % final frequency
fs = 96000;         % sampling frequency
T_ = 10;            % approximative time duration

N = 21;              % order of nonlinearities

% Length of each impulse repsonse (in samples)
len_IR = 2^12;
pre_IR = len_IR/2;

%% -- generation of the signal

L = round(f1/log(f2/f1)*T_)/f1;
T = L*log(f2/f1);
t = (0:ceil(fs*T)-1)./fs;
x = sin(2*pi*f1*L*(exp(t/L)-1));

% fade-in the input signal
fd1 = 480;  % number of samlpes
fade_in = (1-cos((0:fd1-1)/fd1*pi))/2;
index = 1:fd1;
x(index) = x(index).*fade_in;

% fade-out the input signal
fd2 =  480;  % number of samlpes
fade_out = (1-cos((0:fd2-1)/fd2*pi))/2;
index = (1:fd2)-1;
x(end-index) = x(end-index).*fade_out;


%% -- Nonlinear System response
y1 = audioread('sweptsineout13.wav') ; 
% sweptsineout8 je bez aliasingu a prebuzeni
delay = 1; % measured from impulse response
y = transpose(y1(delay:length(x) + delay-2));

figure; plot(y);
title('Response of the Nonlinear System to the Sychronized Swept-Sine Signal');


%% -- Nonlinear convolution
y = y - mean(y);  % signal must not contain the mean value


% Nonlinear convolution in frequency domain:
% len = 2^ceil(log2(length(y)));
len = length(y);
Y = fft(y,len)./fs;   % FFT of y

% frequency axis
f_ax = linspace(0,fs,len+1);
f_ax = f_ax(1:end-1);

% definition of the inferse filter in spectral domain
% (A.Novak IEEE paper "Nonlinear System Identification Using Exponential
% Swept-Sine Signal",  Eq. (30) and (31) leading to Eq. (26) for the amplitude
% and (9) for the phase)
X_ = 2*sqrt(f_ax/L).*exp(-j*2*pi* ...
    f_ax*L.*(1 - log(f_ax/f1)) + j*pi/4);

% Deconvolution
H = Y.*X_;


H(1) = 0;  % avoid Inf at DC

h = ifft(H,'symmetric');     % higher order impulse responses

% x_ = fliplr(x);
% h = conv(y,x_)

%% -- separation of higher impulse responses
% positions of higher harmonic IRs
dt = L.*log(1:N).*fs;




% rounded positions [samples]
dt_ = round(dt);
% non-integer difference
dt_rem = dt - dt_;

% circular periodisation of IR
h_pos = [h h(1:len_IR)];

% frequency axis definition
axe_w = linspace(0,2*pi,len_IR+1);
axe_w = axe_w(1:end-1);

% space localisation for IRs
hs = zeros(N,len_IR);

st0 = length(h_pos);
for n = 1:N
    % start position of n-th IR
    st = len-dt_(n)-pre_IR;
    ed = min(st + len_IR-1, st0);
    st0 = st - 1;
    
    % separation of IRs
    hs(n,1:ed-st+1) = h_pos(st:ed);
    
    % Non-integer sample delay
    Hx = fft(hs(n,:)).* ...
        exp(-j*dt_rem(n)*axe_w);
    hs(n,:) = ifft(Hx,'symmetric');
end




% Higher Harmonics
Hs = fft(hs.').';

figure; axes('FontSize',12);
semilogx(axe_f,20*log10(abs(Hs)),'linewidth',2)
set(gca,'XLim',[n*f1 n*f2]);
grid on;

xlabel('Frequency [kHz]')
ylabel('Amplitude [-]')
title(['Higher Harmonics'])
ylim([-80 40]);

%% -- Conversion to Hammerstein model

% Matrix A generation
A(N,N) = 0;
for l = 1:N
    for k = 1:N
        if ((l>=k) && (mod(l+k,2)==0))
            A(k,l) = (-1)^(2*l+(1-k)/2)/ ...
                2^(l-1)*nchoosek(l,(l-k)/2);
        end
    end
end

% transformation
Gs = inv(A)*Hs;

% ifft with Hermitian property
gs = ifft(Gs.','symmetric').';
Gs = fft(gs.').';


Gs_roll = fft([gs(:,pre_IR+2:end) gs(:,1:pre_IR+1)].').';


% for n = 1:N
%     
%     figure; axes('FontSize',12);
%     %semilogx(axe_f,abs(G_theo(n,:)),'-r','linewidth',2)
%     hold on
%     semilogx(axe_f,abs(gs(n,:)),'-b','linewidth',2)
%     set(gca,'XLim',[N*f1 n*f2]);
%     
%     %legend('Theoretical','Estimated')
%     xlabel('Frequency [kHz]')
%     ylabel('Amplitude [-]')
%     title(['Hammerstein kernel (order ' num2str(n) ')'])
%     grid on;
% 
% end


% for n = 1:N
%     
%     figure; axes('FontSize',12);
% %    semilogx(axe_f,angle(G_theo(n,:)),'-r','linewidth',2)
%     hold on
%     semilogx(axe_f,angle(Gs_roll(n,:)),'-b','linewidth',2)
%     set(gca,'XLim',[N*f1 n*f2]);
%     
%     %legend('Theoretical','Estimated')
%     xlabel('Frequency [kHz]')
%     ylabel('Phase [rad]')
%     title(['Hammerstein kernel (order ' num2str(n) ')'])
%     grid on;
% 
% end




