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



% #+name: fig:micro_station_uniaxial_model
% #+caption: Schematic of the Micro-Station measurement setup and uniaxial model.
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:micro_station_meas_dynamics_schematic}Measurement setup - Schematic}
% #+attr_latex: :options {0.69\textwidth}
% #+begin_subfigure
% #+attr_latex: :scale 1
% [[file:figs/micro_station_meas_dynamics_schematic.png]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:uniaxial_model_micro_station}Uniaxial model of the micro-station}
% #+attr_latex: :options {0.29\textwidth}
% #+begin_subfigure
% #+attr_latex: :scale 1
% [[file:figs/uniaxial_model_micro_station.png]]
% #+end_subfigure
% #+end_figure

% Due to the bad coherence at low frequency, the frequency response functions will only be shown between 20 and 200Hz (solid lines in Figure ref:fig:uniaxial_comp_frf_meas_model).


%% Load measured FRF
load('meas_microstation_frf.mat');



% Masses are estimated from the CAD.

%% Parameters - Mass
mh = 15;   % Micro Hexapod [kg]
mt = 1200; % Ty + Ry + Rz [kg]
mg = 2500; % Granite [kg]



% And stiffnesses from the data-sheet of stage manufacturers.

%% Parameters - Stiffnesses
kh = 6.11e+07; % [N/m]
kt = 5.19e+08; % [N/m]
kg = 9.50e+08; % [N/m]



% The damping coefficients are tuned to match the identified damping from the measurements.

%% Parameters - damping
ch = 2*0.05*sqrt(kh*mh); % [N/(m/s)]
ct = 2*0.05*sqrt(kt*mt); % [N/(m/s)]
cg = 2*0.08*sqrt(kg*mg); % [N/(m/s)]

%% Save model parameters
save('./mat/uniaxial_micro_station_parameters.mat', 'mh', 'mt', 'mg', 'ch', 'ct', 'cg', 'kh', 'kt', 'kg')

%% 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/Fg'], 1, 'openinput');  io_i = io_i + 1; % Hammer on Granite
io(io_i) = linio([mdl, '/micro_station/Fh'], 1, 'openinput');  io_i = io_i + 1; % Hammer on Hexapod
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_id = linearize(mdl, io, 0.0);
G_id.InputName = {'Fg', 'Fh'};
G_id.OutputName = {'Dg', 'Dh'};

% Comparison of the model and measurements
% The comparison between the measurements and the model is shown in Figure ref:fig:uniaxial_comp_frf_meas_model.

% Only three modes are modelled with frequencies at 70Hz, 140Hz and 320Hz.

% As the model is simplistic, the goal is not to match exactly the measurement but to have a first approximation.
% More accurate models will be used later on.


%% Comparison of the measured FRF and identified ones from the uni-axial model
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f(f>20), abs(frf_Fhz_to_Dhz(f>20)), '-', 'color', colors(1,:), 'DisplayName', '$D_{h,z}/F_{h,z}$');
plot(f(f>20), abs(frf_Fgz_to_Dhz(f>20)), '-', 'color', colors(2,:), 'DisplayName', '$D_{h,z}/F_{g,z}$');
plot(f(f>20), abs(frf_Fgz_to_Dgz(f>20)), '-', 'color', colors(3,:), 'DisplayName', '$D_{g,z}/F_{g,z}$');
plot(freqs, abs(squeeze(freqresp(G_id('Dh', 'Fh'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$D_{h,z}/F_{h,z}$ (model)');
plot(freqs, abs(squeeze(freqresp(G_id('Dh', 'Fg'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '$D_{h,z}/F_{g,z}$ (model)');
plot(freqs, abs(squeeze(freqresp(G_id('Dg', 'Fg'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'DisplayName', '$D_{g,z}/F_{g,z}$ (model)');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 2e-7]);
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_id('Dh', 'Fh'), freqs, 'Hz')))), '--', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_id('Dh', 'Fg'), freqs, 'Hz')))), '--', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_id('Dg', 'Fg'), freqs, 'Hz')))), '--', 'color', colors(3,:));
plot(f(f>20), 180/pi*unwrap(angle(frf_Fhx_to_Dhx(f>20))), '-', 'color', colors(1,:));
plot(f(f>30), 180/pi*unwrap(angle(frf_Fgx_to_Dhx(f>30))), '-', 'color', colors(2,:));
plot(f(f>20), 180/pi*unwrap(angle(frf_Fgx_to_Dgx(f>20))), '-', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-360, 90]);

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