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

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



% We then import that on =matlab=, and sort them.

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

% Windowing
% Windowing is used on the force and response signals.

% A boxcar window (figure [[fig:windowing_force_signal]]) is used for the force signal as once the impact on the structure is done, the measured signal is meaningless.
% The parameters are:
% - *Start*: $3\%$
% - *Stop*: $7\%$


figure;
plot(100*[0, 0.03, 0.03, 0.07, 0.07, 1], [0, 0, 1, 1, 0, 0]);
xlabel('Time [%]'); ylabel('Amplitude');
xlim([0, 100]); ylim([0, 1]);



% #+NAME: fig:windowing_force_signal
% #+CAPTION: Window used for the force signal
% [[file:figs/windowing_force_signal.png]]

% An exponential window (figure [[fig:windowing_response_signal]]) is used for the response signal as we are measuring transient signals and most of the information is located at the beginning of the signal.
% The parameters are:
% - FlatTop:
%   - *Start*: $3\%$
%   - *Stop*: $2.96\%$
% - Decreasing point:
%   - *X*: $60.4\%$
%   - *Y*: $14.7\%$


x0 = 0.296;
xd = 0.604;
yd = 0.147;

alpha = log(yd)/(x0 - xd);

t = x0:0.01:1.01;
y = exp(-alpha*(t-x0));

figure;
plot(100*[0, 0.03, 0.03, x0, t], [0, 0, 1, 1, y]);
xlabel('Time [%]'); ylabel('Amplitude');
xlim([0, 100]); ylim([0, 1]);

% Force and Response signals
% Let's load some obtained data to look at the force and response signals.


meas1_raw = load('mat/meas_raw_1.mat');

% Raw Force Data
% The force input for the first measurement is shown on figure [[fig:raw_data_force]]. We can see 10 impacts, one zoom on one impact is shown on figure [[fig:raw_data_force_zoom]].

% The Fourier transform of the force is shown on figure [[fig:fourier_transfor_force_impact]]. This has been obtained without any windowing.


time = linspace(0, meas1_raw.Track1_X_Resolution*length(meas1_raw.Track1), length(meas1_raw.Track1));

figure;
plot(time, meas1_raw.Track1);
xlabel('Time [s]');
ylabel('Force [N]');



% #+NAME: fig:raw_data_force
% #+CAPTION: Raw Force Data from Hammer Blow
% [[file:figs/raw_data_force.png]]


figure;
plot(time, meas1_raw.Track1);
xlabel('Time [s]');
ylabel('Force [N]');
xlim([22.1, 22.3]);



% #+NAME: fig:raw_data_force_zoom
% #+CAPTION: Raw Force Data from Hammer Blow - Zoom
% [[file:figs/raw_data_force_zoom.png]]


Fs = 1/meas1_raw.Track1_X_Resolution; % Sampling Frequency [Hz]
impacts = [5.9, 11.2, 16.6, 22.2, 27.3, 32.7, 38.1, 43.8, 50.4]; % Time just before the impact occurs [s]

L = 8194;
f = Fs*(0:(L/2))/L; % Frequency vector [Hz]

F_fft = zeros((8193+1)/2+1, length(impacts));

for i = 1:length(impacts)
  t0 = impacts(i);
  [~, i_start] = min(abs(time-t0));
  i_end = i_start + 8193;

  Y = fft(meas1_raw.Track1(i_start:i_end));

  P2 = abs(Y/L);
  P1 = P2(1:L/2+1);
  P1(2:end-1) = 2*P1(2:end-1);
  F_fft(:, i) = P1;
end

figure;
hold on;
for i = 1:length(impacts)
  plot(f, F_fft(:, i), '-k');
end
hold off;
xlim([0, 200]);
xlabel('Frequency [Hz]'); ylabel('Force [N]');

% Raw Response Data
% The response signal for the first measurement is shown on figure [[fig:raw_data_acceleration]]. One zoom on one response is shown on figure [[fig:raw_data_acceleration_zoom]].

% The Fourier transform of the response signals is shown on figure [[fig:fourier_transform_response_signals]]. This has been obtained without any windowing.


figure;
plot(time, meas1_raw.Track2);
xlabel('Time [s]');
ylabel('Acceleration [m/s2]');



% #+NAME: fig:raw_data_acceleration
% #+CAPTION: Raw Acceleration Data from Accelerometer
% [[file:figs/raw_data_acceleration.png]]


figure;
plot(time, meas1_raw.Track2);
xlabel('Time [s]');
ylabel('Acceleration [m/s2]');
xlim([22.1, 22.5]);



% #+NAME: fig:raw_data_acceleration_zoom
% #+CAPTION: Raw Acceleration Data from Accelerometer - Zoom
% [[file:figs/raw_data_acceleration_zoom.png]]


X_fft = zeros((8193+1)/2+1, length(impacts));

for i = 1:length(impacts)
  t0 = impacts(i);
  [~, i_start] = min(abs(time-t0));
  i_end = i_start + 8193;

  Y = fft(meas1_raw.Track2(i_start:i_end));

  P2 = abs(Y/L);
  P1 = P2(1:L/2+1);
  P1(2:end-1) = 2*P1(2:end-1);
  X_fft(:, i) = P1;
end

figure;
hold on;
for i = 1:length(impacts)
  plot(f, X_fft(:, i), '-k');
end
hold off;
xlim([0, 200]);
set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Acceleration [$m/s^2$]');

% Computation of one Frequency Response Function
% Now that we have obtained the Fourier transform of both the force input and the response signal, we can compute the Frequency Response Function from the force to the acceleration.

% We then compare the result obtained with the FRF computed by the modal software (figure [[fig:frf_comparison_software]]).

% The slight difference can probably be explained by the use of windows.

% In the following analysis, FRF computed from the software will be used.


meas1 = load('mat/meas_frf_coh_1.mat');

figure;
hold on;
for i = 1:length(impacts)
  plot(f, X_fft(:, i)./F_fft(:, i), '-k');
end
plot(meas1.FFT1_AvSpc_2_RMS_X_Val, meas1.FFT1_AvSpc_2_RMS_Y_Val)
hold off;
xlim([0, 200]);
set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Acceleration/Force [$m/s^2/N$]');

% Frequency Response Functions and Coherence Results
% Let's see one computed Frequency Response Function and one coherence in order to attest the quality of the measurement.

% First, we load the data.

meas1 = load('mat/meas_frf_coh_1.mat');



% And we plot on figure [[fig:frf_result_example]] the frequency response function from the force applied in the $X$ direction at the location of the accelerometer number 11 to the acceleration in the $X$ direction as measured by the first accelerometer located on the top platform of the hexapod.

% The coherence associated is shown on figure [[fig:frf_result_example]].


figure;
ax1 = subplot(2, 1, 1);
plot(meas1.FFT1_AvSpc_2_RMS_X_Val, meas1.FFT1_AvXSpc_2_1_RMS_Y_Mod);
set(gca, 'Yscale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');

ax2 = subplot(2, 1, 2);
plot(meas1.FFT1_AvSpc_2_RMS_X_Val, meas1.FFT1_AvXSpc_2_1_RMS_Y_Phas);
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');

linkaxes([ax1,ax2],'x');



% #+NAME: fig:frf_result_example
% #+CAPTION: Example of one measured FRF
% [[file:figs/frf_result_example.png]]


figure;
plot(meas1.FFT1_AvSpc_2_RMS_X_Val, meas1.FFT1_Coh_2_1_RMS_Y_Val);
xlabel('Frequency [Hz]');
ylabel('Coherence');

% Generation of a FRF matrix and a Coherence matrix from the measurements
% We want here to combine all the Frequency Response Functions measured into one big array called the *Frequency Response Matrix*.

% The frequency response matrix is an $n \times p \times q$:
% - $n$ is the number of measurements: $23 \times 3$ (23 accelerometers measuring 3 directions each)
% - $p$ is the number of excitation inputs: $3$
% - $q$ is the number of frequency points $\omega_i$

% Thus, the FRF matrix is an $69 \times 3 \times 801$ matrix.


% #+begin_important
% For each frequency point $\omega_i$, we obtain a 2D matrix:
% \begin{equation}
%   \text{FRF}(\omega_i) = \begin{bmatrix}
%   \frac{D_{1_x}}{F_x}(\omega_i) & \frac{D_{1_x}}{F_y}(\omega_i) & \frac{D_{1_x}}{F_z}(\omega_i) \\
%   \frac{D_{1_y}}{F_x}(\omega_i) & \frac{D_{1_y}}{F_y}(\omega_i) & \frac{D_{1_y}}{F_z}(\omega_i) \\
%   \frac{D_{1_z}}{F_x}(\omega_i) & \frac{D_{1_z}}{F_y}(\omega_i) & \frac{D_{1_z}}{F_z}(\omega_i) \\
%   \frac{D_{2_x}}{F_x}(\omega_i) & \frac{D_{2_x}}{F_y}(\omega_i) & \frac{D_{2_x}}{F_z}(\omega_i) \\
%   \vdots              & \vdots              & \vdots              \\
%   \frac{D_{23_z}}{F_x}(\omega_i) & \frac{D_{23_z}}{F_y}(\omega_i) & \frac{D_{23_z}}{F_z}(\omega_i) \\
% \end{bmatrix}
% \end{equation}
% #+end_important

% We generate such FRF matrix from the measurements using the following script.

n_meas = 24;
n_acc = 23;

dirs = 'XYZ';

% Number of Accelerometer * DOF for each acccelerometer / Number of excitation / frequency points
FRFs = zeros(3*n_acc, 3, 801);
COHs = zeros(3*n_acc, 3, 801);

% Loop through measurements
for i = 1:n_meas
  % Load the measurement file
  meas = load(sprintf('mat/meas_frf_coh_%i.mat', i));

  % First: determine what is the exitation (direction and sign)
  exc_dir = meas.FFT1_AvXSpc_2_1_RMS_RfName(end);
  exc_sign = meas.FFT1_AvXSpc_2_1_RMS_RfName(end-1);
  % Determine what is the correct excitation sign
  exc_factor = str2num([exc_sign, '1']);
  if exc_dir ~= 'Z'
    exc_factor = exc_factor*(-1);
  end

  % Then: loop through the nine measurements and store them at the correct location
  for j = 2:10
    % Determine what is the accelerometer and direction
    [indices_acc_i] = strfind(meas.(sprintf('FFT1_H1_%i_1_RpName', j)), '.');
    acc_i = str2num(meas.(sprintf('FFT1_H1_%i_1_RpName', j))(indices_acc_i(1)+1:indices_acc_i(2)-1));

    meas_dir  = meas.(sprintf('FFT1_H1_%i_1_RpName', j))(end);
    meas_sign = meas.(sprintf('FFT1_H1_%i_1_RpName', j))(end-1);
    % Determine what is the correct measurement sign
    meas_factor = str2num([meas_sign, '1']);
    if meas_dir ~= 'Z'
      meas_factor = meas_factor*(-1);
    end

    FRFs(find(dirs==meas_dir)+3*(acc_i-1), find(dirs==exc_dir), :) = exc_factor*meas_factor*meas.(sprintf('FFT1_H1_%i_1_Y_ReIm', j));
    COHs(find(dirs==meas_dir)+3*(acc_i-1), find(dirs==exc_dir), :) = meas.(sprintf('FFT1_Coh_%i_1_RMS_Y_Val', j));
  end
end
freqs = meas.FFT1_Coh_10_1_RMS_X_Val;



% And we save the obtained FRF matrix and Coherence matrix in a =.mat= file.

save('./mat/frf_coh_matrices.mat', 'FRFs', 'COHs', 'freqs');

% Plot showing the coherence of all the measurements
% Now that we have defined a Coherence matrix, we can plot each of its elements to have an idea of the overall coherence and thus, quality of the measurement.
% The result is shown on figure [[fig:all_coherence]].


n_acc = 23;

figure;
hold on;
for i = 1:3*n_acc
  plot(freqs, squeeze(COHs(i, 1, :)), 'color', [0, 0, 0, 0.2]);
end
hold off;
xlabel('Frequency [Hz]');
ylabel('Coherence [\%]');

% Solid Bodies considered for further analysis
% We consider the following solid bodies for further analysis:
% - Bottom Granite
% - Top Granite
% - Translation Stage
% - Tilt Stage
% - Spindle
% - Hexapod

% We create a =matlab= structure =solids= that contains the accelerometers ID connected to each solid bodies (as shown on figure [[fig:nass-modal-test]]).

solids = {};
solids.gbot = [17, 18, 19, 20];
solids.gtop = [13, 14, 15, 16];
solids.ty   = [9, 10, 11, 12];
solids.ry   = [5, 6, 7, 8];
solids.rz   = [21, 22, 23];
solids.hexa = [1, 2, 3, 4];

solid_names = fields(solids);



% Finally, we save that into a =.mat= file.

save('mat/geometry.mat', 'solids', 'solid_names', 'acc_pos');



% #+name: fig:aligned_accelerometers
% #+caption: Aligned measurement of the motion of a solid body
% #+RESULTS:
% [[file:figs/aligned_accelerometers.png]]

% The motion of the rigid body of figure [[fig:aligned_accelerometers]] is defined by the velocity $\vec{v}$ and rotation $\vec{\Omega}$ with respect to the reference frame $\{O\}$.

% The motions at points $1$ and $2$ are:
% \begin{align*}
%   v_1 &= v + \Omega \times p_1 \\
%   v_2 &= v + \Omega \times p_2
% \end{align*}

% Taking only the $x$ direction:
% \begin{align*}
%   v_{x1} &= v + \Omega_y p_{z1} - \Omega_z p_{y1} \\
%   v_{x2} &= v + \Omega_y p_{z2} - \Omega_z p_{y2}
% \end{align*}

% However, we have $p_{1y} = p_{2y}$ and $p_{1z} = p_{2z}$ because of the co-linearity of the two sensors in the $x$ direction, and thus we obtain
% \begin{equation}
%   v_{x1} = v_{x2}
% \end{equation}

% #+begin_important
%   Two sensors that are measuring the motion of a rigid body in the direction of the line linking the two sensors should measure the same quantity.
% #+end_important

% We can verify that the rigid body assumption is correct by comparing the measurement of the sensors.

% From the table [[tab:position_accelerometers]], we can guess which sensors will give the same results in the X and Y directions.

% Comparison of such measurements in the X direction is shown on figure [[fig:compare_acc_x_dir]] and in the Y direction on figure [[fig:compare_acc_y_dir]].


meas_dir = 1;
exc_dir = 1;

acc_i = [1 , 4 ;
         2 , 3 ;
         5 , 8 ;
         6 , 7 ;
         9 , 12;
         10, 11;
         14, 15;
         18, 19;
         21, 23];

figure;
for i = 1:size(acc_i, 1)
  subaxis(3, 3, i);
  hold on;
  plot(freqs, abs(squeeze(FRFs(meas_dir+3*(acc_i(i, 1)-1), exc_dir, :))))
  plot(freqs, abs(squeeze(FRFs(meas_dir+3*(acc_i(i, 2)-1), exc_dir, :))))
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  if i > 6
    xlabel('Frequency [Hz]');
  else
    set(gca, 'XTickLabel',[]);
  end

  if rem(i, 3) == 1
    ylabel('Amplitude');
  end
  xlim([1, 200]);
  title(sprintf('Acc %i and %i - X', acc_i(i, 1), acc_i(i, 2)));
end



% #+NAME: fig:compare_acc_x_dir
% #+CAPTION: Compare accelerometers align in the X direction
% [[file:figs/compare_acc_x_dir.png]]



meas_dir = 2;
exc_dir = 1;

acc_i = [1, 2;
         5, 6;
         7, 8;
         9, 10;
         11, 12;
         13, 14;
         15, 16;
         17, 18;
         19, 20];

figure;
for i = 1:size(acc_i, 1)
  subaxis(3, 3, i);
  hold on;
  plot(freqs, abs(squeeze(FRFs(meas_dir+3*(acc_i(i, 1)-1), exc_dir, :))))
  plot(freqs, abs(squeeze(FRFs(meas_dir+3*(acc_i(i, 2)-1), exc_dir, :))))
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  if i > 6
    xlabel('Frequency [Hz]');
  else
    set(gca, 'XTickLabel',[]);
  end

  if rem(i, 3) == 1
    ylabel('Amplitude');
  end
  xlim([1, 200]);
  title(sprintf('Acc %i and %i - Y', acc_i(i, 1), acc_i(i, 2)));
end