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

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

addpath('./mat/');



% #+NAME: fig:velocity_to_voltage
% #+CAPTION: Schematic of the instrumentation used for the measurement
% #+RESULTS: fig:velocity_to_voltage
% [[file:figs/velocity_to_voltage.png]]

% Let's say, we know that the sensitivity of the geophone used is
% \[ G_g(s) = G_0 \frac{\frac{s}{2\pi f_0}}{1 + \frac{s}{2\pi f_0}} \quad \left[\frac{V}{m/s}\right] \]

% The parameters are defined below.

G0 = 88; % Sensitivity [V/(m/s)]
f0 = 2; % Cut-off frequency [Hz]

Gg = G0*(s/2/pi/f0)/(1+s/2/pi/f0);



% And the dynamics of the amplifier in the bandwidth of interest is just a gain: $G_a(s) = 1000$.

Ga = 1000;



% #+NAME: fig:voltage_to_velocity
% #+CAPTION: Schematic of the instrumentation used for the measurement
% #+RESULTS: fig:voltage_to_velocity
% [[file:figs/voltage_to_velocity.png]]


% Let's load the measured $x$.

data = load('mat/data_028.mat', 'data'); data = data.data;

t = data(:, 3); % Time vector [s]
x = data(:, 1)-mean(data(:, 1)); % The offset if removed (coming from the voltage amplifier) [v]

dt = t(2)-t(1); % Sampling Time [s]
Fs = 1/dt; % Sampling Frequency [Hz]



% We simulate this system with matlab using the =lsim= command.

v = lsim(inv(Gg*Ga), x, t);



% And we plot the obtained velocity

figure;
plot(t, v);
xlabel("Time [s]");
ylabel("Velocity [m/s]");

% Power Spectral Density and Amplitude Spectral Density
% From the Matlab documentation:
% #+begin_quote
% The goal of spectral estimation is to describe the distribution (over frequency) of the power contained in a signal, based on a finite set of data.
% #+end_quote

% We now have the velocity $v(t)\ [m/s]$ in the time domain.

% The Power Spectral Density (PSD) $S_v(f)$ of the time domain $v(t)$ can be computed using the following equation:
% \[ S_v(f) = \frac{1}{f_s} \sum_{m=-\infty}^{\infty} R_{xx}(m) e^{-j 2 \pi m f / f_s} \ \left[\frac{(m/s)^2}{Hz}\right] \]
% where
% - $f_s$ is the sampling frequency in Hz
% - $R_{xx}$ is the autocorrelation

% The PSD represents the distribution of the (average) signal power over frequency.
% $S_v(f)df$ is the infinitesimal power in the band $(f-df/2, f+df/2)$ and the total power in the signal is obtained by integrating these infinitesimal contributions.


% To compute the Power Spectral Density with matlab, we use the =pwelch= function ([[https://fr.mathworks.com/help/signal/ref/pwelch.html?s_tid=doc_ta][documentation]]).
% The use of the =pwelch= function is:
% =[pxx,w] = pwelch(x,window,noverlap,nfft, fs)=
% with:
% - =x= is the discrete time signal
% - =window= is a window that is used to smooth the obtained PSD
% - =overlap= can be used to have some overlap from section to section
% - =nfft= specifies the number of FFT points for the PSD
% - =fs= is the sampling frequency of the data =x= in Hertz

% As explained in cite:schmid12_how_to_use_fft_matlab, it is recommended to use the =pwelch= function the following way:

% First, define a window (preferably the =hanning= one) by specifying the averaging factor =na=.

nx = length(v);
na = 8;
win = hanning(floor(nx/na));



% Then, compute the power spectral density $S_v$ and the associated frequency vector $f$ with no overlap.

[Sv, f] = pwelch(v, win, 0, [], Fs);



% The obtained PSD is shown in figure [[fig:psd_velocity]].


figure;
plot(f, Sv);
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]');
ylabel('Power Spectral Density $\left[\frac{(m/s)^2}{Hz}\right]$')
xlim([0.1, 500]);



% #+NAME: fig:psd_velocity
% #+CAPTION: Power Spectral Density of the measured velocity
% #+RESULTS: fig:psd_velocity
% [[file:figs/psd_velocity.png]]

% The Amplitude Spectral Density (ASD) is defined as the square root of the Power Spectral Density and is shown in figure [[fig:asd_velocity]].
% \begin{equation}
%    \Gamma_{vv}(f) = \sqrt{S_{vv}(f)} \quad \left[ \frac{m/s}{\sqrt{Hz}} \right]
% \end{equation}


figure;
plot(f, sqrt(Sv));
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]');
ylabel('Amplitude Spectral Density $\left[\frac{m/s}{\sqrt{Hz}}\right]$')
xlim([0.1, 500]);



% #+NAME: fig:velocity_to_voltage_psd
% #+CAPTION: Schematic of the instrumentation used for the measurement
% #+RESULTS: fig:velocity_to_voltage_psd
% [[file:figs/velocity_to_voltage_psd.png]]

% Similarly, the ASD of $y$ is:
% \begin{equation}
%   \Gamma_{yy}(\omega) = \left|G(j\omega)\right| \Gamma_{uu}(\omega)
% \end{equation}

% Thus, we could have computed the PSD of $x$ and then obtain the PSD of the velocity with:
% \[ S_{v}(\omega) = |G_a(j\omega) G_g(j\omega)|^{-1} S_{x}(\omega) \]

% The PSD of $x$ is computed below.

nx = length(x);
na = 8;
win = hanning(floor(nx/na));
[Sx, f] = pwelch(x, win, 0, [], Fs);



% And the PSD of $v$ is obtained with the below code.

Svv = Sx.*squeeze(abs(freqresp(inv(Gg*Ga), f, 'Hz'))).^2;



% The result is compare with the PSD computed from the $v$ signal obtained with the =lsim= command in figure [[fig:psd_velocity_lti_method]].


figure;
hold on;
plot(f, Sv,  'DisplayName', 'lsim technique');
plot(f, Svv, 'DisplayName', 'LTI technique');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]');
ylabel('Power Spectral Density $\left[\frac{(m/s)^2}{Hz}\right]$');
xlim([0.1, 500]);
legend('location', 'southwest');



% #+NAME: fig:velocity_to_displacement_psd
% #+CAPTION: Schematic of the instrumentation used for the measurement
% #+RESULTS: fig:velocity_to_displacement_psd
% [[file:figs/velocity_to_displacement_psd.png]]

% We then have the relation between the PSD of $d$ and the PSD of $v$:
% \begin{equation}
%   S_{dd}(\omega) = \left|\frac{1}{j \omega}\right|^2 S_{vv}(\omega)
% \end{equation}

% Using a frequency variable $f$ in Hz:
% \begin{equation}
%   S_{dd}(f) = \left| \frac{1}{j 2\pi f} \right|^2 S_{vv}(f)
% \end{equation}

% For the Amplitude Spectral Density:
% \begin{equation}
%   \Gamma_{dd}(f) = \frac{1}{2\pi f} \Gamma_{vv}(f)
% \end{equation}

% Note here that the PSD (and ASD) of one variable and its derivatives/integrals are equal at one particular frequency $f = 1\ rad/s \approx 0.16\ Hz$:
% \begin{equation}
%   S_{xx}(\omega = 1) = S_{vv}(\omega = 1)
% \end{equation}


% With Matlab, the PSD of the displacement can be computed from the PSD of the velocity with the following code.

Sd = Sv.*(1./(2*pi*f)).^2;



% The obtained PSD of the displacement can be seen in figure [[fig:psd_velocity_displacement]].

figure;
hold on;
plot(f, Sd, 'DisplayName', '$S_d$ in $\left[\frac{m^2}{Hz}\right]$');
plot(f, Sv, 'DisplayName', '$S_v$ in $\left[\frac{(m/s)^2}{Hz}\right]$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]');
ylabel('Power Spectral Density');
xlim([0.1, 500]);
legend('location', 'southwest');

% Cumulative Power/Amplitude Spectrum
% The Cumulative Power Spectrum is the cumulative integral of the Power Spectral Density:
% #+NAME: eq:cps
% \begin{equation}
%   CPS_v(f) = \int_0^f PSD_v(\nu) d\nu \quad [(m/s)^2]
% \end{equation}

% It is also possible to integrate from high frequency to low frequency:
% #+NAME: eq:cps_inv
% \begin{equation}
%   CPS_v(f) = \int_f^\infty PSD_v(\nu) d\nu \quad [(m/s)^2]
% \end{equation}

% The Cumulative Power Spectrum taken at frequency $f$ thus represent the power in the signal in the frequency band $0$ to $f$ or $f$ to $\infty$ depending on the above definition taken.


% The choice of the integral direction depends on the shape of the PSD.
% If the power is mostly present at low frequencies, it is preferable to use equation [[eq:cps_inv]].


% The Cumulative Amplitude Spectrum is defined as the square root of the Cumulative Power Spectrum:
% \[ CAS_v(f) = \sqrt{CPS_v(f)} = \sqrt{\int_f^\infty PSD_v(\nu) d\nu} \quad [m/s] \]

% The Root Mean Square value of the velocity corresponds to the Cumulative Amplitude Spectrum when integrated at all frequencies:
% \[ v_{\text{rms}} = \sqrt{\int_0^\infty PSD_v(\nu) d\nu} = CAS_v(0) \quad [m/s \ \text{rms}] \]

% With Matlab, the Cumulative Power Spectrum can be computed with the below formulas and the results are shown in figure [[fig:cps_integral_comp]].

CPS_v  = cumtrapz(f, Sv); % Cumulative Power Spectrum from low to high frequencies
CPS_vv = flip(-cumtrapz(flip(f), flip(Sv))); % Cumulative Power Spectrum from high to low frequencies

figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(f, CPS_v);
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum [$(m/s)^2$]')

ax2 = subplot(1, 2, 2);
hold on;
plot(f, CPS_vv);
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]');

linkaxes([ax1,ax2],'xy');
xlim([0.1, 500]); ylim([1e-15, 1e-11]);