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

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

%% Path for functions, data and scripts
addpath('./src/'); % Path for scripts
addpath('./mat/'); % Path for data
addpath('./STEPS/'); % Path for Simscape Model
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;

%% Open Simscape Model
mdl = 'detail_instrumentation_nass'; % Name of the Simulink File
open(mdl); % Open Simscape Model

%% Colors for the figures
colors = colororder;
freqs = logspace(1,4,1000); % Frequency vector [Hz]

%% Identify the transfer functions from disturbance sources to vertical position error
% Let's initialize all the stages with default parameters.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSample('m', 1);
initializeSimplifiedNanoHexapod();

initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'hac-iff');
initializeReferences();

% Decentralized IFF controller
wz = 2*pi*2;
xiz = 0.7;
Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1);

Kiff = -200 * ...              % Gain
       1/(0.01*2*pi + s) * ... % LPF: provides integral action
       Ghpf * ...              % 2nd order HPF (limit low frequency gain)
       eye(6);                 % Diagonal 6x6 controller (i.e. decentralized)

% Centralized HAC
wc = 2*pi*10; % Wanted crossover [rad/s]
H_int = wc/s; % Integrator
a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); % Lead to increase phase margin
H_lpf = 1/(1 + s/2/pi/80); % Low Pass filter to increase robustness

Khac = -5e4    * ... % Gain
        H_int  * ... % Integrator
        H_lead * ... % Low Pass filter
        H_lpf  * ... % Low Pass filter
        eye(6);      % 6x6 Diagonal

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/dac_noise'],      1, 'input'); io_i = io_i + 1; % DAC noise [V]
io(io_i) = linio([mdl, '/amp_noise'],      1, 'input'); io_i = io_i + 1; % Voltage Amplifier noise [V]
io(io_i) = linio([mdl, '/NASS/adc_noise'], 1, 'input'); io_i = io_i + 1; % ADC noise [V]
io(io_i) = linio([mdl, '/NASS/enc_noise'], 1, 'input'); io_i = io_i + 1; % Encoder noise [m]
io(io_i) = linio([mdl, '/NASS'],           2, 'output', [], 'z');  io_i = io_i + 1; % Vertical error [m]
io(io_i) = linio([mdl, '/NASS'],           2, 'output', [], 'y');  io_i = io_i + 1; % Lateral error [m]

Gd = linearize(mdl, io);
Gd.InputName  = {...
    'nda1',  'nda2',  'nda3',  'nda4',  'nda5',  'nda6',  ... % DAC and Voltage amplifier noise
    'namp1',  'namp2',  'namp3',  'namp4',  'namp5',  'namp6',  ... % DAC and Voltage amplifier noise
    'nad1', 'nad2', 'nad3', 'nad4', 'nad5', 'nad6', ... % ADC noise
    'ddL1', 'ddL2', 'ddL3', 'ddL4', 'ddL5', 'ddL6'  ... % Encoder noise
};
Gd.OutputName = {'y', 'z'}; % Vertical error of the sample

%% Save Requirements
save('./mat/instrumentation_sensitivity.mat', 'Gd');

%% Transfer function from noise sources to vertical motion errors
freqs = logspace(0, 3, 1000);

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(Gd('z', 'nda1' ), freqs, 'Hz'))), 'DisplayName', '$\epsilon_z/n_{da}$');
plot(freqs, abs(squeeze(freqresp(Gd('z', 'namp1'), freqs, 'Hz'))), 'DisplayName', '$\epsilon_z/n_{amp}$');
plot(freqs, abs(squeeze(freqresp(Gd('z', 'nad1' ), freqs, 'Hz'))), 'DisplayName', '$\epsilon_z/n_{ad}$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Sensitivity [m/V]');
ylim([1e-9, 1e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e3]);

% Maximum wanted effect of each noise source on the vertical error
% Specifications: 15nm RMS
% divide by sqrt(6) because 6 struts
% divide by sqrt(3) because 3 considered noise sources
max_asd_z = 15e-9 / sqrt(6) / sqrt(3); % [m/sqrt(Hz)]

% Suppose unitary flat noise ASD => compute the effect on vertical noise
unit_asd = ones(1, length(freqs));
rms_unit_asd_dac = sqrt(sum((unit_asd.*abs(squeeze(freqresp(Gd('z', 'nda1' ), freqs, 'Hz'))).').^2));
rms_unit_asd_amp = sqrt(sum((unit_asd.*abs(squeeze(freqresp(Gd('z', 'namp1'), freqs, 'Hz'))).').^2));
rms_unit_asd_adc = sqrt(sum((unit_asd.*abs(squeeze(freqresp(Gd('z', 'nad1' ), freqs, 'Hz'))).').^2));

% Obtained maximum ASD for different instruments
max_dac_asd = max_asd_z./rms_unit_asd_dac; % [V/sqrt(Hz)]
max_amp_asd = max_asd_z./rms_unit_asd_amp; % [V/sqrt(Hz)]
max_adc_asd = max_asd_z./rms_unit_asd_adc; % [V/sqrt(Hz)]

% Estimation of the equivalent RMS noise
max_dac_rms = 1e3*max_dac_asd*sqrt(5e3) % [mV RMS]
max_amp_rms = 1e3*max_amp_asd*sqrt(5e3) % [mV RMS]
max_adc_rms = 1e3*max_adc_asd*sqrt(5e3) % [mV RMS]

%% Save Requirements
save('./mat/instrumentation_requirements.mat', ...
     'max_dac_asd', 'max_amp_asd', 'max_adc_asd', ...
     'max_dac_rms', 'max_amp_rms', 'max_adc_rms');