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

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

% Simscape Model
% <<sec:simscape_model>>

% The simulink file to do tomography experiments is =sim_nano_station_tomo.slx=.

open('experiment_tomography/matlab/sim_nano_station_tomo.slx')



% We load the shared simulink configuration and we set the =StopTime=.

load('mat/conf_simscape.mat');
set_param(conf_simscape, 'StopTime', '5');



% We first initialize all the stages.

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 1));



% We initialize the reference path for all the stages.
% All stage is set to its zero position except the Spindle which is rotating at 60rpm.

initializeReferences(struct('Rz_type', 'rotating', 'Rz_period', 1));

% Simulation Setup
% And we initialize the disturbances to be equal to zero.

opts = struct(...
    'Dwx', false, ... % Ground Motion - X direction
    'Dwy', false, ... % Ground Motion - Y direction
    'Dwz', false, ... % Ground Motion - Z direction
    'Fty_x', false, ... % Translation Stage - X direction
    'Fty_z', false, ... % Translation Stage - Z direction
    'Frz_z', false  ... % Spindle - Z direction
);
initDisturbances(opts);



% We simulate the model.

sim('sim_nano_station_tomo');



% And we save the obtained data.

tomo_align_no_dist = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'tomo_align_no_dist', '-append');

% Analysis

load('experiment_tomography/mat/experiment.mat', 'tomo_align_no_dist');
t = tomo_align_no_dist.t;
MTr = tomo_align_no_dist.MTr;

Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)),           squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));

figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');

ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');

ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');

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



% #+NAME: fig:exp_tomo_without_dist_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_without_dist_trans.png][png]], [[./figs/exp_tomo_without_dist_trans.pdf][pdf]])
% [[file:figs/exp_tomo_without_dist_trans.png]]


figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');

ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');

ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');

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

% Simulation Setup
% We now activate the disturbances.

opts = struct(...
    'Dwx', true, ... % Ground Motion - X direction
    'Dwy', true, ... % Ground Motion - Y direction
    'Dwz', true, ... % Ground Motion - Z direction
    'Fty_x', true, ... % Translation Stage - X direction
    'Fty_z', true, ... % Translation Stage - Z direction
    'Frz_z', true  ... % Spindle - Z direction
);
initDisturbances(opts);



% We simulate the model.

sim('sim_nano_station_tomo');



% And we save the obtained data.

tomo_align_dist = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'tomo_align_dist', '-append');

% Analysis

load('experiment_tomography/mat/experiment.mat', 'tomo_align_dist');
t = tomo_align_dist.t;
MTr = tomo_align_dist.MTr;

Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)),           squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));

figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');

ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');

ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');

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



% #+NAME: fig:exp_tomo_dist_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. the granite when performing tomography experiment with disturbances ([[./figs/exp_tomo_dist_trans.png][png]], [[./figs/exp_tomo_dist_trans.pdf][pdf]])
% [[file:figs/exp_tomo_dist_trans.png]]


figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');

ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');

ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');

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

% Simulation Setup
% We first set the wanted translation of the Micro Hexapod.

P_micro_hexapod = [0.01; 0; 0]; % [m]



% We initialize the reference path.

initializeReferences(struct('Dh_pos', [P_micro_hexapod; 0; 0; 0], 'Rz_type', 'rotating', 'Rz_period', 1));



% We initialize the stages.

initializeMicroHexapod(struct('AP', P_micro_hexapod));



% And we initialize the disturbances to zero.

opts = struct(...
    'Dwx', false, ... % Ground Motion - X direction
    'Dwy', false, ... % Ground Motion - Y direction
    'Dwz', false, ... % Ground Motion - Z direction
    'Fty_x', false, ... % Translation Stage - X direction
    'Fty_z', false, ... % Translation Stage - Z direction
    'Frz_z', false  ... % Spindle - Z direction
);
initDisturbances(opts);



% We simulate the model.

sim('sim_nano_station_tomo');



% And we save the obtained data.

tomo_not_align = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'tomo_not_align', '-append');

% Analysis

load('experiment_tomography/mat/experiment.mat', 'tomo_not_align');
t = tomo_not_align.t;
MTr = tomo_not_align.MTr;

Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)),           squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));

figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');

ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');

ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');

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



% #+NAME: fig:exp_tomo_offset_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_offset_trans.png][png]], [[./figs/exp_tomo_offset_trans.pdf][pdf]])
% [[file:figs/exp_tomo_offset_trans.png]]


figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');

ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');

ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');

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

% Simulation Setup
% We set the reference path.

initializeReferences(struct('Dy_type', 'triangular', 'Dy_amplitude', 10e-3, 'Dy_period', 1));



% We initialize the stages.

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 1));



% And we initialize the disturbances to zero.

opts = struct(...
    'Dwx', false, ... % Ground Motion - X direction
    'Dwy', false, ... % Ground Motion - Y direction
    'Dwz', false, ... % Ground Motion - Z direction
    'Fty_x', false, ... % Translation Stage - X direction
    'Fty_z', false, ... % Translation Stage - Z direction
    'Frz_z', false  ... % Spindle - Z direction
);
initDisturbances(opts);



% We simulate the model.

sim('sim_nano_station_tomo');



% And we save the obtained data.

ty_scan = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'ty_scan', '-append');

% Analysis

load('experiment_tomography/mat/experiment.mat', 'ty_scan');
t = ty_scan.t;
MTr = ty_scan.MTr;

Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)),           squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));

figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');

ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');

ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');

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



% #+NAME: fig:exp_ty_scan_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_ty_scan_trans.png][png]], [[./figs/exp_ty_scan_trans.pdf][pdf]])
% [[file:figs/exp_ty_scan_trans.png]]


figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');

ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');

ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');

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