% Matlab Init                                              :noexport:ignore:

%% dcm_hac_iff.m
% Development of the HAC-IFF control strategy

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

%% Simscape Model - Nano Hexapod
addpath('./STEPS/')

%% Initialize Parameters for Simscape model
controller.type = 0; % Open Loop Control

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

%% Open Simulink Model
mdl = 'simscape_dcm';

open(mdl)

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(1, 3, 1000);

% System Identification
% Let's identify the damped plant.


%% Input/Output definition
clear io; io_i = 1;

%% Inputs
% Control Input {3x1} [N]
io(io_i) = linio([mdl, '/control_system'], 1, 'input');  io_i = io_i + 1;

%% Outputs
% Force Sensor {3x1} [m]
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;

%% Load DCM Kinematics and IFF controller
load('dcm_kinematics.mat');
load('Kiff.mat');

%% Identification of the damped plant with IFF
controller.type = 1; % IFF
G_dp = J_a_111*inv(J_s_111)*linearize(mdl, io);
G_dp.InputName  = {'u_ur',  'u_uh',  'u_d'};
G_dp.OutputName = {'d_ur',  'd_uh',  'd_d'};

%% Comparison of the damped and undamped plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '-', ...
     'DisplayName', 'd - IFF');
plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '-', ...
     'DisplayName', 'uh - IFF');
plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '-', ...
     'DisplayName', 'ur - IFF');
for i = 1:2
    for j = i+1:3
        plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-12, 1e-8]);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '-');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '-');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 0]);

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

% High Authority Controller
% Let's design a controller with a bandwidth of 100Hz.
% As the plant is well decoupled and well approximated by a constant at low frequency, the high authority controller can easily be designed with SISO loop shaping.


%% Controller design
wc = 2*pi*100; % Wanted crossover frequency [rad/s]
a = 2; % Lead parameter

Khac = diag(1./diag(abs(evalfr(G_dp, 1j*wc)))) * ... % Diagonal controller
        wc/s * ... % Integrator
        1/(sqrt(a))*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))) * ... % Lead
        1/(s^2/(4*wc)^2 + 2*s/(4*wc) + 1); % Low pass filter

%% Save the HAC controller
save('mat/Khac_iff.mat', 'Khac');

%% Loop Gain
L_hac_lac = G_dp * Khac;

%% Bode Plot of the Loop Gain
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(L_hac_lac(1,1), freqs, 'Hz'))), '-', ...
     'DisplayName', 'd');
plot(freqs, abs(squeeze(freqresp(L_hac_lac(2,2), freqs, 'Hz'))), '-', ...
     'DisplayName', 'uh');
plot(freqs, abs(squeeze(freqresp(L_hac_lac(3,3), freqs, 'Hz'))), '-', ...
     'DisplayName', 'ur');
for i = 1:2
    for j = i+1:3
        plot(freqs, abs(squeeze(freqresp(L_hac_lac(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-2, 1e1]);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(1,1), freqs, 'Hz'))), '-');
plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(2,2), freqs, 'Hz'))), '-');
plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(3,3), freqs, 'Hz'))), '-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 0]);

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



% #+name: fig:hac_iff_loop_gain_bode_plot
% #+caption: Bode Plot of the Loop gain for the High Authority Controller
% #+RESULTS:
% [[file:figs/hac_iff_loop_gain_bode_plot.png]]


%% Compute the Eigenvalues of the loop gain
Ldet = zeros(3, length(freqs));

Lmimo = squeeze(freqresp(L_hac_lac, freqs, 'Hz'));
for i_f = 1:length(freqs)
    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end



% As shown in the Root Locus plot in Figure [[fig:loci_hac_iff_fast_jack]], the closed loop system should be stable.


%% Plot of the eigenvalues of L in the complex plane
figure;
hold on;
% Angle used to draw the circles
theta = linspace(0, 2*pi, 100);
% Unit circle
plot(cos(theta), sin(theta), '--');
% Circle for module margin
plot(-1 + min(min(abs(Ldet + 1)))*cos(theta), min(min(abs(Ldet + 1)))*sin(theta), '--');

for i = 1:3
    plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.');
    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
end
% Unstable Point
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
xlabel('Real'); ylabel('Imag');
axis square;
xlim([-3, 1]); ylim([-2, 2]);

% Performances
% In order to estimate the performances of the HAC-IFF control strategy, the transfer function from motion errors of the stepper motors to the motion error of the crystal is identified both in open loop and with the HAC-IFF strategy.


%% Input/Output definition
clear io; io_i = 1;

%% Inputs
% Jack Motion Erros {3x1} [m]
io(io_i) = linio([mdl, '/stepper_errors'], 1, 'input');  io_i = io_i + 1;

%% Outputs
% Interferometer Output {3x1} [m]
io(io_i) = linio([mdl, '/DCM'], 1, 'output'); io_i = io_i + 1;

%% Identification of the transmissibility of errors in open-loop
controller.type = 0; % Open Loop
T_ol = inv(J_s_111)*linearize(mdl, io)*J_a_111;
T_ol.InputName  = {'e_dz',  'e_ry',  'e_rx'};
T_ol.OutputName = {'dx',  'ry',  'rx'};

%% Load DCM Kinematics and IFF controller
load('dcm_kinematics.mat');
load('Kiff.mat');

%% Identification of the transmissibility of errors with HAC-IFF
controller.type = 3; % IFF
T_hl = inv(J_s_111)*linearize(mdl, io)*J_a_111;
T_hl.InputName  = {'e_dz',  'e_ry',  'e_rx'};
T_hl.OutputName = {'dx',  'ry',  'rx'};



% #+RESULTS:
% : 1

% And both transmissibilities are compared in Figure [[fig:stepper_transmissibility_comp_ol_hac_iff]].


%% Transmissibility of stepper errors
f = logspace(0, 3, 1000);

figure;
hold on;
plot(f, abs(squeeze(freqresp(T_ol(1,1), f, 'Hz'))), '-', ...
     'DisplayName', '$d_z$ - OL');
plot(f, abs(squeeze(freqresp(T_ol(2,2), f, 'Hz'))), '-', ...
     'DisplayName', '$r_y$ - OL');
plot(f, abs(squeeze(freqresp(T_ol(3,3), f, 'Hz'))), '-', ...
     'DisplayName', '$r_x$ - OL');
set(gca,'ColorOrderIndex',1)
plot(f, abs(squeeze(freqresp(T_hl(1,1), f, 'Hz'))), '--', ...
     'DisplayName', '$d_z$ - HAC-IFF');
plot(f, abs(squeeze(freqresp(T_hl(2,2), f, 'Hz'))), '--', ...
     'DisplayName', '$r_y$ - HAC-IFF');
plot(f, abs(squeeze(freqresp(T_hl(3,3), f, 'Hz'))), '--', ...
     'DisplayName', '$r_x$ - HAC-IFF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Stepper transmissibility');
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-2, 1e2]);
xlim([f(1), f(end)]);

% Close Loop noise budget

%% Load disturbances
load('asd_noises_disturbances.mat');



% Let's compute the amplitude spectral density of the jack motion errors due to the sensor noise, the actuator noise and disturbances.


%% Computation of ASD of contribution of inputs to the closed-loop motion
% Error due to disturbances
asd_d = abs(squeeze(freqresp(Wd*(1/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
% Error due to actuator noise
asd_u = abs(squeeze(freqresp(Wu*(G_dp(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
% Error due to sensor noise
asd_n = abs(squeeze(freqresp(Wn*(G_dp(1,1)*Khac(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));



% The closed-loop ASD is then:

%% ASD of the closed-loop motion
asd_cl = sqrt(asd_d.^2 + asd_u.^2 + asd_n.^2);



% The obtained ASD are shown in Figure [[fig:close_loop_asd_noise_budget_hac_iff]].


%% Noise Budget (ASD)
f = logspace(-1, 3, 1000);

figure;
hold on;
plot(f, asd_n, 'DisplayName', '$n$');
plot(f, asd_u, 'DisplayName', '$d_u$');
plot(f, asd_d, 'DisplayName', '$d$');
plot(f, asd_cl, 'k--', 'DisplayName', '$y$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
xlim([f(1), f(end)]);
ylim([1e-16, 1e-8]);



% #+name: fig:close_loop_asd_noise_budget_hac_iff
% #+caption: Closed Loop noise budget
% #+RESULTS:
% [[file:figs/close_loop_asd_noise_budget_hac_iff.png]]

% Let's compare the open-loop and close-loop cases (Figure [[fig:cps_comp_ol_cl_hac_iff]]).

% Amplitude spectral density of the open loop motion errors [m/sqrt(Hz)]
asd_ol = abs(squeeze(freqresp(Wd, f, 'Hz')));

% CPS of open-loop motion [m^2]
cps_ol = flip(-cumtrapz(flip(f), flip(asd_ol.^2)));
% CPS of closed-loop motion [m^2]
cps_cl = flip(-cumtrapz(flip(f), flip(asd_cl.^2)));

%% Cumulative Power Spectrum - Motion error of fast jack
figure;
hold on;
plot(f, cps_ol, 'DisplayName', sprintf('OL, $\\epsilon_d = %.0f$ [nm,rms]', 1e9*sqrt(cps_ol(1))));
plot(f, cps_cl, 'DisplayName', sprintf('CL, $\\epsilon_d = %.0f$ [nm,rms]', 1e9*sqrt(cps_cl(1))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('CPS [$m^2$]');
legend('location', 'southwest', 'FontSize', 8);
xlim([f(1), f(end)]);
% ylim([1e-16, 1e-8]);