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

%% Load "Generic" system dynamics
load('rotating_generic_plants.mat', 'Gs', 'Wzs');

%% Bode plot of the direct and coupling term for Integral Force Feedback - Effect of rotation
freqs = logspace(-2, 1, 1000);

Wz_i = [1,3,4];

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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
for i = 1:length(Wz_i)
    plot(freqs, abs(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
ylim([1e-3, 1e2]);
leg = legend('location', 'northwest', 'FontSize', 8);

ax2 = nexttile;
hold on;
for i = 1:length(Wz_i)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([0 180]);
xticks([1e-2,1e-1,1,1e1])
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);

%% Root Locus for the Decentralized Integral Force Feedback controller
Kiff = 1/s*eye(2);

gains = logspace(-2, 4, 300);
Wz_i = [1,3,4];

figure;
hold on;
for i = 1:length(Wz_i)
    plot(real(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(i,:), ...
         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
    plot(real(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(i,:), ...
         'HandleVisibility', 'off','MarkerSize',8);
    for g = gains
        cl_poles = pole(feedback(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff, -1));
        plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(i,:), ...
             'HandleVisibility', 'off','MarkerSize',4);
    end
end
hold off;
axis square;
xlim([-1.8, 0.2]); ylim([0, 2]);
xticks([-1, 0])
xticklabels({'-$\omega_0$', '$0$'})
yticks([0, 1, 2])
yticklabels({'$0$', '$\omega_0$', '$2 \omega_0$'})

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