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

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

%% Load computed requirements
load('instrumentation_requirements.mat')

%% Sensitivity to disturbances
load('instrumentation_sensitivity.mat', 'Gd');

%% ADC noise
adc = load("2023-08-23_15-42_io131_adc_noise.mat");

% Spectral Analysis parameters
Ts = 1e-4;
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% Identification of the transfer function from Va to di
[pxx, f] = pwelch(detrend(adc.adc_1, 0), win, Noverlap, Nfft, 1/Ts);

adc.pxx = pxx;
adc.f = f;

% estimated mean ASD
sprintf('Mean ASD of the ADC: %.1f uV/sqrt(Hz)', 1e6*sqrt(mean(adc.pxx)))
sprintf('Specifications: %.1f uV/sqrt(Hz)', 1e6*max_adc_asd)

% estimated RMS
sprintf('RMS of the ADC: %.2f mV RMS', 1e3*rms(detrend(adc.adc_1,0)))
sprintf('RMS specifications: %.2f mV RMS', max_adc_rms)

% Estimate quantization noise of the IO318 ADC
delta_V = 20; % +/-10 V
n = 16; % number of bits
Fs = 10e3; % [Hz]

adc.q = delta_V/2^n; % Quantization in [V]
adc.q_psd = adc.q^2/12/Fs; % Quantization noise Power Spectral Density [V^2/Hz]
adc.q_asd = sqrt(adc.q_psd); % Quantization noise Amplitude Spectral Density [V/sqrt(Hz)]

%% Measured ADC noise (IO318)
figure;
hold on;
plot(adc.f, sqrt(adc.pxx), 'color', colors(3,:), 'DisplayName', '$\Gamma_{q_{ad}}$')
plot([adc.f(2), adc.f(end)], [max_adc_asd, max_adc_asd], '--', 'color', colors(3,:), 'DisplayName', 'Specs')
plot([adc.f(2), adc.f(end)], [adc.q_asd, adc.q_asd], 'k--', 'DisplayName', 'Quantization noise (16 bits, $\pm 10\,V$)')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [V/$\sqrt{Hz}$]');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
ylim([1e-10, 4e-4]); xlim([1, 5e3]);
xticks([1e0, 1e1, 1e2, 1e3])

%% Read force sensor voltage with the ADC
load('force_sensor_steps.mat', 't', 'encoder', 'u', 'v');

% Exponential fit to compute the time constant
% Fit function
f_exp = @(b,x) b(1).*exp(-b(2).*x) + b(3);

% Three steps are performed at the following time intervals:
t_s = [ 2.5, 23;
       23.8, 35;
       35.8, 50];

tau = zeros(size(t_s, 1),1); % Time constant [s]
V0  = zeros(size(t_s, 1),1); % Offset voltage [V]
a   = zeros(size(t_s, 1),1); %

for t_i = 1:size(t_s, 1)
    t_cur = t(t_s(t_i, 1) < t & t < t_s(t_i, 2));
    t_cur = t_cur - t_cur(1);
    y_cur = v(t_s(t_i, 1) < t & t < t_s(t_i, 2));

    nrmrsd = @(b) norm(y_cur - f_exp(b,t_cur)); % Residual Norm Cost Function
    B0 = [0.5, 0.15, 2.2];                        % Choose Appropriate Initial Estimates
    [B,rnrm] = fminsearch(nrmrsd, B0);     % Estimate Parameters ‘B’

    a(t_i)   = B(1);
    tau(t_i) = 1/B(2);
    V0(t_i)  = B(3);
end

% Data to show the exponential fit
t_fit_1 = linspace(t_s(1,1), t_s(1,2), 100);
y_fit_1 = f_exp([a(1),1/tau(1),V0(1)], t_fit_1-t_s(1,1));

t_fit_2 = linspace(t_s(2,1), t_s(2,2), 100);
y_fit_2 = f_exp([a(2),1/tau(2),V0(2)], t_fit_2-t_s(2,1));

t_fit_3 = linspace(t_s(3,1), t_s(3,2), 100);
y_fit_3 = f_exp([a(3),1/tau(3),V0(3)], t_fit_3-t_s(3,1));

% Speedgoat ADC input impedance
Cp = 4.4e-6; % [F]
Rin = abs(mean(tau))/Cp; % [Ohm]

% Estimated input bias current
in = mean(V0)/Rin; % [A]

% Resistor added in parallel to the force sensor
fc = 0.5; % Wanted corner frequency [Hz]
Ra = Rin/(2*pi*fc*Cp*Rin - 1); % [Ohm]

% New ADC offset voltage
V_offset = Ra*Rin/(Ra + Rin) * in; % [V]

%% Measured voltage accross the sensor stacks - Voltage steps are applied to the actuators
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(t, u, 'DisplayName', '$u$');
plot(t, v, 'DisplayName', '$V_s$');
plot(t_fit_1, y_fit_1, 'k--', 'DisplayName', 'fit');
plot(t_fit_2, y_fit_2, 'k--', 'HandleVisibility', 'off');
plot(t_fit_3, y_fit_3, 'k--', 'HandleVisibility', 'off');
hold off;
xlabel('Time [s]'); ylabel('Voltage [V]');
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([0, 20]);

%% Read force sensor voltage with the ADC with added 82.7kOhm resistor
load('force_sensor_steps_R_82k7.mat', 't', 'encoder', 'u', 'v');

% Step times
t_s = [1.9,  6;
       8.5, 13;
      15.5, 21;
      22.6, 26;
      30.0, 36;
      37.5, 41;
      46.2, 49.5]; % [s]

tau = zeros(size(t_s, 1),1); % Time constant [s]
V0  = zeros(size(t_s, 1),1); % Offset voltage [V]
a   = zeros(size(t_s, 1),1); %

for t_i = 1:size(t_s, 1)
    t_cur = t(t_s(t_i, 1) < t & t < t_s(t_i, 2));
    t_cur = t_cur - t_cur(1);
    y_cur = v(t_s(t_i, 1) < t & t < t_s(t_i, 2));

    nrmrsd = @(b) norm(y_cur - f_exp(b,t_cur)); % Residual Norm Cost Function
    B0 = [0.5, 0.1, 2.2];                        % Choose Appropriate Initial Estimates
    [B,rnrm] = fminsearch(nrmrsd, B0);     % Estimate Parameters ‘B’

    a(t_i)   = B(1);
    tau(t_i) = 1/B(2);
    V0(t_i)  = B(3);
end

% Data to show the exponential fit
t_fit_1 = linspace(t_s(1,1), t_s(1,2), 100);
y_fit_1 = f_exp([a(1),1/tau(1),V0(1)], t_fit_1-t_s(1,1));

t_fit_2 = linspace(t_s(2,1), t_s(2,2), 100);
y_fit_2 = f_exp([a(2),1/tau(2),V0(2)], t_fit_2-t_s(2,1));

t_fit_3 = linspace(t_s(3,1), t_s(3,2), 100);
y_fit_3 = f_exp([a(3),1/tau(3),V0(3)], t_fit_3-t_s(3,1));

%% Measured voltage accross the sensor stacks - Voltage steps are applied to the actuators
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(t, u, 'DisplayName', '$u$');
plot(t, v, 'DisplayName', '$V_s$');
plot(t_fit_1, y_fit_1, 'k--', 'DisplayName', 'fit');
plot(t_fit_2, y_fit_2, 'k--', 'HandleVisibility', 'off');
plot(t_fit_3, y_fit_3, 'k--', 'HandleVisibility', 'off');
hold off;
xlabel('Time [s]'); ylabel('Voltage [V]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([0, 20]);

%% Femto Input Voltage Noise
femto = load('noise_femto.mat', 't', 'Vout', 'notes'); % Load Data

% Compute the equivalent voltage at the input of the amplifier
femto.Vout = femto.Vout/femto.notes.pre_amp.gain;
femto.Vout = femto.Vout - mean(femto.Vout);

Ts = (femto.t(end) - femto.t(1))/(length(femto.t) - 1);
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% Power Spectral Density
[pxx, f] = pwelch(detrend(femto.Vout, 0), win, Noverlap, Nfft, 1/Ts);

% Save the results inside the struct
femto.pxx = pxx(f<=5e3);
femto.f = f(f<=5e3);

%% Measured input voltage noise of the Femto voltage pre-amplifier
figure;
hold on;
plot(femto.f, sqrt(femto.pxx), 'color', colors(5,:), 'DisplayName', '$\Gamma_{n_a}$');
plot(adc.f, sqrt(adc.pxx)./femto.notes.pre_amp.gain, 'color', colors(3,:), 'DisplayName', '$\Gamma_{q_{ad}}/|G_a|$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$V/\sqrt{Hz}$]');
legend('location', 'northeast');
xlim([1, 5e3]); ylim([2e-10, 1e-7]);
xticks([1e0, 1e1, 1e2, 1e3]);
yticks([1e-9, 1e-8]);

%% DAC Output Voltage Noise
dac = load('mat/noise_dac.mat', 't', 'Vn', 'notes');

% Take input acount the gain of the pre-amplifier
dac.Vn = dac.Vn/dac.notes.pre_amp.gain;
dac.Vn = dac.Vn - mean(dac.Vn);

Ts = (dac.t(end) - dac.t(1))/(length(dac.t) - 1);
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
% Identification of the transfer function from Va to di
[pxx, f] = pwelch(dac.Vn, win, Noverlap, Nfft, 1/Ts);

dac.pxx = pxx(f<=5e3);
dac.f = f(f<=5e3);

% Estimated mean ASD
sprintf('Mean ASD of the DAC: %.1f uV/sqrt(Hz)', 1e6*sqrt(mean(dac.pxx)))
sprintf('Specifications: %.1f uV/sqrt(Hz)', 1e6*max_dac_asd)

% Estimated RMS
sprintf('RMS of the DAC: %.2f mV RMS', 1e3*rms(dac.Vn))
sprintf('RMS specifications: %.2f mV RMS', max_dac_rms)

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(femto.f, sqrt(femto.pxx), 'color', colors(5,:), 'DisplayName', '$\Gamma_{n_a}$');
plot(dac.f, sqrt(dac.pxx), 'color', colors(1,:), 'DisplayName', '$\Gamma_{n_{da}}$');
plot([dac.f(2), dac.f(end)], [max_dac_asd, max_dac_asd], '--', 'color', colors(1,:), 'DisplayName', 'DAC specs')
plot(adc.f, sqrt(adc.pxx)./dac.notes.pre_amp.gain, 'color', colors(3,:), 'DisplayName', '$\Gamma_{q_{ad}}/|G_a|$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$V/\sqrt{Hz}$]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 5e3]); ylim([2e-10, 4e-4]);
xticks([1e0, 1e1, 1e2, 1e3]);

%% Measure transfer function from DAC to ADC
data_dac_adc = load("2023-08-22_15-52_io131_dac_to_adc.mat");

% Frequency analysis parameters
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

[G_dac_adc, f] = tfestimate(data_dac_adc.dac_1, data_dac_adc.adc_1, win, Noverlap, Nfft, 1/Ts);

%
G_delay = exp(-Ts*s);

%% Measure transfer function from DAC to ADC - It fits a pure "1-sample" delay
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_dac_adc), 'color', colors(2,:), 'DisplayName', 'Measurement');
plot(f, abs(squeeze(freqresp(G_delay, f, 'Hz'))), 'k--', 'DisplayName', 'Pure Delay');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-1, 1e1]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile();
hold on;
plot(f, 180/pi*unwrap(angle(G_dac_adc)), 'color', colors(2,:));
plot(f, 180/pi*unwrap(angle(squeeze(freqresp(G_delay, f, 'Hz')))), 'k--', 'DisplayName', 'Pure Delay');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-200, 20])

linkaxes([ax1,ax2],'x');
xlim([1, 5e3]);
xticks([1e0, 1e1, 1e2, 1e3]);

%% PD200 Output Voltage Noise
% Load all the measurements
pd200 = {};
for i = 1:6
    pd200(i) = {load(['mat/noise_PD200_' num2str(i) '_10uF.mat'], 't', 'Vout', 'notes')};
end

% Take into account the pre-amplifier gain
for i = 1:6
    pd200{i}.Vout = pd200{i}.Vout/pd200{i}.notes.pre_amp.gain;
end

% Sampling time / frequency
Ts = (pd200{1}.t(end) - pd200{1}.t(1))/(length(pd200{1}.t) - 1);

% Compute the PSD of the measured noise
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

for i = 1:6
    % Identification of the transfer function from Va to di
    [pxx, f] = pwelch(pd200{i}.Vout, win, Noverlap, Nfft, 1/Ts);
    pd200{i}.pxx = pxx(f<=5e3);
    pd200{i}.f = f(f<=5e3);
end

% Estimated RMS
sprintf('RMS of the PD200: %.2f mV RMS', 1e3*rms(detrend(pd200{1}.Vout,0)))
sprintf('RMS specifications: %.2f mV RMS', max_amp_rms)

%% Measured output voltage noise of the PD200 amplifiers
figure;
hold on;
plot([1 Fs/2], [max_amp_asd, max_amp_asd], '--', 'color', colors(2,:), 'DisplayName', 'Specs')
plot(pd200{1}.f, sqrt(pd200{1}.pxx), 'color', [colors(2, :), 0.5], 'DisplayName', '$\Gamma_{n_p}$');
for i = 2:6
    plot(pd200{i}.f, sqrt(pd200{i}.pxx), 'color', [colors(2, :), 0.5], 'HandleVisibility', 'off');
end
plot(femto.f, sqrt(femto.pxx), 'color', [colors(5, :)], 'DisplayName', '$\Gamma_{n_a}$');
plot(adc.f, sqrt(adc.pxx)./pd200{1}.notes.pre_amp.gain, 'color', colors(3,:), 'DisplayName', '$\Gamma_{q_{ad}}/|G_a|$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$V/\sqrt{Hz}$]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 4e-4]); xlim([1, 5e3]);
xticks([1e0, 1e1, 1e2, 1e3])

%% Load all the measurements
pd200_tf = {};
for i = 1:6
    pd200_tf(i) = {load(['tf_pd200_' num2str(i) '_10uF_small_signal.mat'], 't', 'Vin', 'Vout', 'notes')};
end

% Compute sampling Frequency
Ts = (pd200_tf{1}.t(end) - pd200_tf{1}.t(1))/(length(pd200_tf{1}.t)-1);

% Compute all the transfer functions
Nfft = floor(1.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

for i = 1:length(pd200_tf)
    [tf_est, f] = tfestimate(pd200_tf{i}.Vin, 20*pd200_tf{i}.Vout, win, Noverlap, Nfft, 1/Ts);
    pd200_tf{i}.tf = tf_est(f<=5e3);
    pd200_tf{i}.f = f(f<=5e3);
end

% Amplified model
Gp = 20/(1 + s/2/pi/25e3);

figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(pd200_tf{1}.f, abs(pd200_tf{1}.tf), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'Measurement')
plot(pd200_tf{1}.f, abs(squeeze(freqresp(Gp, pd200_tf{1}.f, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '$1^{st}$ order LPF')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(pd200_tf{1}.f, 180/pi*unwrap(angle(pd200_tf{1}.tf)), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5)
plot(pd200_tf{1}.f, 180/pi*unwrap(angle(squeeze(freqresp(Gp, pd200_tf{1}.f, 'Hz')))), '--', 'color', colors(2,:))
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:2:360);
ylim([-13, 1]);

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

%% Load all the measurements
enc = {};
for i = 1:6
    enc(i) = {load(['mat/noise_meas_100s_20kHz_' num2str(i) '.mat'], 't', 'x')};
end

% Compute sampling Frequency
Ts = (enc{1}.t(end) - enc{1}.t(1))/(length(enc{1}.t)-1);
Nfft = floor(1.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

for i = 1:length(enc)
    [pxx, f] = pwelch(detrend(enc{i}.x, 0), win, Noverlap, Nfft, 1/Ts);
    enc{i}.pxx = pxx(f<=5e3);
    enc{i}.pxx(2) = enc{i}.pxx(3); % Remove first point which corresponds to drifts
    enc{i}.f = f(f<=5e3);
end

%% Measured Amplitude Spectral Density of the encoder position noise
figure;
hold on;
plot(enc{1}.f, sqrt(enc{1}.pxx), 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
xlim([1, 5e3]); ylim([1e-12, 1e-8]);

%% Estimate the resulting errors induced by noise of instruments
f = dac.f;

% Vertical direction
psd_z_dac = 6*(abs(squeeze(freqresp(Gd('z', 'nda1' ), f, 'Hz'))).^2).*dac.pxx;
psd_z_adc = 6*(abs(squeeze(freqresp(Gd('z', 'nad1' ), f, 'Hz'))).^2).*adc.pxx;
psd_z_amp = 6*(abs(squeeze(freqresp(Gd('z', 'namp1'), f, 'Hz'))).^2).*pd200{1}.pxx;
psd_z_enc = 6*(abs(squeeze(freqresp(Gd('z', 'ddL1' ), f, 'Hz'))).^2).*enc{1}.pxx;
psd_z_tot = psd_z_dac + psd_z_adc + psd_z_amp + psd_z_enc;

rms_z_dac = sqrt(trapz(f, psd_z_dac));
rms_z_adc = sqrt(trapz(f, psd_z_adc));
rms_z_amp = sqrt(trapz(f, psd_z_amp));
rms_z_enc = sqrt(trapz(f, psd_z_enc));
rms_z_tot = sqrt(trapz(f, psd_z_tot));

% Lateral direction
psd_y_dac = 6*(abs(squeeze(freqresp(Gd('y', 'nda1' ), f, 'Hz'))).^2).*dac.pxx;
psd_y_adc = 6*(abs(squeeze(freqresp(Gd('y', 'nad1' ), f, 'Hz'))).^2).*adc.pxx;
psd_y_amp = 6*(abs(squeeze(freqresp(Gd('y', 'namp1'), f, 'Hz'))).^2).*pd200{1}.pxx;
psd_y_enc = 6*(abs(squeeze(freqresp(Gd('y', 'ddL1' ), f, 'Hz'))).^2).*enc{1}.pxx;
psd_y_tot = psd_y_dac + psd_y_adc + psd_y_amp + psd_y_enc;

rms_y_tot = sqrt(trapz(f, psd_y_tot));

%% Closed-loop noise budgeting using measured noise of instrumentation
figure;
hold on;
plot(f, sqrt(psd_z_amp), 'color', [colors(2,:)], 'linewidth', 2.5, 'DisplayName', 'PD200');
plot(f, sqrt(psd_z_dac), 'color', [colors(1,:)], 'linewidth', 2.5, 'DisplayName', 'DAC')
plot(f, sqrt(psd_z_adc), 'color', [colors(3,:)], 'linewidth', 2.5, 'DisplayName', 'ADC')
plot(f, sqrt(psd_z_tot), 'k-', 'DisplayName', sprintf('Total: %.1f nm RMS', 1e9*rms_z_tot));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 5e3]); ylim([1e-14, 1e-9]);
xticks([1e0, 1e1, 1e2, 1e3]);