%% Huddle Test
ht = load('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');

% Detrend Data
ht.d = detrend(ht.d, 0);
ht.acc_1 = detrend(ht.acc_1, 0);
ht.acc_2 = detrend(ht.acc_2, 0);
ht.geo_1 = detrend(ht.geo_1, 0);
ht.geo_2 = detrend(ht.geo_2, 0);
ht.f_meas = detrend(ht.f_meas, 0);

% Compute PSD
run setup;

win = hann(ceil(10/Ts));

[p_d, f] = pwelch(ht.d, win, [], [], 1/Ts);
[p_acc1, ~] = pwelch(ht.acc_1, win, [], [], 1/Ts);
[p_acc2, ~] = pwelch(ht.acc_2, win, [], [], 1/Ts);
[p_geo1, ~] = pwelch(ht.geo_1, win, [], [], 1/Ts);
[p_geo2, ~] = pwelch(ht.geo_2, win, [], [], 1/Ts);
[p_fmeas, ~] = pwelch(ht.f_meas, win, [], [], 1/Ts);

% Plot PSD
figure;
hold on;
plot(f, p_acc1);
plot(f, p_acc2);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
title('Huddle Test - Accelerometers')

figure;
hold on;
plot(f, p_geo1);
plot(f, p_geo2);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
title('Huddle Test - Geophones')

figure;
hold on;
plot(f, p_d);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
title('Huddle Test - Interferometers')

figure;
hold on;
plot(f, p_fmeas);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
title('Huddle Test - Force Sensor')

%% Accelerometer and Geophone Models
% Accelerometer used: https://www.pcb.com/products?model=393B05
% Geophone used: L22 https://www.sercel.com/products/Lists/ProductSpecification/Geophones_brochure_Sercel_EN.pdf

G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
G_geo = 120*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [[V/(m/s)]

% PSD of intertial sensors in [m^2/Hz]
figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, p_acc1./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
    'DisplayName', 'Accelerometer');
set(gca, 'ColorOrderIndex', 1);
plot(f, p_acc2./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
    'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 2);
plot(f, p_geo1./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
    'DisplayName', 'Geophone');
set(gca, 'ColorOrderIndex', 2);
plot(f, p_geo2./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
    'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 3);
plot(f, p_d, 'DisplayName', 'Interferometer');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
title('Huddle Test')
legend();

%% Compare Theoretical model with identified one
id_ol = load('./mat/identification_chirp_40_400.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');

% Detrend Data
id_ol.d = detrend(id_ol.d, 0);
id_ol.acc_1 = detrend(id_ol.acc_1, 0);
id_ol.acc_2 = detrend(id_ol.acc_2, 0);
id_ol.geo_1 = detrend(id_ol.geo_1, 0);
id_ol.geo_2 = detrend(id_ol.geo_2, 0);
id_ol.f_meas = detrend(id_ol.f_meas, 0);
id_ol.u = detrend(id_ol.u, 0);

% Identification Parameters
run setup;
win = hann(ceil(10/Ts));

% IFF Plant
[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/m]
[co_fmeas_est, ~] = mscohere(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts);

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_fmeas_est), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;

ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_fmeas_est)), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

linkaxes([ax1,ax2], 'x');
xlim([40, 400]);

% Geophones
[tf_geo1_est, ~] = tfestimate(id_ol.d, id_ol.geo_1, win, [], [], 1/Ts); % [V/m]
[co_geo1_est, ~] = mscohere(id_ol.d, id_ol.geo_1, win, [], [], 1/Ts);

[tf_geo2_est, ~] = tfestimate(id_ol.d, id_ol.geo_2, win, [], [], 1/Ts); % [V/m]
[co_geo2_est, ~] = mscohere(id_ol.d, id_ol.geo_2, win, [], [], 1/Ts);

figure;
ax1 = subplot(2, 1, 1);
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, abs(tf_geo1_est), '.')
set(gca, 'ColorOrderIndex', 1);
plot(f, abs(tf_geo2_est), '.')
plot(f, abs(squeeze(freqresp(G_geo, f, 'Hz')).*(1i*2*pi*f)), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;

ax2 = subplot(2, 1, 2);
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, 180/pi*angle(tf_geo1_est), '.')
set(gca, 'ColorOrderIndex', 1);
plot(f, 180/pi*angle(tf_geo2_est), '.')
plot(f, 180/pi*angle(-squeeze(freqresp(G_geo, f, 'Hz')).*(1i*2*pi*f)), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

linkaxes([ax1,ax2], 'x');
xlim([40, 400]);

% Accelerometers
[tf_acc1_est, ~] = tfestimate(id_ol.d, id_ol.acc_1, win, [], [], 1/Ts); % [V/m]
[co_acc1_est, ~] = mscohere(id_ol.d, id_ol.acc_1, win, [], [], 1/Ts);

[tf_acc2_est, ~] = tfestimate(id_ol.d, id_ol.acc_2, win, [], [], 1/Ts); % [V/m]
[co_acc2_est, ~] = mscohere(id_ol.d, id_ol.acc_2, win, [], [], 1/Ts);

figure;
ax1 = subplot(2, 1, 1);
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, abs(tf_acc1_est), '.')
set(gca, 'ColorOrderIndex', 1);
plot(f, abs(tf_acc2_est), '.')
plot(f, abs(squeeze(freqresp(G_acc, f, 'Hz')).*(1i*2*pi*f).^2), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;

ax2 = subplot(2, 1, 2);
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, 180/pi*angle(tf_acc1_est), '.')
set(gca, 'ColorOrderIndex', 1);
plot(f, 180/pi*angle(tf_acc2_est), '.')
plot(f, 180/pi*angle(squeeze(freqresp(G_acc, f, 'Hz')).*(1i*2*pi*f).^2), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

linkaxes([ax1,ax2], 'x');
xlim([40, 400]);

%% IFF development
[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/m]
[co_fmeas_est, ~] = mscohere(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts);

% Model
wz = 2*pi*103;
xi_z = 0.01;
wp = 2*pi*237;
xi_p = 0.015;

Giff = -20*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2);

% Comparison model and identification
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_fmeas_est), '.')
plot(f, abs(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;

ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*angle(tf_fmeas_est), '.')
plot(f, 180/pi*angle(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

linkaxes([ax1,ax2], 'x');
xlim([40, 400]);

% Root Locus
gains = logspace(0, 5, 1000);

figure;
hold on;
plot(real(pole(Giff)),  imag(pole(Giff)),  'kx');
plot(real(tzero(Giff)),  imag(tzero(Giff)),  'ko');
for i = 1:length(gains)
    cl_poles = pole(feedback(Giff, -gains(i)/(s + 2*pi*2)));
    plot(real(cl_poles), imag(cl_poles), 'k.');
end
ylim([0, 1800]);
xlim([-1600,200]); 
xlabel('Real Part')
ylabel('Imaginary Part')
axis square

% Optimal Controller
Kiff_opt = -110/(s + 2*pi*2);

%% New identification 
id_ol = load('./mat/identification_chirp_40_400.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
id_cl = load('./mat/identification_chirp_40_400_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');

% Used controller
Kiff = -110/(s + 2*pi*2);

[tf_G_ol_est, f] = tfestimate(id_ol.u, id_ol.d, win, [], [], 1/Ts);
[co_G_ol_est, ~] = mscohere(id_ol.u, id_ol.d, win, [], [], 1/Ts);
[tf_G_cl_est, ~] = tfestimate(id_cl.u, id_cl.d, win, [], [], 1/Ts);
[co_G_cl_est, ~] = mscohere(id_cl.u, id_cl.d, win, [], [], 1/Ts);

figure;
hold on;
plot(f, co_G_ol_est, '-')
plot(f, co_G_cl_est, '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Coherence'); xlabel('Frequency [Hz]');
hold off;
xlim([40, 400]); ylim([0, 1])

% Comparison model and identification
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_G_ol_est), '-')
plot(f, abs(tf_G_cl_est), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;

ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*angle(tf_G_ol_est), '-')
plot(f, 180/pi*angle(tf_G_cl_est), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

linkaxes([ax1,ax2], 'x');
xlim([40, 400]);

%% Estimation of the inertial sensor transfer functions
id = load('./mat/identification_noise_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
ht = load('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');

% Compare PSD
run setup;
win = hann(ceil(1/Ts));

[p_id_d, f] = pwelch(id.d, win, [], [], 1/Ts);
[p_id_acc1, ~] = pwelch(id.acc_1, win, [], [], 1/Ts);
[p_id_acc2, ~] = pwelch(id.acc_2, win, [], [], 1/Ts);
[p_id_geo1, ~] = pwelch(id.geo_1, win, [], [], 1/Ts);
[p_id_geo2, ~] = pwelch(id.geo_2, win, [], [], 1/Ts);
[p_id_fmeas, ~] = pwelch(id.f_meas, win, [], [], 1/Ts);

[p_ht_d, ~] = pwelch(ht.d, win, [], [], 1/Ts);
[p_ht_acc1, ~] = pwelch(ht.acc_1, win, [], [], 1/Ts);
[p_ht_acc2, ~] = pwelch(ht.acc_2, win, [], [], 1/Ts);
[p_ht_geo1, ~] = pwelch(ht.geo_1, win, [], [], 1/Ts);
[p_ht_geo2, ~] = pwelch(ht.geo_2, win, [], [], 1/Ts);
[p_ht_fmeas, ~] = pwelch(ht.f_meas, win, [], [], 1/Ts);

figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, p_ht_acc1, 'DisplayName', 'Huddle Test');
set(gca, 'ColorOrderIndex', 1);
plot(f, p_ht_acc2, 'HandleVisibility',  'off');
set(gca, 'ColorOrderIndex', 2);
plot(f, p_id_acc1, 'DisplayName', 'Identification Test');
set(gca, 'ColorOrderIndex', 2);
plot(f, p_id_acc2, 'HandleVisibility',  'off');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
title('Huddle Test - Accelerometers')
legend();

figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, p_ht_geo1, 'DisplayName', 'Huddle Test');
set(gca, 'ColorOrderIndex', 1);
plot(f, p_ht_geo2, 'HandleVisibility',  'off');
set(gca, 'ColorOrderIndex', 2);
plot(f, p_id_geo1, 'DisplayName', 'Identification Test');
set(gca, 'ColorOrderIndex', 2);
plot(f, p_id_geo2, 'HandleVisibility',  'off');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
title('Huddle Test - Geophones')
legend();

figure;
hold on;
plot(f, p_ht_d, 'DisplayName', 'Huddle Test');
plot(f, p_id_d, 'DisplayName', 'Identification Test');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
title('Huddle Test - Interferometers')
legend();

% tf and coh computation
[tf_acc1_est, f] = tfestimate(id.d, id.acc_1, win, [], [], 1/Ts);
[co_acc1_est, ~] = mscohere(id.d, id.acc_1, win, [], [], 1/Ts);
[tf_acc2_est, ~] = tfestimate(id.d, id.acc_2, win, [], [], 1/Ts);
[co_acc2_est, ~] = mscohere(id.d, id.acc_2, win, [], [], 1/Ts);

[tf_geo1_est, ~] = tfestimate(id.d, id.geo_1, win, [], [], 1/Ts);
[co_geo1_est, ~] = mscohere(id.d, id.geo_1, win, [], [], 1/Ts);
[tf_geo2_est, ~] = tfestimate(id.d, id.geo_2, win, [], [], 1/Ts);
[co_geo2_est, ~] = mscohere(id.d, id.geo_2, win, [], [], 1/Ts);

% Coherence
figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, co_acc1_est, '-', 'DisplayName', 'Accelerometer')
set(gca, 'ColorOrderIndex', 1);
plot(f, co_acc2_est, '-',  'HandleVisibility', 'off')
set(gca, 'ColorOrderIndex', 2);
plot(f, co_geo1_est, '-', 'DisplayName', 'Geophone')
set(gca, 'ColorOrderIndex', 2);
plot(f, co_geo2_est, '-',  'HandleVisibility', 'off')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Coherence'); xlabel('Frequency [Hz]');
hold off;
xlim([2, 2e3]); ylim([0, 1])
legend();

% Transfer Functions
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_acc1_est), '-')
plot(f, abs(tf_acc2_est), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;

ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*angle(tf_acc1_est), '-')
plot(f, 180/pi*angle(tf_acc2_est), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

linkaxes([ax1,ax2], 'x');
xlim([2, 2e3]);


figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_geo1_est), '-')
plot(f, abs(tf_geo2_est), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;

ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*angle(tf_geo1_est), '-')
plot(f, 180/pi*angle(tf_geo2_est), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

linkaxes([ax1,ax2], 'x');
xlim([2, 2e3]);