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


% with:
% - =dcgain=: low frequency gain
% - =[freq,mag]=: frequency =freq= at which the gain is =mag=
% - =hfgain=: high frequency gain

% #+begin_exampl
% The Matlab code below produces a weighting function with the following characteristics (Figure [[fig:first_order_weight]]):
% - Low frequency gain of 100
% - Gain of 1 at 10Hz
% - High frequency gain of 0.5


Ws = makeweight(1e2, [2*pi*10, 1], 1/2);

freqs = logspace(-2, 2, 1000);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(Ws, freqs, 'Hz'))), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;



% Let's use this function to generate three weights with the same high and low frequency gains, but but different slopes.


W1 = generateWeight('G0', 1e2, 'G1', 1/2, 'Gc', 1, 'wc', 2*pi*10, 'n', 1);
W2 = generateWeight('G0', 1e2, 'G1', 1/2, 'Gc', 1, 'wc', 2*pi*10, 'n', 2);
W3 = generateWeight('G0', 1e2, 'G1', 1/2, 'Gc', 1, 'wc', 2*pi*10, 'n', 3);



% The obtained shapes are shown in Figure [[fig:high_order_weight]].


freqs = logspace(-2, 2, 1000);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), ...
     'DisplayName', '$n = 1$');
plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), ...
     'DisplayName', '$n = 2$');
plot(freqs, abs(squeeze(freqresp(W3, freqs, 'Hz'))), ...
     'DisplayName', '$n = 3$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
legend('location', 'northeast');


% #+HTML: </details>
% #+end_exercice

% Let's say we came up with the following weighting function:

Ws = generateWeight('G0', 1e3, ...
                    'G1', 1/2, ...
                    'Gc', sqrt(2), 'wc', 2*pi*10, ...
                    'n', 2);



% The weighting function is then added to the generalized plant.

P = [1 -G;
     1 -G];
Pw = blkdiag(Ws, 1)*P;



% And the $\mathcal{H}_\infty$ synthesis is performed on the /weighted/ generalized plant.

K = hinfsyn(Pw, 1, 1, 'Display', 'on');



% #+RESULTS:
% #+begin_example
%   Test bounds:  0.5 <=  gamma  <=  0.51

%    gamma        X>=0        Y>=0       rho(XY)<1    p/f
%   5.05e-01     0.0e+00     0.0e+00     3.000e-16     p
%   Limiting gains...
%   5.05e-01     0.0e+00     0.0e+00     3.461e-16     p
%   5.05e-01    -3.5e+01 #  -4.9e-14     1.732e-26     f

%   Best performance (actual): 0.503
% #+end_example

% $\gamma \approx 0.5$ means that the $\mathcal{H}_\infty$ synthesis generated a controller $K(s)$ that stabilizes the closed-loop system, and such that the $\mathcal{H}_\infty$ norm of the closed-loop transfer function from $w$ to $z$ is less than $\gamma$:
% \begin{align*}
%   & \| W_s(s) S(s) \|_\infty \approx 0.5 \\
%   & \Leftrightarrow |S(j\omega)| < \frac{0.5}{|W_s(j\omega)|} \quad \forall \omega
% \end{align*}

% This is indeed what we can see by comparing $|S|$ and $|W_S|$ in Figure [[fig:results_sensitivity_hinf]].

% #+begin_important
% Obtaining $\gamma < 1$ means that the $\mathcal{H}_\infty$ synthesis found a controller such that the specified closed-loop transfer functions are bellow the specified upper bounds.

% Yet, obtaining a $\gamma$ slightly above one does not necessary means the synthesis is unsuccessful.
% It just means that at some frequency, one of the closed-loop transfer functions is above the specified upper bound by a factor $\gamma$.
% #+end_important


figure;
hold on;
plot(freqs, 1./abs(squeeze(freqresp(Ws, freqs, 'Hz'))), 'k--', 'DisplayName', '$|W_s|^{-1}$');
plot(freqs, abs(squeeze(freqresp(1/(1 + K*G), freqs, 'Hz'))), 'k-', 'DisplayName', '$|S|$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
legend('location', 'southeast', 'FontSize', 8);