%% Script Description
% Identification of the NASS from cartesian actuation
% to cartesian displacement.

%%
clear; close all; clc;

%%
initializeSample(struct('mass', 1));

G_nass_1 = identifyNass();

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

G_nass_20 = identifyNass();

%%
initializeSample(struct('mass', 50));

G_nass_50 = identifyNass();

%%
freqs = logspace(1, 4, 1000);

bodeFig({G_nass_1(1, 1), G_nass_1(2, 2), G_nass_1(3, 3)}, struct('phase', true))
legend({'$F_{n_x} \rightarrow D_{n_x}$ - $M = 1Kg$', ...
        '$F_{n_y} \rightarrow D_{n_y}$ - $M = 1Kg$', ...
        '$F_{n_z} \rightarrow D_{n_z}$ - $M = 1Kg$'})
legend('location', 'southwest')
exportFig('nass_cart_xyz', 'normal-normal')

bodeFig({G_nass_1(1, 1), G_nass_20(1, 1), G_nass_50(1, 1)}, struct('phase', true))
legend({'$F_{n_x} \rightarrow D_{n_x}$ - $M = 1Kg$', ...
        '$F_{n_x} \rightarrow D_{n_x}$ - $M = 20Kg$', ...
        '$F_{n_x} \rightarrow D_{n_x}$ - $M = 50Kg$'})
legend('location', 'southwest')
exportFig('nass_cart_x_mass', 'normal-normal')

bodeFig({G_nass_1(2, 2), G_nass_20(2, 2), G_nass_50(2, 2)}, struct('phase', true))
legend({'$F_{n_x} \rightarrow D_{n_x}$ - $M = 1Kg$', ...
        '$F_{n_x} \rightarrow D_{n_x}$ - $M = 20Kg$', ...
        '$F_{n_x} \rightarrow D_{n_x}$ - $M = 50Kg$'})
legend('location', 'southwest')
exportFig('nass_cart_y_mass', 'normal-normal')

bodeFig({G_nass_1(3, 3), G_nass_20(3, 3), G_nass_50(3, 3)}, struct('phase', true))
legend({'$F_{n_z} \rightarrow D_{n_z}$ - $M = 1Kg$', ...
        '$F_{n_z} \rightarrow D_{n_z}$ - $M = 20Kg$', ...
        '$F_{n_z} \rightarrow D_{n_z}$ - $M = 50Kg$'})
legend('location', 'southwest')
exportFig('nass_cart_z_mass', 'normal-normal')

%% Save Transfer Functions
save('./mat/G_nass.mat', 'G_nass_1', 'G_nass_20', 'G_nass_50');