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

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

% Effect of the Sercalo angle error on the measured distance by the Attocube
% <<sec:sercalo_angle_error>>
% To simplify, we suppose that the Newport mirror is a flat mirror (instead of a concave one).

% The geometry of the setup is shown in Fig. [[fig:angle_error_schematic_sercalo]] where:
% - $O$ is the reference surface of the Attocube
% - $S$ is the point where the beam first hits the Sercalo mirror
% - $X$ is the point where the beam first hits the Newport mirror
% - $\delta \theta_c$ is the angle error from its ideal 45 degrees

% We define the angle error range $\delta \theta_c$ where we want to evaluate the distance error $\delta L$ measured by the Attocube.

thetas_c = logspace(-7, -4, 100); % [rad]



% The geometrical parameters of the setup are defined below.

H = 0.05; % [m]
L = 0.05; % [m]



% #+NAME: fig:angle_error_schematic_sercalo
% #+CAPTION: Schematic of the geometry used to evaluate the effect of $\delta \theta_c$ on the measured distance $\delta L$
% #+RESULTS:
% [[file:figs/angle_error_schematic_sercalo.png]]

% The nominal points $O$, $S$ and $X$ are defined.

O = [-L, 0];
S = [0, 0];
X = [0, H];



% Thus, the initial path length $L$ is:

path_nominal = norm(S-O) + norm(X-S) + norm(S-X) + norm(O-S);



% We now compute the new path length when there is an error angle $\delta \theta_c$ on the Sercalo mirror angle.

path_length = zeros(size(thetas_c));

for i = 1:length(thetas_c)
    theta_c = thetas_c(i);
    Y = [H*tan(2*theta_c), H];
    M = 2*H/(tan(pi/4-theta_c)+1/tan(2*theta_c))*[1, tan(pi/4-theta_c)];
    T = [-L, M(2)+(L+M(1))*tan(4*theta_c)];

    path_length(i) = norm(S-O) + norm(Y-S) + norm(M-Y) + norm(T-M);
end



% We then compute the distance error and we plot it as a function of the Sercalo angle error (Fig. [[fig:effect_sercalo_angle_distance_meas]]).

path_error = path_length - path_nominal;

figure;
plot(thetas_c, path_error)
set(gca,'xscale','log');
set(gca,'yscale','log');
xlabel('Sercalo angle error [rad]');
ylabel('Attocube measurement error [m]');



% #+NAME: fig:effect_sercalo_angle_distance_meas
% #+CAPTION: Effect of an angle error of the Sercalo on the distance error measured by the Attocube ([[./figs/effect_sercalo_angle_distance_meas.png][png]], [[./figs/effect_sercalo_angle_distance_meas.pdf][pdf]])
% [[file:figs/effect_sercalo_angle_distance_meas.png]]

% And we plot the beam path using Matlab for an high angle to verify that the code is working (Fig. [[fig:simulation_beam_path_high_angle]]).

theta = 2*2*pi/360; % [rad]
H = 0.05; % [m]
L = 0.05; % [m]

O = [-L, 0];
S = [0, 0];
X = [0, H];
Y = [H*tan(2*theta), H];
M = 2*H/(tan(pi/4-theta)+1/tan(2*theta))*[1, tan(pi/4-theta)];
T = [-L, M(2)+(L+M(1))*tan(4*theta)];

figure;
hold on;
plot([-L, -L], [0, H], 'k-'); % Interferometer
plot([-L, 0.1*L], [H, H], 'k-'); % Reflector
plot(0.5*min(L, H)*[-cos(pi/4-theta), cos(pi/4-theta)], 0.5*min(L, H)*[-sin(pi/4-theta), sin(pi/4-theta)], 'k-'); % Tilt-Mirror
plot(0.5*min(L, H)*[-cos(pi/4), cos(pi/4)], 0.5*min(L, H)*[-sin(pi/4), sin(pi/4)], 'k--'); % Initial position of tilt mirror
plot([O(1), S(1), Y(1), M(1), T(1)], [O(2), S(2), Y(2), M(2), T(2)], 'r-');
plot([O(1), S(1), X(1), S(1), O(1)], [O(2), S(2), X(2), S(2), O(2)], 'b--');
hold off;
xlabel('X [m]'); ylabel('Y [m]');
axis equal

% Error due to DAC noise used for the Sercalo

load('./mat/plant.mat', 'Gi', 'Gc', 'Gd');

G = inv(Gd)*Gc*Gi;



% Dynamical estimation:
% - ASD of DAC noise used for the Sercalo
% - Multiply by transfer function from Sercalo voltage to angle estimation using the 4QD


freqs = logspace(1, 3, 1000);

fs = 1e4;

% ASD of the DAC voltage going to the Sercalo in [V/sqrt(Hz)]
asd_uc = (20/2^16)/sqrt(12*fs)*ones(length(freqs), 1);

% ASD of the measured angle by the QD in [rad/sqrt(Hz)]
asd_theta = asd_uc.*abs(squeeze(freqresp(G(1,1), freqs, 'Hz')));

figure;
loglog(freqs, asd_theta)



% Then the corresponding ASD of the measured displacement by the interferometer is:

asd_L = asd_theta*10^(-6); % [m/sqrt(Hz)]



% And we integrate that to have the RMS value:

cps_L = 1/pi*cumtrapz(2*pi*freqs, (asd_L).^2);



% The RMS value is:

sqrt(cps_L(end))



% #+RESULTS:
% : 1.647e-11


figure;
loglog(freqs, cps_L)



% Let's estimate the beam angle error corresponding to 1 LSB of the sercalo's DAC.
% Gain of the Sercalo is approximatively 5 degrees for 10V.
% However the beam angle deviation is 4 times the angle deviation of the sercalo mirror, thus:

d_alpha = 4*(20/2^16)*(5*pi/180)/10 % [rad]



% #+RESULTS:
% : 1.0653e-05

% This corresponds to a measurement error of the Attocube equals to (in [m])

1e-6*d_alpha % [m]

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

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

% Coprime Factorization

load('mat/plant.mat', 'sys', 'Gi', 'Zc', 'Ga', 'Gc', 'Gn', 'Gd');

[fact, Ml, Nl] = lncf(Gc*Gi);

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

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

% Load Plant

load('mat/plant.mat', 'G');

% RGA-Number

freqs = logspace(2, 4, 1000);
G_resp = freqresp(G, freqs, 'Hz');
A = zeros(size(G_resp));
RGAnum = zeros(1, length(freqs));

for i = 1:length(freqs)
  A(:, :, i) = G_resp(:, :, i).*inv(G_resp(:, :, i))';
  RGAnum(i) = sum(sum(abs(A(:, :, i)-eye(2))));
end
% RGA = G0.*inv(G0)';

figure;
plot(freqs, RGAnum);
set(gca, 'xscale', 'log');

U = zeros(2, 2, length(freqs));
S = zeros(2, 2, length(freqs))
V = zeros(2, 2, length(freqs));

for i = 1:length(freqs)
  [Ui, Si, Vi] = svd(G_resp(:, :, i));
  U(:, :, i) = Ui;
  S(:, :, i) = Si;
  V(:, :, i) = Vi;
end

% Rotation Matrix

G0 = freqresp(G, 0);

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

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

freqs = logspace(0, 2, 1000);

% Load Plant

load('mat/plant.mat', 'Gn', 'Gd');

% Analysis
% The plant is basically a constant until frequencies up to the required bandwidth.

% We get that constant value.

Gn0 = freqresp(inv(Gd)*Gn, 0);



% We design two controller containing 2 integrators and one lead near the crossover frequency set to 10Hz.

h = 2;
w0 = 2*pi*10;

Knh = 1/Gn0(1,1) * (w0/s)^2 * (1 + s/w0*h)/(1 + s/w0/h)/h;
Knv = 1/Gn0(2,2) * (w0/s)^2 * (1 + s/w0*h)/(1 + s/w0/h)/h;

figure;

hold on;
plot(freqs, abs(squeeze(freqresp(Gn0(1,1)*Knh, freqs, 'Hz'))))
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Loop Gain');

% Save

Kn = blkdiag(Knh, Knv);
Knd = c2d(Kn, 1e-4, 'tustin');




% The controllers can be downloaded [[./mat/K_newport.mat][here]].


save('mat/K_newport.mat', 'Kn', 'Knd');

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

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

fs = 1e4;

% Data Load and pre-processing

uh = load('mat/data_rep_h.mat', ...
     't', 'Uch', 'Ucv', ...
     'Unh', 'Unv', ...
     'Vph', 'Vpv', ...
     'Vnh', 'Vnv', ...
     'Va');

uv = load('mat/data_rep_v.mat', ...
     't', 'Uch', 'Ucv', ...
     'Unh', 'Unv', ...
     'Vph', 'Vpv', ...
     'Vnh', 'Vnv', ...
     'Va');

% Let's start one second after the first command in the system
i0 = find(uh.Unh ~= 0, 1) + fs;
iend = i0+fs*floor((length(uh.t)-i0)/fs);

uh.Uch([1:i0-1, iend:end]) = [];
uh.Ucv([1:i0-1, iend:end]) = [];
uh.Unh([1:i0-1, iend:end]) = [];
uh.Unv([1:i0-1, iend:end]) = [];
uh.Vph([1:i0-1, iend:end]) = [];
uh.Vpv([1:i0-1, iend:end]) = [];
uh.Vnh([1:i0-1, iend:end]) = [];
uh.Vnv([1:i0-1, iend:end]) = [];
uh.Va ([1:i0-1, iend:end]) = [];
uh.t  ([1:i0-1, iend:end]) = [];

% We reset the time t
uh.t = uh.t - uh.t(1);

% Let's start one second after the first command in the system
i0 = find(uv.Unv ~= 0, 1) + fs;
iend = i0+fs*floor((length(uv.t)-i0)/fs);

uv.Uch([1:i0-1, iend:end]) = [];
uv.Ucv([1:i0-1, iend:end]) = [];
uv.Unh([1:i0-1, iend:end]) = [];
uv.Unv([1:i0-1, iend:end]) = [];
uv.Vph([1:i0-1, iend:end]) = [];
uv.Vpv([1:i0-1, iend:end]) = [];
uv.Vnh([1:i0-1, iend:end]) = [];
uv.Vnv([1:i0-1, iend:end]) = [];
uv.Va ([1:i0-1, iend:end]) = [];
uv.t  ([1:i0-1, iend:end]) = [];

% We reset the time t
uv.t = uv.t - uv.t(1);

% Some Time domain plots

tend = 5; % [s]

figure;
ax1 = subplot(2, 2, 1);
hold on;
plot(uh.t(1:tend*fs), uh.Unh(1:tend*fs));
hold off;
xlabel('Time [s]'); ylabel('Voltage [V]');
title('Newport Tilt - Horizontal Direction');

ax3 = subplot(2, 2, 3);
hold on;
plot(uh.t(1:tend*fs), 1e9*uh.Va(1:tend*fs));
hold off;
xlabel('Time [s]'); ylabel('Distance [nm]');
title('Attocube - Horizontal Direction');

ax2 = subplot(2, 2, 2);
hold on;
plot(uv.t(1:tend*fs), uv.Unv(1:tend*fs));
hold off;
xlabel('Time [s]'); ylabel('Voltage [V]');
title('Newport Tilt - Vertical Direction');

ax4 = subplot(2, 2, 4);
hold on;
plot(uv.t(1:tend*fs), 1e9*uv.Va(1:tend*fs));
hold off;
xlabel('Time [s]'); ylabel('Distance [nm]');
title('Attocube - Vertical Direction');

linkaxes([ax1,ax2,ax3,ax4],'x');
linkaxes([ax1,ax2],'xy');
linkaxes([ax3,ax4],'xy');

% Verify Tracking Angle Error
% Let's verify that the positioning error of the beam is small and what could be the effect on the distance measured by the intereferometer.


load('./mat/plant.mat', 'Gd');

figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(uh.t(1:2*fs), 1e6*uh.Vph(1:2*fs)/freqresp(Gd(1,1), 0), 'DisplayName', '$\theta_{h}$');
plot(uh.t(1:2*fs), 1e6*uh.Vpv(1:2*fs)/freqresp(Gd(2,2), 0), 'DisplayName', '$\theta_{v}$');
hold off;
xlabel('Time [s]'); ylabel('$\theta$ [$\mu$ rad]');
title('Newport Tilt - Horizontal Direction');
legend();

ax2 = subplot(1, 2, 2);
hold on;
plot(uv.t(1:2*fs), 1e6*uv.Vph(1:2*fs)/freqresp(Gd(1,1), 0), 'DisplayName', '$\theta_{h}$');
plot(uv.t(1:2*fs), 1e6*uv.Vpv(1:2*fs)/freqresp(Gd(2,2), 0), 'DisplayName', '$\theta_{v}$');
hold off;
xlabel('Time [s]'); ylabel('$\theta$ [$\mu$ rad]');
title('Newport Tilt - Vertical Direction');
legend();

linkaxes([ax1,ax2],'xy');



% #+NAME: fig:repeat_tracking_errors
% #+CAPTION: Tracking errors during the repeatability measurement ([[./figs/repeat_tracking_errors.png][png]], [[./figs/repeat_tracking_errors.pdf][pdf]])
% [[file:figs/repeat_tracking_errors.png]]

% Let's compute the PSD of the error to see the frequency content.


[psd_UhRh, f] = pwelch(uh.Vph/freqresp(Gd(1,1), 0), hanning(ceil(1*fs)), [], [], fs);
[psd_UhRv, ~] = pwelch(uh.Vpv/freqresp(Gd(2,2), 0), hanning(ceil(1*fs)), [], [], fs);
[psd_UvRh, ~] = pwelch(uv.Vph/freqresp(Gd(1,1), 0), hanning(ceil(1*fs)), [], [], fs);
[psd_UvRv, ~] = pwelch(uv.Vpv/freqresp(Gd(2,2), 0), hanning(ceil(1*fs)), [], [], fs);

figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(f, sqrt(psd_UhRh), 'DisplayName', '$\Gamma_{\theta_h}$');
plot(f, sqrt(psd_UhRv), 'DisplayName', '$\Gamma_{\theta_v}$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{rad}{\sqrt{Hz}}\right]$')
legend('Location', 'southwest');
title('Newport Tilt - Horizontal Direction');

ax2 = subplot(1, 2, 2);
hold on;
plot(f, sqrt(psd_UvRh), 'DisplayName', '$\Gamma_{\theta_h}$');
plot(f, sqrt(psd_UvRv), 'DisplayName', '$\Gamma_{\theta_v}$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{rad}{\sqrt{Hz}}\right]$')
legend('Location', 'southwest');
title('Newport Tilt - Vertical Direction');

linkaxes([ax1,ax2],'xy');
xlim([1, 1000]);



% #+NAME: fig:psd_tracking_error_rad
% #+CAPTION: Power Spectral Density of the tracking errors ([[./figs/psd_tracking_error_rad.png][png]], [[./figs/psd_tracking_error_rad.pdf][pdf]])
% [[file:figs/psd_tracking_error_rad.png]]


% Let's convert that to errors in distance

% \[ \Delta L = L^\prime - L = \frac{L}{\cos(\alpha)} - L \approx \frac{L \alpha^2}{2} \]
% with
% - $L$ is the nominal distance traveled by the beam
% - $L^\prime$ is the distance traveled by the beam with an angle error
% - $\alpha$ is the angle error


L = 0.1; % [m]

[psd_UhLh, f] = pwelch(0.5*L*(uh.Vph/freqresp(Gd(1,1), 0)).^2, hanning(ceil(1*fs)), [], [], fs);
[psd_UhLv, ~] = pwelch(0.5*L*(uh.Vpv/freqresp(Gd(2,2), 0)).^2, hanning(ceil(1*fs)), [], [], fs);
[psd_UvLh, ~] = pwelch(0.5*L*(uv.Vph/freqresp(Gd(1,1), 0)).^2, hanning(ceil(1*fs)), [], [], fs);
[psd_UvLv, ~] = pwelch(0.5*L*(uv.Vpv/freqresp(Gd(2,2), 0)).^2, hanning(ceil(1*fs)), [], [], fs);



% Now, compare that with the PSD of the measured distance by the interferometer (Fig. [[fig:compare_tracking_error_attocube_meas]]).

[psd_Lh, f] = pwelch(uh.Va, hanning(ceil(1*fs)), [], [], fs);
[psd_Lv, ~] = pwelch(uv.Va, hanning(ceil(1*fs)), [], [], fs);

figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(f, sqrt(psd_UhLh), 'DisplayName', '$\Gamma_{L_h}$');
plot(f, sqrt(psd_UhLv), 'DisplayName', '$\Gamma_{L_v}$');
plot(f, sqrt(psd_Lh), '--k', 'DisplayName', '$\Gamma_{L_h}$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$')
legend('Location', 'southwest');
title('Newport Tilt - Horizontal Direction');

ax2 = subplot(1, 2, 2);
hold on;
plot(f, sqrt(psd_UvLh), 'DisplayName', '$\Gamma_{L_h}$');
plot(f, sqrt(psd_UvLv), 'DisplayName', '$\Gamma_{L_v}$');
plot(f, sqrt(psd_Lv), '--k', 'DisplayName', '$\Gamma_{L_h}$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$')
legend('Location', 'southwest');
title('Newport Tilt - Vertical Direction');

linkaxes([ax1,ax2],'xy');
xlim([1, 1000]);

% Processing
% First, we get the mean value as measured by the interferometer for each value of the Newport angle.

Vahm = mean(reshape(uh.Va, [fs floor(length(uh.t)/fs)]),2);
Unhm = mean(reshape(uh.Unh, [fs floor(length(uh.t)/fs)]),2);

Vavm = mean(reshape(uv.Va, [fs floor(length(uv.t)/fs)]),2);
Unvm = mean(reshape(uv.Unv, [fs floor(length(uv.t)/fs)]),2);



% #+RESULTS:
% | Va - Horizontal [nm rms] | Va - Vertical [nm rms] |
% |--------------------------+------------------------|
% |                     19.6 |                   13.9 |


figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(uh.Unh, uh.Va);
plot(Unhm, Vahm)
hold off;
xlabel('$V_{n,h}$ [V]'); ylabel('$V_a$ [m]');

ax2 = subplot(1, 2, 2);
hold on;
plot(uv.Unv, uv.Va);
plot(Unvm, Vavm)
hold off;
xlabel('$V_{n,v}$ [V]'); ylabel('$V_a$ [m]');

linkaxes([ax1,ax2],'xy');



% #+NAME: fig:repeat_plot_raw
% #+CAPTION: Repeatability of the measurement ([[./figs/repeat_plot_raw.png][png]], [[./figs/repeat_plot_raw.pdf][pdf]])
% [[file:figs/repeat_plot_raw.png]]



figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(uh.Unh, 1e9*(uh.Va - repmat(Vahm, length(uh.t)/length(Vahm),1)));
hold off;
xlabel('$V_{n,h}$ [V]'); ylabel('$V_a$ [nm]');

ax2 = subplot(1, 2, 2);
hold on;
plot(uv.Unv, 1e9*(uv.Va - repmat(Vavm, length(uv.t)/length(Vavm),1)));
hold off;
xlabel('$V_{n,v}$ [V]'); ylabel('$V_a$ [nm]');

linkaxes([ax1,ax2],'xy');
ylim([-100 100]);

% Analysis of the non-repeatable contributions
% Let's know try to determine where does the non-repeatability comes from.

% From the 4QD signal, we can compute the angle error of the beam and thus determine the corresponding displacement measured by the attocube.

% We then take the non-repeatable part of this displacement and we compare that with the total non-repeatability.

% We also plot the displacement measured during the huddle test.

% All the signals are shown on Fig. [[]].


Vphm = mean(reshape(uh.Vph/freqresp(Gd(1,1), 0), [fs floor(length(uh.t)/fs)]),2);
Unhm = mean(reshape(uh.Unh, [fs floor(length(uh.t)/fs)]),2);

Vpvm = mean(reshape(uv.Vpv/freqresp(Gd(2,2), 0), [fs floor(length(uv.t)/fs)]),2);
Unvm = mean(reshape(uv.Unv, [fs floor(length(uv.t)/fs)]),2);

% =Vaheq= is the equivalent measurement error in [m] due to error angle of the Sercalo.
Vaheq = uh.Vph/freqresp(Gd(1,1), 0) - repmat(Vphm, length(uh.t)/length(Vphm),1);
Vaveq = uv.Vpv/freqresp(Gd(2,2), 0) - repmat(Vpvm, length(uv.t)/length(Vpvm),1);

Vaheq = sign(Vaheq).*Vaheq.^2;
Vaveq = sign(Vaveq).*Vaveq.^2;

ht = load('./mat/data_huddle_test_3.mat', 't', 'Va');

htm = 1e9*ht.Va(1:length(Vaheq)) - repmat(mean(1e9*ht.Va(1:length(Vaheq))), length(uh.t)/length(Vaheq),1);

figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(uh.Unh, 1e9*(uh.Va - repmat(Vahm, length(uh.t)/length(Vahm),1)));
plot(uh.Unh, 1e9*ht.Va(1:length(Vaheq))-mean(1e9*ht.Va(1:length(Vaheq))));
plot(uh.Unh, 1e9*Vaheq);
hold off;
xlabel('$V_{n,h}$ [V]'); ylabel('$V_a$ [nm]');

ax2 = subplot(1, 2, 2);
hold on;
plot(uv.Unv, 1e9*(uv.Va - repmat(Vavm, length(uv.t)/length(Vavm),1)), 'DisplayName', 'Measured Non-Repeatability');
plot(uv.Unv, 1e9*ht.Va(1:length(Vaveq))-mean(1e9*ht.Va(1:length(Vaveq))), 'DisplayName', 'Huddle Test');
plot(uv.Unv, 1e9*Vaveq, 'DisplayName', 'Due to Sercalo Angle Error');
hold off;
xlabel('$V_{n,v}$ [V]'); ylabel('$V_a$ [nm]');
legend('location', 'northeast');

linkaxes([ax1,ax2],'xy');
ylim([-100 100]);