%% test_nhexa_2_dynamics.m
% Identification of the nano-hexapod dynamics from u to de and to Vs
% Encoders are fixed to the plates

%% 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

%% Colors for the figures
colors = colororder;

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

%% Identification of the transfer function from u to de and from u to Vs without payload
% Load identification data
load('test_nhexa_identification_data_mass_0.mat', 'data');

% Setup useful variables
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1/Ts); % Number of points for the FFT computation
win = hanning(Nfft); % Hanning window
Noverlap = floor(Nfft/2); % Overlap between frequency analysis

% And we get the frequency vector
[~, f] = tfestimate(data{1}.u, data{1}.de, win, Noverlap, Nfft, 1/Ts);

% Transfer function from u to de
G_de = zeros(length(f), 6, 6);

for i = 1:6
    G_de(:,:,i) = tfestimate(data{i}.u, data{i}.de, win, Noverlap, Nfft, 1/Ts);
end

% Transfer function from u to Vs
G_Vs = zeros(length(f), 6, 6);

for i = 1:6
    G_Vs(:,:,i) = tfestimate(data{i}.u, data{i}.Vs, win, Noverlap, Nfft, 1/Ts);
end

%% Bode plot for the transfer function from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_de(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, abs(G_de(:,i, i)), ...
         'DisplayName', sprintf('$d_{e,%i}/u_%i$', i, i));
end
plot(f, abs(G_de(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$d_{e,i}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 5e-4]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 4);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, 180/pi*angle(G_de(:,i, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);

%% Bode plot of the IFF Plant (transfer function from u to Vs)
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_Vs(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, abs(G_Vs(:,i , i)), ...
         'DisplayName', sprintf('$V_{s%i}/u_%i$', i, i));
end
plot(f, abs(G_Vs(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$V_{si}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 4);
leg.ItemTokenSize(1) = 15;
ylim([1e-3, 6e1]);

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, 180/pi*angle(G_Vs(:,i, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);

%% Set to true only if all the FRF matrices should again computed
% from the experimental data
compute_frf = false;
if compute_frf

    %% Identification of the FRF matrices from u to de and to Vs
    % Load identification Data
    meas_added_mass = {...
        load('test_nhexa_identification_data_mass_0.mat', 'data'), ....
        load('test_nhexa_identification_data_mass_1.mat', 'data'), ....
        load('test_nhexa_identification_data_mass_2.mat', 'data'), ....
        load('test_nhexa_identification_data_mass_3.mat', 'data')};

    % Setup useful variables
    Ts = 1e-4; % Sampling Time [s]
    Nfft = floor(1/Ts); % Number of points for the FFT computation
    win = hanning(Nfft); % Hanning window
    Noverlap = floor(Nfft/2); % Overlap between frequency analysis

    % And we get the frequency vector
    [~, f] = tfestimate(meas_added_mass{1}.data{1}.u, meas_added_mass{1}.data{1}.de, win, Noverlap, Nfft, 1/Ts);

    % FRF from u to de
    G_de = {};

    for i_mass = [0:3]
        G_de(i_mass+1) = {zeros(length(f), 6, 6)};
        for i_strut = 1:6
            G_de{i_mass+1}(:,:,i_strut) = tfestimate(meas_added_mass{i_mass+1}.data{i_strut}.u, meas_added_mass{i_mass+1}.data{i_strut}.de, win, Noverlap, Nfft, 1/Ts);
        end
    end

    % FRF from u to Vs
    G_Vs = {};

    for i_mass = [0:3]
        G_Vs(i_mass+1) = {zeros(length(f), 6, 6)};
        for i_strut = 1:6
            G_Vs{i_mass+1}(:,:,i_strut) = tfestimate(meas_added_mass{i_mass+1}.data{i_strut}.u, meas_added_mass{i_mass+1}.data{i_strut}.Vs, win, Noverlap, Nfft, 1/Ts);
        end
    end

    % The identified dynamics are then saved for further use.
    save('./mat/test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de')

end

%% Load the identified transfer functions
frf_ol = load('test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de');

%% Bode plot for the transfer function from u to de - Several payloads
masses = [0, 13, 26, 39];
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = [0:3]
    % Diagonal terms
    plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.5], ...
         'DisplayName', sprintf('$d_{ei}/u_i$ - %i kg', masses(i_mass+1)));
    for i = 2:6
        plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5], ...
             'HandleVisibility', 'off');
    end
    % % Off-Diagonal terms
    % for i = 1:5
    %     for j = i+1:6
    %         plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,i,j)), 'color', [colors(i_mass+1,:), 0.2], ...
    %              'HandleVisibility', 'off');
    %     end
    % end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 5e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i_mass = [0:3]
    for i =1:6
        plot(frf_ol.f, 180/pi*angle(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5]);
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])

linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);

%% Bode plot for the transfer function from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = [0:3]
    % Diagonal terms
    plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.5], ...
         'DisplayName', sprintf('$V_{si}/u_i$ - %i kg', masses(i_mass+1)));
    for i = 2:6
        plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5], ...
             'HandleVisibility', 'off');
    end
    % % Off-Diagonal terms
    % for i = 1:5
    %     for j = i+1:6
    %         plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,i,j)), 'color', [colors(i_mass+1,:), 0.2], ...
    %              'HandleVisibility', 'off');
    %     end
    % end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i_mass = [0:3]
    for i =1:6
        plot(frf_ol.f, 180/pi*angle(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.5]);
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);