%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

addpath('./mat/');

% Transfer function from excitation signal to displacement
% Let's first estimate the transfer function from the excitation signal in [V] to the generated displacement in [m] as measured by the inteferometer.


id_cl = load('identification_noise_iff_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');

id_cl.d      = detrend(id_cl.d, 0);
id_cl.acc_1  = detrend(id_cl.acc_1, 0);
id_cl.acc_2  = detrend(id_cl.acc_2, 0);
id_cl.geo_1  = detrend(id_cl.geo_1, 0);
id_cl.geo_2  = detrend(id_cl.geo_2, 0);
id_cl.f_meas = detrend(id_cl.f_meas, 0);
id_cl.u      = detrend(id_cl.u, 0);

Ts = id_cl.t(2) - id_cl.t(1);
win = hann(ceil(10/Ts));

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



% Approximate transfer function from voltage output to generated displacement when IFF is used, in [m/V].

G_d_est = -5e-6*(2*pi*230)^2/(s^2 + 2*0.3*2*pi*240*s + (2*pi*240)^2);

figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(tf_G_cl_est), '-')
plot(f, abs(squeeze(freqresp(G_d_est, f, 'Hz'))), '--')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
hold off;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_G_cl_est), '-')
plot(f, 180/pi*angle(squeeze(freqresp(G_d_est, f, 'Hz'))), '--')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);

linkaxes([ax1,ax2], 'x');
xlim([10, 1000]);

% Motion measured during Huddle test
% We now compute the PSD of the measured motion by the inertial sensors during the huddle test.

ht = load('huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
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);

[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);



% Using an estimated model of the sensor dynamics from the documentation of the sensors, we can compute the ASD of the motion in $m/\sqrt{Hz}$ measured by the sensors.

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)]

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



% #+name: fig:huddle_test_psd_motion
% #+caption: ASD of the motion measured by the sensors
% #+RESULTS:
% [[file:figs/huddle_test_psd_motion.png]]

% From the ASD of the motion measured by the sensors, we can create an excitation signal that will generate much motion motion that the motion under no excitation.

% We create =G_exc= that corresponds to the wanted generated motion.

G_exc = 0.2e-6/(1 + s/2/pi/2)/(1 + s/2/pi/50);



% And we create a time domain signal =y_d= that have the spectral density described by =G_exc=.

Fs = 1/Ts;
t = 0:Ts:180; % Time Vector [s]
u = sqrt(Fs/2)*randn(length(t), 1); % Signal with an ASD equal to one

y_d = lsim(G_exc, u, t);

[pxx, ~] = pwelch(y_d, win, 0, [], Fs);

figure;
hold on;
set(gca, 'ColorOrderIndex', 1);
plot(f, sqrt(p_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
     'DisplayName', 'Accelerometer');
set(gca, 'ColorOrderIndex', 1);
plot(f, sqrt(p_acc2)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
     'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 2);
plot(f, sqrt(p_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
     'DisplayName', 'Geophone');
set(gca, 'ColorOrderIndex', 2);
plot(f, sqrt(p_geo2)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
     'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 3);
plot(f, sqrt(pxx), 'k-', ...
     'DisplayName', 'Excitation');
plot(f, sqrt(p_d), 'DisplayName', 'Interferometer');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
title('Huddle Test')
legend();



% #+name: fig:comp_huddle_test_excited_motion_psd
% #+caption: Comparison of the ASD of the motion during Huddle and the wanted generated motion
% #+RESULTS:
% [[file:figs/comp_huddle_test_excited_motion_psd.png]]


% We can now generate the voltage signal that will generate the wanted motion.

y_v = lsim(G_exc * ... % from unit PSD to shaped PSD
           (1 + s/2/pi/50) * ... % Inverse of pre-filter included in the Simulink file
           1/G_d_est * ... % Wanted displacement => required voltage
           1/(1 + s/2/pi/5e3), ... %  Add some high frequency filtering
           u, t);

figure;
plot(t, y_v)
xlabel('Time [s]'); ylabel('Voltage [V]');