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

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

% Load Data

load('mat/frf_coh_matrices.mat', 'FRFs', 'COHs', 'freqs');
load('mat/geometry.mat', 'solids', 'solid_names', 'acc_pos');
load('mat/frf_o.mat', 'FRFs_O');

% 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 [[fig:cmif]].
% The exact same curve is obtained when computed using the OROS software.


MIF = zeros(size(FRFs, 2), size(FRFs, 2), size(FRFs, 3));

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

figure;
hold on;
for i = 1:size(MIF, 1)
  plot(freqs, squeeze(MIF(i, i, :)), 'DisplayName', sprintf('MDIF - %i', i));
end
hold off;
set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('CMIF Amplitude');
xlim([1, 200]);
legend('location', 'southeast');



% #+NAME: fig:cmif
% #+CAPTION: Complex Mode Indicator Function
% [[file:figs/cmif.png]]

% We can also compute the CMIF using the FRF matrix expressed in the same global frame.
% We compare the two CMIF on figure [[fig:cmif_compare]].

% They do not indicate the same resonance frequencies, especially around 110Hz.


MIF_O = zeros(size(FRFs_O, 2), size(FRFs_O, 2), size(FRFs_O, 3));
for i = 1:length(freqs)
  [~,S,~] = svd(FRFs_O(:, :, i));
  MIF_O(:, :, i) = S'*S;
end

figure;
hold on;
for i = 1:size(MIF, 1)
  set(gca,'ColorOrderIndex',i)
  plot(freqs, squeeze(MIF(i, i, :)), '-');
  set(gca,'ColorOrderIndex',i)
  plot(freqs, squeeze(MIF_O(i, i, :)), '--');
end
hold off;
set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('CMIF Amplitude');
xlim([1, 200]);

% 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 FRFs:
% \begin{equation}
%   HH(\omega) = \sum_j\sum_k \left|H_{jk}(\omega) \right|
% \end{equation}

% The result is shown on figure [[fig:composite_response_function]].


figure;
hold on;
plot(freqs, squeeze(sum(sum(abs(FRFs)))), '-k');
plot(freqs, squeeze(sum(sum(abs(FRFs_O)))), '--k');
hold off;
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([1, 200]);

% Importation of the modal parameters on Matlab
% Then we import the obtained =.txt= files on Matlab using =readtable= function.


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]



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



% As the 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(69, 69, 801);

for i = 1:length(freqs)
  Hsyn(:, :, i) = eigen_vec_ext_M*(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 = 15; dir_o = 1; dir_i = 1;

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(FRFs(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, 'xscale', 'log'); set(gca, 'yscale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [$\frac{m/s^2}{N}$]');
legend('location', 'northwest');

ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, mod(180/pi*phase(squeeze(FRFs(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]);
set(gca, 'xscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');

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

% Synthesize FRF that has not yet been measured

dir_names = {'X', 'Y', 'Z'};

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, dir_names{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]);

% Modal Complexity
%   <<sec:modal_complexity>>
% A method of displaying *modal complexity* is by plotting the elements of the eigenvector on an *Argand diagram* (complex plane), such as the ones shown in figure [[fig:modal_complexity_small]].

% To evaluate the complexity of the modes, we plot a polygon around the extremities of the individual vectors.
% The obtained area of this polygon is then compared with the area of the circle which is based on the length of the largest vector element. The resulting ratio is used as an *indication of the complexity of the mode*.

% A mode with small complexity is shown on figure [[fig:modal_complexity_small]] whereas an highly complex mode is shown on figure [[fig:modal_complexity_high]].
% The complexity of all the modes are compared on figure [[fig:modal_complexities]].


figure;
mod_i = 1;
i_max = convhull(real(eigen_vector_M(:, mod_i)), imag(eigen_vector_M(:, mod_i)));
radius = max(abs(eigen_vector_M(:, mod_i)));
theta = linspace(0, 2*pi, 100);

hold on;
plot(radius*cos(theta), radius*sin(theta), '-');
plot(real(eigen_vector_M(i_max, mod_i)), imag(eigen_vector_M(i_max, mod_i)), '-');
plot(real(eigen_vector_M(:, mod_i)), imag(eigen_vector_M(:, mod_i)), 'ko');
hold off;
xlabel('Real Part'); ylabel('Imaginary Part');
title(sprintf('Mode %i', mod_i));
axis manual equal



% #+NAME: fig:modal_complexity_small
% #+CAPTION: Modal Complexity of one mode with small complexity
% [[file:figs/modal_complexity_small.png]]


mod_i = 8;
i_max = convhull(real(eigen_vector_M(:, mod_i)), imag(eigen_vector_M(:, mod_i)));
radius = max(abs(eigen_vector_M(:, mod_i)));
theta = linspace(0, 2*pi, 100);

figure;
hold on;
plot(radius*cos(theta), radius*sin(theta), '-');
plot(real(eigen_vector_M(i_max, mod_i)), imag(eigen_vector_M(i_max, mod_i)), '-');
plot(real(eigen_vector_M(:, mod_i)), imag(eigen_vector_M(:, mod_i)), 'ko');
hold off;
xlabel('Real Part'); ylabel('Imaginary Part');
title(sprintf('Mode %i', mod_i));
axis manual equal



% #+NAME: fig:modal_complexity_high
% #+CAPTION: Modal Complexity of one higly complex mode
% [[file:figs/modal_complexity_high.png]]


modes_complexity = zeros(mod_n, 1);
for mod_i = 1:mod_n
  i = convhull(real(eigen_vector_M(:, mod_i)), imag(eigen_vector_M(:, mod_i)));
  area_complex = polyarea(real(eigen_vector_M(i, mod_i)), imag(eigen_vector_M(i, mod_i)));
  area_circle = pi*max(abs(eigen_vector_M(:, mod_i)))^2;
  modes_complexity(mod_i) = area_complex/area_circle;
end

figure;
plot(1:mod_n, modes_complexity, 'ok');
ylim([0, 1]);
xlabel('Mode Number'); ylabel('Modal Complexity');

% Modal Assurance Criterion (MAC)
% The MAC is calculated as the normalized scalar product of the two sets of vectors $\{\psi_A\}$ and $\{\psi_X\}$.
% The resulting scalars are arranged into the MAC matrix cite:pastor12_modal_assur_criter:
% \begin{equation}
%   \text{MAC}(r, q) = \frac{\left| \{\psi_A\}_r^T \{\psi_X\}_q^* \right|^2}{\left( \{\psi_A\}_r^T \{\psi_A\}_r^* \right) \left( \{\psi_X\}_q^T \{\psi_X\}_q^* \right)}
% \end{equation}

% An equivalent formulation is:
% \begin{equation}
%   \text{MAC}(r, q) = \frac{\left| \sum_{j=1}^n \{\psi_A\}_j \{\psi_X\}_j^* \right|^2}{\left( \sum_{j=1}^n |\{\psi_A\}_j|^2 \right) \left( \sum_{j=1}^n |\{\psi_X\}_j|^2 \right)}
% \end{equation}

% The MAC takes value between 0 (representing no consistent correspondence) and 1 (representing a consistent correspondence).

% We compute the autoMAC matrix that compares all the possible combinations of mode shape pairs for only one set of mode shapes. The result is shown on figure [[fig:automac]].


autoMAC = eye(size(eigen_vec_M, 2));

for r = 1:size(eigen_vec_M, 2)
  for q = r+1:size(eigen_vec_M, 2)
    autoMAC(r, q) = abs(eigen_vec_M(r, :)*eigen_vec_M(q, :)')^2/((eigen_vec_M(r, :)*eigen_vec_M(r, :)')*(eigen_vec_M(q, :)*eigen_vec_M(q, :)'));
    autoMAC(q, r) = autoMAC(r, q);
  end
end

figure;
imagesc(autoMAC);
colormap('parula');
colorbar;
xticks(1:1:size(eigen_val_M, 1)); yticks(1:1:size(eigen_val_M, 1));
xticklabels(num2str(round(imag(diag(eigen_val_M))/2/pi, 1)));
xtickangle(90);
yticklabels(num2str(round(imag(diag(eigen_val_M))/2/pi, 1)));
set(gca,'YDir','normal');

% Matlab Implementation
% The obtained mode shapes matrix that gives the mode shapes of each solid bodies with respect to the fixed frame $\{O\}$, =mode_shapes_O=, is an $n \times p \times q$ with:
% - $n$ is the number of modes: 21
% - $p$ is the number of DOFs for each solid body: 6
% - $q$ is the number of solid bodies: 6


mode_shapes_O = zeros(mod_n, 6, length(solid_names));

for mod_i = 1:mod_n
  for solid_i = 1:length(solid_names)
    acc_i = solids.(solid_names{solid_i});

    Y = mode_shapes(mod_i, :, acc_i);
    Y = Y(:);

    A = zeros(3*length(acc_i), 6);
    for i = 1:length(acc_i)
      A(3*(i-1)+1:3*i, 1:3) = eye(3);

      A(3*(i-1)+1:3*i, 4:6) = [ 0              acc_pos(i, 3) -acc_pos(i, 2) ;
                               -acc_pos(i, 3)  0              acc_pos(i, 1) ;
                                acc_pos(i, 2) -acc_pos(i, 1)  0];
    end

    mode_shapes_O(mod_i, :, solid_i) = A\Y;
  end
end



% We then rearrange the eigen vectors in another way:
% \[ \Psi_O = \begin{bmatrix}
%   & & \\
%   \{\psi_1\} & \dots & \{\psi_N\} \\
%   & &
% \end{bmatrix}_{M \times N} \]
% with
% \[ \{\psi\}_r = \begin{Bmatrix}
%   \psi_{1, x} & \psi_{1, y} & \psi_{1, z} & \psi_{1, \theta_x} & \psi_{1, \theta_y} & \psi_{1, \theta_z} & \psi_{2, x} & \dots & \psi_{6, \theta_z}
% \end{Bmatrix}^T \]

% With $M = 6 \times 6$ is the new number of DOFs and $N=21$ is the number of modes.


eigen_vec_O = reshape(mode_shapes_O, [mod_n, 6*length(solid_names)]).';

eigen_vec_ext_O = [eigen_vec_O, conj(eigen_vec_O)];

% Matlab Implementation

Hsyn_O = zeros(36, 36, 801);

for i = 1:length(freqs)
  Hsyn_O(:, :, i) = eigen_vec_ext_O*(inv(modal_a_ext_M)/(diag(j*2*pi*freqs(i) - diag(eigen_val_ext_M))))*eigen_vec_ext_O.';
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_O, 1)
  Hsyn_O(i, :, :) = -squeeze(Hsyn_O(i, :, :)).*(j*2*pi*freqs).^2;
end

% TODO Test to have the original inputs

Hsyn_O = zeros(36, 3, 801);

for i = 1:length(freqs)
  Hsyn_O(:, :, i) = eigen_vec_ext_O*(inv(modal_a_ext_M)/(diag(j*2*pi*freqs(i) - diag(eigen_val_ext_M))))*eigen_vec_ext_M(10*3+1:11*3, :).';
end

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

% TODO Verification

solid_o = 6; dir_o = 3;
solid_i = 3; dir_i = 3;

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(FRFs_O((solid_o-1)*6+dir_o, dir_i, :))), 'DisplayName', 'Original');
plot(freqs, abs(squeeze(Hsyn_O((solid_o-1)*6+dir_o, dir_i, :))), 'DisplayName', 'Synthesize');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [$\frac{m/s^2}{N}$]');
legend('location', 'northwest');

ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, mod(180/pi*phase(squeeze(FRFs_O((solid_o-1)*6+dir_o, dir_i, :)))+180, 360)-180);
plot(freqs, mod(180/pi*phase(squeeze(Hsyn_O((solid_o-1)*6+dir_o, dir_i, :)))+180, 360)-180);
hold off;
yticks(-360:90:360);
set(gca, 'xscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');

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

% New FRFs

dof_names = {'X', 'Y', 'Z', '\theta_X', '\theta_Y', '\theta_Z'};
solid_i = 6; dirs = [1:3];

figure;
ax1 = subplot(2, 1, 1);
hold on;
for dir_i = dirs
  plot(freqs, abs(squeeze(Hsyn_O((solid_i-1)*6+dir_i, (solid_i-1)*6+dir_i, :))), 'DisplayName', sprintf('Solid %s - %s', solid_names{solid_i}, dof_names{dir_i}));
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [$\frac{m/s^2}{N}$]');
legend('location', 'northwest');

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

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

% Compare Mode Shapes
% Let's say we want to see for the first mode which DOFs can be neglected.
% In order to do so, we should estimate the motion of each stage in particular directions.
% If we look at the z motion for instance, we will find that we cannot neglect that motion (because of the tilt causing z motion).


mode_i = 3;
dof_i = 6;

mode = eigen_vector_M(dof_i:6:end, mode_i);

figure;
hold on;
for i=1:length(mode)
  plot([0, real(mode(i))], [0, imag(mode(i))], '-', 'DisplayName', solid_names{i});
end
hold off;
legend();

figure;
subplot(2, 1, 1);
hold on;
for i=1:length(mode)
  plot(1, norm(mode(i)), 'o');
end
hold off;
ylabel('Amplitude');

subplot(2, 1, 2);
hold on;
for i=1:length(mode)
  plot(1, 180/pi*angle(mode(i)), 'o', 'DisplayName', solid_names{i});
end
hold off;
ylim([-180, 180]); yticks([-180:90:180]);
ylabel('Phase [deg]');
legend();

test = mode_shapes_O(10, 1, :)/norm(squeeze(mode_shapes_O(10, 1, :)));
test = mode_shapes_O(10, 2, :)/norm(squeeze(mode_shapes_O(10, 2, :)));

% Modes shapes transformation
% First, we load the position of the Center of Mass of each solid body with respect to the point of interest.

model_com = table2array(readtable('mat/model_solidworks_com.txt', 'ReadVariableNames', false));



% Then, we reshape the data.

model_com = reshape(model_com , [3, 6]);



% Now we convert the mode shapes expressed in the DOF of the accelerometers to the DoF of each solid body (translations and rotations) with respect to their own CoM.

mode_shapes_CoM = zeros(mod_n, 6, length(solid_names));

for mod_i = 1:mod_n
  for solid_i = 1:length(solid_names)
    acc_i = solids.(solid_names{solid_i});

    Y = mode_shapes(mod_i, :, acc_i);
    Y = Y(:);

    A = zeros(3*length(acc_i), 6);
    for i = 1:length(acc_i)
      acc_pos_com = acc_pos(i, :).' - model_com(:, i);

      A(3*(i-1)+1:3*i, 1:3) = eye(3);
      A(3*(i-1)+1:3*i, 4:6) = [ 0               acc_pos_com(3) -acc_pos_com(2) ;
                               -acc_pos_com(3)  0               acc_pos_com(1) ;
                                acc_pos_com(2) -acc_pos_com(1)  0];
    end

    mode_shapes_CoM(mod_i, :, solid_i) = A\Y;
  end
end



% We then rearrange the eigen vectors in another way:
% \[ \Psi_{\text{CoM}} = \begin{bmatrix}
%   & & \\
%   \{\psi_1\} & \dots & \{\psi_N\} \\
%   & &
% \end{bmatrix}_{M \times N} \]
% with
% \[ \{\psi\}_r = \begin{Bmatrix}
%   \psi_{1, x} & \psi_{1, y} & \psi_{1, z} & \psi_{1, \theta_x} & \psi_{1, \theta_y} & \psi_{1, \theta_z} & \psi_{2, x} & \dots & \psi_{6, \theta_z}
% \end{Bmatrix}^T \]

% With $M = 6 \times 6$ is the new number of DOFs and $N=21$ is the number of modes.

% Each eigen vector is normalized.


eigen_vec_CoM = reshape(mode_shapes_CoM, [mod_n, 6*length(solid_names)]).';
% eigen_vec_CoM = eigen_vec_CoM./vecnorm(eigen_vec_CoM);
eigen_vec_ext_CoM = [eigen_vec_CoM, conj(eigen_vec_CoM)];

model_mass = table2array(readtable('mat/model_solidworks_mass.txt', 'ReadVariableNames', false));
model_inertia = table2array(readtable('mat/model_solidworks_inertia.txt', 'ReadVariableNames', false));



% \[ M = \begin{bmatrix}
%   M_1 &     &     &     &     &     \\
%       & M_2 &     &     &     &     \\
%       &     & M_3 &     &     &     \\
%       &     &     & M_4 &     &     \\
%       &     &     &     & M_5 &     \\
%       &     &     &     &     & M_6 \\
% \end{bmatrix}, \text{ with } M_i = \begin{bmatrix}
%   m_i &     &     &          &          &          \\
%       & m_i &     &          &          &          \\
%       &     & m_i &          &          &          \\
%       &     &     & I_{i,xx} & I_{i,yx} & I_{i,zx} \\
%       &     &     & I_{i,xy} & I_{i,yy} & I_{i,zy} \\
%       &     &     & I_{i,xz} & I_{i,yz} & I_{i,zz} \\
% \end{bmatrix} \]


M = zeros(6*6, 6*6);

for i = 1:6
  M((i-1)*6+1:i*6, (i-1)*6+1:i*6) = blkdiag(model_mass(i)*eye(3), model_inertia((i-1)*3+1:i*3, 1:3));
end

% Mass-normalized Eigen Vectors
% To do so, it seems that we need to know the mass matrix $[M]$. Then:
% \[ \{\phi\}_r = \frac{1}{\sqrt{m_r}} \{\psi\}_r \text{ where } m_r = \{\psi\}_r^T [M] \{\psi\}_r \]

% Mass-normalized eigen vectors are very important for the synthesis and spatial model extraction.

% Mass Matrix could be estimated from the SolidWorks model.

% We compute the modal masses that will be used for normalization.

mr = zeros(size(eigen_vector_matrix_CoM, 2), 1);

for i = 1:length(mr)
  mr(i) = real(eigen_vector_matrix_CoM(:, i).' * M * eigen_vector_matrix_CoM(:, i));
end

figure;
plot(freqs_m, mr, 'ko');
xlabel('Mode Frequency [Hz]'); ylabel('Modal mass [kg]');



% #+NAME: fig:modal_mass
% #+CAPTION: Modal masses
% [[file:figs/modal_mass.png]]

% And finally, we compute the mass-normalized eigen vectors.

eigen_vector_mass_CoM = zeros(size(eigen_vector_matrix_CoM));

for i = 1:size(eigen_vector_matrix_CoM, 2)
  eigen_vector_mass_CoM(:, i) = 1/sqrt(mr(i)) * eigen_vector_matrix_CoM(:, i);
end



% Verification that
% \[ [\Phi]^T [M] [\Phi] = [I] \]


eigen_vector_matrix_CoM.'*M*eigen_vector_matrix_CoM

eigen_vector_mass_CoM*M*eigen_vector_mass_CoM.'



% Other test for normalized eigen vectors

eigen_vector_mass_CoM = (eigen_vector_matrix_CoM.'*diag(diag(M))*eigen_vector_matrix_CoM)^(-0.5) * eigen_vector_matrix_CoM';
eigen_vector_mass_CoM = eigen_vector_mass_CoM.';

% Full Response Model from modal model (synthesis)
% In general, the form of response model with which we are concerned is an *FRF matrix* whose order is dictated by the number of coordinates $n$ included in the test.
% Also, as explained, it is normal in practice to measured and to analyze just a *subset of the FRF matrix* but rather to measure the full FRF matrix.
% Usually *one column* or *one row* with a few additional elements are measured.

% Thus, if we are to construct an acceptable response model, it will be necessary to synthesize those elements which have not been directly measured.
% However, in principle, this need present no major problem as it is possible to compute the full FRF matrix from a modal model using:
% \begin{equation}
%   [H]_{n\times n} = [\Phi]_{n\times m} [\lambda_r^2 - \omega^2]_{m\times m}^{-1} [\Phi]_{m\times n}^T
% \end{equation}

% $\{\Phi\}$ is a *mass-normalized* eigen vector.


FRF_matrix_CoM = zeros(size(eigen_vector_mass_CoM, 1), size(eigen_vector_mass_CoM, 1), length(freqs));

for i = 1:length(freqs)
  FRF_matrix_CoM(:, :, i) = eigen_vector_mass_CoM*inv(eigen_value_M^2 - (2*pi*freqs(i)*eye(size(eigen_value_M, 1)))^2)*eigen_vector_mass_CoM.';
end

exc_dir = 3;
meas_mass = 6;
meas_dir = 3;

figure;
hold on;
plot(freqs, abs(squeeze(FRF_matrix_CoM((meas_mass-1)*6 + meas_dir, 6*2+exc_dir, :))));
plot(freqs, abs(squeeze(FRFs_CoM((meas_mass-1)*6 + meas_dir, exc_dir, :))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([1, 200]);

FRF_matrix = zeros(size(eigen_vector_mass, 1), size(eigen_vector_mass, 1), length(freqs));

for i = 1:length(freqs)
  FRF_matrix(:, :, i) = eigen_vector_mass*inv(eigen_value_M - (freqs(i)*eye(size(eigen_value_M, 1)))^2)*eigen_vector_mass.';
end

figure;
hold on;
plot(freqs, abs(squeeze(FRFs_O(1, 1, :))));
plot(freqs, abs(squeeze(FRF_matrix(1, 1, :))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([1, 200]);