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

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

% Load data

ty_of = load('mat/data_050.mat', 'data'); ty_of = ty_of.data;
ty_on = load('mat/data_051.mat', 'data'); ty_on = ty_on.data;
ty_1h = load('mat/data_052.mat', 'data'); ty_1h = ty_1h.data;

% Voltage to Velocity
% We convert the measured voltage to velocity using the function =voltageToVelocityL22= (accessible [[file:~/MEGA/These/meas/src/index.org][here]]).


gain = 40; % [dB]

ty_of(:, 1) = voltageToVelocityL22(ty_of(:, 1), ty_of(:, 3), gain);
ty_on(:, 1) = voltageToVelocityL22(ty_on(:, 1), ty_on(:, 3), gain);
ty_1h(:, 1) = voltageToVelocityL22(ty_1h(:, 1), ty_1h(:, 3), gain);

ty_of(:, 2) = voltageToVelocityL22(ty_of(:, 2), ty_of(:, 3), gain);
ty_on(:, 2) = voltageToVelocityL22(ty_on(:, 2), ty_on(:, 3), gain);
ty_1h(:, 2) = voltageToVelocityL22(ty_1h(:, 2), ty_1h(:, 3), gain);

% Time domain plots

figure;
hold on;
plot(ty_1h(:, 3), ty_1h(:, 1), 'DisplayName', 'Marble - Ty 1Hz');
plot(ty_on(:, 3), ty_on(:, 1), 'DisplayName', 'Marble - Ty ON');
plot(ty_of(:, 3), ty_of(:, 1), 'DisplayName', 'Marble - Ty OFF');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
xlim([0, 100]);
legend('Location', 'southwest');



% #+NAME: fig:ty_marble_time_zoom
% #+CAPTION: caption
% #+RESULTS: fig:ty_marble_time_zoom
% [[file:figs/ty_marble_time_zoom.png]]


figure;
hold on;
plot(ty_1h(:, 3), ty_1h(:, 2), 'DisplayName', 'Sample - Ty - 1Hz');
plot(ty_on(:, 3), ty_on(:, 2), 'DisplayName', 'Sample - Ty - ON');
plot(ty_of(:, 3), ty_of(:, 2), 'DisplayName', 'Sample - Ty - OFF');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
xlim([0, 100]);
legend('Location', 'southwest');

% Relative Velocity

figure;
hold on;
plot(ty_1h(:, 3), ty_1h(:, 2)-ty_1h(:, 1), 'DisplayName', 'Relative Velocity - Ty - 1Hz');
plot(ty_on(:, 3), ty_on(:, 2)-ty_on(:, 1), 'DisplayName', 'Relative Velocity - Ty - ON');
plot(ty_of(:, 3), ty_of(:, 2)-ty_of(:, 1), 'DisplayName', 'Relative Velocity - Ty - OFF');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
xlim([0, 100]);
legend('Location', 'southwest');

% Frequency Domain
% We first compute some parameters that will be used for the PSD computation.

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

Fs = 1/dt; % [Hz]

win = hanning(ceil(10*Fs));



% Then we compute the Power Spectral Density using =pwelch= function.

% First for the geophone located on the marble

[pxof_m, f] = pwelch(ty_of(:, 1), win, [], [], Fs);
[pxon_m, ~] = pwelch(ty_on(:, 1), win, [], [], Fs);
[px1h_m, ~] = pwelch(ty_1h(:, 1), win, [], [], Fs);



% And for the geophone located at the sample position.

[pxof_s, f] = pwelch(ty_of(:, 2), win, [], [], Fs);
[pxon_s, ~] = pwelch(ty_on(:, 2), win, [], [], Fs);
[px1h_s, ~] = pwelch(ty_1h(:, 2), win, [], [], Fs);



% Finally, for the relative velocity.

[pxof_r, f] = pwelch(ty_of(:, 2)-ty_of(:, 1), win, [], [], Fs);
[pxon_r, ~] = pwelch(ty_on(:, 2)-ty_on(:, 1), win, [], [], Fs);
[px1h_r, ~] = pwelch(ty_1h(:, 2)-ty_1h(:, 1), win, [], [], Fs);



% And we plot the ASD of the measured velocities:
% - figure [[fig:psd_marble_compare]] for the geophone located on the marble
% - figure [[fig:psd_sample_compare]] for the geophone at the sample position
% - figure [[fig:psd_relative_compare]] for the relative velocity


figure;
hold on;
plot(f, sqrt(px1h_m), 'DisplayName', 'Marble - Ty 1Hz');
plot(f, sqrt(pxon_m), 'DisplayName', 'Marble - Ty ON');
plot(f, sqrt(pxof_m), 'DisplayName', 'Marble - Ty OFF');
hold off;
set(gca, 'xscale', 'log');
set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD of the measured velocity $\left[\frac{m/s}{\sqrt{Hz}}\right]$')
legend('Location', 'southwest');
xlim([1, 500]);



% #+NAME: fig:psd_marble_compare
% #+CAPTION: Comparison of the ASD of the measured velocities from the Geophone on the marble
% #+RESULTS: fig:psd_marble_compare
% [[file:figs/psd_marble_compare.png]]


figure;
hold on;
plot(f, sqrt(px1h_s), 'DisplayName', 'Sample - Ty 1Hz');
plot(f, sqrt(pxon_s), 'DisplayName', 'Sample - Ty ON');
plot(f, sqrt(pxof_s), 'DisplayName', 'Sample - Ty OFF');
hold off;
set(gca, 'xscale', 'log');
set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD of the measured velocity $\left[\frac{m/s}{\sqrt{Hz}}\right]$')
legend('Location', 'southwest');
xlim([2, 500]);



% #+NAME: fig:psd_sample_compare
% #+CAPTION: Comparison of the ASD of the measured velocities from the Geophone at the sample location
% #+RESULTS: fig:psd_sample_compare
% [[file:figs/psd_sample_compare.png]]


figure;
hold on;
plot(f, sqrt(px1h_r), 'DisplayName', 'Relative - Ty 1Hz');
plot(f, sqrt(pxon_r), 'DisplayName', 'Relative - Ty ON');
plot(f, sqrt(pxof_r), 'DisplayName', 'Relative - Ty OFF');
hold off;
set(gca, 'xscale', 'log');
set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD of the measured velocity $\left[\frac{m/s}{\sqrt{Hz}}\right]$')
legend('Location', 'southwest');
xlim([2, 500]);



% #+RESULTS:
% #+begin_example
%      1	Elmo txt chart ver 2.0
%      2
%      3	[File Properties]
%      4	Creation Time,2019-05-13 05:33:43
%      5	Last Updated,2019-05-13 05:33:43
%      6	Resolution,0.001
%      7	Sampling Time,5E-05
%      8	Recording Time,5.461
%      9
%     10	[Chart Properties]
%     11	No.,Name,X Linear,X No.
%     12	1,Chart #1,True,0
%     13	2,Chart #2,True,0
%     14
%     15	[Chart Data]
%     16	Display No.,X No.,Y No.,X Unit,Y Unit,Color,Style,Width
%     17	1,1,2,sec,N/A,ff0000ff,Solid,TwoPoint
%     18	2,1,3,sec,N/A,ff0000ff,Solid,TwoPoint
%     19	2,1,4,sec,N/A,ff007f00,Solid,TwoPoint
%     20
%     21	[Signal Names]
%     22	1,Time (sec)
%     23	2,Position [cnt]
%     24	3,Current Command [A]
%     25	4,Total Current Command [A]
%     26
%     27	[Signals Data Group 1]
%     28	1,2,3,4,
%     29	0,-141044,-0.537239575086517,-0.537239575086517,
%     30	0.001,-143127,-0.530803752974691,-0.530803752974691,
% #+end_example

% The real data starts at line 29.
% We then load this =cvs= file starting at line 29.


ty_on = csvread("mat/Ty-when-Rz-1Hz.csv", 29, 0);
ty_1h = csvread("mat/Ty-when-Rz-1Hz-and-Ty-1Hz.csv", 29, 0);

% Time domain data
% We plot the position of the translation stage measured by the encoders.

% There is 200000 encoder count for each mm, we then divide by 200000 to obtain mm.

% The result is shown on figure [[fig:ty_position_time]].


figure;
subplot(1, 2, 1);
plot(ty_on(:, 1), (ty_on(:, 2)-mean(ty_on(:, 2)))/200000);
xlim([0, 5]);
xlabel('Time [s]'); ylabel('Position [mm]');
legend({'Ty - ON'}, 'Location', 'northeast');

subplot(1, 2, 2);
plot(ty_1h(:, 1), (ty_1h(:, 2)-mean(ty_1h(:, 2)))/200000);
xlim([0, 5]);
xlabel('Time [s]'); ylabel('Position [mm]');
legend({'Ty - 1Hz'}, 'Location', 'northeast');



% #+NAME: fig:ty_position_time
% #+CAPTION: Y position of the translation stage measured by the encoders
% #+RESULTS: fig:ty_position_time
% [[file:figs/ty_position_time.png]]


% We also plot the current as function of the time on figure [[fig:ty_current_time]].


figure;
subplot(1, 2, 1);
plot(ty_on(:, 1), ty_on(:, 3)-mean(ty_on(:, 3)));
xlim([0, 5]);
xlabel('Time [s]'); ylabel('Current [A]');
legend({'Ty - ON'}, 'Location', 'northeast');

subplot(1, 2, 2);
plot(ty_1h(:, 1), ty_1h(:, 3)-mean(ty_1h(:, 3)));
xlim([0, 5]);
xlabel('Time [s]'); ylabel('Current [A]');
legend({'Ty - 1Hz'}, 'Location', 'northeast');