%% 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;

% Number of modes determination
% <<ssec:modal_number_of_modes>>
% The acrshort:mif is here applied to the $n\times p$ acrshort:frf matrix where $n$ is a relatively large number of measurement DOFs (here $n=69$) and $p$ is the number of excitation DOFs (here $p=3$).

% The complex modal indication function is defined in equation eqref:eq:modal_cmif where the diagonal matrix $\Sigma$ is obtained from a acrlong:svd of the acrshort:frf matrix as shown in equation eqref:eq:modal_svd.
% \begin{equation} \label{eq:modal_cmif}
%   [CMIF(\omega)]_{p\times p} = [\Sigma(\omega)]_{p\times n}^T [\Sigma(\omega)]_{n\times p}
% \end{equation}

% \begin{equation} \label{eq:modal_svd}
% [H(\omega)]_{n\times p} = [U(\omega)]_{n\times n} [\Sigma(\omega)]_{n\times p} [V(\omega)]_{p\times p}^H
% \end{equation}

% The acrshort:mif therefore yields to $p$ values that are also frequency dependent.
% A peak in the acrshort:mif plot indicates the presence of a mode.
% Repeated modes can also be detected by multiple singular values are having peaks at the same frequency.
% The obtained acrshort:mif is shown on Figure ref:fig:modal_indication_function.
% A total of 16 modes are found between 0 and $200\,\text{Hz}$.
% The obtained natural frequencies and associated modal damping are summarized in Table ref:tab:modal_obtained_modes_freqs_damps.


%% Load frequency response matrix
load('frf_matrix.mat', 'freqs', 'frf');

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

% Verification of the modal model validity
% <<ssec:modal_model_validity>>

% In order to check the validity of the modal model, the complete $n \times n$ acrshort:frf matrix $\mathbf{H}_{\text{syn}}$ is first synthesized from the modal parameters.
% Then, the elements of this acrshort:frf matrix $\mathbf{H}_{\text{syn}}$ that were already measured can be compared with the measured acrshort:frf matrix $\mathbf{H}$.

% In order to synthesize the full acrshort:frf matrix, the eigenvectors $\phi_r$ are first organized in a matrix from as shown in equation eqref:eq:modal_eigvector_matrix.
% \begin{equation}\label{eq:modal_eigvector_matrix}
% \Phi = \begin{bmatrix}
%   & & & & &\\
%   \phi_1 & \dots & \phi_N & \phi_1^* & \dots & \phi_N^* \\
%   & & & & &
% \end{bmatrix}_{n \times 2m}
% \end{equation}

% The full acrshort:frf matrix $\mathbf{H}_{\text{syn}}$ can be synthesize using eqref:eq:modal_synthesized_frf.

% \begin{equation}\label{eq:modal_synthesized_frf}
%   [\mathbf{H}_{\text{syn}}(\omega)]_{n\times n} = [\Phi]_{n\times2m} [\mathbf{H}_{\text{mod}}(\omega)]_{2m\times2m} [\Phi]_{2m\times n}^T
% \end{equation}

% With $\mathbf{H}_{\text{mod}}(\omega)$ a diagonal matrix representing the response of the different modes eqref:eq:modal_modal_resp.
% \begin{equation}\label{eq:modal_modal_resp}
% \mathbf{H}_{\text{mod}}(\omega) = \text{diag}\left(\frac{1}{a_1 (j\omega - s_1)},\ \dots,\ \frac{1}{a_m (j\omega - s_m)}, \frac{1}{a_1^* (j\omega - s_1^*)},\ \dots,\ \frac{1}{a_m^* (j\omega - s_m^*)} \right)_{2m\times 2m}
% \end{equation}


%% 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

%% Create the eigenvalue and eigenvector matrices
eigen_val_M = diag(2*pi*freqs_m.*(-damps_m/100 + j*sqrt(1 - (damps_m/100).^2))); % Lambda = diagonal matrix
eigen_vec_M = reshape(mode_shapes, [mod_n, acc_n*dir_n]).'; % Phi, vecnorm(eigen_vec_M) = 1

% Add complex conjugate eigenvalues and eigenvectors
eigen_val_ext_M = blkdiag(eigen_val_M, conj(eigen_val_M));
eigen_vec_ext_M = [eigen_vec_M, conj(eigen_vec_M)];

%% "Modal A" and "Modal B" matrices
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));

%% Synthesize the full FRF matrix from the modal model
Hsyn = zeros(acc_n*dir_n, acc_n*dir_n, length(freqs));

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

%% Derivate two times to to have the acceleration response
for i = 1:size(Hsyn, 1)
  Hsyn(i, :, :) = squeeze(Hsyn(i, :, :)).*(j*2*pi*freqs).^2;
end



% The comparison between the original measured frequency response functions and the synthesized ones from the modal model is done in Figure ref:fig:modal_comp_acc_frf_modal.
% Whether the obtained match can be considered good or bad is quite arbitrary.
% Yet, the modal model seems to be able to represent the coupling between different nodes and different direction which is quite important in a control point of view.
% This can be seen in Figure ref:fig:modal_comp_acc_frf_modal_3 that shows the frequency response function between a force applied on node 11 (i.e. on the translation stage) in the $y$ direction to the measured acceleration at node $2$ (i.e. at the top of the micro-hexapod) in the $x$ direction.


acc_o = 11; dir_o = 3;
acc_i = 11; dir_i = 3;

figure;
hold on;
plot(freqs, abs(squeeze(frf( 3*(acc_o-1)+dir_o,             dir_i, :))), 'DisplayName', 'Measured');
plot(freqs, abs(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(acc_i-1)+dir_i, :))), 'DisplayName', 'Synthesized');
hold off;
set(gca, 'xscale', 'lin');
set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]');
ylabel('Magnitude [$\frac{m/s^2}{N}$]');
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [10, 1];
xticks([0:40:200]);
xlim([1, 200]);
ylim([1e-6, 1e-1]);

acc_o = 15; dir_o = 3;
acc_i = 11; dir_i = 3;

figure;
hold on;
plot(freqs, abs(squeeze(frf( 3*(acc_o-1)+dir_o,             dir_i, :))), 'DisplayName', 'Measured');
plot(freqs, abs(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(acc_i-1)+dir_i, :))), 'DisplayName', 'Synthesized');
hold off;
set(gca, 'xscale', 'lin');
set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]');
ylabel('Magnitude [$\frac{m/s^2}{N}$]');
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [10, 1];
xticks([0:40:200]);
xlim([1, 200]);
ylim([1e-6, 1e-1]);

acc_o = 2; dir_o = 1;
acc_i = 11; dir_i = 2;

figure;
hold on;
plot(freqs, abs(squeeze(frf( 3*(acc_o-1)+dir_o,             dir_i, :))), 'DisplayName', 'Measured');
plot(freqs, abs(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(acc_i-1)+dir_i, :))), 'DisplayName', 'Synthesized');
hold off;
set(gca, 'xscale', 'lin');
set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]');
ylabel('Magnitude [$\frac{m/s^2}{N}$]');
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [10, 1];
xticks([0:40:200]);
xlim([1, 200]);
ylim([1e-6, 1e-1]);