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

%% Colors for the figures
colors = colororder;

%% Simscape model name
mdl = 'rotating_model';

%% The rotating speed is set to $\Omega = 0.1 \omega_0$.
Wz = 0.1; % [rad/s]

%% Masses
ms = 0.5; % Sample mass [kg]
mn = 0.5; % Tuv mass [kg]

%% General Configuration
model_config = struct();
model_config.controller = "open_loop"; % Default: Open-Loop

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],        1, 'openinput');  io_i = io_i + 1; % [Fu, Fv]
io(io_i) = linio([mdl, '/fd'],                1, 'openinput');  io_i = io_i + 1; % [Fdu, Fdv]
io(io_i) = linio([mdl, '/xf'],                1, 'openinput');  io_i = io_i + 1; % [Dfx, Dfy]
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
io(io_i) = linio([mdl, '/ext_metrology'],     1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]

%% Identifying plant with parallel stiffness
model_config.Tuv_type   = "parallel_k";

% Parallel stiffness
kp = 2*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]

% Tuv Stage
kn = 1 - kp; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

% Linearize
G_kp = linearize(mdl, io, 0);
G_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% Identifying plant with no parallel stiffness
model_config.Tuv_type   = "normal";

% Tuv Stage
kn = 1; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

% Linearize
G = linearize(mdl, io, 0);
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% IFF Controller
Kiff = (2.2/(s + 0.1))*eye(2);
Kiff.InputName = {'fu', 'fv'};
Kiff.OutputName = {'Fu', 'Fv'};

%% IFF Controller with added stiffness
Kiff_kp = (2.2/(s + 0.1))*eye(2);
Kiff_kp.InputName = {'fu', 'fv'};
Kiff_kp.OutputName = {'Fu', 'Fv'};

%% Relative Damping Controller
Krdc = 2*s*eye(2);
Krdc.InputName = {'Du', 'Dv'};
Krdc.OutputName = {'Fu', 'Fv'};

%% Comparison of active damping techniques for rotating platform - Root Locus
gains = logspace(-2, 2, 500);

figure;
hold on;
% IFF
plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(1,:), ...
     'DisplayName', 'IFF with HPF', 'MarkerSize', 8);
plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off', 'MarkerSize', 8);
for g = gains
    cl_poles = pole(feedback(G({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:),'MarkerSize',4, ...
         'HandleVisibility', 'off');
end
% IFF with parallel stiffness
plot(real(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)),  imag(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'x', 'color', colors(2,:), ...
     'DisplayName', 'IFF with $k_p$', 'MarkerSize', 8);
plot(real(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)),  imag(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off', 'MarkerSize', 8);
for g = gains
    cl_poles = pole(feedback(G_kp({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff_kp));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:),'MarkerSize',4, ...
         'HandleVisibility', 'off');
end
% RDC
plot(real(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)),  imag(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(3,:), ...
     'DisplayName', 'RDC', 'MarkerSize', 8);
plot(real(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)),  imag(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off', 'MarkerSize', 8);
for g = gains
    cl_poles = pole(feedback(G({'Du', 'Dv'}, {'Fu', 'Fv'}), g*Krdc));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:),'MarkerSize',4, ...
         'HandleVisibility', 'off');
end
hold off;
axis square;
xlim([-1.15, 0.05]); ylim([0, 1.2]);

xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 12;

%% Compute Damped plants
G_cl_iff    = feedback(G,    Kiff,    'name');
G_cl_iff_kp = feedback(G_kp, Kiff_kp, 'name');
G_cl_rdc    = feedback(G,    Krdc,    'name');

%% Comparison of the damped plants obtained with the three active damping techniques
freqs = logspace(-2, 2, 1000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G(          'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
    'DisplayName', 'OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
    'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
    'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
    'DisplayName', 'RDC')
plot(freqs, abs(squeeze(freqresp(G(          'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros(1,3), 0.5], ...
    'DisplayName', 'Coupling')
plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
    'DisplayName', 'Coupling')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(2,:), 0.5], ...
    'DisplayName', 'Coupling')
plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(3,:), 0.5], ...
    'DisplayName', 'Coupling')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
ldg.ItemTokenSize = [10, 1];
ylim([1e-6, 1e2])

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G(          'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_rdc(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(3,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 15]);

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([1e-2,1e-1,1,1e1,1e2])
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$', '$100 \omega_0$'})

%% Comparison of the obtained transmissibility and compliance for the three tested active damping techniques
freqs = logspace(-2, 2, 1000);

% transmissibility
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G(          'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
    'DisplayName', 'OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
    'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
    'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
    'DisplayName', 'RDC')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]');
xlim([freqs(1), freqs(end)]);

% Compliance
figure;
ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G(          'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
    'DisplayName', 'OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
    'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
    'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
    'DisplayName', 'RDC')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
ldg.ItemTokenSize = [10, 1];
xlim([freqs(1), freqs(end)]);
