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

% Effect of the rotation speed on the IFF plant dynamics
% The transfer functions from actuator forces $[F_u,\ F_v]$ to the measured force sensors $[f_u,\ f_v]$ are identified for several rotating velocities and shown in Figure ref:fig:rotating_iff_bode_plot_effect_rot.

% As was expected from the derived equations of motion:
% - when $0 < \Omega < \omega_0$: the low frequency gain is no longer zero and two (non-minimum phase) real zero appears at low frequency.
%   The low frequency gain increases with $\Omega$.
%   A pair of (minimum phase) complex conjugate zeros appears between the two complex conjugate poles that are split further apart as $\Omega$ increases.
% - when $\omega_0 < \Omega$: the low frequency pole becomes unstable.


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

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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
for i = 1:length(Wzs)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
title('Direct terms: $f_u/F_u$, $f_v/F_v$');
ylim([1e-3, 1e2]);

ax2 = nexttile([2, 1]);
hold on;
for i = 1:length(Wzs)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('fv', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0$', Wzs(i)))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
title('Coupling Terms: $f_u/F_v$, $f_v/F_u$');
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [10, 1];
ylim([1e-3, 1e2]);

ax3 = nexttile;
hold on;
for i = 1:length(Wzs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{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([-180 180]);
xticks([1e-2,1e-1,1,1e1])
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

ax4 = nexttile;
hold on;
for i = 1:length(Wzs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('fv', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); set(gca, 'YTickLabel',[]);
yticks(-180:90:180);
ylim([-180 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,ax3,ax4],'x');
xlim([freqs(1), freqs(end)]);

linkaxes([ax1,ax2],'y');



% #+name: fig:rotating_iff_diagram
% #+caption: Control diagram for decentralized Integral Force Feedback
% #+RESULTS:
% [[file:figs/rotating_iff_diagram.png]]

% The decentralized IFF controller $\bm{K}_F$ corresponds to a diagonal controller with integrators:
% #+name: eq:Kf_pure_int
% \begin{equation}
% \begin{aligned}
%   \mathbf{K}_{F}(s) &= \begin{bmatrix} K_{F}(s) & 0 \\ 0 & K_{F}(s) \end{bmatrix} \\
%   K_{F}(s) &= g \cdot \frac{1}{s}
% \end{aligned}
% \end{equation}

% In order to see how the IFF controller affects the poles of the closed loop system, a Root Locus plot (Figure ref:fig:rotating_root_locus_iff_pure_int) is constructed as follows: the poles of the closed-loop system are drawn in the complex plane as the controller gain $g$ varies from $0$ to $\infty$ for the two controllers $K_{F}$ simultaneously.
% As explained in cite:preumont08_trans_zeros_struc_contr_with,skogestad07_multiv_feedb_contr, the closed-loop poles start at the open-loop poles (shown by $\tikz[baseline=-0.6ex] \node[cross out, draw=black, minimum size=1ex, line width=2pt, inner sep=0pt, outer sep=0pt] at (0, 0){};$) for $g = 0$ and coincide with the transmission zeros (shown by $\tikz[baseline=-0.6ex] \draw[line width=2pt, inner sep=0pt, outer sep=0pt] (0,0) circle[radius=3pt];$) as $g \to \infty$.

% #+begin_important
% Whereas collocated IFF is usually associated with unconditional stability cite:preumont91_activ, this property is lost due to gyroscopic effects as soon as the rotation velocity in non-null.
% This can be seen in the Root Locus plot (Figure ref:fig:rotating_root_locus_iff_pure_int) where poles corresponding to the controller are bound to the right half plane implying closed-loop system instability.
% #+end_important

% Physically, this can be explained like so: at low frequency, the loop gain is very large due to the pure integrator in $K_{F}$ and the finite gain of the plant (Figure ref:fig:rotating_iff_bode_plot_effect_rot).
% The control system is thus canceling the spring forces which makes the suspended platform no able to hold the payload against centrifugal forces, hence the instability.


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