%% ustation_1_kinematics.m

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

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

%% Path for functions, data and scripts
addpath('./mat/'); % Path for Data
addpath('./src/'); % Path for functions
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files

% Simulink Model name
mdl = 'ustation_simscape';

load('nass_model_conf_simulink.mat');

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(log10(10), log10(2e3), 1000);

%% Stage setpoints
Dy = 1e-3; % Translation Stage [m]
Ry = 3*pi/180; % Tilt Stage [rad]
Rz = 180*pi/180; % Spindle [rad]

%% Stage individual Homogeneous transformations
% Translation Stage
Rty = [1 0 0 0;
       0 1 0 Dy;
       0 0 1 0;
       0 0 0 1];

% Tilt Stage - Pure rotating aligned with Ob
Rry = [ cos(Ry) 0 sin(Ry) 0;
        0       1 0       0;
       -sin(Ry) 0 cos(Ry) 0;
        0       0 0       1];

% Spindle - Rotation along the Z axis
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
       sin(Rz)  cos(Rz) 0 0 ;
       0        0       1 0 ;
       0        0       0 1 ];

% Micro-Station homogeneous transformation
Ttot = Rty*Rry*Rrz;

%% Compute translations and rotations (Euler angles) induced by the micro-station
ustation_dx = Ttot(1,4);
ustation_dy = Ttot(2,4);
ustation_dz = Ttot(3,4);
ustation_ry = atan2( Ttot(1, 3),                  sqrt(Ttot(1, 1)^2 + Ttot(1, 2)^2));
ustation_rx = atan2(-Ttot(2, 3)/cos(ustation_ry), Ttot(3, 3)/cos(ustation_ry));
ustation_rz = atan2(-Ttot(1, 2)/cos(ustation_ry), Ttot(1, 1)/cos(ustation_ry));

%% Verification using the Simscape model
% All stages are initialized as rigid bodies to avoid any guiding error
initializeGround(      'type', 'rigid');
initializeGranite(     'type', 'rigid');
initializeTy(          'type', 'rigid');
initializeRy(          'type', 'rigid');
initializeRz(          'type', 'rigid');
initializeMicroHexapod('type', 'rigid');

initializeLoggingConfiguration('log', 'all');

initializeReferences('Dy_amplitude', Dy, ...
                     'Ry_amplitude', Ry, ...
                     'Rz_amplitude', Rz);

initializeDisturbances('enable', false);

set_param(conf_simulink, 'StopTime', '0.5');

% Simulation is performed
sim(mdl);

% Sample's motion is computed from "external metrology"
T_sim = [simout.y.R.Data(:,:,end), [simout.y.x.Data(end); simout.y.y.Data(end); simout.y.z.Data(end)]; [0,0,0,1]];
sim_dx = T_sim(1,4);
sim_dy = T_sim(2,4);
sim_dz = T_sim(3,4);
sim_ry = atan2( T_sim(1, 3),             sqrt(T_sim(1, 1)^2 + T_sim(1, 2)^2));
sim_rx = atan2(-T_sim(2, 3)/cos(sim_ry), T_sim(3, 3)/cos(sim_ry));
sim_rz = atan2(-T_sim(1, 2)/cos(sim_ry), T_sim(1, 1)/cos(sim_ry));