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

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

addpath('./mat/');

fs = 1e4;
Ts = 1/fs;
freqs = logspace(1, 3, 1000);

% Input / Output data
% The identification data is loaded

uh = load('mat/data_cal_pd_h.mat', 't', 'Vph', 'Vpv', 'Vnh');
uv = load('mat/data_cal_pd_v.mat', 't', 'Vph', 'Vpv', 'Vnv');



% We remove the first seconds where the Sercalo is turned on.

t0 = 1;

uh.Vph(uh.t<t0) = [];
uh.Vpv(uh.t<t0) = [];
uh.Vnh(uh.t<t0) = [];
uh.t(uh.t<t0)   = [];
uh.t = uh.t - uh.t(1); % We start at t=0

t0 = 1;

uv.Vph(uv.t<t0) = [];
uv.Vpv(uv.t<t0) = [];
uv.Vnv(uv.t<t0) = [];
uv.t(uv.t<t0) = [];
uv.t = uv.t - uv.t(1); % We start at t=0

figure;
ax1 = subplot(1, 2, 1);
plot(uh.t, uh.Vnh, 'DisplayName', '$Vn_h$');
xlabel('Time [s]');
ylabel('Amplitude [V]');
legend();

ax2 = subplot(1, 2, 2);
hold on;
plot(uh.t, uh.Vph, 'DisplayName', '$Vp_h$');
plot(uh.t, uh.Vpv, 'DisplayName', '$Vp_v$');
hold off;
xlabel('Time [s]');
ylabel('Amplitude [V]');
legend();

linkaxes([ax1,ax2],'x');
xlim([uh.t(1), uh.t(end)]);



% #+NAME: fig:calib_4qd_h
% #+CAPTION: Identification signals when exciting the horizontal direction ([[./figs/calib_4qd_h.png][png]], [[./figs/calib_4qd_h.pdf][pdf]])
% [[file:figs/calib_4qd_h.png]]



figure;
ax1 = subplot(1, 2, 1);
plot(uv.t, uv.Vnv, 'DisplayName', '$Vn_v$');
xlabel('Time [s]');
ylabel('Amplitude [V]');

ax2 = subplot(1, 2, 2);
hold on;
plot(uv.t, uv.Vpv, 'DisplayName', '$Vp_v$');
plot(uv.t, uv.Vph, 'DisplayName', '$Vp_h$');
hold off;
xlabel('Time [s]');
ylabel('Amplitude [V]');
legend();

linkaxes([ax1,ax2],'x');
xlim([uv.t(1), uv.t(end)]);

% Linear Regression to obtain the gain of the 4QD
% We plot the angle of mirror

% Gain of the Newport metrology in [rad/V].

gn0 = 2.62e-3;



% The angular displacement of the beam is twice the angular displacement of the Newport mirror.

% We do a linear regression
% \[ y = a x + b \]
% where:
% - $y$ is the measured voltage of the 4QD in [V]
% - $x$ is the beam angle (twice the mirror angle) in [rad]
% - $a$ is the identified gain of the 4QD in [rad/V]

% The linear regression is shown in Fig. [[fig:4qd_linear_reg]].


bh = [ones(size(uh.Vnh)) 2*gn0*uh.Vnh]\uh.Vph;
bv = [ones(size(uv.Vnv)) 2*gn0*uv.Vnv]\uv.Vpv;

figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(2*gn0*uh.Vnh, uh.Vph, 'o', 'DisplayName', 'Exp. data');
plot(2*gn0*[min(uh.Vnh) max(uh.Vnh)], 2*gn0*[min(uh.Vnh) max(uh.Vnh)].*bh(2) + bh(1), 'k--', 'DisplayName', sprintf('%.1e x + %.1e', bh(2), bh(1)))
hold off;
xlabel('$\alpha_{0,h}$ [rad]'); ylabel('$Vp_h$ [V]');
legend();

ax2 = subplot(1, 2, 2);
hold on;
plot(2*gn0*uv.Vnv, uv.Vpv, 'o', 'DisplayName', 'Exp. data');
plot(2*gn0*[min(uv.Vnv) max(uv.Vnv)], 2*gn0*[min(uv.Vnv) max(uv.Vnv)].*bv(2) + bv(1), 'k--', 'DisplayName', sprintf('%.1e x + %.1e', bv(2), bv(1)))
hold off;
xlabel('$\alpha_{0,v}$ [rad]'); ylabel('$Vp_v$ [V]');
legend();



% #+name: tab:gain_4qd
% #+attr_latex: :environment tabularx :width 0.5\linewidth :align lX
% #+attr_latex: :center t :booktabs t :float t
% #+caption: Identified Gain of the 4 quadrant diode
% #+RESULTS:
% | Horizontal [V/rad] | Vertical [V/rad] |
% |--------------------+------------------|
% |              -31.0 |             36.3 |


Gd = tf([bh(2) 0 ;
         0     bv(2)]);

% Theoretical Transfer Functions
% The values of the components in the current amplifier have been measured.

Rh = 41; % [Ohm]
Lch = 0.1e-3; % [H]
Rch = 9.3; % [Ohm]

Rv = 41; % [Ohm]
Lcv = 0.1e-3; % [H]
Rcv = 8.3; % [Ohm]



% \begin{align*}
%   G_i(s) &= \frac{1}{(R + R_c) + L_c s} \\
%   Z_c(s) &= R_c + L_c s \\
%   G_a(s) &= \frac{1000}{1 + s/\omega_c}
% \end{align*}


Gi = blkdiag(1/(Rh + Rch + Lch * s), 1/(Rv + Rcv + Lcv * s));
Zc = blkdiag(Rch+Lch*s, Rcv+Lcv*s);
Ga = blkdiag(1000/(1 + s/2/pi/1000), 1000/(1 + s/2/pi/1000));

freqs = logspace(1, 4, 1000);

figure;

ax1 = subplot(1, 3, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gi(1,1), freqs, 'Hz'))), 'DisplayName', '$G_{i, h} = \frac{I_{c,h}}{U_{c,h}}$')
plot(freqs, abs(squeeze(freqresp(Gi(2,2), freqs, 'Hz'))), 'DisplayName', '$G_{i, v} = \frac{I_{c,v}}{U_{c,v}}$')
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [A/V]');
legend('location', 'northwest');
ylim([0.01, 1]);

ax2 = subplot(1, 3, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(Zc(1,1), freqs, 'Hz'))), 'DisplayName', '$Z_{c, h} = \frac{\tilde{V}_{c,h}}{I_{c,h}}$')
plot(freqs, abs(squeeze(freqresp(Zc(2,2), freqs, 'Hz'))), 'DisplayName', '$Z_{c, v} = \frac{\tilde{V}_{c,v}}{I_{c,v}}$')
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [V/A]');
legend('location', 'southwest');
ylim([1, 100]);

ax3 = subplot(1, 3, 3);
hold on;
plot(freqs, abs(squeeze(freqresp(Ga(1,1), freqs, 'Hz'))), 'DisplayName', '$G_{a, h} = \frac{V_{c,h}}{\tilde{V}_{c,h}}$')
plot(freqs, abs(squeeze(freqresp(Ga(2,2), freqs, 'Hz'))), 'DisplayName', '$G_{a, v} = \frac{V_{c,v}}{\tilde{V}_{c,v}}$')
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [V/V]');
legend('location', 'southwest');
ylim([10, 1000]);

linkaxes([ax1, ax2, ax3], 'x');
xlim([freqs(1), freqs(end)]);



% #+NAME: fig:current_amplifier_tf
% #+CAPTION: Transfer function for the current amplifier ([[./figs/current_amplifier_tf.png][png]], [[./figs/current_amplifier_tf.pdf][pdf]])
% [[file:figs/current_amplifier_tf.png]]

% #+begin_important
% Over the frequency band of interest, the current amplifier transfer function $G_i$ can be considered as constant.
% This is the same for the impedance $Z_c$.
% #+end_important


Gi = tf(blkdiag(1/(Rh + Rch), 1/(Rv + Rcv)));
Zc = tf(blkdiag(Rch, Rcv));

% Identified Transfer Functions
% Noise is generated using the DAC ($[U_{c,h}\ U_{c,v}]$) and we measure the output of the voltage amplifier $[V_{c,h}, V_{c,v}]$.
% From that, we should be able to identify $G_a Z_c G_i$.

% The identification data is loaded.

uh = load('mat/data_uch.mat', 't', 'Uch', 'Vch');
uv = load('mat/data_ucv.mat', 't', 'Ucv', 'Vcv');



% We remove the first seconds where the Sercalo is turned on.

t0 = 1;

uh.Uch(uh.t<t0) = [];
uh.Vch(uh.t<t0) = [];
uh.t(uh.t<t0)   = [];
uh.t = uh.t - uh.t(1); % We start at t=0

t0 = 1;

uv.Ucv(uv.t<t0) = [];
uv.Vcv(uv.t<t0) = [];
uv.t(uv.t<t0)   = [];
uv.t = uv.t - uv.t(1); % We start at t=0

win = hanning(ceil(1*fs));
[GaZcGi_h, f] = tfestimate(uh.Uch, uh.Vch, win, [], [], fs);
[GaZcGi_v, ~] = tfestimate(uv.Ucv, uv.Vcv, win, [], [], fs);

GaZcGi_resp = abs(squeeze(freqresp(Ga*Zc*Gi, f, 'Hz')));

figure;

ax1 = subplot(1, 2, 1);
hold on;
plot(f, abs(GaZcGi_h), 'k-', 'DisplayName', 'Identified')
plot(f, squeeze(GaZcGi_resp(1,1,:)), 'k--', 'DisplayName', 'Theoretical')
title('FRF $G_{i,h} Z_{c,h} G_{a,h} = \frac{V_{c,h}}{U_{c,h}}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
legend();
hold off;

ax2 = subplot(1, 2, 2);
hold on;
plot(f, abs(GaZcGi_v), 'k-', 'DisplayName', 'Identified')
plot(f, squeeze(GaZcGi_resp(2,2,:)), 'k--', 'DisplayName', 'Theoretical')
title('FRF $G_{a,v} Z_{c,v} G_{i,v} = \frac{V_{c,v}}{U_{c,v}}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
legend();
hold off;

linkaxes([ax1,ax2],'xy');
xlim([1, 2000]);



% #+NAME: fig:current_amplifier_comp_theory_id
% #+CAPTION: Identified and Theoretical Transfer Function $G_a G_i$ ([[./figs/current_amplifier_comp_theory_id.png][png]], [[./figs/current_amplifier_comp_theory_id.pdf][pdf]])
% [[file:figs/current_amplifier_comp_theory_id.png]]

% There is a gain mismatch, that is probably due to bad identification of the inductance and resistance measurement of the sercalo inductors.
% Thus, we suppose $G_a$ is perfectly known (the gain and cut-off frequency of the voltage amplifier is very accurate) and that $G_i$ is also well determined as it mainly depends on the resistor used in the amplifier that is well measured.

Gi_resp_h = abs(GaZcGi_h)./squeeze(abs(freqresp(Ga(1,1)*Zc(1,1), f, 'Hz')));
Gi_resp_v = abs(GaZcGi_v)./squeeze(abs(freqresp(Ga(2,2)*Zc(2,2), f, 'Hz')));
Gi = tf(blkdiag(mean(Gi_resp_h(f>20 & f<200)), mean(Gi_resp_v(f>20 & f<200))));

GaZcGi_resp = abs(squeeze(freqresp(Ga*Zc*Gi, f, 'Hz')));

figure;

ax1 = subplot(1, 2, 1);
hold on;
plot(f, abs(GaZcGi_h), 'k-', 'DisplayName', 'Identified')
plot(f, squeeze(GaZcGi_resp(1,1,:)), 'k--', 'DisplayName', 'Theoretical')
title('FRF $G_{i,h} Z_{c,h} G_{a,h} = \frac{V_{c,h}}{U_{c,h}}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
legend();
hold off;

ax2 = subplot(1, 2, 2);
hold on;
plot(f, abs(GaZcGi_v), 'k-', 'DisplayName', 'Identified')
plot(f, squeeze(GaZcGi_resp(2,2,:)), 'k--', 'DisplayName', 'Theoretical')
title('FRF $G_{a,v} Z_{c,v} G_{i,v} = \frac{V_{c,v}}{U_{c,v}}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
legend();
hold off;

linkaxes([ax1,ax2],'xy');
xlim([1, 2000]);



% #+NAME: fig:current_amplifier_comp_theory_id_bis
% #+CAPTION: Identified and Theoretical Transfer Function $G_a G_i$ ([[./figs/current_amplifier_comp_theory_id_bis.png][png]], [[./figs/current_amplifier_comp_theory_id_bis.pdf][pdf]])
% [[file:figs/current_amplifier_comp_theory_id_bis.png]]

% Finally, we have the following transfer functions:

Gi,Zc,Ga

% Input / Output data
% The identification data is loaded

uh = load('mat/data_uch.mat', 't', 'Uch', 'Vph', 'Vpv');
uv = load('mat/data_ucv.mat', 't', 'Ucv', 'Vph', 'Vpv');



% We remove the first seconds where the Sercalo is turned on.

t0 = 1;

uh.Uch(uh.t<t0) = [];
uh.Vph(uh.t<t0) = [];
uh.Vpv(uh.t<t0) = [];
uh.t(uh.t<t0)   = [];
uh.t = uh.t - uh.t(1); % We start at t=0

t0 = 1;

uv.Ucv(uv.t<t0) = [];
uv.Vph(uv.t<t0) = [];
uv.Vpv(uv.t<t0) = [];
uv.t(uv.t<t0)   = [];

uv.t = uv.t - uv.t(1); % We start at t=0

figure;
ax1 = subplot(1, 2, 1);
plot(uh.t, uh.Uch, 'DisplayName', '$Uc_h$');
xlabel('Time [s]');
ylabel('Amplitude [V]');
legend();

ax2 = subplot(1, 2, 2);
hold on;
plot(uh.t, uh.Vph, 'DisplayName', '$Vp_h$');
plot(uh.t, uh.Vpv, 'DisplayName', '$Vp_v$');
hold off;
xlabel('Time [s]');
ylabel('Amplitude [V]');
legend();

linkaxes([ax1,ax2],'x');
xlim([uh.t(1), uh.t(end)]);



% #+NAME: fig:identification_uh
% #+CAPTION: Identification signals when exciting the horizontal direction ([[./figs/identification_uh.png][png]], [[./figs/identification_uh.pdf][pdf]])
% [[file:figs/identification_uh.png]]



figure;
ax1 = subplot(1, 2, 1);
plot(uv.t, uv.Ucv, 'DisplayName', '$Uc_v$');
xlabel('Time [s]');
ylabel('Amplitude [V]');

ax2 = subplot(1, 2, 2);
hold on;
plot(uv.t, uv.Vpv, 'DisplayName', '$Vp_v$');
plot(uv.t, uv.Vph, 'DisplayName', '$Vp_h$');
hold off;
xlabel('Time [s]');
ylabel('Amplitude [V]');
legend();

linkaxes([ax1,ax2],'x');
xlim([uv.t(1), uv.t(end)]);

% Coherence
% The window used for the spectral analysis is an =hanning= windows with temporal size equal to 1 second.

win = hanning(ceil(1*fs));

[coh_Uch_Vph, f] = mscohere(uh.Uch, uh.Vph, win, [], [], fs);
[coh_Uch_Vpv, ~] = mscohere(uh.Uch, uh.Vpv, win, [], [], fs);
[coh_Ucv_Vph, ~] = mscohere(uv.Ucv, uv.Vph, win, [], [], fs);
[coh_Ucv_Vpv, ~] = mscohere(uv.Ucv, uv.Vpv, win, [], [], fs);

figure;
ax11 = subplot(2, 2, 1);
hold on;
plot(f, coh_Uch_Vph)
set(gca, 'Xscale', 'log');
title('Coherence $\frac{Vp_h}{Uc_h}$')
ylabel('Coherence')
hold off;

ax12 = subplot(2, 2, 2);
hold on;
plot(f, coh_Ucv_Vph)
set(gca, 'Xscale', 'log');
title('Coherence $\frac{Vp_h}{Uc_v}$')
hold off;

ax21 = subplot(2, 2, 3);
hold on;
plot(f, coh_Uch_Vpv)
set(gca, 'Xscale', 'log');
title('Coherence $\frac{Vp_v}{Uc_h}$')
ylabel('Coherence')
xlabel('Frequency [Hz]')
hold off;

ax22 = subplot(2, 2, 4);
hold on;
plot(f, coh_Ucv_Vpv)
set(gca, 'Xscale', 'log');
title('Coherence $\frac{Vp_v}{Uc_v}$')
xlabel('Frequency [Hz]')
hold off;

linkaxes([ax11,ax12,ax21,ax22],'xy');
xlim([10, 1000]); ylim([0, 1]);

% Estimation of the Frequency Response Function Matrix
% We compute an estimate of the transfer functions.

[tf_Uch_Vph, f] = tfestimate(uh.Uch, uh.Vph, win, [], [], fs);
[tf_Uch_Vpv, ~] = tfestimate(uh.Uch, uh.Vpv, win, [], [], fs);
[tf_Ucv_Vph, ~] = tfestimate(uv.Ucv, uv.Vph, win, [], [], fs);
[tf_Ucv_Vpv, ~] = tfestimate(uv.Ucv, uv.Vpv, win, [], [], fs);

figure;
ax11 = subplot(2, 2, 1);
hold on;
plot(f, abs(tf_Uch_Vph))
title('Frequency Response Function $\frac{Vp_h}{Uc_h}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [V/V]')
hold off;

ax12 = subplot(2, 2, 2);
hold on;
plot(f, abs(tf_Ucv_Vph))
title('Frequency Response Function $\frac{Vp_h}{Uc_v}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
hold off;

ax21 = subplot(2, 2, 3);
hold on;
plot(f, abs(tf_Uch_Vpv))
title('Frequency Response Function $\frac{Vp_v}{Uc_h}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [V/V]')
xlabel('Frequency [Hz]')
hold off;

ax22 = subplot(2, 2, 4);
hold on;
plot(f, abs(tf_Ucv_Vpv))
title('Frequency Response Function $\frac{Vp_v}{Uc_v}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]')
hold off;

linkaxes([ax11,ax12,ax21,ax22],'xy');
xlim([10, 1000]); ylim([1e-2, 1e3]);



% #+NAME: fig:frf_sercalo_gain
% #+CAPTION: Frequency Response Matrix ([[./figs/frf_sercalo_gain.png][png]], [[./figs/frf_sercalo_gain.pdf][pdf]])
% [[file:figs/frf_sercalo_gain.png]]


figure;
ax11 = subplot(2, 2, 1);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Uch_Vph)))
title('Frequency Response Function $\frac{Vp_h}{Uc_h}$')
set(gca, 'Xscale', 'log');
ylabel('Phase [deg]')
yticks(-180:90:180);
hold off;

ax12 = subplot(2, 2, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Ucv_Vph)))
title('Frequency Response Function $\frac{Vp_h}{Uc_v}$')
set(gca, 'Xscale', 'log');
yticks(-180:90:180);
hold off;

ax21 = subplot(2, 2, 3);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Uch_Vpv)))
title('Frequency Response Function $\frac{Vp_v}{Uc_h}$')
set(gca, 'Xscale', 'log');
ylabel('Phase [deg]')
xlabel('Frequency [Hz]')
yticks(-180:90:180);
hold off;

ax22 = subplot(2, 2, 4);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Ucv_Vpv)))
title('Frequency Response Function $\frac{Vp_v}{Uc_v}$')
set(gca, 'Xscale', 'log');
xlabel('Frequency [Hz]')
yticks(-180:90:180);
hold off;

linkaxes([ax11,ax12,ax21,ax22],'xy');
xlim([10, 1000]); ylim([-200, 200]);

% Time Delay
% Now, we would like to remove the time delay included in the FRF prior to the model extraction.

% Estimation of the time delay:

Ts_delay = Ts; % [s]

G_delay = tf(1, 1, 'InputDelay', Ts_delay);

G_delay_resp = squeeze(freqresp(G_delay, f, 'Hz'));



% We then remove the time delay from the frequency response function.

tf_Uch_Vph = tf_Uch_Vph./G_delay_resp;
tf_Uch_Vpv = tf_Uch_Vpv./G_delay_resp;
tf_Ucv_Vph = tf_Ucv_Vph./G_delay_resp;
tf_Ucv_Vpv = tf_Ucv_Vpv./G_delay_resp;

% Extraction of a transfer function matrix
% First we define the initial guess for the resonance frequencies and the weights associated.

freqs_res_uh = [410]; % [Hz]
freqs_res_uv = [250]; % [Hz]



% We then make an initial guess on the complex values of the poles.

xi = 0.001; % Approximate modal damping
poles_uh = [2*pi*freqs_res_uh*(xi + 1i), 2*pi*freqs_res_uh*(xi - 1i)];
poles_uv = [2*pi*freqs_res_uv*(xi + 1i), 2*pi*freqs_res_uv*(xi - 1i)];



% We then define the weight that will be used for the fitting.
% Basically, we want more weight around the resonance and at low frequency (below the first resonance).
% Also, we want more importance where we have a better coherence.
% Finally, we ignore data above some frequency.

weight_Uch_Vph = coh_Uch_Vph';
weight_Uch_Vpv = coh_Uch_Vpv';
weight_Ucv_Vph = coh_Ucv_Vph';
weight_Ucv_Vpv = coh_Ucv_Vpv';

alpha = 0.1;

for freq_i = 1:length(freqs_res_uh)
  weight_Uch_Vph(f>(1-alpha)*freqs_res_uh(freq_i) & f<(1 + alpha)*freqs_res_uh(freq_i)) = 10;
  weight_Uch_Vpv(f>(1-alpha)*freqs_res_uh(freq_i) & f<(1 + alpha)*freqs_res_uh(freq_i)) = 10;
  weight_Ucv_Vph(f>(1-alpha)*freqs_res_uv(freq_i) & f<(1 + alpha)*freqs_res_uv(freq_i)) = 10;
  weight_Ucv_Vpv(f>(1-alpha)*freqs_res_uv(freq_i) & f<(1 + alpha)*freqs_res_uv(freq_i)) = 10;
end

weight_Uch_Vph(f>1000) = 0;
weight_Uch_Vpv(f>1000) = 0;
weight_Ucv_Vph(f>1000) = 0;
weight_Ucv_Vpv(f>1000) = 0;



% The weights are shown in Fig. [[fig:weights_sercalo]].


figure;
hold on;
plot(f, weight_Uch_Vph, 'DisplayName', '$W_{U_{ch},V_{ph}}$');
plot(f, weight_Uch_Vpv, 'DisplayName', '$W_{U_{ch},V_{pv}}$');
plot(f, weight_Ucv_Vph, 'DisplayName', '$W_{U_{cv},V_{ph}}$');
plot(f, weight_Ucv_Vpv, 'DisplayName', '$W_{U_{cv},V_{pv}}$');
hold off;
xlabel('Frequency [Hz]'); ylabel('Weight Amplitude');
set(gca, 'xscale', 'log');
xlim([f(1), f(end)]);
legend('location', 'northwest');



% #+NAME: fig:weights_sercalo
% #+CAPTION: Weights amplitude ([[./figs/weights_sercalo.png][png]], [[./figs/weights_sercalo.pdf][pdf]])
% [[file:figs/weights_sercalo.png]]

% When we set some options for =vfit3=.

opts = struct();

opts.stable = 1;    % Enforce stable poles
opts.asymp = 1;     % Force D matrix to be null
opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
opts.skip_pole = 0; % Do NOT skip pole identification
opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
opts.cmplx_ss = 0;  % Create real state space model with block diagonal A

opts.spy1 = 0;      % No plotting for first stage of vector fitting
opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)



% We define the number of iteration.

Niter = 5;



% An we run the =vectfit3= algorithm.

for iter = 1:Niter
  [SER_Uch_Vph, poles, ~, fit_Uch_Vph] = vectfit3(tf_Uch_Vph.', 1i*2*pi*f, poles_uh, weight_Uch_Vph, opts);
end
for iter = 1:Niter
  [SER_Uch_Vpv, poles, ~, fit_Uch_Vpv] = vectfit3(tf_Uch_Vpv.', 1i*2*pi*f, poles_uh, weight_Uch_Vpv, opts);
end
for iter = 1:Niter
  [SER_Ucv_Vph, poles, ~, fit_Ucv_Vph] = vectfit3(tf_Ucv_Vph.', 1i*2*pi*f, poles_uv, weight_Ucv_Vph, opts);
end
for iter = 1:Niter
  [SER_Ucv_Vpv, poles, ~, fit_Ucv_Vpv] = vectfit3(tf_Ucv_Vpv.', 1i*2*pi*f, poles_uv, weight_Ucv_Vpv, opts);
end

figure;
ax11 = subplot(2, 2, 1);
hold on;
plot(f, abs(tf_Uch_Vph))
plot(f, abs(fit_Uch_Vph))
title('Frequency Response Function $\frac{Vp_h}{Uc_h}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [V/V]')
hold off;

ax12 = subplot(2, 2, 2);
hold on;
plot(f, abs(tf_Ucv_Vph))
plot(f, abs(fit_Ucv_Vph))
title('Frequency Response Function $\frac{Vp_h}{Uc_v}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
hold off;

ax21 = subplot(2, 2, 3);
hold on;
plot(f, abs(tf_Uch_Vpv))
plot(f, abs(fit_Uch_Vpv))
title('Frequency Response Function $\frac{Vp_v}{Uc_h}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [V/V]')
xlabel('Frequency [Hz]')
hold off;

ax22 = subplot(2, 2, 4);
hold on;
plot(f, abs(tf_Ucv_Vpv))
plot(f, abs(fit_Ucv_Vpv))
title('Frequency Response Function $\frac{Vp_v}{Uc_v}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]')
hold off;

linkaxes([ax11,ax12,ax21,ax22],'xy');
xlim([10, 1000]); ylim([1e-2, 1e3]);



% #+NAME: fig:identification_matrix_fit
% #+CAPTION: Transfer Function Extraction of the FRF matrix ([[./figs/identification_matrix_fit.png][png]], [[./figs/identification_matrix_fit.pdf][pdf]])
% [[file:figs/identification_matrix_fit.png]]



figure;
ax11 = subplot(2, 2, 1);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Uch_Vph)))
plot(f, 180/pi*unwrap(angle(fit_Uch_Vph)))
title('Frequency Response Function $\frac{Vp_h}{Uc_h}$')
set(gca, 'Xscale', 'log');
ylabel('Phase [deg]')
yticks(-180:90:180);
hold off;

ax12 = subplot(2, 2, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Ucv_Vph)))
plot(f, 180/pi*unwrap(angle(fit_Ucv_Vph)))
title('Frequency Response Function $\frac{Vp_h}{Uc_v}$')
set(gca, 'Xscale', 'log');
yticks(-180:90:180);
hold off;

ax21 = subplot(2, 2, 3);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Uch_Vpv)))
plot(f, 180/pi*unwrap(angle(fit_Uch_Vpv)))
title('Frequency Response Function $\frac{Vp_v}{Uc_h}$')
set(gca, 'Xscale', 'log');
ylabel('Phase [deg]')
xlabel('Frequency [Hz]')
yticks(-180:90:180);
hold off;

ax22 = subplot(2, 2, 4);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Ucv_Vpv)))
plot(f, 180/pi*unwrap(angle(fit_Ucv_Vpv)))
title('Frequency Response Function $\frac{Vp_v}{Uc_v}$')
set(gca, 'Xscale', 'log');
xlabel('Frequency [Hz]')
yticks(-180:90:180);
hold off;

linkaxes([ax11,ax12,ax21,ax22],'xy');
xlim([10, 1000]); ylim([-200, 200]);



% #+NAME: fig:identification_matrix_fit_phase
% #+CAPTION: Transfer Function Extraction of the FRF matrix ([[./figs/identification_matrix_fit_phase.png][png]], [[./figs/identification_matrix_fit_phase.pdf][pdf]])
% [[file:figs/identification_matrix_fit_phase.png]]

% And finally, we create the identified $G_c$ matrix by multiplying by ${G_i}^{-1}$.

G_Uch_Vph = tf(minreal(ss(full(SER_Uch_Vph.A),SER_Uch_Vph.B,SER_Uch_Vph.C,SER_Uch_Vph.D)));
G_Ucv_Vph = tf(minreal(ss(full(SER_Ucv_Vph.A),SER_Ucv_Vph.B,SER_Ucv_Vph.C,SER_Ucv_Vph.D)));
G_Uch_Vpv = tf(minreal(ss(full(SER_Uch_Vpv.A),SER_Uch_Vpv.B,SER_Uch_Vpv.C,SER_Uch_Vpv.D)));
G_Ucv_Vpv = tf(minreal(ss(full(SER_Ucv_Vpv.A),SER_Ucv_Vpv.B,SER_Ucv_Vpv.C,SER_Ucv_Vpv.D)));

Gc = [G_Uch_Vph, G_Ucv_Vph;
      G_Uch_Vpv, G_Ucv_Vpv]*inv(Gi);

% Input / Output data
% The identification data is loaded

uh = load('mat/data_unh.mat', 't', 'Unh', 'Vph', 'Vpv');
uv = load('mat/data_unv.mat', 't', 'Unv', 'Vph', 'Vpv');



% We remove the first seconds where the Sercalo is turned on.

t0 = 3;

uh.Unh(uh.t<t0) = [];
uh.Vph(uh.t<t0) = [];
uh.Vpv(uh.t<t0) = [];
uh.t(uh.t<t0)   = [];
uh.t = uh.t - uh.t(1); % We start at t=0

t0 = 1.5;

uv.Unv(uv.t<t0) = [];
uv.Vph(uv.t<t0) = [];
uv.Vpv(uv.t<t0) = [];
uv.t(uv.t<t0)   = [];

uv.t = uv.t - uv.t(1); % We start at t=0

figure;
ax1 = subplot(1, 2, 1);
plot(uh.t, uh.Unh, 'DisplayName', '$Un_h$');
xlabel('Time [s]');
ylabel('Amplitude [V]');
legend();

ax2 = subplot(1, 2, 2);
hold on;
plot(uh.t, uh.Vph, 'DisplayName', '$Vp_h$');
plot(uh.t, uh.Vpv, 'DisplayName', '$Vp_v$');
hold off;
xlabel('Time [s]');
ylabel('Amplitude [V]');
legend();

linkaxes([ax1,ax2],'x');
xlim([uh.t(1), uh.t(end)]);



% #+NAME: fig:identification_unh
% #+CAPTION: Identification signals when exciting the horizontal direction ([[./figs/identification_unh.png][png]], [[./figs/identification_unh.pdf][pdf]])
% [[file:figs/identification_unh.png]]


figure;
ax1 = subplot(1, 2, 1);
plot(uv.t, uv.Unv, 'DisplayName', '$Un_v$');
xlabel('Time [s]');
ylabel('Amplitude [V]');

ax2 = subplot(1, 2, 2);
hold on;
plot(uv.t, uv.Vpv, 'DisplayName', '$Vp_v$');
plot(uv.t, uv.Vph, 'DisplayName', '$Vp_h$');
hold off;
xlabel('Time [s]');
ylabel('Amplitude [V]');
legend();

linkaxes([ax1,ax2],'x');
xlim([uv.t(1), uv.t(end)]);

% Coherence
% The window used for the spectral analysis is an =hanning= windows with temporal size equal to 1 second.

win = hanning(ceil(1*fs));

[coh_Unh_Vph, f] = mscohere(uh.Unh, uh.Vph, win, [], [], fs);
[coh_Unh_Vpv, ~] = mscohere(uh.Unh, uh.Vpv, win, [], [], fs);
[coh_Unv_Vph, ~] = mscohere(uv.Unv, uv.Vph, win, [], [], fs);
[coh_Unv_Vpv, ~] = mscohere(uv.Unv, uv.Vpv, win, [], [], fs);

figure;
ax11 = subplot(2, 2, 1);
hold on;
plot(f, coh_Unh_Vph)
set(gca, 'Xscale', 'log');
title('Coherence $\frac{Vp_h}{Un_h}$')
ylabel('Coherence')
hold off;

ax12 = subplot(2, 2, 2);
hold on;
plot(f, coh_Unv_Vph)
set(gca, 'Xscale', 'log');
title('Coherence $\frac{Vp_h}{Un_v}$')
hold off;

ax21 = subplot(2, 2, 3);
hold on;
plot(f, coh_Unh_Vpv)
set(gca, 'Xscale', 'log');
title('Coherence $\frac{Vp_v}{Un_h}$')
ylabel('Coherence')
xlabel('Frequency [Hz]')
hold off;

ax22 = subplot(2, 2, 4);
hold on;
plot(f, coh_Unv_Vpv)
set(gca, 'Xscale', 'log');
title('Coherence $\frac{Vp_v}{Un_v}$')
xlabel('Frequency [Hz]')
hold off;

linkaxes([ax11,ax12,ax21,ax22],'xy');
xlim([10, 1000]); ylim([0, 1]);

% Estimation of the Frequency Response Function Matrix
% We compute an estimate of the transfer functions.

[tf_Unh_Vph, f] = tfestimate(uh.Unh, uh.Vph, win, [], [], fs);
[tf_Unh_Vpv, ~] = tfestimate(uh.Unh, uh.Vpv, win, [], [], fs);
[tf_Unv_Vph, ~] = tfestimate(uv.Unv, uv.Vph, win, [], [], fs);
[tf_Unv_Vpv, ~] = tfestimate(uv.Unv, uv.Vpv, win, [], [], fs);

figure;
ax11 = subplot(2, 2, 1);
hold on;
plot(f, abs(tf_Unh_Vph))
title('Frequency Response Function $\frac{Vp_h}{Un_h}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [V/V]')
hold off;

ax12 = subplot(2, 2, 2);
hold on;
plot(f, abs(tf_Unv_Vph))
title('Frequency Response Function $\frac{Vp_h}{Un_v}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
hold off;

ax21 = subplot(2, 2, 3);
hold on;
plot(f, abs(tf_Unh_Vpv))
title('Frequency Response Function $\frac{Vp_v}{Un_h}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [V/V]')
xlabel('Frequency [Hz]')
hold off;

ax22 = subplot(2, 2, 4);
hold on;
plot(f, abs(tf_Unv_Vpv))
title('Frequency Response Function $\frac{Vp_v}{Un_v}$')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]')
hold off;

linkaxes([ax11,ax12,ax21,ax22],'xy');
xlim([10, 1000]); ylim([1e-4, 1e1]);



% #+NAME: fig:frf_newport_gain
% #+CAPTION: Frequency Response Matrix ([[./figs/frf_newport_gain.png][png]], [[./figs/frf_newport_gain.pdf][pdf]])
% [[file:figs/frf_newport_gain.png]]


figure;
ax11 = subplot(2, 2, 1);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Unh_Vph)))
title('Frequency Response Function $\frac{Vp_h}{Un_h}$')
set(gca, 'Xscale', 'log');
ylabel('Phase [deg]')
yticks(-180:90:180);
hold off;

ax12 = subplot(2, 2, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Unv_Vph)))
title('Frequency Response Function $\frac{Vp_h}{Un_v}$')
set(gca, 'Xscale', 'log');
yticks(-180:90:180);
hold off;

ax21 = subplot(2, 2, 3);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Unh_Vpv)))
title('Frequency Response Function $\frac{Vp_v}{Un_h}$')
set(gca, 'Xscale', 'log');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
yticks(-180:90:180);
hold off;

ax22 = subplot(2, 2, 4);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Unv_Vpv)))
title('Frequency Response Function $\frac{Vp_v}{Un_v}$')
set(gca, 'Xscale', 'log');
xlabel('Frequency [Hz]');
yticks(-180:90:180);
hold off;

linkaxes([ax11,ax12,ax21,ax22],'xy');
xlim([10, 1000]); ylim([-200, 200]);

% Time Delay
% Now, we would like to remove the time delay included in the FRF prior to the model extraction.

% Estimation of the time delay:

Ts_delay = 0.0005; % [s]

G_delay = tf(1, 1, 'InputDelay', Ts_delay);

G_delay_resp = squeeze(freqresp(G_delay, f, 'Hz'));



% We then remove the time delay from the frequency response function.

figure;
ax11 = subplot(2, 2, 1);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Unh_Vph)))
plot(f, 180/pi*unwrap(angle(tf_Unh_Vph./G_delay_resp)))
title('Frequency Response Function $\frac{Vp_h}{Un_h}$')
set(gca, 'Xscale', 'log');
ylabel('Phase [deg]')
yticks(-180:90:180);
hold off;

ax12 = subplot(2, 2, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Unv_Vph)))
plot(f, 180/pi*unwrap(angle(tf_Unv_Vph./G_delay_resp)))
title('Frequency Response Function $\frac{Vp_h}{Un_v}$')
set(gca, 'Xscale', 'log');
yticks(-180:90:180);
hold off;

ax21 = subplot(2, 2, 3);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Unh_Vpv)))
plot(f, 180/pi*unwrap(angle(tf_Unh_Vpv./G_delay_resp)))
title('Frequency Response Function $\frac{Vp_v}{Un_h}$')
set(gca, 'Xscale', 'log');
ylabel('Phase [deg]')
xlabel('Frequency [Hz]')
yticks(-180:90:180);
hold off;

ax22 = subplot(2, 2, 4);
hold on;
plot(f, 180/pi*unwrap(angle(tf_Unv_Vpv)))
plot(f, 180/pi*unwrap(angle(tf_Unv_Vpv./G_delay_resp)))
title('Frequency Response Function $\frac{Vp_v}{Un_v}$')
set(gca, 'Xscale', 'log');
xlabel('Frequency [Hz]')
yticks(-180:90:180);
hold off;

linkaxes([ax11,ax12,ax21,ax22],'xy');
xlim([10, 1000]); ylim([-200, 200]);

% Extraction of a transfer function matrix
% From Fig. [[fig:frf_newport_gain]], it seems reasonable to model the Newport dynamics as diagonal and constant.


Gn = blkdiag(tf(mean(abs(tf_Unh_Vph(f>10 & f<100)))), tf(mean(abs(tf_Unv_Vpv(f>10 & f<100)))));

% Full System
% We now have identified:
% - $G_i$
% - $G_a$
% - $G_c$
% - $G_n$
% - $G_d$

% We name the input and output of each transfer function:

Gi.InputName = {'Uch', 'Ucv'};
Gi.OutputName = {'Ich', 'Icv'};

Zc.InputName = {'Ich', 'Icv'};
Zc.OutputName = {'Vtch', 'Vtcv'};

Ga.InputName = {'Vtch', 'Vtcv'};
Ga.OutputName = {'Vch', 'Vcv'};

Gc.InputName = {'Ich', 'Icv'};
Gc.OutputName = {'Vpch', 'Vpcv'};

Gn.InputName = {'Unh', 'Unv'};
Gn.OutputName = {'Vpnh', 'Vpnv'};

Gd.InputName = {'Rh', 'Rv'};
Gd.OutputName = {'Vph', 'Vpv'};

Sh = sumblk('Vph = Vpch + Vpnh');
Sv = sumblk('Vpv = Vpcv + Vpnv');

inputs  = {'Uch', 'Ucv', 'Unh', 'Unv'};
outputs = {'Vch', 'Vcv', 'Ich', 'Icv', 'Rh', 'Rv', 'Vph', 'Vpv'};

sys = connect(Gi, Zc, Ga, Gc, Gn, inv(Gd), Sh, Sv, inputs, outputs);



% The file =mat/plant.mat= is accessible [[./mat/plant.mat][here]].

save('mat/plant.mat', 'sys', 'Gi', 'Zc', 'Ga', 'Gc', 'Gn', 'Gd');