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

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

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

%% Add functions to path
addpath('./src');

%% Upper bounds for the complementary filters
figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot([0.0001, 0.008], [8e-3, 8e-3], ':', 'DisplayName', 'Spec. on $H_H$');
set(gca,'ColorOrderIndex',1)
plot([0.008 0.04], [8e-3, 1], ':', 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',1)
plot([0.04 0.1], [3, 3], ':', 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2)
plot([0.1, 10], [0.045, 0.045], ':', 'DisplayName', 'Spec. on $H_L$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-4, 10]);
leg = legend('location', 'southeast', 'FontSize', 8);
leg.ItemTokenSize(1) = 18;

%% Initialized CVX
cvx_startup;
cvx_solver sedumi;

%% Frequency vectors
w1 = 0:4.06e-4:0.008;
w2 = 0.008:4.06e-4:0.04;
w3 = 0.04:8.12e-4:0.1;
w4 = 0.1:8.12e-4:0.83;

%% Filter order
n = 512;

%% Initialization of filter responses
A1 = [ones(length(w1),1),  cos(kron(w1'.*(2*pi),[1:n-1]))];
A2 = [ones(length(w2),1),  cos(kron(w2'.*(2*pi),[1:n-1]))];
A3 = [ones(length(w3),1),  cos(kron(w3'.*(2*pi),[1:n-1]))];
A4 = [ones(length(w4),1),  cos(kron(w4'.*(2*pi),[1:n-1]))];

B1 = [zeros(length(w1),1), sin(kron(w1'.*(2*pi),[1:n-1]))];
B2 = [zeros(length(w2),1), sin(kron(w2'.*(2*pi),[1:n-1]))];
B3 = [zeros(length(w3),1), sin(kron(w3'.*(2*pi),[1:n-1]))];
B4 = [zeros(length(w4),1), sin(kron(w4'.*(2*pi),[1:n-1]))];

%% Convex optimization
cvx_begin

variable y(n+1,1)

% t
maximize(-y(1))

for i = 1:length(w1)
    norm([0 A1(i,:); 0 B1(i,:)]*y) <= 8e-3;
end

for  i = 1:length(w2)
    norm([0 A2(i,:); 0 B2(i,:)]*y) <= 8e-3*(2*pi*w2(i)/(0.008*2*pi))^3;
end

for i = 1:length(w3)
    norm([0 A3(i,:); 0 B3(i,:)]*y) <= 3;
end

for i = 1:length(w4)
    norm([[1 0]'- [0 A4(i,:); 0 B4(i,:)]*y]) <= y(1);
end

cvx_end

h = y(2:end);

%% Combine the frequency vectors to form the obtained filter
w = [w1 w2 w3 w4];
H = [exp(-j*kron(w'.*2*pi,[0:n-1]))]*h;

%% Bode plot of the obtained complementary filters
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
set(gca,'ColorOrderIndex',1)
plot(w, abs(1-H), '-', 'DisplayName', '$L_1$');
plot([0.1, 10], [0.045, 0.045], 'k:', 'DisplayName', 'Spec. on $L_1$');

set(gca,'ColorOrderIndex',2)
plot(w, abs(H), '-', 'DisplayName', '$H_1$');
plot([0.0001, 0.008], [8e-3, 8e-3], 'k--', 'DisplayName', 'Spec. on $H_1$');
plot([0.008 0.04], [8e-3, 1],       'k--', 'HandleVisibility', 'off');
plot([0.04 0.1], [3, 3],            'k--', 'HandleVisibility', 'off');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
hold off;
ylim([5e-3, 10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 16;

% Phase
ax2 = nexttile;
hold on;
plot(w, 180/pi*unwrap(angle(1-H)), '-');
plot(w, 180/pi*unwrap(angle(H)), '-');
hold off;
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
set(gca, 'XScale', 'log');
yticks([-360:180:180]); ylim([-380, 200]);

linkaxes([ax1,ax2],'x');
xlim([1e-3, 1]);

%% Design of the weight for the high pass filter
w1 = 2*pi*0.008; x1 = 0.35;
w2 = 2*pi*0.04;  x2 = 0.5;
w3 = 2*pi*0.05;  x3 = 0.5;

% Slope of +3 from w1
wH = 0.008*(s^2/w1^2 + 2*x1/w1*s + 1)*(s/w1 + 1);
% Little bump from w2 to w3
wH = wH*(s^2/w2^2 + 2*x2/w2*s + 1)/(s^2/w3^2 + 2*x3/w3*s + 1);
% No Slope at high frequencies
wH = wH/(s^2/w3^2 + 2*x3/w3*s + 1)/(s/w3 + 1);
% Little bump between w2 and w3
w0 = 2*pi*0.045; xi = 0.1; A = 2; n = 1;
wH = wH*((s^2 + 2*w0*xi*A^(1/n)*s + w0^2)/(s^2 + 2*w0*xi*s + w0^2))^n;

wH = 1/wH;
wH = minreal(ss(wH));

%% Design of the weight for the low pass filter
n = 20; % Filter order
Rp = 1; % Peak to peak passband ripple
Wp = 2*pi*0.102; % Edge frequency

% Chebyshev Type I filter design
[b,a] = cheby1(n, Rp, Wp, 'high', 's');
wL = 0.04*tf(a, b);

wL = 1/wL;
wL = minreal(ss(wL));

%% Magnitude of the designed Weights and initial specifications
figure;
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(inv(wL), freqs, 'Hz'))), '-', 'DisplayName', '$|W_L|^{-1}$');
plot([0.1, 10], [0.045, 0.045],     'k:',  'DisplayName', 'Spec. on $L_1$');

set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(inv(wH), freqs, 'Hz'))), '-', 'DisplayName', '$|W_H|^{-1}$');
plot([0.0001, 0.008], [8e-3, 8e-3], 'k--', 'DisplayName', 'Spec. on $H_1$');
plot([0.008 0.04], [8e-3, 1],       'k--', 'HandleVisibility', 'off');
plot([0.04 0.1], [3, 3],            'k--', 'HandleVisibility', 'off');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([5e-3, 10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 16;

%% Generalized plant for the H-infinity Synthesis
P = [0   wL;
     wH -wH;
     1   0];

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

%% High pass filter as the complementary of the low pass filter
Hh = 1 - Hl;

%% Minimum realization of the filters
Hh = minreal(Hh);
Hl = minreal(Hl);

%% Bode plot of the obtained filters and comparison with the upper bounds
figure;
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Hl, freqs, 'Hz'))), '-', 'DisplayName', '$L_1^\prime$');
plot([0.1, 10], [0.045, 0.045],     'k:',  'DisplayName', 'Spec. on $L_1$');

set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Hh, freqs, 'Hz'))), '-', 'DisplayName', '$H_1^\prime$');
plot([0.0001, 0.008], [8e-3, 8e-3], 'k--', 'DisplayName', 'Spec. on $H_1$');
plot([0.008 0.04], [8e-3, 1],       'k--', 'HandleVisibility', 'off');
plot([0.04 0.1], [3, 3],            'k--', 'HandleVisibility', 'off');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([5e-3, 10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 16;

%% Comparison of the complementary filters obtained with H-infinity and with CVX
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Hl, freqs, 'Hz'))), '-', ...
     'DisplayName', '$L_1(s)$ - $\mathcal{H}_\infty$');
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Hh, freqs, 'Hz'))), '-', ...
     'DisplayName', '$H_1(s)$ - $\mathcal{H}_\infty$');

set(gca,'ColorOrderIndex',1);
plot(w, abs(1-H), '--', ...
     'DisplayName', '$L_1(s)$ - FIR');
set(gca,'ColorOrderIndex',2);
plot(w, abs(H), '--', ...
     'DisplayName', '$H_1(s)$ - FIR');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude');
set(gca, 'XTickLabel',[]);
ylim([5e-3, 10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 16;

% Phase
ax2 = nexttile;
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Hl, freqs, 'Hz')))), '-');
set(gca,'ColorOrderIndex',2);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Hh, freqs, 'Hz')))), '-');

set(gca,'ColorOrderIndex',1);
plot(w, 180/pi*unwrap(angle(1-H)), '--');
set(gca,'ColorOrderIndex',2);
plot(w, 180/pi*unwrap(angle(H)), '--');
set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks([-360:180:180]); ylim([-380, 200]);

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