clear
close all

mes_dir = 'measurements';
files = dir;
dx = 0.088;
dy = 0.0775;
x = (0:9)'*dx;
y = (0:6)'*dy;
[X,Y] = meshgrid(x,y);
X = X(:);
Y = Y(:);

H = [];
for n = 1 : length(X)
    Results = load(fullfile(mes_dir,strcat('Untitled_',num2str(round(X(n)/dx), '%02d'),mat2str(round(Y(n)/dy)),'.mat')));
    N = Results.Result.AcqSetup.nSamples;
    H(n,:) = Results.Result.FrequencyResponseFunction(:,2);
    h(n,:) = ifft(Results.Result.FrequencyResponseFunction(:,2),N,'symmetric');
end
%%
fs = Results.Result.AcqSetup.Fs;
t = (0:N-1)'/fs;
flim = [1 400];
Fs2 = 2*flim(2);
[h_res, t] = filtresamp(h, t, flim, Fs2, 1024);
h_res(isnan(h_res)) = 0;
omvec = 2*pi*(0 : N/2)/N*fs;
indicator = 20*log10(sum(abs(H),1));
%lambda = stabilization(h, t(2)-t(1), [1 60], 2*pi*flim, 1e-2, omvec, indicator);

stab_diag = generate_stabilization_diagram(h_res, t(2)-t(1), [1 30], 2*pi*flim, 1e-2, omvec, indicator);
%save('stab_diag_0_100_ver1','stab_diag')
%pause
%%
lambda = get_poles(stab_diag);
res = lstd(lambda, t,h_res);

f_modal = imag(lambda(1:2:end))/(2*pi);
I_modal = sqrt(sum( res( :,1:2:end ).*conj(res( :,1:2:end ) ), 1)');
shapes = real(bsxfun(@times, res(:,1:2:end), 1./res(1,1:2:end)));

[~,ix] = sort(f_modal);
f_modal = f_modal(ix);
shapes = shapes(:,ix);
%
x_new = (x(1):0.01:x(end))';
y_new = (y(1):0.01:y(end))';
[X_new,Y_new] = meshgrid(x_new,y_new);

Nmode = size(shapes,2);
Nmode0 = (floor(sqrt(size(shapes,2)))).^2;
Ns = size(shapes,2);

figure

for ix = 1 : size(shapes,2)
    subplot(3,3,ix)
    F = scatteredInterpolant(X,Y,shapes(:,ix));
    out = reshape(F([X_new(:),Y_new(:)]), size(X_new,1), size(X_new,2));
    pcolor(x_new,y_new,out);
    %surf(x,y,reshape(shapes(:,ix),length(y),length(x)))
    shading interp
    axis equal tight
    colorbar
    title(sprintf('%.2f Hz',f_modal(ix)))
    xlabel('x -> [m]')
    ylabel('y -> [m]')
end 