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

%% Uniaxial Simscape model name
mdl = 'nass_uniaxial_model';

%% Frequency Vector [Hz]
freqs = logspace(0, 3, 1000);

%% Load the micro-station parameters
load('uniaxial_micro_station_parameters.mat');

% Ground Motion
% The geophone fixed to the floor to measure the floor motion.


%% Load floor motion data
% t: time in [s]
% V: measured voltage genrated by the geophone and amplified by a 60dB gain voltage amplifier [V]
load('ground_motion_measurement.mat', 't', 'V');



% The voltage generated by each geophone is amplified using a voltage amplifier (gain of 60dB) before going to the ADC.
% The sensitivity of the geophone as well as the gain of the voltage amplifier are then taken into account to reconstruct the floor displacement.

%% Sensitivity of the geophone
S0 = 88; % Sensitivity [V/(m/s)]
f0 = 2; % Cut-off frequency [Hz]

S = S0*(s/2/pi/f0)/(1+s/2/pi/f0); % Geophone's transfer function [V/(m/s)]

%% Gain of the voltage amplifier
G0_db = 60; % [dB]
G0 = 10^(G0_db/20); % [abs]

%% Transfer function from measured voltage to displacement
G_geo = 1/S/G0/s; % [m/V]



% The PSD $S_{V_f}$ of the measured voltage $V_f$ is computed.

%% Compute measured voltage PSD
Fs = 1/(t(2)-t(1)); % Sampling Frequency [Hz]
win = hanning(ceil(2*Fs)); % Hanning window

[psd_V, f] = pwelch(V, win, [], [], Fs); % [V^2/Hz]



% The PSD of the corresponding displacement can be computed as follows:
% \begin{equation}
% S_{x_f}(\omega) = \frac{S_{V_f}(\omega)}{|S_{\text{geo}}(j\omega)| \cdot G_{\text{amp}} \cdot \omega}
% \end{equation}
% with:
% - $S_{\text{geo}}$ the sensitivity of the Geophone in $[Vs/m]$
% - $G_{\text{amp}}$ the gain of the voltage amplifier
% - $\omega$ is here to integrate and have the displacement instead of the velocity


%% Ground Motion ASD
psd_xf = psd_V.*abs(squeeze(freqresp(G_geo, f, 'Hz'))).^2; % [m^2/Hz]



% The amplitude spectral density $\Gamma_{x_f}$ of the measured displacement $x_f$ is shown in Figure ref:fig:uniaxial_asd_floor_motion_id31.


%% Amplitude Spectral Density of the measured Floor motion on ID31
figure;
hold on;
plot(f, sqrt(psd_xf), 'DisplayName', '$\Gamma_{x_{f}}$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Ampl. Spectral Density $\left[\frac{m}{\sqrt{Hz}}\right]$')
xlim([1, 500]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

% Stage Vibration
% During Spindle rotation (here at 6rpm), the granite velocity and micro-hexapod's top platform velocity are measured with the geophones.


%% Measured velocity of granite and hexapod during spindle rotation
% t: time in [s]
% vg: measured granite velocity [m/s]
% vg: measured micro-hexapod's top platform velocity [m/s]
load('meas_spindle_on.mat', 't', 'vg', 'vh');
spindle_off = load('meas_spindle_off.mat', 't', 'vg', 'vh'); % No Rotation



% The Power Spectral Density of the relative velocity between the hexapod and the granite is computed.

%% Compute Power Spectral Density of the relative velocity between granite and hexapod during spindle rotation
Fs = 1/(t(2)-t(1)); % Sampling Frequency [Hz]
win = hanning(ceil(2*Fs)); % Hanning window

[psd_vft, f] = pwelch(vh-vg, win, [], [], Fs); % [(m/s)^2/Hz]
[psd_off, ~] = pwelch(spindle_off.vh-spindle_off.vg, win, [], [], Fs); % [(m/s)^2/Hz]



% It is then integrated to obtain the Amplitude Spectral Density of the relative motion which is compared with a non-rotating case (Figure ref:fig:uniaxial_asd_vibration_spindle_rotation).
% It is shown that the spindle rotation induces vibrations in a wide frequency spectrum.


%% Amplitude Spectral Density of the relative motion measured between the granite and the micro-hexapod's top platform during Spindle rotating
figure;
hold on;
plot(f, sqrt(psd_vft)./(2*pi*f), 'DisplayName', '6rpm');
plot(f, sqrt(psd_off)./(2*pi*f), 'DisplayName', '0rpm');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Ampl. Spectral Density $\left[\frac{m}{\sqrt{Hz}}\right]$')
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([1, 500]); ylim([1e-12, 1e-7])



% #+name: fig:uniaxial_asd_vibration_spindle_rotation
% #+caption: Amplitude Spectral Density of the relative motion measured between the granite and the micro-hexapod's top platform during Spindle rotating
% #+RESULTS:
% [[file:figs/uniaxial_asd_vibration_spindle_rotation.png]]

% In order to compute the equivalent disturbance force $f_t$ that induces such motion, the transfer function from $f_t$ to the relative motion of the hexapod's top platform and the granite is extracted from the model.

%% Disable the Nano-Hexpod for now
model_config = struct();
model_config.nhexa = "none";
model_config.controller = "open_loop";

%% Identify the transfer function from u to taum
clear io; io_i = 1;
io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput');  io_i = io_i + 1; % Stage Disturbance Force
io(io_i) = linio([mdl, '/micro_station/xg'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion of Granite
io(io_i) = linio([mdl, '/micro_station/xh'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion of Hexapod

%% Perform the model extraction
G = linearize(mdl, io, 0.0);
G.InputName = {'ft'};
G.OutputName = {'Dg', 'Dh'};



% The power spectral density $\Gamma_{f_{t}}$ of the disturbance force can be computed as follows:
% \begin{equation}
% \Gamma_{f_{t}}(\omega) = \frac{\Gamma_{v_{t}}(\omega)}{|G_{\text{model}}(j\omega)|^2}
% \end{equation}
% with:
% - $\Gamma_{v_{t}}$ the measured power spectral density of the relative motion between the micro-hexapod's top platform and the granite during the spindle's rotation
% - $G_{\text{model}}$ the transfer function (extracted from the uniaxial model) from $f_t$ to the relative motion between the micro-hexapod's top platform and the granite


%% Power Spectral Density of the equivalent force ft
psd_ft = (psd_vft./(2*pi*f).^2)./abs(squeeze(freqresp(G('Dh', 'ft') - G('Dg', 'ft'), f, 'Hz'))).^2;



% The obtained amplitude spectral density of the disturbance force $f_t$ is shown in Figure ref:fig:uniaxial_asd_disturbance_force.

%% Estimated disturbance force ft from measurement and uniaxial model
figure;
hold on;
plot(f, sqrt(psd_ft), 'DisplayName', '$\Gamma_{f_{t}}$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Ampl. Spectral Density $\left[\frac{N}{\sqrt{Hz}}\right]$')
xlim([1, 500]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

%% Save PSD of disturbances
save('./mat/uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');