%%
run init_sim_configuration.m
run init_data.m

%%
time_vector = 0:Ts:Tsim;

%% Set point [m, rad]
setpoint = zeros(length(time_vector), 6);

% setpoint(ceil(1/Ts):end, 2) = 1e-6;

r_setpoint = timeseries(setpoint, time_vector);

%% Ground motion
xg = zeros(length(time_vector), 3);

% Wxg = 1e-5*(s/(2e2)^(1/3) + 2*pi*0.1)^3/(s + 2*pi*0.1)^3;
% Wxg = Wxg*(s/(0.5e6)^(1/3) + 2*pi*10)^3/(s + 2*pi*10)^3;
% Wxg = Wxg/(1+s/(2*pi*2000));
% 
% xg = 1/sqrt(2)*100*random('norm', 0, 1, length(time_vector), 3);
% xg(:, 1) = lsim(Wxg, xg(:, 1), time_vector);
% xg(:, 2) = lsim(Wxg, xg(:, 2), time_vector);
% xg(:, 3) = lsim(Wxg, xg(:, 3), time_vector);

r_Gm = timeseries(xg, time_vector);

% figure;
% plot(r_Gm)

%% Translation stage [m]
r_Ty = timeseries(zeros(length(time_vector), 1), time_vector);

%% Tilt Stage [deg]
r_My = timeseries(zeros(length(time_vector), 1), time_vector);

%% Spindle [deg]
% r_spindle = zeros(length(time_vector), 1);

r_spindle = 2*pi*time_vector;

r_Mz = timeseries(r_spindle, time_vector);

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

%% Center of gravity compensation
r_mass = timeseries(zeros(length(time_vector), 2), time_vector);

%% Nano Hexapod
r_n_hexa = timeseries(zeros(length(time_vector), 6), time_vector);

%%
save('./mat/inputs_setpoint.mat', 'r_setpoint', 'r_Gm', 'r_Ty', 'r_My', 'r_u_hexa', 'r_mass', 'r_n_hexa');