clear; close all; clc;

%%
run init_solidworks_data.m

%% Solver Configuration
Ts   = 1e-4; % Sampling time [s]
Tsim = 1;    % Simulation time [s]

%% Gravity
g = 0 ; % Gravity along the z axis [m/s^2]

%% Ground
ground = struct();

ground.shape = [2, 2, 0.5]; % m

%% Granite
granite = struct();

granite.m = smiData.Solid(5).mass;
granite.k.ax = 1e8; % x-y-z Stiffness of the granite [N/m]
granite.ksi.ax = 10;

granite = updateDamping(granite);

%% Translation stage
ty = struct();

ty.m = smiData.Solid(4).mass+smiData.Solid(6).mass+smiData.Solid(7).mass+smiData.Solid(8).mass+smiData.Solid(9).mass+4*smiData.Solid(11).mass+smiData.Solid(24).mass+smiData.Solid(25).mass+smiData.Solid(28).mass;

ty.k.ax   = 1e7/4; % Axial Stiffness for each of the 4 guidance (y) [N/m]
ty.k.rad  = 9e9/4; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]

ty.ksi.ax   = 10;
ty.ksi.rad  = 10;

ty = updateDamping(ty);

%% Tilt Stage
ry = struct();

ry.m = smiData.Solid(26).mass+smiData.Solid(18).mass+smiData.Solid(10).mass;

ry.k.h    = 357e6/4; % Stiffness in the direction of the guidance [N/m]
ry.k.rad  = 555e6/4; % Stiffness in the top direction [N/m]
ry.k.rrad = 238e6/4; % Stiffness in the side direction [N/m]
ry.k.tilt = 1e4 ;    % Rotation stiffness around y [N*m/deg]

ry.ksi.h    = 10;
ry.ksi.rad  = 10;
ry.ksi.rrad = 10;
ry.ksi.tilt = 10;

ry = updateDamping(ry);


%% Spindle
rz = struct();

rz.m = smiData.Solid(12).mass+6*smiData.Solid(20).mass+smiData.Solid(19).mass;

rz.k.ax   = 2e9; % Axial Stiffness [N/m]
rz.k.rad  = 7e8; % Radial Stiffness [N/m]
rz.k.tilt = 1e5; % TODO
rz.k.rot  = 1e5; % TODO

rz.ksi.ax   = 10;
rz.ksi.rad  = 10;
rz.ksi.tilt = 1;
rz.ksi.rot  = 1;

rz = updateDamping(rz);

%% Hexapod Symétrie
run stewart-simscape\params_micro_hexapod.m;
micro_hexapod = stewart;
clear stewart;

%% Center of gravity compensation
axisc = struct();

axisc.m      = smiData.Solid(30).mass;
axisc.k.ax   = 1; % TODO [N*m/deg)]
axisc.ksi.ax = 1;

axisc = updateDamping(axisc);

%% NASS
run stewart-simscape\params_micro_hexapod.m;
nano_hexapod = stewart;
clear stewart;

%% Sample
sample = struct();

sample.radius = 100; % Sample radius [mm]
sample.height = 300; % Sample height [mm]
sample.mass   = 50; % Sample mass [kg]
sample.offset = 0; % Decentralization offset [mm]
sample.color  = [0.9 0.1 0.1]; % Sample color

%%
function element = updateDamping(element)
    field = fieldnames(element.k);
    for i = 1:length(field)
        element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
    end
end