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

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

% Load data
% We load the data of the z axis of two geophones.

d3 = load('mat/data_003.mat', 'data'); d3 = d3.data;
d4 = load('mat/data_004.mat', 'data'); d4 = d4.data;
d5 = load('mat/data_005.mat', 'data'); d5 = d5.data;
d6 = load('mat/data_006.mat', 'data'); d6 = d6.data;
d7 = load('mat/data_007.mat', 'data'); d7 = d7.data;
d8 = load('mat/data_008.mat', 'data'); d8 = d8.data;

% Analysis - Time Domain
% First, we can look at the time domain data and compare all the measurements:
% - comparison for the geophone at the sample location (figure [[fig:time_domain_sample]])
% - comparison for the geophone on the granite (figure [[fig:time_domain_marble]])


figure;
hold on;
plot(d3(:, 3), d3(:, 2), 'DisplayName', 'All ON');
plot(d4(:, 3), d4(:, 2), 'DisplayName', 'Ty OFF');
plot(d5(:, 3), d5(:, 2), 'DisplayName', 'Ry OFF');
plot(d6(:, 3), d6(:, 2), 'DisplayName', 'S-R OFF');
plot(d7(:, 3), d7(:, 2), 'DisplayName', 'Rz OFF');
plot(d8(:, 3), d8(:, 2), 'DisplayName', 'Hexa OFF');
hold off;
xlabel('Time [s]'); ylabel('Voltage [V]');
xlim([0, 50]);
legend('Location', 'bestoutside');



% #+NAME: fig:time_domain_sample
% #+CAPTION: Comparison of the time domain data when turning off the control system of the stages - Geophone at the sample location
% #+RESULTS: fig:time_domain_sample
% [[file:figs/time_domain_sample.png]]



figure;
hold on;
plot(d3(:, 3), d3(:, 1), 'DisplayName', 'All ON');
plot(d4(:, 3), d4(:, 1), 'DisplayName', 'Ty OFF');
plot(d5(:, 3), d5(:, 1), 'DisplayName', 'Ry OFF');
plot(d6(:, 3), d6(:, 1), 'DisplayName', 'S-R OFF');
plot(d7(:, 3), d7(:, 1), 'DisplayName', 'Rz OFF');
plot(d8(:, 3), d8(:, 1), 'DisplayName', 'Hexa OFF');
hold off;
xlabel('Time [s]'); ylabel('Voltage [V]');
xlim([0, 50]);
legend('Location', 'bestoutside');

% Analysis - Frequency Domain

dt = d3(2, 3) - d3(1, 3);

Fs = 1/dt;
win = hanning(ceil(10*Fs));

% Vibrations at the sample location
% First, we compute the Power Spectral Density of the signals coming from the Geophone located at the sample location.

[px3, f] = pwelch(d3(:, 2), win, [], [], Fs);
[px4, ~] = pwelch(d4(:, 2), win, [], [], Fs);
[px5, ~] = pwelch(d5(:, 2), win, [], [], Fs);
[px6, ~] = pwelch(d6(:, 2), win, [], [], Fs);
[px7, ~] = pwelch(d7(:, 2), win, [], [], Fs);
[px8, ~] = pwelch(d8(:, 2), win, [], [], Fs);



% And we compare all the signals (figures [[fig:psd_sample_comp]] and [[fig:psd_sample_comp_high_freq]]).

figure;
hold on;
plot(f, sqrt(px3), 'DisplayName', 'All ON');
plot(f, sqrt(px4), 'DisplayName', 'Ty OFF');
plot(f, sqrt(px5), 'DisplayName', 'Ry OFF');
plot(f, sqrt(px6), 'DisplayName', 'S-R OFF');
plot(f, sqrt(px7), 'DisplayName', 'Rz OFF');
plot(f, sqrt(px8), 'DisplayName', 'Hexa OFF');
hold off;
set(gca, 'xscale', 'log');
set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude Spectral Density $\left[\frac{V}{\sqrt{Hz}}\right]$')
xlim([0.1, 500]);
legend('Location', 'southwest');

% Vibrations on the marble
% Now we plot the same curves for the geophone located on the marble.

[px3, f] = pwelch(d3(:, 1), win, [], [], Fs);
[px4, ~] = pwelch(d4(:, 1), win, [], [], Fs);
[px5, ~] = pwelch(d5(:, 1), win, [], [], Fs);
[px6, ~] = pwelch(d6(:, 1), win, [], [], Fs);
[px7, ~] = pwelch(d7(:, 1), win, [], [], Fs);
[px8, ~] = pwelch(d8(:, 1), win, [], [], Fs);



% And we compare the Amplitude Spectral Densities (figures [[fig:psd_marble_comp]] and [[fig:psd_marble_comp_high_freq]])

figure;
hold on;
plot(f, sqrt(px3), 'DisplayName', 'All ON');
plot(f, sqrt(px4), 'DisplayName', 'Ty OFF');
plot(f, sqrt(px5), 'DisplayName', 'Ry OFF');
plot(f, sqrt(px6), 'DisplayName', 'S-R OFF');
plot(f, sqrt(px7), 'DisplayName', 'Rz OFF');
plot(f, sqrt(px8), 'DisplayName', 'Hexa OFF');
hold off;
set(gca, 'xscale', 'log');
set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude Spectral Density $\left[\frac{V}{\sqrt{Hz}}\right]$')
xlim([0.1, 500]);
legend('Location', 'northeast');

% Effect of the control system on the transmissibility from ground to sample
% As the feedback loops change the dynamics of the system, we should see differences on the transfer function from marble velocity to sample velocity when turning off the control systems (figure [[fig:trans_comp]]).


dt = d3(2, 3) - d3(1, 3);

Fs = 1/dt;
win = hanning(ceil(1*Fs));



% First, we compute the Power Spectral Density of the signals coming from the Geophone located at the sample location.

[T3, f] = tfestimate(d3(:, 1), d3(:, 2), win, [], [], Fs);
[T4, ~] = tfestimate(d4(:, 1), d4(:, 2), win, [], [], Fs);
[T5, ~] = tfestimate(d5(:, 1), d5(:, 2), win, [], [], Fs);
[T6, ~] = tfestimate(d6(:, 1), d6(:, 2), win, [], [], Fs);
[T7, ~] = tfestimate(d7(:, 1), d7(:, 2), win, [], [], Fs);
[T8, ~] = tfestimate(d8(:, 1), d8(:, 2), win, [], [], Fs);

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(T3), 'DisplayName', 'All ON');
plot(f, abs(T4), 'DisplayName', 'Ty OFF');
plot(f, abs(T5), 'DisplayName', 'Ry OFF');
plot(f, abs(T6), 'DisplayName', 'S-R OFF');
plot(f, abs(T7), 'DisplayName', 'Rz OFF');
plot(f, abs(T8), 'DisplayName', 'Hexa OFF');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
legend('Location', 'northwest');

ax2 = subplot(2, 1, 2);
hold on;
plot(f, mod(180+180/pi*phase(T3), 360)-180);
plot(f, mod(180+180/pi*phase(T4), 360)-180);
plot(f, mod(180+180/pi*phase(T5), 360)-180);
plot(f, mod(180+180/pi*phase(T6), 360)-180);
plot(f, mod(180+180/pi*phase(T7), 360)-180);
plot(f, mod(180+180/pi*phase(T8), 360)-180);
hold off;
set(gca, 'xscale', 'log');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
xlabel('Frequency [Hz]'); ylabel('Phase');

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