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

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

addpath('./mat/');

% Load Data

ht_1 = load('./mat/data_huddle_test_1.mat', 't', 'Vph', 'Vpv', 'Va');
ht_2 = load('./mat/data_huddle_test_2.mat', 't', 'Vph', 'Vpv', 'Va');
ht_3 = load('./mat/data_huddle_test_3.mat', 't', 'Uch', 'Ucv', 'Vph', 'Vpv', 'Va');
ht_4 = load('./mat/data_huddle_test_4.mat', 't', 'Vph', 'Vpv', 'Va');
% ht_5 = load('./mat/data_huddle_test_5.mat', 't', 'Uch', 'Ucv', 'Vph', 'Vpv', 'Va');

fs = 1e4;

% Pre-processing

t0 = 1; % [s]

tend = 100; % [s]

ht_s = {ht_1 ht_2 ht_3 ht_4}

for i = 1:length(ht_s)
  ht_s{i}.Vph(ht_s{i}.t<t0) = [];
  ht_s{i}.Vpv(ht_s{i}.t<t0) = [];
  ht_s{i}.Va(ht_s{i}.t<t0)  = [];
  ht_s{i}.t(ht_s{i}.t<t0)   = [];

  ht_s{i}.t = ht_s{i}.t - ht_s{i}.t(1); % We start at t=0

  ht_s{i}.Vph(tend*fs+1:end) = [];
  ht_s{i}.Vpv(tend*fs+1:end) = [];
  ht_s{i}.Va(tend*fs+1:end)  = [];
  ht_s{i}.t(tend*fs+1:end)   = [];

  ht_s{i}.Va = ht_s{i}.Va - mean(ht_s{i}.Va);
end

ht_1 = ht_s{1};
ht_2 = ht_s{2};
ht_3 = ht_s{3};
ht_4 = ht_s{4};

% Filter data with low pass filter
% We filter the data with a first order low pass filter with a crossover frequency of $\omega_0$.


w0 = 50; % [Hz]

G_lpf = 1/(1 + s/2/pi/w0);

ht_1.Vaf = lsim(G_lpf, ht_1.Va, ht_1.t);
ht_2.Vaf = lsim(G_lpf, ht_2.Va, ht_2.t);
ht_3.Vaf = lsim(G_lpf, ht_3.Va, ht_3.t);
ht_4.Vaf = lsim(G_lpf, ht_4.Va, ht_4.t);

% Time domain plots

figure;
ax1 = subplot(2, 2, 1);
hold on;
plot(ht_1.t, 1e9*ht_1.Va);
plot(ht_1.t, 1e9*ht_1.Vaf);
hold off;
ylabel('Displacement [nm]');
set(gca, 'XTickLabel',[]);
title('OL');

ax2 = subplot(2, 2, 2);
hold on;
plot(ht_2.t, 1e9*ht_2.Va);
plot(ht_2.t, 1e9*ht_2.Vaf);
hold off;
set(gca, 'XTickLabel',[]);
set(gca, 'YTickLabel',[]);
title('OL + CU');

ax3 = subplot(2, 2, 3);
hold on;
plot(ht_3.t, 1e9*ht_3.Va);
plot(ht_3.t, 1e9*ht_3.Vaf);
hold off;
xlabel('Time [s]');
ylabel('Displacement [nm]');
title('CL + CU');

ax4 = subplot(2, 2, 4);
hold on;
plot(ht_4.t, 1e9*ht_4.Va);
plot(ht_4.t, 1e9*ht_4.Vaf);
hold off;
xlabel('Time [s]');
set(gca, 'YTickLabel',[]);
title('OL + CU + AL');

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



% #+NAME: fig:huddle_test_Va
% #+CAPTION: Measurement of the Attocube during Huddle Test ([[./figs/huddle_test_Va.png][png]], [[./figs/huddle_test_Va.pdf][pdf]])
% [[file:figs/huddle_test_Va.png]]


figure;
ax1 = subplot(2, 2, 1)
hold on;
plot(ht_1.t, ht_1.Vph);
plot(ht_1.t, ht_1.Vpv);
hold off;
ylabel('Voltage [V]');
set(gca, 'XTickLabel',[]);
title('OL');

ax2 = subplot(2, 2, 2)
hold on;
plot(ht_2.t, ht_2.Vph);
plot(ht_2.t, ht_2.Vpv);
hold off;
set(gca, 'XTickLabel',[]);
set(gca, 'YTickLabel',[]);
title('OL + CU');

ax3 = subplot(2, 2, 3)
hold on;
plot(ht_3.t, ht_3.Vph);
plot(ht_3.t, ht_3.Vpv);
hold off;
xlabel('Time [s]');
ylabel('Voltage [V]');
title('CL + CU');

ax4 = subplot(2, 2, 4)
hold on;
plot(ht_4.t, ht_4.Vph);
plot(ht_4.t, ht_4.Vpv);
hold off;
xlabel('Time [s]');
set(gca, 'YTickLabel',[]);
title('OL + CU + AL');

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

% Power Spectral Density

win = hanning(ceil(1*fs));

[psd_Va1, f] = pwelch(ht_1.Va, win, [], [], fs);
[psd_Va2, ~] = pwelch(ht_2.Va, win, [], [], fs);
[psd_Va3, ~] = pwelch(ht_3.Va, win, [], [], fs);
[psd_Va4, ~] = pwelch(ht_4.Va, win, [], [], fs);

[psd_Vph1, ~] = pwelch(ht_1.Vph, win, [], [], fs);
[psd_Vph2, ~] = pwelch(ht_2.Vph, win, [], [], fs);
[psd_Vph3, ~] = pwelch(ht_3.Vph, win, [], [], fs);
[psd_Vph4, ~] = pwelch(ht_4.Vph, win, [], [], fs);

[psd_Vpv1, ~] = pwelch(ht_1.Vpv, win, [], [], fs);
[psd_Vpv2, ~] = pwelch(ht_2.Vpv, win, [], [], fs);
[psd_Vpv3, ~] = pwelch(ht_3.Vpv, win, [], [], fs);
[psd_Vpv4, ~] = pwelch(ht_4.Vpv, win, [], [], fs);

figure;
hold on;
plot(f, sqrt(psd_Va1), 'DisplayName', 'OL');
plot(f, sqrt(psd_Va2), 'DisplayName', 'OL + CU');
plot(f, sqrt(psd_Va3), 'DisplayName', 'CL + CU');
plot(f, sqrt(psd_Va4), 'DisplayName', 'OL + CU + AL');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]');
ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$');
legend('location', 'northeast');
xlim([1, 1000]);



% #+NAME: fig:huddle_test_psd_va
% #+CAPTION: PSD of the Interferometer measurement during Huddle tests ([[./figs/huddle_test_psd_va.png][png]], [[./figs/huddle_test_psd_va.pdf][pdf]])
% [[file:figs/huddle_test_psd_va.png]]



figure;
hold on;
plot(f, sqrt(psd_Vph1), 'DisplayName', 'OL');
plot(f, sqrt(psd_Vph2), 'DisplayName', 'OL + CU');
plot(f, sqrt(psd_Vph3), 'DisplayName', 'CL + CU');
plot(f, sqrt(psd_Vph4), 'DisplayName', 'OL + CU + AL');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]');
ylabel('ASD $\left[\frac{1}{\sqrt{Hz}}\right]$');
legend('location', 'northeast');
xlim([1, 1000]);