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

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

% Load Plant

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

% Test

bodeFig({Ga*Zc*Gi}, struct('phase', true));

% TODO Huddle Test
% We load the data taken during the Huddle Test.

load('mat/data_huddle_test.mat', ...
     't', 'Uch', 'Ucv', ...
     'Unh', 'Unv', ...
     'Vph', 'Vpv', ...
     'Vch', 'Vcv', ...
     'Vnh', 'Vnv', ...
     'Va');



% We remove the first second of data where everything is settling down.

t0 = 1;

Uch(t<t0) = [];
Ucv(t<t0) = [];
Unh(t<t0) = [];
Unv(t<t0) = [];
Vph(t<t0) = [];
Vpv(t<t0) = [];
Vch(t<t0) = [];
Vcv(t<t0) = [];
Vnh(t<t0) = [];
Vnv(t<t0) = [];
Va(t<t0)  = [];
t(t<t0)   = [];

t = t - t(1); % We start at t=0

figure;
hold on;
plot(t, Vph, 'DisplayName', '$Vp_h$');
plot(t, Vpv, 'DisplayName', '$Vp_v$');
hold off;
xlabel('Time [s]');
ylabel('Amplitude [V]');
xlim([t(1), t(end)]);
legend();



% We compute the Power Spectral Density of the horizontal and vertical positions of the beam as measured by the 4 quadrant diode.

[psd_Vph, f] = pwelch(Vph, hanning(ceil(1*fs)), [], [], fs);
[psd_Vpv, ~] = pwelch(Vpv, hanning(ceil(1*fs)), [], [], fs);

figure;
hold on;
plot(f, sqrt(psd_Vph), 'DisplayName', '$\Gamma_{Vp_h}$');
plot(f, sqrt(psd_Vpv), 'DisplayName', '$\Gamma_{Vp_v}$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{V}{\sqrt{Hz}}\right]$')
legend('Location', 'southwest');
xlim([1, 1000]);

figure;
hold on;
plot(t, Vch, 'DisplayName', '$Vc_h$');
plot(t, Vcv, 'DisplayName', '$Vc_v$');
hold off;
xlabel('Time [s]');
ylabel('Amplitude [V]');
xlim([t(1), t(end)]);
legend();



% We compute the Power Spectral Density of the voltage across the inductance used for horizontal and vertical positioning of the Cercalo.

[psd_Vch, f] = pwelch(Vch, hanning(ceil(1*fs)), [], [], fs);
[psd_Vcv, ~] = pwelch(Vcv, hanning(ceil(1*fs)), [], [], fs);

figure;
hold on;
plot(f, sqrt(psd_Vch), 'DisplayName', '$\Gamma_{Vc_h}$');
plot(f, sqrt(psd_Vcv), 'DisplayName', '$\Gamma_{Vc_v}$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{V}{\sqrt{Hz}}\right]$')
legend('Location', 'southwest');
xlim([1, 1000]);

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