clear;
clc;

%% import FRFs from measurements
load data/frf H freqvec
omvec = 2*pi*freqvec;
Nom = size(H,2)-1;
Fs = max(freqvec)*2;
indicator = 20*log10(sum(abs(H),1));

%% inverse fourer transform
h = ifft([H conj(fliplr(H(:,2:end-1)))], [], 2, 'symmetric');
t = (0:size(h,2)-1)/Fs;

%% Resampling for the selected frequency range
flim = [100 500];
Fs2 = 2*flim(2)*4;
[h, t] = filtresamp(h, t, flim, Fs2, 1024);

%% Plot resampled IRs
XL = [0 .35];
YL = [-600 600];
figure;
for j = 1:10
subplot(5,2,j);
plot(t, h(j,:));
xlabel('Time [s]');
% set(gca, 'xlim', XL);
% set(gca, 'ylim', YL);
end

%% stabilization
lambda = stabilization(h, t(2)-t(1), [1 40], 2*pi*flim, 1e-2, omvec, indicator);

%% estimate the FRFs IRs and shapes
res = lstd(lambda, t, h);
Hest = res * (1./(repmat(1i*omvec,length(lambda),1) - repmat(lambda,1,length(omvec))));
hest = res * exp(lambda*t);
shapes = imag(res(:,1:2:end));
om = imag(lambda(1:2:end));
alpha = real(lambda(1:2:end));
%shapes = res(:,1:2:end);
%shapes = shapes ./ repmat(max(shapes),142,1);

%% Plot the synthesized FRFs
j = 15;
figure;
subplot(2,1,1)
plot(omvec/2/pi, 20*log10(abs(H(j,:))), omvec/2/pi, 20*log10(abs(Hest(j,:))));
xlabel('Frequency [Hz]');
ylabel('FRF [dB]');
subplot(2,1,2)
plot(omvec/2/pi, angle(H(j,:)), omvec/2/pi, angle(Hest(j,:)));
xlabel('Frequency [Hz]');
ylabel('angle(FRF) [rad]');

%% Plot the synthesized IRs
j = 2;
figure;
plot(t, h(1,:), t, hest(1,:));
xlabel('time [s]');
ylabel('IR');

%% Plot the modes
load data/guitarmesh x y tri;
im = importdata('data/gitar.png');
plotmode(om, alpha, shapes, x, y, tri, im);