%% 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_simscape.mat');

% Initialize the Simulation
% We initialize all the stages with the default parameters.

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();



% No disturbances.

initializeDisturbances('enable', false);



% The nano-hexapod is a piezoelectric hexapod.

initializeNanoHexapod('actuator', 'piezo');



% We set the references to zero.

initializeReferences();



% And all the controllers are set to 0.

K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_ine = tf(zeros(6));
save('./mat/controllers.mat', 'K_ine', '-append');
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
K_dvf = tf(zeros(6));
save('./mat/controllers.mat', 'K_dvf', '-append');

% Identification
% First, we identify the dynamics of the system using the =linearize= function.

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

masses = [1, 10, 50];

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.1, 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', 'Gm_iff', 'Gm_dvf', 'Gm_ine', '-append');

% Plots

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

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gm)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))));
  for j = 2:6
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gm_iff{i}(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
  end
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)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
  for j = 2:6
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i}(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))), 'HandleVisibility', 'off');
  end
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');



% #+NAME: fig:act_damp_variability_iff_sample_mass
% #+CAPTION: Variability of the IFF plant with the Spindle Angle ([[./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)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))));
  for j = 2:6
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
  end
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)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
  for j = 2:6
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_dvf{i}(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))), 'HandleVisibility', 'off');
  end
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');



% #+NAME: fig:act_damp_variability_dvf_sample_mass
% #+CAPTION: Variability of the DVF plant with the Spindle Angle ([[./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)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gm_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))));
  for j = 2:6
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gm_ine{i}(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
  end
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)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$M = %.0f$ [kg]', masses(i)));
  for j = 2:6
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_ine{i}(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))), 'HandleVisibility', 'off');
  end
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');

% Initialize the Simulation
% We initialize all the stages with the default parameters.

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();



% No disturbances.

initializeDisturbances('enable', false);



% The nano-hexapod is a piezoelectric hexapod.

initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 50);



% And all the controllers are set to 0.

K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_ine = tf(zeros(6));
save('./mat/controllers.mat', 'K_ine', '-append');
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
K_dvf = tf(zeros(6));
save('./mat/controllers.mat', 'K_dvf', '-append');

% Identification
% First, we identify the dynamics of the system using the =linearize= function.

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

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.1, 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', 'Ga_iff', 'Ga_dvf', 'Ga_ine', '-append');

% Plots

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

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ga)
  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)
  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');



% #+NAME: fig:act_damp_variability_iff_spindle_angle
% #+CAPTION: Variability of the IFF plant 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)
  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)
  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');



% #+NAME: fig:act_damp_variability_dvf_spindle_angle
% #+CAPTION: Variability of the DVF plant 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)
  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)
  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');

% Initialize the Simulation
% We initialize all the stages with the default parameters.

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();



% No disturbances.

initializeDisturbances('enable', false);



% The nano-hexapod is a piezoelectric hexapod.

initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 50);



% And all the controllers are set to 0.

K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_ine = tf(zeros(6));
save('./mat/controllers.mat', 'K_ine', '-append');
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
K_dvf = tf(zeros(6));
save('./mat/controllers.mat', 'K_dvf', '-append');

% Identification
% First, we identify the dynamics of the system using the =linearize= function.

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

Rz_periods = [60, 10, 1]; % [s]

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

  %% Run the linearization
  G = linearize(mdl, io, 0.5, 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', 'Gw_iff', 'Gw_dvf', 'Gw_ine', '-append');

% Plots

load('./active_damping/mat/plants_variable.mat', 'Gw_iff', 'Gw_dvf', 'Gw_ine');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gw)
  plot(freqs, abs(squeeze(freqresp(Gw_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(Gw)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gw_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [rpm]', 60/Rz_periods(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');



% #+NAME: fig:act_damp_variability_iff_spindle_speed
% #+CAPTION: Variability of the IFF plant with the Spindle Angle ([[./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]]


freqs = logspace(0, 3, 1000);

figure;

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

for i = 1:length(Gw)
  plot(freqs, abs(squeeze(freqresp(Gw_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(Gw)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gw_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [rpm]', 60/Rz_periods(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');



% #+NAME: fig:act_damp_variability_dvf_spindle_speed
% #+CAPTION: Variability of the DVF plant with the Spindle Angle ([[./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]]


freqs = logspace(0, 2, 5000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gw)
  plot(freqs, abs(squeeze(freqresp(Gw_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(Gw)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gw_ine{i}('Vnlm1', 'Fnl1'), freqs, 'Hz'))), 'DisplayName', sprintf('$Rz = %.0f$ [rpm]', 60/Rz_periods(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');

% Initialize the Simulation
% We initialize all the stages with the default parameters.

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();



% No disturbances.

initializeDisturbances('enable', false);



% The nano-hexapod is a piezoelectric hexapod.

initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 50);



% And all the controllers are set to 0.

K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_ine = tf(zeros(6));
save('./mat/controllers.mat', 'K_ine', '-append');
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
K_dvf = tf(zeros(6));
save('./mat/controllers.mat', 'K_dvf', '-append');

% Identification
% First, we identify the dynamics of the system using the =linearize= function.

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

Ry_amplitudes = [0, 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.1, 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', 'Gy_iff', 'Gy_dvf', 'Gy_ine', '-append');

% Plots

load('./active_damping/mat/plants_variable.mat', 'Gy_iff', 'Gy_dvf', 'Gy_ine');

freqs = logspace(0, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gy)
  plot(freqs, abs(squeeze(freqresp(Gy_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(Gy)
  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
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');



% #+NAME: fig:act_damp_variability_iff_tilt_angle
% #+CAPTION: Variability of the IFF plant with the Spindle 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)
  plot(freqs, abs(squeeze(freqresp(Gy_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(Gy)
  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
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');



% #+NAME: fig:act_damp_variability_dvf_tilt_angle
% #+CAPTION: Variability of the DVF plant with the Spindle 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)
  plot(freqs, abs(squeeze(freqresp(Gy_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(Gy)
  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
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');