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

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

% Data - Plot

% First, we plot the straightness error as a function of the position (figure [[fig:raw_data_tyz]]).


figure;
hold on;
for i=1:data(end, 1)
  plot(data(data(:, 1) == i, 3), data(data(:, 1) == i, 4), '-k');
end
hold off;
xlabel('Target Value [mm]'); ylabel('Error Value [$\mu m$]');



% #+NAME: fig:raw_data_tyz
% #+CAPTION: Time domain Data
% #+RESULTS: fig:raw_data_tyz
% [[file:figs/raw_data_tyz.png]]

% Then, we compute mean value of each position, and we remove this mean value from the data.
% The results are shown on figure [[fig:processed_data_tyz]].

mean_pos = zeros(sum(data(:, 1)==1), 1);
for i=1:sum(data(:, 1)==1)
  mean_pos(i) = mean(data(data(:, 2)==i, 4));
end

figure;
hold on;
for i=1:data(end, 1)
  filt = data(:, 1) == i;
  plot(data(filt, 3), data(filt, 4) - mean_pos, '-k');
end
hold off;
xlabel('Target Value [mm]'); ylabel('Error Value [$\mu m$]');

% Translate to time domain
% We here make the assumptions that, during a scan with the translation stage, the Z motion of the translation stage will follow the guiding error measured.

% We then create a time vector $t$ from 0 to 1 second that corresponds to a typical scan, and we plot the guiding error as a function of the time on figure [[fig:time_domain_tyz]].


t = linspace(0, 1, length(data(data(:, 1)==1, 4)));

figure;
hold on;
plot(t, data(data(:, 1) == 1, 4) - mean_pos, '-k');
hold off;
xlabel('Time [s]'); ylabel('Error Value [um]');

% Compute the PSD

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

dt = t(2)-t(1);

Fs = 1/dt; % [Hz]

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



% We remove the mean position from the data.

x = data(data(:, 1) == 1, 4) - mean_pos;



% And finally, we compute the power spectral density of the displacement obtained in the time domain.

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

[pxx, f] = pwelch(x, win, [], [], Fs);

pxx_t = zeros(length(pxx), data(end, 1));

for i=1:data(end, 1)
  x = data(data(:, 1) == i, 4) - mean_pos;
  [pxx, f] = pwelch(x, win, [], [], Fs);
  pxx_t(:, i) = pxx;
end

figure;
hold on;
plot(f, sqrt(mean(pxx_t, 2)), 'k-');
hold off;
xlabel('Frequency (Hz)');
ylabel('Amplitude Spectral Density $\left[\frac{m}{\sqrt{Hz}}\right]$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');