clear;
%close all;
clc;
addpath(genpath('Convolution_tools\'))
addpath(genpath('Functions\'))
addpath(genpath('Samples\'))
addpath(genpath('measurements\'))

%% data load in

meas_data = load("TS_proba_000.mat");
result = meas_data.Result;

f1 = 0.1;
f2 = 22e3;
Rate = log(f2/f1);

fs = result.AcqSetup.Fs;
N = result.AcqSetup.nSamples;
T = N / fs;
t = (0:N-1)' / fs;
Ube = result.ExcitationSignal;
Uki = mean( result.ResponseSignal(:,:,2), 2 );

Nl = length(Ube)+length(Uki)-1;
Nifft = Nl;
h = fftshift(ifft( fft(Uki,Nl)./fft(Ube,Nl),Nifft,'symmetric' ) );

len_h = length(h);
center = floor(len_h/2) + 1;
t_h = ((1:len_h) - center) / fs;

figure
plot(t_h,h);
xlabel('idő [s]');
ylabel('Impulzusválasz');
title('Az impulzusválasz');
grid on


% harmonikusok közti távolság számítása
nHarm = 10;
pks = zeros(1,nHarm);
for n_block = 1:nHarm
    tau = (T*log(n_block)/Rate);  % Minden harmonikusra külön tau
    pks(n_block) = round(length(h)/2 - tau*fs);
end

hold on
 for i=1:length(pks)
     xline(t_h(pks(i)),'r--');
 end


%% ablakozás
L = 128;
if any(pks - floor(L/2) < 1) || any(pks + ceil(L/2)-1 > len_h)
    error('Ablak lépne ki a határokon. Növeld Nfft-et vagy csökkentsd L-et.');
end
h_window = zeros(L, nHarm);
for m = 1:nHarm
    idx_center = pks(m);
    i1 = idx_center - floor(L/2) + 1;
    i2 = i1 + L - 1;
    h_window(:, m) = h(i1:i2);   % megtartjuk az időirendet
end


Nmax = nHarm;                 % maximalis rend amihez a trig. mátrix készül
Csin = zeros(Nmax+1);
for n_block = 0:Nmax
    if rem(n_block,2) == 0
        if n_block==0
            Csin(n_block+1,1) = 1;
        else
            k = 0:(n_block/2-1);
            c0 = factorial(n_block)./(factorial(n_block-k).*factorial(k));
            k0 = (n_block - 2*k);
            Csin(n_block+1, k0 + 1) = 2/2^n_block * c0 .* (-1).^(n_block/2 - k) * 1i;
            Csin(n_block+1,1) = Csin(n_block+1,1) + 1/(2^n_block) * nchoosek(n_block, n_block/2);
        end
    else
        k = 0:((n_block-1)/2);
        c0 = factorial(n_block)./(factorial(n_block-k).*factorial(k));
        k0 = (n_block - 2*k);
        Csin(n_block+1, k0 + 1) = 2/2^n_block * c0 .* (-1).^((n_block-1)/2 - k);
    end
end
Coeff_mtx = Csin(2:end, 2:end).';   % dim = nHarm x nHarm


 H = fft(h_window,[],1); % kiablakozott imp. válaszok F-transzformációja
 H_volterra = (pinv(Coeff_mtx)*H.').'; % kiablakozott imp. válaszok súlyozása a megfelelő amplitúdókkal
 h_volterra = ifft(H_volterra,L,1,'symmetric'); % inv. F-transzformáció, Volterra-kernelek mtx-a


%% Konvolúció Volterra-tagokkal
L = size(h_volterra, 1);  % Kernel hossza
N = size(h_volterra, 2);  % Volterra-rendek száma

input = Ube;
Y_volterra = zeros(size(input));
for n = 1:N
    input_n = input.^n;  % bemenet n-edik hatványa
    y_n = conv(input_n, h_volterra(:,n), 'same');  % konvolúció
    Y_volterra = Y_volterra + y_n;  % összegzés
end

block_size = 128;
for n = 1:N
    h_volterra_filters{n} = OLS_convolver(h_volterra(:,n), block_size);
end
N_b = floor(length(input)/block_size);
output = zeros(length(input),1);
for n_block = 1 : N_b
    curr_ixs = (n_block-1)*block_size+1:n_block*block_size;
    input_block = input( curr_ixs );
    for n_volt = 1:N
        output(curr_ixs) = output(curr_ixs) + h_volterra_filters{n_volt}.convolve(input_block.^n_volt);
    end
end
figure
plot(output)
hold on
plot(Y_volterra)

filename = 'sweep.wav'; % Use a MATLAB example file

info = audioinfo(filename);
totalSamples = info.TotalSamples;
numChannels = info.NumChannels;
fs = info.SampleRate;
fileReader = dsp.AudioFileReader(filename, ...
    'SamplesPerFrame', block_size);

outputVector = zeros(block_size,1);
disp('Processing audio block-by-block...');

block_size = 128;
h_volterra_filters = {};
for n = 1:N
    h_volterra_filters{n} = OLS_convolver(h_volterra(:,n), block_size);
end

idx = 1;
while ~isDone(fileReader)
    input_block = fileReader();
    output = zeros(block_size,1);
    for n_volt = 1:N
        output = output + h_volterra_filters{n_volt}.convolve(input_block.^n_volt);
    end
    outputVector(idx : idx + block_size - 1, :) = output;
    idx = idx + block_size;
end

disp('Processing complete.');
figure
plot(outputVector)
hold on
plot(Y_volterra)