%%
clear; close all; clc;

%% Initialize simulation configuration
opts_sim = struct(...
    'Tsim', 1     ...
);

initializeSimConf(opts_sim);

%% Initialize Inputs
load('./mat/sim_conf.mat', 'sim_conf')

time_vector = 0:sim_conf.Ts:sim_conf.Tsim;

% Translation Stage
ty = 0*ones(length(time_vector), 1);

% Tilt Stage
ry = 2*pi*(0/360)*ones(length(time_vector), 1);
% ry = 2*pi*(3/360)*sin(2*pi*time_vector);

% Spindle
rz = 2*pi*1*(time_vector);
% rz = 2*pi*(190/360)*ones(length(time_vector), 1);

% Micro Hexapod
u_hexa = zeros(length(time_vector), 6);

% Gravity Compensator system
mass = zeros(length(time_vector), 2);
mass(:, 2) = pi;

opts_inputs = struct(...
    'ty', ty, ...
    'ry', ry, ...
    'rz', rz, ...
    'u_hexa', u_hexa, ...
    'mass', mass ...
);

initializeInputs(opts_inputs);

%% Initialize SolidWorks Data
initializeSolids();

%% Initialize Ground
initializeGround();

%% Initialize Granite
initializeGranite();

%% Initialize Translation stage
initializeTy();

%% Initialize Tilt Stage
initializeRy();

%% Initialize Spindle
initializeRz();

%% Initialize Hexapod Symétrie
initializeMicroHexapod();

%% Initialize Center of Gravity compensation
initializeAxisc();

%% Initialize NASS
initializeNanoHexapod(struct('actuator', 'piezo'));

%% Initialize Sample
initializeSample(struct('mass', 20));