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

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

% Load data

z_ty = load('mat/data_040.mat', 'data'); z_ty = z_ty.data;
e_ty = load('mat/data_041.mat', 'data'); e_ty = e_ty.data;
e_of = load('mat/data_042.mat', 'data'); e_of = e_of.data;

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


z_ty(:, 1) = voltageToVelocityL22(z_ty(:, 1), z_ty(:, 3), 40);
e_ty(:, 1) = voltageToVelocityL22(e_ty(:, 1), e_ty(:, 3), 40);
e_of(:, 1) = voltageToVelocityL22(e_of(:, 1), e_of(:, 3), 40);

z_ty(:, 2) = voltageToVelocityL22(z_ty(:, 2), z_ty(:, 3), 40);
e_ty(:, 2) = voltageToVelocityL22(e_ty(:, 2), e_ty(:, 3), 40);
e_of(:, 2) = voltageToVelocityL22(e_of(:, 2), e_of(:, 3), 40);

% Time domain plots
% We plot the measured velocity of the marble and sample in the vertical direction (figure [[fig:ty_z_time]]) and in the X direction (figure [[fig:ty_e_time]]).

% We also integrate the relative velocity to obtain the relative displacement (figure [[fig:x_relative_disp]] in the X direction and figure [[fig:z_relative_disp]] in the Z direction).


figure;
hold on;
plot(z_ty(:, 3), z_ty(:, 1), 'DisplayName', 'Marble - Z');
plot(z_ty(:, 3), z_ty(:, 2), 'DisplayName', 'Sample - Z');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
xlim([0, 2]);
legend('Location', 'northeast');



% #+NAME: fig:ty_z_time
% #+CAPTION: Z velocity of the sample and marble when scanning with the translation stage
% #+RESULTS: fig:ty_z_time
% [[file:figs/ty_z_time.png]]


figure;
hold on;
plot(e_ty(:, 3), e_ty(:, 1), 'DisplayName', 'Marble - X');
plot(e_ty(:, 3), e_ty(:, 2), 'DisplayName', 'Sample - X');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
xlim([0, 2]);
legend('Location', 'northeast');



% #+NAME: fig:ty_e_time
% #+CAPTION: Velocity of the sample and marble in the east direction when scanning with the translation stage
% #+RESULTS: fig:ty_e_time
% [[file:figs/ty_e_time.png]]


figure;
plot(e_ty(:, 3), 1e6*lsim(1/s, e_ty(:, 2)-e_ty(:, 1), e_ty(:, 3)));
xlabel('Time [s]'); ylabel('X Relative Displacement [$\mu m$]');
xlim([0, 2]);



% #+NAME: fig:x_relative_disp
% #+CAPTION: X relative displacement of the sample with respect to the marble
% #+RESULTS: fig:x_relative_disp
% [[file:figs/x_relative_disp.png]]


figure;
plot(z_ty(:, 3), 1e6*lsim(1/s, z_ty(:, 2)-z_ty(:, 1), z_ty(:, 3)));
xlabel('Time [s]'); ylabel('Z Relative Displacement [$\mu m$]');
xlim([0, 2]);

% Frequency Domain analysis
% We get the typical ground velocity to compare with the velocities measured.

[pxx_gm, f_gm] = getPSDGroundVelocity();



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

dt = z_ty(2, 3)-z_ty(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

[pxz_ty_m, f] = pwelch(z_ty(:, 1), win, [], [], Fs);
[pxe_ty_m, ~] = pwelch(e_ty(:, 1), win, [], [], Fs);
[pxe_of_m, ~] = pwelch(e_of(:, 1), win, [], [], Fs);



% And for the geophone located at the sample position.

[pxz_ty_s, ~] = pwelch(z_ty(:, 2), win, [], [], Fs);
[pxe_ty_s, ~] = pwelch(e_ty(:, 2), win, [], [], Fs);
[pxe_of_s, ~] = pwelch(e_of(:, 2), win, [], [], Fs);



% And finally for the relative velocity between the sample and the marble.

[pxz_ty_r, ~] = pwelch(z_ty(:, 2)-z_ty(:, 1), win, [], [], Fs);
[pxe_ty_r, ~] = pwelch(e_ty(:, 2)-e_ty(:, 1), win, [], [], Fs);
[pxe_of_r, ~] = pwelch(e_of(:, 2)-e_of(:, 1), win, [], [], Fs);



% And we plot the ASD of the measured velocities:
% - figure [[fig:asd_east_marble]] compares the marble velocity in the east direction when scanning and when Ty is OFF
% - figure [[fig:asd_east_sample]] compares the sample velocity in the east direction when scanning and when Ty is OFF
% - figure [[fig:asd_z_direction]] shows the marble and sample velocities in the Z direction when scanning with the translation stage
% - figure [[fig:asd_e_relative]] shows the relative velocity of the sample with respect to the granite in the X direction when the translation stage is OFF and when it is scanning at 1Hz


figure;
hold on;
plot(f, sqrt(pxe_ty_m), 'DisplayName', 'Ty 1Hz - Marble - X');
plot(f, sqrt(pxe_of_m), 'DisplayName', 'Ty OFF - Marble - X');
plot(f_gm, sqrt(pxx_gm), 'k--', 'DisplayName', 'Ground Motion');
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', 'northwest');
xlim([0.1, 500]);



% #+NAME: fig:asd_east_marble
% #+CAPTION: Amplitude spectral density of the measured velocities corresponding to the geophone in the east direction located on the marble when the translation stage is OFF and when it is scanning at 1Hz
% #+RESULTS: fig:asd_east_marble
% [[file:figs/asd_east_marble.png]]



figure;
hold on;
plot(f, sqrt(pxe_ty_s), 'DisplayName', 'Ty 1Hz - Sample - X');
plot(f, sqrt(pxe_of_s), 'DisplayName', 'Ty OFF - Sample - X');
plot(f_gm, sqrt(pxx_gm), 'k--', 'DisplayName', 'Ground Motion');
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', 'northwest');
xlim([0.1, 500]);



% #+NAME: fig:asd_east_sample
% #+CAPTION: Amplitude spectral density of the measured velocities corresponding to the geophone in the east direction located at the sample location when the translation stage is OFF and when it is scanning at 1Hz
% #+RESULTS: fig:asd_east_sample
% [[file:figs/asd_east_sample.png]]



figure;
hold on;
plot(f, sqrt(pxz_ty_m), 'DisplayName', 'Ty 1Hz - Marble - Z');
plot(f, sqrt(pxz_ty_s), 'DisplayName', 'Ty 1Hz - Sample - Z');
plot(f_gm, sqrt(pxx_gm), 'k--', 'DisplayName', 'Ground Motion');
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', 'northwest');
xlim([0.1, 500]);



% #+NAME: fig:asd_z_direction
% #+CAPTION: Amplitude spectral density of the measure velocity corresponding to the geophone in the vertical direction located on the granite and at the sample location when the translation stage is scanning at 1Hz
% #+RESULTS: fig:asd_z_direction
% [[file:figs/asd_z_direction.png]]



figure;
hold on;
plot(f, sqrt(pxe_of_r), 'DisplayName', 'Ty OFF - Relative - E');
plot(f, sqrt(pxe_ty_r), 'DisplayName', 'Ty 1Hz - Relative - E');
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', 'northwest');
xlim([0.1, 500]);

% Transfer function from marble motion in the East direction to sample motion in the East direction
% Let's compute the transfer function for the marble velocity in the east direction to the sample velocity in the east direction.

% We first plot the time domain motions when every stage is off (figure [[fig:east_marble_sample]]).


figure;
hold on;
plot(e_of(:, 3), e_of(:, 2), 'DisplayName', 'Sample - X');
plot(e_of(:, 3), e_of(:, 1), 'DisplayName', 'Marble - X');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
xlim([0, 100]);
legend('Location', 'southwest');



% #+NAME: fig:east_marble_sample
% #+CAPTION: Velocity in the east direction of the marble and sample when all the stages are OFF
% #+RESULTS: fig:east_marble_sample
% [[file:figs/east_marble_sample.png]]


% We then compute the transfer function using =tfestimate=.

dt = e_of(2, 3)-e_of(1, 3);
Fs = 1/dt; % [Hz]
win = hanning(ceil(10*Fs));

[T, f] = tfestimate(e_of(:, 1), e_of(:, 2), win, [], [], Fs);




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

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(T));
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');

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

linkaxes([ax1,ax2],'x');
xlim([10, 100]);



% #+RESULTS:
% #+begin_example
%      1	Elmo txt chart ver 2.0
%      2
%      3	[File Properties]
%      4	Creation Time,2019-05-13 05:11:45
%      5	Last Updated,2019-05-13 05:11:45
%      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,1110769,-0.320872406596209,-0.320872406596209,
%     30	0.001,1108743,-0.319658428261391,-0.319658428261391,
% #+end_example

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


data = csvread("mat/sin_elmo.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;
hold on;
plot(data(:, 1), data(:, 2)/200000);
hold off;
xlim([0, 5]);
xlabel('Time [s]'); ylabel('Position [mm]');



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

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


figure;
hold on;
plot(data(:, 1), data(:, 3));
hold off;
xlim([0, 5]); ylim([-10, 10]);
xlabel('Time [s]'); ylabel('Current [A]');