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

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

open('active_damping/matlab/sim_nass_active_damping.slx')
load('mat/conf_simulink.mat');

% Identification                                                  :ignore:
% We initialize all the stages with the default parameters.

prepareLinearizeIdentification();

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'sim_nass_active_damping';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fnl'],           1, 'openinput');              io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm');  io_i = io_i + 1;



% We identify the dynamics for the following sample mass.

masses = [1, 10, 50]; % [kg]

Gm     = {zeros(length(masses))};
Gm_iff = {zeros(length(masses))};
Gm_dvf = {zeros(length(masses))};
Gm_ine = {zeros(length(masses))};

for i = 1:length(masses)
  initializeSample('mass', masses(i));

  %% Run the linearization
  G = linearize(mdl, io, 0.3, options);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                  'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
  Gm(i) = {G};
  Gm_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Gm_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Gm_ine(i) = {minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
end

save('./active_damping/mat/plants_variable.mat', 'masses', 'Gm_iff', 'Gm_dvf', 'Gm_ine', '-append');

% Plots                                                           :ignore:

load('./active_damping/mat/plants_variable.mat', 'masses', 'Gm_iff', 'Gm_dvf', 'Gm_ine');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gm_iff)
  plot(freqs, abs(squeeze(freqresp(Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gm_iff)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), ...
       'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_iff_sample_mass
% #+caption: Variability of the dynamics from actuator force to force sensor with the Sample Mass ([[./figs/act_damp_variability_iff_sample_mass.png][png]], [[./figs/act_damp_variability_iff_sample_mass.pdf][pdf]])
% [[file:figs/act_damp_variability_iff_sample_mass.png]]


freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;

for i = 1:length(Gm_dvf)
  plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gm_dvf)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_dvf_sample_mass
% #+caption: Variability of the dynamics from actuator force to relative motion sensor with the Sample Mass ([[./figs/act_damp_variability_dvf_sample_mass.png][png]], [[./figs/act_damp_variability_dvf_sample_mass.pdf][pdf]])
% [[file:figs/act_damp_variability_dvf_sample_mass.png]]


freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gm_ine)
  plot(freqs, abs(squeeze(freqresp(Gm_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gm_ine)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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

% Identification                                                  :ignore:
% We initialize all the stages with the default parameters.

prepareLinearizeIdentification();

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'sim_nass_active_damping';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fnl'],           1, 'openinput');              io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm');  io_i = io_i + 1;



% We identify the dynamics for the following Spindle angles.

Rz_amplitudes = [0, pi/4, pi/2, pi]; % [rad]

Ga     = {zeros(length(Rz_amplitudes))};
Ga_iff = {zeros(length(Rz_amplitudes))};
Ga_dvf = {zeros(length(Rz_amplitudes))};
Ga_ine = {zeros(length(Rz_amplitudes))};

for i = 1:length(Rz_amplitudes)
  initializeReferences('Rz_type', 'constant', 'Rz_amplitude', Rz_amplitudes(i))

  %% Run the linearization
  G = linearize(mdl, io, 0.3, options);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                  'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
  Ga(i) = {G};
  Ga_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Ga_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Ga_ine(i) = {minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
end

save('./active_damping/mat/plants_variable.mat', 'Rz_amplitudes', 'Ga_iff', 'Ga_dvf', 'Ga_ine', '-append');

% Plots                                                           :ignore:

load('./active_damping/mat/plants_variable.mat', 'Rz_amplitudes', 'Ga_iff', 'Ga_dvf', 'Ga_ine');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ga_iff)
  plot(freqs, abs(squeeze(freqresp(Ga_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ga_iff)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Ga_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [deg]', Rz_amplitudes(i)*180/pi));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_iff_spindle_angle
% #+caption: Variability of the dynamics from the actuator force to the force sensor with the Spindle Angle ([[./figs/act_damp_variability_iff_spindle_angle.png][png]], [[./figs/act_damp_variability_iff_spindle_angle.pdf][pdf]])
% [[file:figs/act_damp_variability_iff_spindle_angle.png]]


freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;

for i = 1:length(Ga_dvf)
  plot(freqs, abs(squeeze(freqresp(Ga_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ga_dvf)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Ga_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [deg]', Rz_amplitudes(i)*180/pi));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_dvf_spindle_angle
% #+caption: Variability of the dynamics from actuator force to relative motion sensor with the Spindle Angle ([[./figs/act_damp_variability_dvf_spindle_angle.png][png]], [[./figs/act_damp_variability_dvf_spindle_angle.pdf][pdf]])
% [[file:figs/act_damp_variability_dvf_spindle_angle.png]]


freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ga_ine)
  plot(freqs, abs(squeeze(freqresp(Ga_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ga_ine)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Ga_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [deg]', Rz_amplitudes(i)*180/pi));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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

% Identification                                                  :ignore:
% We initialize all the stages with the default parameters.

prepareLinearizeIdentification();

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'sim_nass_active_damping';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fnl'],           1, 'openinput');              io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm');  io_i = io_i + 1;



% We identify the dynamics for the following Spindle rotation periods.

Rz_periods = [60, 6, 2, 1]; % [s]



% The identification of the dynamics is done at the same Spindle angle position.


Gw     = {zeros(length(Rz_periods))};
Gw_iff = {zeros(length(Rz_periods))};
Gw_dvf = {zeros(length(Rz_periods))};
Gw_ine = {zeros(length(Rz_periods))};

for i = 1:length(Rz_periods)
  initializeReferences('Rz_type', 'rotating', ...
                       'Rz_period', Rz_periods(i), ... % Rotation period [s]
                       'Rz_amplitude', -0.5*(2*pi/Rz_periods(i))); % Angle offset [rad]

  load('mat/nass_references.mat', 'Rz'); % We load the reference for the Spindle
  [~, i_end] = min(abs(Rz.signals.values)); % Obtain the indice where the spindle angle is zero
  t_sim = Rz.time(i_end) % Simulation time before identification [s]

  %% Run the linearization
  G = linearize(mdl, io, t_sim, options);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                  'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};

  Gw(i) = {G};
  Gw_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Gw_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Gw_ine(i) = {minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
end

save('./active_damping/mat/plants_variable.mat', 'Rz_periods', 'Gw_iff', 'Gw_dvf', 'Gw_ine', '-append');

% Dynamics of the Active Damping plants

load('./active_damping/mat/plants_variable.mat', 'Rz_periods', 'Gw_iff', 'Gw_dvf', 'Gw_ine');
load('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');

freqs = logspace(0, 3, 10000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gw_iff)
  plot(freqs, abs(squeeze(freqresp(Gw_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))));
end
plot(freqs, abs(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gw_iff)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gw_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [rpm]', 60/Rz_periods(i)));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', 'No Rotation');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_iff_spindle_speed
% #+caption: Variability of the dynamics from the actuator force to the force sensor with the Spindle rotation speed ([[./figs/act_damp_variability_iff_spindle_speed.png][png]], [[./figs/act_damp_variability_iff_spindle_speed.pdf][pdf]])
% [[file:figs/act_damp_variability_iff_spindle_speed.png]]


xlim([20, 30]);



% #+name: fig:act_damp_variability_iff_spindle_speed_zoom
% #+caption: Variability of the dynamics from the actuator force to the force sensor with the Spindle rotation speed ([[./figs/act_damp_variability_iff_spindle_speed_zoom.png][png]], [[./figs/act_damp_variability_iff_spindle_speed_zoom.pdf][pdf]])
% [[file:figs/act_damp_variability_iff_spindle_speed_zoom.png]]


freqs = logspace(0, 3, 5000);

figure;

ax1 = subplot(2, 1, 1);
hold on;

for i = 1:length(Gw_dvf)
  plot(freqs, abs(squeeze(freqresp(Gw_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))));
end
plot(freqs, abs(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gw_dvf)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gw_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [rpm]', 60/Rz_periods(i)));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', 'No Rotation');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_dvf_spindle_speed
% #+caption: Variability of the dynamics from the actuator force to the relative motion sensor with the Spindle rotation speed ([[./figs/act_damp_variability_dvf_spindle_speed.png][png]], [[./figs/act_damp_variability_dvf_spindle_speed.pdf][pdf]])
% [[file:figs/act_damp_variability_dvf_spindle_speed.png]]


xlim([20, 30]);



% #+name: fig:act_damp_variability_dvf_spindle_speed_zoom
% #+caption: Variability of the dynamics from the actuator force to the relative motion sensor with the Spindle rotation speed ([[./figs/act_damp_variability_dvf_spindle_speed_zoom.png][png]], [[./figs/act_damp_variability_dvf_spindle_speed_zoom.pdf][pdf]])
% [[file:figs/act_damp_variability_dvf_spindle_speed_zoom.png]]


freqs = logspace(0, 3, 5000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gw_ine)
  plot(freqs, abs(squeeze(freqresp(Gw_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))));
end
plot(freqs, abs(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gw_ine)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gw_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [rpm]', 60/Rz_periods(i)));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', 'No Rotation');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_ine_spindle_speed
% #+caption: Variability of the dynamics from the actuator force to the absolute velocity sensor with the Spindle rotation speed ([[./figs/act_damp_variability_ine_spindle_speed.png][png]], [[./figs/act_damp_variability_ine_spindle_speed.pdf][pdf]])
% [[file:figs/act_damp_variability_ine_spindle_speed.png]]


xlim([20, 30]);

% Variation of the poles and zeros with the Spindle rotation frequency

load('./active_damping/mat/plants_variable.mat', 'Rz_periods', 'Gw_iff', 'Gw_dvf', 'Gw_ine');
load('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');

figure;

subplot(1,2,1);
hold on;
for i = 1:length(Gw_iff)
  G_poles = pole(Gw_iff{i}('Fnlm1', 'Fnl1'));
  plot(1/Rz_periods(i), real(G_poles(imag(G_poles)<2*pi*30 & imag(G_poles)>2*pi*22)), 'kx');
end
G_poles = pole(G_iff('Fnlm1', 'Fnl1'));
plot(0, real(G_poles(imag(G_poles)<2*pi*30 & imag(G_poles)>2*pi*22)), 'kx');
hold off;
ylim([-inf, 0]);
xlabel('Rotation Speed [Hz]');
ylabel('Real Part');

subplot(1,2,2);
hold on;
for i = 1:length(Gw_iff)
  G_poles = pole(Gw_iff{i}('Fnlm1', 'Fnl1'));
  plot(1/Rz_periods(i), imag(G_poles(imag(G_poles)<2*pi*30 & imag(G_poles)>2*pi*22)), 'kx');
end
G_poles = pole(G_iff('Fnlm1', 'Fnl1'));
plot(0, imag(G_poles(imag(G_poles)<2*pi*30 & imag(G_poles)>2*pi*22)), 'kx');
hold off;
ylim([0, inf]);
xlabel('Rotation Speed [Hz]');
ylabel('Imaginary Part');



% #+name: fig:campbell_diagram_spindle_rotation
% #+caption: Evolution of the pole with respect to the spindle rotation speed ([[./figs/campbell_diagram_spindle_rotation.png][png]], [[./figs/campbell_diagram_spindle_rotation.pdf][pdf]])
% [[file:figs/campbell_diagram_spindle_rotation.png]]


figure;

subplot(1,2,1);
hold on;
for i = 1:length(Gw_ine)
  set(gca,'ColorOrderIndex',1);
  G_zeros = zero(Gw_ine{i}('Vnlm1', 'Fnl1'));
  plot(1/Rz_periods(i), real(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o');

  set(gca,'ColorOrderIndex',2);
  G_zeros = zero(Gw_iff{i}('Fnlm1', 'Fnl1'));
  plot(1/Rz_periods(i), real(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o');

  set(gca,'ColorOrderIndex',3);
  G_zeros = zero(Gw_dvf{i}('Dnlm1', 'Fnl1'));
  plot(1/Rz_periods(i), real(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o');
end
hold off;
xlabel('Rotation Speed [Hz]');
ylabel('Real Part');

subplot(1,2,2);
hold on;
for i = 1:length(Gw_ine)
  set(gca,'ColorOrderIndex',1);
  G_zeros = zero(Gw_ine{i}('Vnlm1', 'Fnl1'));
  p_ine = plot(1/Rz_periods(i), imag(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o');

  set(gca,'ColorOrderIndex',2);
  G_zeros = zero(Gw_iff{i}('Fnlm1', 'Fnl1'));
  p_iff = plot(1/Rz_periods(i), imag(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o');

  set(gca,'ColorOrderIndex',3);
  G_zeros = zero(Gw_dvf{i}('Dnlm1', 'Fnl1'));
  p_dvf = plot(1/Rz_periods(i), imag(G_zeros(imag(G_zeros)<2*pi*25 & imag(G_zeros)>2*pi*22)), 'o');
end
hold off;
xlabel('Rotation Speed [Hz]');
ylabel('Imaginary Part');
legend([p_ine p_iff p_dvf],{'Inertial Sensor','Force Sensor', 'Relative Motion Sensor'}, 'location', 'southwest');

% Identification                                                  :ignore:
% We initialize all the stages with the default parameters.

prepareLinearizeIdentification();

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'sim_nass_active_damping';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fnl'],           1, 'openinput');              io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm');  io_i = io_i + 1;



% We identify the dynamics for the following Tilt stage angles.

Ry_amplitudes = [-3*pi/180, 3*pi/180]; % [rad]

Gy     = {zeros(length(Ry_amplitudes))};
Gy_iff = {zeros(length(Ry_amplitudes))};
Gy_dvf = {zeros(length(Ry_amplitudes))};
Gy_ine = {zeros(length(Ry_amplitudes))};

for i = 1:length(Ry_amplitudes)
  initializeReferences('Ry_type', 'constant', 'Ry_amplitude', Ry_amplitudes(i))

  %% Run the linearization
  G = linearize(mdl, io, 0.3, options);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                  'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
  Gy(i) = {G};
  Gy_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Gy_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Gy_ine(i) = {minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
end

save('./active_damping/mat/plants_variable.mat', 'Ry_amplitudes', 'Gy_iff', 'Gy_dvf', 'Gy_ine', '-append');

% Plots                                                           :ignore:

load('./active_damping/mat/plants_variable.mat', 'Ry_amplitudes', 'Gy_iff', 'Gy_dvf', 'Gy_ine');
load('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gy_iff)
  plot(freqs, abs(squeeze(freqresp(Gy_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))));
end
plot(freqs, abs(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gy_iff)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gy_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Ry = %.0f$ [deg]', Ry_amplitudes(i)*180/pi));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_iff_tilt_angle
% #+caption: Variability of the dynamics from the actuator force to the force sensor with the Tilt stage Angle ([[./figs/act_damp_variability_iff_tilt_angle.png][png]], [[./figs/act_damp_variability_iff_tilt_angle.pdf][pdf]])
% [[file:figs/act_damp_variability_iff_tilt_angle.png]]


freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;

for i = 1:length(Gy_dvf)
  plot(freqs, abs(squeeze(freqresp(Gy_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))));
end
plot(freqs, abs(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gy_dvf)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gy_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Ry = %.0f$ [deg]', Ry_amplitudes(i)*180/pi));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_dvf_tilt_angle
% #+caption: Variability of the dynamics from the actuator force to the relative motion sensor with the Tilt Angle ([[./figs/act_damp_variability_dvf_tilt_angle.png][png]], [[./figs/act_damp_variability_dvf_tilt_angle.pdf][pdf]])
% [[file:figs/act_damp_variability_dvf_tilt_angle.png]]


freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gy_ine)
  plot(freqs, abs(squeeze(freqresp(Gy_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))));
end
plot(freqs, abs(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gy_ine)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gy_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Ry = %.0f$ [deg]', Ry_amplitudes(i)*180/pi));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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

% Identification                                                  :ignore:
% We initialize all the stages with the default parameters.

prepareLinearizeIdentification();

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'sim_nass_active_damping';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fnl'],           1, 'openinput');              io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm');  io_i = io_i + 1;



% We initialize the translation stage reference to be a sinus with an amplitude of 5mm and a period of 1s (Figure [[fig:ty_scanning_reference_sinus]]).

initializeReferences('Dy_type', 'sinusoidal', ...
                     'Dy_amplitude', 5e-3, ... % [m]
                     'Dy_period', 1); % [s]

load('mat/nass_references.mat', 'Dy');
figure;
plot(Dy.time, Dy.signals.values);
xlabel('Time [s]'); ylabel('Dy - Position [m]');
xlim([0, 2]);



% #+name: fig:ty_scanning_reference_sinus
% #+caption: Reference path for the translation stage ([[./figs/ty_scanning_reference_sinus.png][png]], [[./figs/ty_scanning_reference_sinus.pdf][pdf]])
% [[file:figs/ty_scanning_reference_sinus.png]]

% We identify the dynamics at different positions (times) when scanning with the Translation stage.

t_lin = [0.5, 0.75, 1, 1.25];

Gty     = {zeros(length(t_lin))};
Gty_iff = {zeros(length(t_lin))};
Gty_dvf = {zeros(length(t_lin))};
Gty_ine = {zeros(length(t_lin))};

%% Run the linearization
G = linearize(mdl, io, t_lin, options);
G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};

for i = 1:length(t_lin)
  Gty(i) = {G(:,:,i)};
  Gty_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}, i))};
  Gty_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}, i))};
  Gty_ine(i) = {minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}, i))};
end

Gty_tlin = t_lin;
save('./active_damping/mat/plants_variable.mat', 'Gty_tlin', 'Dy', 'Gty_iff', 'Gty_dvf', 'Gty_ine', '-append');

% Plots                                                           :ignore:

load('./active_damping/mat/plants_variable.mat', 'Gty_tlin', 'Dy', 'Gty_iff', 'Gty_dvf', 'Gty_ine');
load('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gty_iff)
  plot(freqs, abs(squeeze(freqresp(Gty_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))));
end
plot(freqs, abs(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gty_iff)
  [~, i_t] = min(abs(Dy.time - Gty_tlin(i)));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gty_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Dy = %.0f$ [mm]', 1e3*Dy.signals.values(i_t)));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_iff_ty_scans
% #+caption: Variability of the dynamics from the actuator force to the absolute velocity sensor plant at different Ty scan positions ([[./figs/act_damp_variability_iff_ty_scans.png][png]], [[./figs/act_damp_variability_iff_ty_scans.pdf][pdf]])
% [[file:figs/act_damp_variability_iff_ty_scans.png]]


freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;

for i = 1:length(Gty_dvf)
  plot(freqs, abs(squeeze(freqresp(Gty_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))));
end
plot(freqs, abs(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gty_dvf)
  [~, i_t] = min(abs(Dy.time - Gty_tlin(i)));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gty_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Dy = %.0f$ [mm]', 1e3*Dy.signals.values(i_t)));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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



% #+name: fig:act_damp_variability_dvf_ty_scans
% #+caption: Variability of the dynamics from actuator force to relative displacement sensor at different Ty scan positions ([[./figs/act_damp_variability_dvf_ty_scans.png][png]], [[./figs/act_damp_variability_dvf_ty_scans.pdf][pdf]])
% [[file:figs/act_damp_variability_dvf_ty_scans.png]]


freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gty_ine)
  plot(freqs, abs(squeeze(freqresp(Gty_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))));
end
plot(freqs, abs(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gty_ine)
  [~, i_t] = min(abs(Dy.time - Gty_tlin(i)));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gty_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Dy = %.0f$ [mm]', 1e3*Dy.signals.values(i_t)));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'k--', 'DisplayName', '$Ry = 0$ [deg]');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');

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