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

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

%% Path for functions, data and scripts
addpath('./mat/'); % Path for data

%% Colors for the figures
colors = colororder;

% Singular Value Decomposition - Modal Indication Function
% The Mode Indicator Functions are usually used on $n\times p$ FRF matrix where $n$ is a relatively large number of measurement DOFs and $p$ is the number of excitation DOFs, typically 3 or 4.

% In these methods, the frequency dependent FRF matrix is subjected to a singular value decomposition analysis which thus yields a small number (3 or 4) of singular values, these also being frequency dependent.

% These methods are used to *determine the number of modes* present in a given frequency range, to *identify repeated natural frequencies* and to pre-process the FRF data prior to modal analysis.

% From the documentation of the modal software:
% #+begin_quote
% The MIF consist of the singular values of the Frequency response function matrix. The number of MIFs equals the number of excitations.
% By the powerful singular value decomposition, the real signal space is separated from the noise space. Therefore, the MIFs exhibit the modes effectively.
% A peak in the MIFs plot usually indicate the existence of a structural mode, and two peaks at the same frequency point means the existence of two repeated modes.
% Moreover, the magnitude of the MIFs implies the strength of the a mode.
% #+end_quote

% #+begin_important
% The *Complex Mode Indicator Function* is defined simply by the SVD of the FRF (sub) matrix:
% \begin{align*}
%   [H(\omega)]_{n\times p} &= [U(\omega)]_{n\times n} [\Sigma(\omega)]_{n\times p} [V(\omega)]_{p\times p}^H\\
%   [CMIF(\omega)]_{p\times p} &= [\Sigma(\omega)]_{p\times n}^T [\Sigma(\omega)]_{n\times p}
% \end{align*}
% #+end_important

% We compute the Complex Mode Indicator Function.
% The result is shown on Figure ref:fig:modal_indication_function.


%% Computation of the modal indication function
MIF = zeros(size(frf, 2), size(frf, 2), size(frf, 3));

for i = 1:length(freqs)
  [~,S,~] = svd(frf(:, :, i));
  MIF(:, :, i) = S'*S;
end

%% Modal Indication Function
figure;
hold on;
for i = 1:size(MIF, 1)
  plot(freqs, squeeze(MIF(i, i, :)));
end
hold off;
set(gca, 'Xscale', 'lin'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('CMIF Amplitude');
xticks([0:20:200]);
xlim([0, 200]);
ylim([1e-6, 2e-2]);

% Composite Response Function
% An alternative is the Composite Response Function $HH(\omega)$ defined as the sum of all the measured FRF:
% \begin{equation}
%   HH(\omega) = \sum_j\sum_kH_{jk}(\omega)
% \end{equation}

% Instead, we choose here to use the sum of the norms of the measured frf:
% \begin{equation}
%   HH(\omega) = \sum_j\sum_k \left|H_{jk}(\omega) \right|
% \end{equation}

% The result is shown on figure ref:fig:modal_composite_reponse_function.


%% Composite Response Function
figure;
hold on;
plot(freqs, squeeze(sum(sum(abs(frf)))), '-k');
hold off;
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([0, 200]);
xticks([0:20:200]);

% Importation of the modal parameters on Matlab
% The obtained modal parameters are:
% - Resonance frequencies in Hertz
% - Modal damping ratio in percentage
% - (complex) Modes shapes for each measured DoF
% - Modal A and modal B which are parameters important for further normalization


%% Load modal parameters
shapes_m = readtable('mat/mode_shapes.txt',              'ReadVariableNames', false);  % [Sign / Real / Imag]
freqs_m  = table2array(readtable('mat/mode_freqs.txt',   'ReadVariableNames', false)); % in [Hz]
damps_m  = table2array(readtable('mat/mode_damps.txt',   'ReadVariableNames', false)); % in [%]
modal_a  = table2array(readtable('mat/mode_modal_a.txt', 'ReadVariableNames', false)); % [Real / Imag]
modal_b  = table2array(readtable('mat/mode_modal_b.txt', 'ReadVariableNames', false)); % [Real / Imag]

%% Guess the number of modes identified from the length of the imported data.
acc_n = 23; % Number of accelerometers
dir_n = 3; % Number of directions
dirs = 'XYZ';

mod_n = size(shapes_m,1)/acc_n/dir_n; % Number of modes

%% Mode shapes are split into 3 parts (direction plus sign, real part and imaginary part)
% we aggregate them into one array of complex numbers
T_sign = table2array(shapes_m(:, 1));
T_real = table2array(shapes_m(:, 2));
T_imag = table2array(shapes_m(:, 3));

mode_shapes = zeros(mod_n, dir_n, acc_n);

for mod_i = 1:mod_n
  for acc_i = 1:acc_n
    % Get the correct section of the signs
    T = T_sign(acc_n*dir_n*(mod_i-1)+1:acc_n*dir_n*mod_i);
    for dir_i = 1:dir_n
      % Get the line corresponding to the sensor
      i = find(contains(T, sprintf('%i%s',acc_i, dirs(dir_i))), 1, 'first')+acc_n*dir_n*(mod_i-1);
      mode_shapes(mod_i, dir_i, acc_i) = str2num([T_sign{i}(end-1), '1'])*complex(T_real(i),T_imag(i));
    end
  end
end

% Modal Matrices
% We would like to arrange the obtained modal parameters into two modal matrices:
% \[ \Lambda = \begin{bmatrix}
%   s_1 &        & 0 \\
%       & \ddots &   \\
%   0   &        & s_N
% \end{bmatrix}_{N \times N}; \quad \Psi = \begin{bmatrix}
%   & & \\
%   \{\psi_1\} & \dots & \{\psi_N\} \\
%   & &
% \end{bmatrix}_{M \times N} \]
% \[ \{\psi_i\} = \begin{Bmatrix} \psi_{i, 1_x} & \psi_{i, 1_y} & \psi_{i, 1_z} & \psi_{i, 2_x} & \dots & \psi_{i, 23_z} \end{Bmatrix}^T \]

% $M$ is the number of DoF: here it is $23 \times 3 = 69$.
% $N$ is the number of mode


eigen_val_M = diag(2*pi*freqs_m.*(-damps_m/100 + j*sqrt(1 - (damps_m/100).^2)));
eigen_vec_M = reshape(mode_shapes, [mod_n, acc_n*dir_n]).';



% Each eigen vector is normalized: $\| \{\psi_i\} \|_2 = 1$


% However, the eigen values and eigen vectors appears as complex conjugates:
% \[ s_r, s_r^*, \{\psi\}_r, \{\psi\}_r^*, \quad r = 1, N \]

% In the end, they are $2N$ eigen values.
% We then build two extended eigen matrices as follow:
% \[ \mathcal{S} = \begin{bmatrix}
%   s_1 &        &     &       &        &      \\
%       & \ddots &     &       & 0      &      \\
%       &        & s_N &       &        &      \\
%       &        &     & s_1^* &        &      \\
%       & 0      &     &       & \ddots &      \\
%       &        &     &       &        & s_N^*
% \end{bmatrix}_{2N \times 2N}; \quad \Phi = \begin{bmatrix}
%   & & & & &\\
%   \{\psi_1\} & \dots & \{\psi_N\} & \{\psi_1^*\} & \dots & \{\psi_N^*\} \\
%   & & & & &
% \end{bmatrix}_{M \times 2N} \]


eigen_val_ext_M = blkdiag(eigen_val_M, conj(eigen_val_M));
eigen_vec_ext_M = [eigen_vec_M, conj(eigen_vec_M)];



% We also build the Modal A and Modal B matrices:
% \begin{equation}
%   A = \begin{bmatrix}
%     a_1 &        & 0   \\
%         & \ddots &     \\
%     0   &        & a_N
%   \end{bmatrix}_{N \times N}; \quad B = \begin{bmatrix}
%     b_1 &        & 0   \\
%         & \ddots &     \\
%     0   &        & b_N
%   \end{bmatrix}_{N \times N}
% \end{equation}
% With $a_i$ is the "Modal A" parameter linked to mode i.


modal_a_M = diag(complex(modal_a(:, 1), modal_a(:, 2)));
modal_b_M = diag(complex(modal_b(:, 1), modal_b(:, 2)));

modal_a_ext_M = blkdiag(modal_a_M, conj(modal_a_M));
modal_b_ext_M = blkdiag(modal_b_M, conj(modal_b_M));

% Matlab Implementation

Hsyn = zeros(acc_n*dir_n, acc_n*dir_n, length(freqs));

for i = 1:length(freqs)
  Hsyn(:, :, i) = eigen_vec_ext_M*((j*2*pi*freqs(i)).^2*inv(modal_a_ext_M)/(diag(j*2*pi*freqs(i) - diag(eigen_val_ext_M))))*eigen_vec_ext_M.';
end



% Because the synthesize frequency response functions are representing the displacement response in $[m/N]$, we multiply each element of the FRF matrix by $(j \omega)^2$ in order to obtain the acceleration response in $[m/s^2/N]$.


for i = 1:size(Hsyn, 1)
  Hsyn(i, :, :) = squeeze(Hsyn(i, :, :)).*(j*2*pi*freqs).^2;
end

% Original and Synthesize FRF matrix comparison

acc_o = 1; dir_o = 1; dir_i = 1;

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(frf(3*(acc_o-1)+dir_o,          dir_i, :))), 'DisplayName', 'Original');
plot(freqs, abs(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(11-1)+dir_i, :))), 'DisplayName', 'Synthesize');
hold off;
set(gca, 'yscale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [$\frac{m/s^2}{N}$]');
title(sprintf('From acc %i %s to acc %i %s', 11, dirs(dir_i), acc_o, dirs(dir_o)))
legend('location', 'northwest');

ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, mod(180/pi*phase(squeeze(frf(3*(acc_o-1)+dir_o,          dir_i, :)))+180, 360)-180);
plot(freqs, mod(180/pi*phase(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(11-1)+dir_i, :)))+180, 360)-180);
hold off;
yticks(-360:90:360); ylim([-180, 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');

linkaxes([ax1,ax2],'x');
xlim([1, 200]);

% Synthesize FRF that has not yet been measured

accs = [1]; dirs = [1:3];

figure;
ax1 = subplot(2, 1, 1);
hold on;
for acc_i = accs
  for dir_i = dirs
    plot(freqs, abs((1./(j*2*pi*freqs').^2).*squeeze(Hsyn(3*(acc_i-1)+dir_i, 3*(acc_i-1)+dir_i, :))), 'DisplayName', sprintf('Acc %i - %s', acc_i, dirs(dir_i)));
  end
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [$\frac{m}{N}$]');
legend('location', 'southwest');

ax2 = subplot(2, 1, 2);
hold on;
for acc_i = accs
  for dir_i = dirs
    plot(freqs, mod(180/pi*phase((1./(j*2*pi*freqs').^2).*squeeze(Hsyn(3*(acc_i-1)+dir_i, 3*(acc_i-1)+dir_i, :)))+180, 360)-180);
  end
end
hold off;
yticks(-360:90:360); ylim([-180, 180]);
set(gca, 'xscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');

linkaxes([ax1,ax2],'x');
xlim([1, 200]);