%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');



% Let's define the system parameters on Matlab.

k = 1e6; % Stiffness [N/m]
c = 4e2; % Damping [N/(m/s)]
m = 10; % Mass [kg]



% And now the system dynamics $G(s)$ and $G_d(s)$.

G = 1/(m*s^2 + c*s + k); % Plant
Gd = (c*s + k)/(m*s^2 + c*s + k); % Disturbance



% #+HTML: <details><summary>Hint</summary>
% #+LATEX: \tcbsubtitle{Hint}
% You can follow this procedure:
% 1. In order to have good disturbance rejection at low frequency, add a simple or double *integrator*
% 2. In terms of the loop gain, the *bandwidth* can be defined at the frequency $\omega_c$ where $|l(j\omega_c)|$ first crosses 1 from above.
%    Therefore, adjust the *gain* such that $L(j\omega)$ crosses 1 at around 10Hz
% 3. The roll-off at high frequency for noise attenuation should already be good enough.
%    If not, add a *low pass filter*
% 4. Add a *Lead* centered around the crossover frequency (10 Hz) and tune it such that sufficient phase margin is added.
%    Verify that the gain margin is good enough.
% #+HTML: </details>
% #+end_exercice

% Let's say we came up with the following controller.

K = 14e8 * ...              % Gain
    1/(s^2) * ...           % Double Integrator
    1/(1 + s/2/pi/40) * ... % Low Pass Filter
    (1 + s/(2*pi*10/sqrt(8)))/(1 + s/(2*pi*10*sqrt(8))); % Lead



% The bode plot of the Loop Gain is shown in Figure [[fig:loop_gain_manual_afm]] and we can verify that we have the wanted stability margins using the =margin= command:

[Gm, Pm, ~, Wc] = margin(G*K);



% #+RESULTS:
% | Requirements                | Manual Method |
% |-----------------------------+---------------|
% | Gain Margin $> 3$ [dB]      |           3.1 |
% | Phase Margin $> 30$ [deg]   |          35.4 |
% | Crossover $\approx 10$ [Hz] |          10.1 |


freqs = logspace(0, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G*K, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-4, 1e1])

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G*K, freqs, 'Hz')))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
yticks(-360:90:360); ylim([-360, 0]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;

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

% Example of the $\mathcal{H}_\infty$ Loop Shaping Synthesis
% <<sec:h_infinity_open_loop_shaping_example>>

% To apply the $\mathcal{H}_\infty$ Loop Shaping Synthesis, the wanted shape of the loop gain should be determined from the specifications.
% This is summarized in Table [[tab:open_loop_shaping_specifications]].

% Such shape corresponds to the typical wanted Loop gain Shape shown in Figure [[fig:open_loop_shaping_shape]].

% #+name: tab:open_loop_shaping_specifications
% #+caption: Wanted Loop Shape corresponding to each specification
% #+attr_latex: :environment tabularx :width \linewidth :align lXX
% |              | *Specification*                        | *Corresponding Loop Shape*               |
% |--------------+----------------------------------------+------------------------------------------|
% | *Dist. Rej.* | Highest possible rejection below 1Hz   | Slope of -40dB/dec at low frequency      |
% | *Pos. Speed* | Bandwidth of approximately 10Hz        | $L$ crosses 1 at 10Hz                    |
% | *Noise Att.* | Roll-off of -40dB/decade past 30Hz     | Roll-off of -40dB/decade past 30Hz       |
% | *Robustness* | $\Delta G > 3dB$, $\Delta \phi > 30^o$ | Slope of -20dB/decade near the crossover |

% Then, a (stable, minimum phase) transfer function $L_w(s)$ should be created that has the same gain as the wanted shape of the Loop gain.
% For this example, a double integrator and a lead centered on 10Hz are used.
% Then the gain is adjusted such that the $|L_w(j2 \pi 10)| = 1$.

% Using Matlab, we have:

Lw = 2.3e3 * ...
     1/(s^2) * ... % Double Integrator
     (1 + s/(2*pi*10/sqrt(3)))/(1 + s/(2*pi*10*sqrt(3))); % Lead



% The $\mathcal{H}_\infty$ open loop shaping synthesis is then performed using the =loopsyn= command:

[K, ~, GAM] = loopsyn(G, Lw);



% The obtained Loop Gain is shown in Figure [[fig:open_loop_shaping_hinf_L]] and matches the specified one by a factor $\gamma \approx 2$.


freqs = logspace(0, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G*K, freqs, 'Hz'))), 'DisplayName', '$L(s)$');
plot(freqs, abs(squeeze(freqresp(Lw, freqs, 'Hz'))), 'k--', 'DisplayName', '$L_w(s)$');
plot(freqs, abs(squeeze(freqresp(Lw, freqs, 'Hz')))*GAM, 'k-.', 'DisplayName', '$L_w(s) / \gamma$, $L_w(s) \cdot \gamma$');
plot(freqs, abs(squeeze(freqresp(Lw, freqs, 'Hz')))/GAM, 'k-.', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast');
ylim([1e-4, 1e2]);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G*K, freqs, 'Hz')))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
yticks(-360:90:360); ylim([-360, 0]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;

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



% #+name: fig:open_loop_shaping_hinf_L
% #+caption: Obtained Open Loop Gain $L(s) = G(s) K(s)$ and comparison with the wanted Loop gain $L_w$
% #+RESULTS:
% [[file:figs/open_loop_shaping_hinf_L.png]]


% #+begin_important
% When using the $\mathcal{H}_\infty$ Synthesis, it is usually recommended to analyze the obtained controller.

% This is usually done by breaking down the controller into simple elements such as low pass filters, high pass filters, notches, leads, etc.
% #+end_important

% Let's briefly analyze the obtained controller which bode plot is shown in Figure [[fig:open_loop_shaping_hinf_K]]:
% - two integrators are used at low frequency to have the wanted low frequency high gain
% - a lead is added centered with the crossover frequency to increase the phase margin
% - a notch is added at the resonance of the plant to increase the gain margin (this is very typical of $\mathcal{H}_\infty$ controllers, and can be an issue, more info on that latter)


freqs = logspace(0, 3, 1000);

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

ax1 = nexttile([2,1]);
plot(freqs, abs(squeeze(freqresp(K, freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
hold off;

ax2 = nexttile;
plot(freqs, 180/pi*angle(squeeze(freqresp(K, freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
yticks(-360:90:360); ylim([-180, 90]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');

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



% #+name: fig:open_loop_shaping_hinf_K
% #+caption: Obtained controller $K$ using the open-loop $\mathcal{H}_\infty$ shaping
% #+RESULTS:
% [[file:figs/open_loop_shaping_hinf_K.png]]


% Let's finally compare the obtained stability margins of the $\mathcal{H}_\infty$ controller and of the manually developed controller in Table [[tab:open_loop_shaping_compare]].


[Gm_2, Pm_2, ~, Wc_2] = margin(G*K);