G = [87.8, -86.4; 108.2, -109.6]*tf(1,[75,1]);
Wp = 0.5*tf([10,1], [10,1.e-5])*eye(2);
Wi = tf([1, 0.2], [0.5,1])*eye(2);

G.InputName = 'Gi';
G.OutputName = 'Go';
Wp.InputName = 'Wpi';
Wp.OutputName = 'z';
Wi.InputName = 'u';
Wi.OutputName = 'v';

% interconnection
ic1 = sumblk('y = -Wpi',2);
ic2 = sumblk('Wpi = w + Go',2);
ic3 = sumblk('Gi = u + eta',2);

% input and output
input_var = {'eta', 'w', 'u'};
output_var = {'v', 'z', 'y'};

P = connect(G,Wp,Wi,ic1,ic2,ic3,input_var,output_var);

omega = logspace(-3,3,61);
blk = [1 1; 1 1; 2 2];

opts = hinfsynOptions('Display','on','AbsTol',1e-6,'RelTol',1e-3);
Niter= 4;
d1 = 1;
d2 = 1;
legendEntries = cell(1,Niter);
h = [];

for iter = 1 : Niter
    disp(['Running iteration step ',num2str(iter)]);
    % Step 1: K-step: Hinf synthesis
    D = append(d1, d2, tf(eye(2)));
    Daug = append(D, eye(2));
    [K, Nsc, gamma, info] = hinfsyn(Daug*P*inv(Daug),2,2,opts);

    % Step 2: D-step: mussv
    N = frd(lft(P,K),omega);
    [mu_bnds,mu_info] = mussv(N,blk);
    bodemag(mu_bnds(1,1),omega);
    legendEntries{iter} = sprintf('Iter %d',iter);
    hold on;
    % extract D
    [~,VSigma,~] = mussvextract(mu_info);
    D = VSigma.DLeft;
    D = D / D(3,3);
    d1 = fitfrd(genphase(D(1,1)),4);
    d2 = fitfrd(genphase(D(2,2)),4);

end

ax = gca;
lines = findobj(ax,'Type','line');
legend(lines(end:-1:1),legendEntries, 'Location','best');
title('Upper bound of $\mu$','Interpreter','latex')




