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

% Location of the Accelerometers
% <<ssec:modal_accelerometers>>
% 4 tri-axis accelerometers are used for each solid body.

% Only 2 could have been used as only 6DOF have to be measured, however, we have chosen to have some *redundancy*.

% This could also help us identify measurement problems or flexible modes is present.

% The position of the accelerometers are:
% - 4 on the first granite
% - 4 on the second granite
% - 4 on top of the translation stage (figure ref:fig:accelerometers_ty)
% - 4 on top of the tilt stage
% - 3 on top of the spindle
% - 4 on top of the hexapod (figure ref:fig:accelerometers_hexapod)

% In total, 23 accelerometers are used: *69 DOFs are thus measured*.

% The precise determination of the position of each accelerometer is done using the SolidWorks model (shown on figure ref:fig:location_accelerometers).

% #+name: fig:accelerometer_pictures
% #+caption: Accelerometers fixed on the micro-station
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:accelerometers_ty}$T_y$ stage}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :height 6cm
% [[file:figs/accelerometers_ty.jpg]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:accelerometers_hexapod}Micro-Hexapod}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :height 6cm
% [[file:figs/accelerometers_hexapod.jpg]]
% #+end_subfigure
% #+end_figure

% #+name: fig:location_accelerometers
% #+caption: Position of the accelerometers using SolidWorks
% #+attr_latex: :width \linewidth
% [[file:figs/location_accelerometers.png]]

% The precise position of all the 23 accelerometer with respect to a frame located at the point of interest (located 270mm above the top platform of the hexapod) are shown in table ref:tab:position_accelerometers.


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

% Signal Processing                                               :noexport:
% <<ssec:modal_signal_processing>>
% The measurements are averaged 10 times corresponding to 10 hammer impacts in order to reduce the effect of random noise.

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

% A boxcar window (figure ref:fig:modal_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\%$


%% Boxcar window used for the force signal
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:modal_windowing_force_signal
% #+caption: Boxcar window used for the force signal
% #+RESULTS:
% [[file:figs/modal_windowing_force_signal.png]]

% An exponential window (figure ref:fig:modal_windowing_acc_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\%$


%% Exponential window used for acceleration signal
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
% <<ssec:modal_measured_signals>>

%% Load raw data
meas1_raw = load('mat/meas_raw_1.mat');

% Sampling Frequency [Hz]
Fs = 1/meas1_raw.Track1_X_Resolution;

% Time just before the impact occurs [s]
impacts = [5.937, 11.228, 16.681, 22.205, 27.350, 32.714, 38.115, 43.888, 50.407]-0.01;

% Time vector [s]
time = linspace(0, meas1_raw.Track1_X_Resolution*length(meas1_raw.Track1), length(meas1_raw.Track1));



% The force sensor and the accelerometers signals are shown in the time domain in Figure ref:fig:modal_raw_meas.
% Sharp "impacts" can be seen for the force sensor, indicating wide frequency band excitation.
% For the accelerometer, many resonances can be seen on the right, indicating complex dynamics


%% Raw measurement of the Accelerometer
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
plot(time, meas1_raw.Track2, 'DisplayName', 'Acceleration [$m/s^2$]');
plot(time, 1e-3*meas1_raw.Track1, 'DisplayName', 'Force [kN]');
hold off;
xlabel('Time [s]');
ylabel('Amplitude');
xlim([0, time(end)]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(time, meas1_raw.Track2);
plot(time, 1e-3*meas1_raw.Track1);
hold off;
xlabel('Time [s]');
set(gca, 'YTickLabel',[]);
xlim([22.19, 22.4]);

linkaxes([ax1,ax2],'y');
ylim([-2, 2]);



% #+name: fig:modal_raw_meas
% #+caption: Raw measurement of the acceleromter (blue) and of the force sensor at the Hammer tip (red). Zoom on one impact is shown on the right.
% #+RESULTS:
% [[file:figs/modal_raw_meas.png]]


%% Frequency Analysis
Nfft = floor(5.0*Fs); % Number of frequency points
win = hanning(Nfft); % Windowing
Noverlap = floor(Nfft/2); % Overlap for frequency analysis

%% Comnpute the power spectral density of the force and acceleration
[pxx_force, f] = pwelch(meas1_raw.Track1, win, Noverlap, Nfft, Fs);
[pxx_acc,   ~] = pwelch(meas1_raw.Track2, win, Noverlap, Nfft, Fs);



% The "normalized" amplitude spectral density of the two signals are computed and shown in Figure ref:fig:modal_asd_acc_force.
% Conclusions based on the time domain signals can be clearly seen in the frequency domain (wide frequency content for the force signal and complex dynamics for the accelerometer).


%% Normalized Amplitude Spectral Density of the measured force and acceleration
figure;
hold on;
plot(f, sqrt(pxx_force./max(pxx_force(f<200))), 'DisplayName', 'Force');
plot(f, sqrt(pxx_acc./max(pxx_acc(f<200))), 'DisplayName', 'Acceleration');
hold off;
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Normalized Spectral Density');
xlim([0, 200]);
xticks([0:20:200]);
ylim([0, 1])
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);



% #+name: fig:modal_asd_acc_force
% #+caption: Normalized Amplitude Spectral Density of the measured force and acceleration
% #+RESULTS:
% [[file:figs/modal_asd_acc_force.png]]

% The frequency response function from the applied force to the measured acceleration can then be computed (Figure ref:fig:modal_frf_acc_force).


%% Compute the transfer function and Coherence
[G1,   f] = tfestimate(meas1_raw.Track1, meas1_raw.Track2, win, Noverlap, Nfft, Fs);
[coh1, ~] = mscohere(  meas1_raw.Track1, meas1_raw.Track2, win, Noverlap, Nfft, Fs);

%% Frequency Response Function between the force and the acceleration
figure;
plot(f, abs(G1));
xlabel('Frequency [Hz]'); ylabel('FRF [$m/s^2/N$]')
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
xlim([0, 200]);
xticks([0:20:200]);



% #+name: fig:modal_frf_acc_force
% #+caption: Frequency Response Function between the measured force and acceleration
% #+RESULTS:
% [[file:figs/modal_frf_acc_force.png]]

% The coherence between the input and output signals is also computed and found to be good between 20 and 200Hz (Figure ref:fig:modal_coh_acc_force).


%% Frequency Response Function between the force and the acceleration
figure;
plot(f, coh1);
xlabel('Frequency [Hz]'); ylabel('Coherence [-]')
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
xlim([0, 200]); ylim([0,1]);
xticks([0:20:200]);