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

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

freqs = logspace(-1, 3, 1000);

addpath('./src');

% Design of Weighting Function
% A formula is proposed to help the design of the weighting functions:
% \begin{equation}
%   W(s) = \left( \frac{
%            \frac{1}{\omega_0} \sqrt{\frac{1 - \left(\frac{G_0}{G_c}\right)^{\frac{2}{n}}}{1 - \left(\frac{G_c}{G_\infty}\right)^{\frac{2}{n}}}} s + \left(\frac{G_0}{G_c}\right)^{\frac{1}{n}}
%          }{
%            \left(\frac{1}{G_\infty}\right)^{\frac{1}{n}} \frac{1}{\omega_0} \sqrt{\frac{1 - \left(\frac{G_0}{G_c}\right)^{\frac{2}{n}}}{1 - \left(\frac{G_c}{G_\infty}\right)^{\frac{2}{n}}}} s + \left(\frac{1}{G_c}\right)^{\frac{1}{n}}
%          }\right)^n
% \end{equation}

% The parameters permits to specify:
% - the low frequency gain: $G_0 = lim_{\omega \to 0} |W(j\omega)|$
% - the high frequency gain: $G_\infty = lim_{\omega \to \infty} |W(j\omega)|$
% - the absolute gain at $\omega_0$: $G_c = |W(j\omega_0)|$
% - the absolute slope between high and low frequency: $n$

% The general shape of a weighting function generated using the formula is shown in figure [[fig:weight_formula]].


n = 3; w0 = 2*pi*10; G0 = 1e-3; G1 = 1e1; Gc = 2;

W = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(W, freqs, 'Hz'))), 'k-');

plot([1e-3 1e0], [G0 G0], 'k--', 'LineWidth', 1)
text(1e0, G0, '$\quad G_0$')

plot([1e1 1e3], [G1 G1], 'k--', 'LineWidth', 1)
text(1e1,G1,'$G_{\infty}\quad$','HorizontalAlignment', 'right')

plot([w0/2/pi w0/2/pi], [1 2*Gc], 'k--', 'LineWidth', 1)
text(w0/2/pi,1,'$\omega_c$','VerticalAlignment', 'top', 'HorizontalAlignment', 'center')

plot([w0/2/pi/2  2*w0/2/pi], [Gc Gc], 'k--', 'LineWidth', 1)
text(w0/2/pi/2, Gc, '$G_c \quad$','HorizontalAlignment', 'right')

text(w0/5/pi/2, abs(evalfr(W, j*w0/5)), 'Slope: $n \quad$', 'HorizontalAlignment', 'right')

text(w0/2/pi, abs(evalfr(W, j*w0)), '$\bullet$', 'HorizontalAlignment', 'center')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([5e-4, 20]);



% #+name: fig:weight_formula
% #+caption: Gain of the Weighting Function formula
% #+RESULTS:
% [[file:figs/weight_formula.png]]


n = 2; w0 = 2*pi*10; G0 = 1/10; G1 = 1000; Gc = 0.45;
W1 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;

n = 3; w0 = 2*pi*10; G0 = 1000; G1 = 0.1; Gc = 0.45;
W2 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;

figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$|W_1|^{-1}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-4, 20]);
xticks([0.1, 1, 10, 100, 1000]);
leg = legend('location', 'southeast', 'FontSize', 8);
leg.ItemTokenSize(1) = 18;

% H-Infinity Synthesis
% We define the generalized plant $P$ on matlab.

P = [W1 -W1;
     0   W2;
     1   0];



% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.

[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');



% #+RESULTS:
% #+begin_example
% [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');

%   Test bounds:  0.3223 <=  gamma  <=  1000

%     gamma        X>=0        Y>=0       rho(XY)<1    p/f
%   1.795e+01     1.4e-07     0.0e+00     1.481e-16     p
%   2.406e+00     1.4e-07     0.0e+00     3.604e-15     p
%   8.806e-01    -3.1e+02 #  -1.4e-16     7.370e-19     f
%   1.456e+00     1.4e-07     0.0e+00     1.499e-18     p
%   1.132e+00     1.4e-07     0.0e+00     8.587e-15     p
%   9.985e-01     1.4e-07     0.0e+00     2.331e-13     p
%   9.377e-01    -7.7e+02 #  -6.6e-17     3.744e-14     f
%   9.676e-01    -2.0e+03 #  -5.7e-17     1.046e-13     f
%   9.829e-01    -6.6e+03 #  -1.1e-16     2.949e-13     f
%   9.907e-01     1.4e-07     0.0e+00     2.374e-19     p
%   9.868e-01    -1.6e+04 #  -6.4e-17     5.331e-14     f
%   9.887e-01    -5.1e+04 #  -1.5e-17     2.703e-19     f
%   9.897e-01     1.4e-07     0.0e+00     1.583e-11     p
%   Limiting gains...
%   9.897e-01     1.5e-07     0.0e+00     1.183e-12     p
%   9.897e-01     6.9e-07     0.0e+00     1.365e-12     p

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

% We then define the high pass filter $H_1 = 1 - H_2$. The bode plot of both $H_1$ and $H_2$ is shown on figure [[fig:hinf_filters_results]].


H1 = 1 - H2;

% Obtained Complementary Filters
% The obtained complementary filters are shown on figure [[fig:hinf_filters_results]].


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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$');

set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude');
set(gca, 'XTickLabel',[]);
ylim([1e-4, 20]);
yticks([1e-4, 1e-3, 1e-2, 1e-1, 1, 1e1]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 18;

% Phase
ax2 = nexttile;
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
hold off;
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
set(gca, 'XScale', 'log');
yticks([-180:90:180]);

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