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



% #+name: fig:modal_local_to_global_coordinates
% #+caption: Schematic of the measured motions of a solid body
% #+RESULTS:
% [[file:figs/modal_local_to_global_coordinates.png]]

% The motion of the rigid body of figure ref:fig:modal_local_to_global_coordinates can be described by its displacement $\vec{\delta}p = [\delta p_x,\ \delta p_y,\ \delta p_z]$ and (small) rotations $[\delta \Omega_x,\ \delta \Omega_y,\ \delta \Omega_z]$ with respect to the reference frame $\{O\}$.

% The motion $\vec{\delta} p_{i}$ of a point $p_i$ can be computed from $\vec{\delta} p$ and $\bm{\delta \Omega}$ using equation eqref:eq:modal_compute_point_response, with $\bm{\delta\Omega}$ defined in equation eqref:eq:modal_rotation_matrix.

% \begin{equation}\label{eq:modal_compute_point_response}
%   \vec{\delta} p_{i} &= \vec{\delta} p + \bm{\delta \Omega} \cdot \vec{p}_{i} \\
% \end{equation}

% \begin{equation}\label{eq:modal_rotation_matrix}
%   \bm{\delta\Omega} = \begin{bmatrix}
%    0              & -\delta\Omega_z &  \delta\Omega_y \\
%    \delta\Omega_z &  0              & -\delta\Omega_x \\
%   -\delta\Omega_y &  \delta\Omega_x &  0
% \end{bmatrix}
% \end{equation}

% Writing this in matrix form for the four points gives eqref:eq:modal_cart_to_acc.

% \begin{equation}\label{eq:modal_cart_to_acc}
% \left[\begin{array}{c}
%   \delta p_{1x} \\ \delta p_{1y} \\ \delta p_{1z} \\\hline \vdots \\\hline \delta p_{4x} \\ \delta p_{4y} \\ \delta p_{4z}
% \end{array}\right] =
% \left[\begin{array}{ccc|ccc}
%   1 & 0 & 0 & 0 & p_{1z} & -p_{1y} \\
%   0 & 1 & 0 & -p_{1z} & 0 & p_{1x} \\
%   0 & 0 & 1 & p_{1y} & -p_{1x} & 0 \\ \hline
%     & \vdots &   &    & \vdots &   \\ \hline
%   1 & 0 & 0 & 0 & p_{4z} & -p_{4y} \\
%   0 & 1 & 0 & -p_{4z} & 0 & p_{4x} \\
%   0 & 0 & 1 & p_{4y} & -p_{4x} & 0
% \end{array}\right] \left[\begin{array}{c}
%   \delta p_x \\ \delta p_y \\ \delta p_z \\ \hline \delta\Omega_x \\ \delta\Omega_y \\ \delta\Omega_z
% \end{array}\right]
% \end{equation}

% Provided that the four sensors are properly located, the system of equation eqref:eq:modal_cart_to_acc can be solved by matrix inversion[fn:5].
% The motion of the solid body expressed in a chosen frame $\{O\}$ can be determined using equation eqref:eq:modal_determine_global_disp.
% Note that this matrix inversion is equivalent to resolving a mean square problem.
% Therefore, having more accelerometers permits better approximation of the motion of a solid body.

% \begin{equation}
% \left[\begin{array}{c}
%   \delta p_x \\ \delta p_y \\ \delta p_z \\ \hline \delta\Omega_x \\ \delta\Omega_y \\ \delta\Omega_z
% \end{array}\right] =
% \left[\begin{array}{ccc|ccc}
%   1 & 0 & 0 & 0 & p_{1z} & -p_{1y} \\
%   0 & 1 & 0 & -p_{1z} & 0 & p_{1x} \\
%   0 & 0 & 1 & p_{1y} & -p_{1x} & 0 \\ \hline
%     & \vdots &   &    & \vdots &   \\ \hline
%   1 & 0 & 0 & 0 & p_{4z} & -p_{4y} \\
%   0 & 1 & 0 & -p_{4z} & 0 & p_{4x} \\
%   0 & 0 & 1 & p_{4y} & -p_{4x} & 0
% \end{array}\right]^{-1} \left[\begin{array}{c}
%   \delta p_{1x} \\ \delta p_{1y} \\ \delta p_{1z} \\\hline \vdots \\\hline \delta p_{4x} \\ \delta p_{4y} \\ \delta p_{4z}
% \end{array}\right] \label{eq:modal_determine_global_disp}
% \end{equation}


% From the CAD model, the position of the center of mass of each considered solid body is computed (see Table ref:tab:modal_com_solid_bodies).
% The position of each accelerometer with respect to the center of mass of the corresponding solid body can easily be determined.


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

%% Load Accelerometer positions
acc_pos = readtable('mat/acc_pos.txt', 'ReadVariableNames', false);
acc_pos = table2array(acc_pos(:, 1:4));
[~, i] = sort(acc_pos(:, 1));
acc_pos = acc_pos(i, 2:4);

%% Accelerometers ID connected to each solid body
solids = {};
solids.gbot = [17, 18, 19, 20]; % bottom granite
solids.gtop = [13, 14, 15, 16]; % top granite
solids.ty   = [9, 10, 11, 12]; % Ty stage
solids.ry   = [5, 6, 7, 8]; % Ry stage
solids.rz   = [21, 22, 23]; % Rz stage
solids.hexa = [1, 2, 3, 4]; % Hexapod

% Names of the solid bodies
solid_names = fields(solids);

%% Save the accelerometer positions are well as the solid bodies
save('mat/geometry.mat', 'solids', 'solid_names', 'acc_pos');

%% Extract the CoM of considered solid bodies
model_com = reshape(table2array(readtable('mat/model_solidworks_com.txt', 'ReadVariableNames', false)), [3, 6]);



% #+name: tab:modal_com_solid_bodies
% #+caption: Center of mass of considered solid bodies with respect to the "point of interest"
% #+attr_latex: :environment tabularx :width 0.6\linewidth :align lXXX
% #+attr_latex: :center t :booktabs t
% #+RESULTS:
% |                   | $X$ [mm] | $Y$ [mm] | $Z$ [mm] |
% |-------------------+----------+----------+----------|
% | Bottom Granite    |       45 |      144 |    -1251 |
% | Top granite       |       52 |      258 |     -778 |
% | Translation stage |        0 |       14 |     -600 |
% | Tilt Stage        |        0 |       -5 |     -628 |
% | Spindle           |        0 |        0 |     -580 |
% | Hexapod           |       -4 |        6 |     -319 |

% Using eqref:eq:modal_determine_global_disp, the frequency response matrix $\mathbf{H}_\text{CoM}$ eqref:eq:modal_frf_matrix_com expressing the response at the center of mass of each solid body $D_i$ ($i$ from $1$ to $6$ for the $6$ considered solid bodies) can be computed from the initial acrshort:frf matrix $\mathbf{H}$.

% \begin{equation}\label{eq:modal_frf_matrix_com}
%   \mathbf{H}_\text{CoM}(\omega_i) = \begin{bmatrix}
%   \frac{D_{1,T_x}}{F_x}(\omega_i) & \frac{D_{1,T_x}}{F_y}(\omega_i) & \frac{D_{1,T_x}}{F_z}(\omega_i) \\
%   \frac{D_{1,T_y}}{F_x}(\omega_i) & \frac{D_{1,T_y}}{F_y}(\omega_i) & \frac{D_{1,T_y}}{F_z}(\omega_i) \\
%   \frac{D_{1,T_z}}{F_x}(\omega_i) & \frac{D_{1,T_z}}{F_y}(\omega_i) & \frac{D_{1,T_z}}{F_z}(\omega_i) \\
%   \frac{D_{1,R_x}}{F_x}(\omega_i) & \frac{D_{1,R_x}}{F_y}(\omega_i) & \frac{D_{1,R_x}}{F_z}(\omega_i) \\
%   \frac{D_{1,R_y}}{F_x}(\omega_i) & \frac{D_{1,R_y}}{F_y}(\omega_i) & \frac{D_{1,R_y}}{F_z}(\omega_i) \\
%   \frac{D_{1,R_z}}{F_x}(\omega_i) & \frac{D_{1,R_z}}{F_y}(\omega_i) & \frac{D_{1,R_z}}{F_z}(\omega_i) \\
%   \frac{D_{2,T_x}}{F_x}(\omega_i) & \frac{D_{2,T_x}}{F_y}(\omega_i) & \frac{D_{2,T_x}}{F_z}(\omega_i) \\
%   \vdots              & \vdots              & \vdots              \\
%   \frac{D_{6,R_z}}{F_x}(\omega_i) & \frac{D_{6,R_z}}{F_y}(\omega_i) & \frac{D_{6,R_z}}{F_z}(\omega_i)
% \end{bmatrix}
% \end{equation}


%% Frequency Response Matrix - Response expressed at the CoM of the solid bodies
frfs_CoM = zeros(length(solid_names)*6, 3, 801);

for solid_i = 1:length(solid_names)
  % Number of accelerometers fixed to this solid body
  solids_i = solids.(solid_names{solid_i});

  % "Jacobian" matrix to go from accelerometer frame to CoM frame
  A = zeros(3*length(solids_i), 6);
  for i = 1:length(solids_i)
    acc_i = solids_i(i);

    acc_pos_com = acc_pos(acc_i, :).' - model_com(:, solid_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

  for exc_dir = 1:3
    frfs_CoM((solid_i-1)*6+1:solid_i*6, exc_dir, :) = A\squeeze(frf((solids_i(1)-1)*3+1:solids_i(end)*3, exc_dir, :));
  end
end

%% Save the computed FRF at the CoM
save('mat/frf_com.mat', 'frfs_CoM');

% Verification of solid body assumption
% <<ssec:modal_solid_body_assumption>>

% From the response of one solid body expressed by its 6 acrshortpl:dof (i.e. from $\mathbf{H}_{\text{CoM}}$), and using equation eqref:eq:modal_cart_to_acc, it is possible to compute the response of the same solid body at any considered location.
% In particular, the responses at the locations of the four accelerometers can be computed and compared with the original measurements $\mathbf{H}$.
% This is what is done here to check whether the solid body assumption is correct in the frequency band of interest.


%% Compute the FRF at the accelerometer location from the CoM reponses
frfs_A = zeros(size(frf));

% For each excitation direction
for exc_dir = 1:3
  % For each solid
  for solid_i = 1:length(solid_names)
    v0 = squeeze(frfs_CoM((solid_i-1)*6+1:(solid_i-1)*6+3, exc_dir, :));
    W0 = squeeze(frfs_CoM((solid_i-1)*6+4:(solid_i-1)*6+6, exc_dir, :));

    % For each accelerometer attached to the current solid
    for acc_i = solids.(solid_names{solid_i})
      % We get the position of the accelerometer expressed in frame O
      pos = acc_pos(acc_i, :).' - model_com(:, solid_i);
      % pos = acc_pos(acc_i, :).';
      posX = [0 pos(3) -pos(2); -pos(3) 0 pos(1) ; pos(2) -pos(1) 0];

      frfs_A(3*(acc_i-1)+1:3*(acc_i-1)+3, exc_dir, :) = v0 + posX*W0;
    end
  end
end



% The comparison is made for the 4 accelerometers fixed on the micro-hexapod (Figure ref:fig:modal_comp_acc_solid_body_frf).
% The original frequency response functions and those computed from the CoM responses match well in the frequency range of interest.
% Similar results were obtained for the other solid bodies, indicating that the solid body assumption is valid and that a multi-body model can be used to represent the dynamics of the micro-station.
% This also validates the reduction in the number of degrees of freedom from 69 (23 accelerometers with each 3 acrshort:dof) to 36 (6 solid bodies with 6 acrshort:dof).


%% Comparaison of the original accelerometer response and reconstructed response from the solid body response
exc_names = {'$F_x$', '$F_y$', '$F_z$'};
DOFs = {'x', 'y', 'z', '\theta_x', '\theta_y', '\theta_z'};

solid_i = 6; % Considered solid body
exc_dir = 1; % Excited direction

accs_i = solids.(solid_names{solid_i}); % Accelerometers fixed to this solid body

figure;
tiledlayout(2, 2, 'TileSpacing', 'Tight', 'Padding', 'None');

for i = 1:length(accs_i)
  acc_i = accs_i(i);
  nexttile();

  hold on;
  for dir_i = 1:3
    plot(freqs, abs(squeeze(frf(3*(acc_i-1)+dir_i, exc_dir, :))), '-', 'DisplayName', sprintf('$a_{%i,%s}$', acc_i, DOFs{dir_i}));
  end
  set(gca,'ColorOrderIndex',1)
  for dir_i = 1:3
    plot(freqs, abs(squeeze(frfs_A(3*(acc_i-1)+dir_i, exc_dir, :))), '--', 'HandleVisibility', 'off');
  end
  hold off;

  if i > 2
    xlabel('Frequency [Hz]');
  else
    set(gca, 'XTickLabel',[]);
  end

  if rem(i, 2) == 1
    ylabel('Amplitude [$\frac{m/s^2}{N}$]');
  else
    set(gca, 'YTickLabel',[]);
  end

  set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
  xlim([0, 200]); ylim([1e-6, 3e-2]);
  xticks([0:20:200]);
  legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
end