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

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

%% Path for functions, data and scripts
addpath('./src/'); % Path for functions

%% Colors for the figures
colors = colororder;

%% Initialize Frequency Vector
freqs = logspace(-1, 3, 1000);

%% Weighting Function Design
% Parameters
n = 3; w0 = 2*pi*10; G0 = 1e-3; G1 = 1e1; Gc = 2;

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

% Function generateWF can be used to easily design the weighting filters
% W = generateWF('n', 3, 'w0', 2*pi*10, 'G0', 1e-3, 'Ginf', 10, 'Gc', 2);

%% Magnitude of the weighting function with parameters
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]);
yticks([1e-4, 1e-3, 1e-2, 1e-1, 1, 1e1]);

%% Synthesis of Complementary Filters using H-infinity synthesis
% Design of the Weighting Functions
W1 = generateWF('n', 3, 'w0', 2*pi*10, 'G0', 1000, 'Ginf', 1/10, 'Gc', 0.45);
W2 = generateWF('n', 2, 'w0', 2*pi*10, 'G0', 1/10, 'Ginf', 1000, 'Gc', 0.45);

% Generalized Plant
P = [W1 -W1;
     0   W2;
     1   0];

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

% Define H1 to be the complementary of H2
H1 = 1 - H2;

% The function generateCF can also be used to synthesize the complementary filters.
% [H1, H2] = generateCF(W1, W2);

%% Bode plot of the obtained complementary filters
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', '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|^{-1}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');

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$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
ylim([8e-4, 20]);
yticks([1e-3, 1e-2, 1e-1, 1, 1e1]);
yticklabels({'', '$10^{-2}$', '', '$10^0$', ''})
leg = legend('location', 'south', '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;
set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
yticks([-180:90:180]);
ylim([-180, 200])
yticklabels({'-180', '', '0', '', '180'})

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

%% Design of "Closed-loop" complementary filters
% Design of the Weighting Functions
W1 = generateWF('n', 3, 'w0', 2*pi*10, 'G0', 1000, 'Ginf', 1/10, 'Gc', 0.45);
W2 = generateWF('n', 2, 'w0', 2*pi*10, 'G0', 1/10, 'Ginf', 1000, 'Gc', 0.45);

% Generalized plant for "closed-loop" complementary filter synthesis
P = [ W1 0   1;
     -W1 W2 -1];

% Standard H-Infinity Synthesis
[L, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');

% Complementary filters
H1 = inv(1 + L);
H2 = 1 - H1;

%% Bode plot of the obtained complementary filters after H-infinity mixed-sensitivity synthesis
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', '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|^{-1}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');

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

plot(freqs, abs(squeeze(freqresp(L, freqs, 'Hz'))), 'k--', 'DisplayName', '$|L|$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
ylim([1e-3, 1e3]);
yticks([1e-3, 1e-2, 1e-1, 1, 1e1, 1e2, 1e3]);
yticklabels({'', '$10^{-2}$', '', '$10^0$', '', '$10^2$', ''});
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
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;
set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
yticks([-180:90:180]);
ylim([-180, 200])
yticklabels({'-180', '', '0', '', '180'})

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

%% Synthesis of a set of three complementary filters
% Design of the Weighting Functions
W1 = generateWF('n', 2, 'w0', 2*pi*1, 'G0', 1/10, 'Ginf', 1000, 'Gc', 0.5);
W2 = 0.22*(1 + s/2/pi/1)^2/(sqrt(1e-4) + s/2/pi/1)^2*(1 + s/2/pi/10)^2/(1 + s/2/pi/1000)^2;
W3 = generateWF('n', 3, 'w0', 2*pi*10, 'G0', 1000, 'Ginf', 1/10, 'Gc', 0.5);

% Generalized plant for the synthesis of 3 complementary filters
P = [W1 -W1 -W1;
     0   W2  0 ;
     0   0   W3;
     1   0   0];

% Standard H-Infinity Synthesis
[H, ~, gamma, ~] = hinfsyn(P, 1, 2,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');

% Synthesized H2 and H3 filters
H2 = tf(H(1));
H3 = tf(H(2));

% H1 is defined as the complementary filter of H2 and H3
H1 = 1 - H2 - H3;

%% Bode plot of the inverse weighting functions and of the three complementary filters obtained using the H-infinity synthesis
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', '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|^{-1}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');
set(gca,'ColorOrderIndex',3)
plot(freqs, 1./abs(squeeze(freqresp(W3, freqs, 'Hz'))), '--', 'DisplayName', '$|W_3|^{-1}$');
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,'ColorOrderIndex',3)
plot(freqs, abs(squeeze(freqresp(H3, freqs, 'Hz'))), '-', 'DisplayName', '$H_3$');
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]);
leg = legend('location', 'northeast', 'FontSize', 8);
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'))));
set(gca,'ColorOrderIndex',3)
plot(freqs, 180/pi*phase(squeeze(freqresp(H3, freqs, 'Hz'))));
hold off;
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
set(gca, 'XScale', 'log');
yticks([-180:90:180]); ylim([-220, 220]);

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