% ### Robust Daisy-Chained Laser PLL Phase Tracking Simulation ###
%
% SPDX-FileCopyrightText: 2023 Deutsches Elektronen-Synchrotron DESY, MSK, LbSync <lbsync-expert@desy.de>
% SPDX-License-Identifier: GPL-3.0-or-later
%
% Author: Maximilian Schütte <maximilian.schuette@desy.de>
%
% Control studies for chained VCO PLL phase tracking with uncertain damping using IQC & nonsmooth methods. Gridding.

yalmip('clear');
warning("off", "Robust:umodel:ureal9");
%clear;

% CASES
% 1 - Simple 2nd-order plant, no input delay
% 2 - Extra pole at 50kHz to induce finite gain margin
CASE = 2;

% SCALINGS
ctrl_scale = 1e-2; % to make solution ~1
noise_scale = 1e1; % to make nominal H2-norm ~1

% CHAIN LENGTH
N = 3; % Chain length

%%
reso_dp = ureal("damp", 0.25, Range=[0.05, 0.5]); % 0.5 mechanical resonance frequency damping
zeta = normalized2actual(reso_dp, linspace(-1, 1, 50+1)');
[zeta_lb, zeta_ub] = meshgrid(zeta);

%%
drv_bw = 25e3; % 25 kHz driver bandwidth (typical DESY PZT4)
drv_k = 40; % 40 V/V gain (typical PZT4 gain in unipolar mode)
vco_k = 250; % (2*pi * k_omega=6.5 * n_harm=6) ~= 250
reso_cf = 1e5; % mechanical resonance frequency
reso_dp = ureal("damp", 0.25, Range=[0.05, 0.5]); % 0.5 mechanical resonance frequency damping
reso_dp.NominalValue = mean(reso_dp.Range);
reso = tf([1, 2*2*pi*reso_cf, (2*pi*reso_cf)^2], [1, 2*reso_dp*(2*pi*reso_cf), (2*pi*reso_cf)^2], ...
          InputName="u", OutputName="uu"); % added later, so parameters are independent

% Plant model in mrad @ 1.3GHz
G = ctrl_scale * 1e3 * zpk([], [0, -2*pi*drv_bw], (2*pi*drv_bw) * drv_k * vco_k);
if CASE == 2
    G = G * zpk([], -2*pi*50e3, 2*pi*50e3);
end

Fr = noise_scale * zpk([], 0, 2);
Fd = noise_scale * zpk(-2*pi*1e5, [0, -2*pi*5], 500 / (2*pi*1e5) * (2*pi*5));
Fn = noise_scale * zpk([], -2*pi*1e6, 2*pi*1e6*db2mag(-100));  % full-block S-procedure doesn't allow feedthrough here.

% Build generalized VCO model
[P, Pinfo] = build_pll_model(G, Fr, Fd, Fn, Rolloff=10); %
reso = chgTimeUnit(reso, P.TimeUnit); % adapt TimeUnit of resonance (which is added later)

Pr = ss(getIOTransfer(P, 'w(1)', ["r", "r"])); Pr.OutputName = ["r", "y"]; % Equiv: Pr = [1; 1] * Pinfo.Fr; Pr.OutputName = ["r", "y"];
P_ = getIOTransfer(P, ["r", "w(2)", "w(3)", "u"], ["y", "e"]);

P.InputName{end} = 'uu';
P = connect(zpk(P), reso, ["w", "u"], P.OutputName);
[M, Delta, BlkStruct] = lftdata(P);
M = nameMissingIO(M, "p", "q");

P_.InputName{end} = 'uu';
P_ = connect(zpk(P_), reso, ["w", "r", "u"], P_.OutputName);
[M_, Delta_, BlkStruct_] = lftdata(P_);
M_ = nameMissingIO(M_, "p", "q");

% Construct repeated VCOs for daisy chaining
M_list = [{Pr}, repmat({M_}, 1, N)];

% Build chain interconnection
Mchain = chain(M_list{:});

% Network performance signal
Mchain = lft([1, -1], Mchain, 2, 0);
Mchain.OutputName{1} = 'z';

Mchain = connect(Mchain, ["p", "w", "u"], ["q", "z", "e"]); % Reorder inputs/outputs

pidx = startsWith(Mchain.InputName, 'p'); np = sum(pidx);
widx = startsWith(Mchain.InputName, 'w'); nw = sum(widx);
uidx = startsWith(Mchain.InputName, 'u'); nu = sum(uidx);
qidx = startsWith(Mchain.OutputName, 'q'); nq = sum(qidx);
zidx = startsWith(Mchain.OutputName, 'z'); nz = sum(zidx);
yidx = startsWith(Mchain.OutputName, 'e'); ny = sum(yidx);
nx = order(Mchain);

Mchain = prescale(Mchain);
Mchain = truncate_coeff(Mchain, 1e-9);

Delta = repmat({Delta}, [1,N]); Delta = blkdiag(Delta{:});
BlkStruct = repmat(BlkStruct, [1,N]);

% Controller
K = arrayfun(@(i) tunablePID("K" + i, 'P'), 1:N, UniformOutput=false); K = blkdiag(K{:});


% Augmented Multiplier
Sigma = tf();
Psi = tf();
phi0 = tf();

%- Robustness
%-- Nx arbitrarily fast time-varying repeated parameter
D = arrayfun(@(i) realp("D" + i, 0), 1:N, UniformOutput=false);
for i = 1:N, D{i}.Minimum = 0; end; clear i;
D = blkdiag(D{:});
W = zeros(nq);

Sigma = append(Sigma, D);
phi0 = append(phi0, W);
Psi = append(Psi, -(D' * D) / 2);

%- Performance
%-- H2
PsiY = 0;
for i = 1:1
    a = realp("a" + i, 0);
    b = realp("b" + i, 0);
    X = realp("X" + i, zeros(nw));
    X.Free = eye(nw); % Try diagonal only, sufficient degrees of freedom
    Y = realp("Y" + i, zeros(nw));
    Y.Free = eye(nw); % same
    % a.Minimum = 0; % technically this should be enforced, but makes random sampling linear instead of log.
    a.Maximum = 2*pi*1e6; % Leads to a in [0, aMax] with log sampling.
    % b.Minimum = 0; % doesn't matter if positive or negative
    % abs(b) should be bounded, but this would make sampling linear instead
    % of log. In any case, sign may be changed by Y.
    b.Maximum = 1e3; % lower bound damping
    X.Minimum = 0;

    A = [-a, b;
         -b, -a];
    A = fKron(eye(nw), A);
    B = [0; 1];
    B = fKron(eye(nw), B);
    C = [Y, X];
    C = C(:, reshape([1:nw; nw+1:2*nw], [], 1)'); % interlace.

    PsiY = PsiY + ss(A, B, C, zeros(nw));
    %tmp2 = X * tf([1, a], [1, 2*a, a*a + b*b]) + Y * tf(b, [1, 2*a, a*a + b*b]);
end; clear X Y a b A B C;
PsiY.InputName = 'k'; PsiY.OutputName = 'l';

Sigma = append(Sigma, eye(nz));
phi0 = append(phi0, zeros(nw, nz));
Psi = append(Psi, -PsiY);

% Nonsmooth IQC Synthesis
opt = systuneOptions(RandomStart=50, Display='final');

% Goals
sigma_max_goal = TuningGoal.Gain('v', 'c', 1);
%sigma_max_goal.Focus = [0, 2*pi*1e6];
pos_real_goal = TuningGoal.Passivity('k', 'l');
h2_goal = HermIntegral('k', 'l', 1);

gamma = struct([]);
for ii = 1:numel(zeta_lb)
    if zeta_lb(ii) >= zeta_ub(ii)
        continue;
    end

    range_ = [zeta_lb(ii), zeta_ub(ii)];
    reso.Blocks.damp.Range = range_;
    reso.Blocks.damp.NominalValue = mean(range_);

    % Build generalized VCO model
    [P, Pinfo] = build_pll_model(G, Fr, Fd, Fn, Rolloff=10); %
    reso = chgTimeUnit(reso, P.TimeUnit); % adapt TimeUnit of resonance (which is added later)
    
    Pr = ss(getIOTransfer(P, 'w(1)', ["r", "r"])); Pr.OutputName = ["r", "y"]; % Equiv: Pr = [1; 1] * Pinfo.Fr; Pr.OutputName = ["r", "y"];
    P_ = getIOTransfer(P, ["r", "w(2)", "w(3)", "u"], ["y", "e"]);
    
    P.InputName{end} = 'uu';
    P = connect(zpk(P), reso, ["w", "u"], P.OutputName);
    [M, Delta, BlkStruct] = lftdata(P);
    M = nameMissingIO(M, "p", "q");
    
    P_.InputName{end} = 'uu';
    P_ = connect(zpk(P_), reso, ["w", "r", "u"], P_.OutputName);
    [M_, Delta_, BlkStruct_] = lftdata(P_);
    M_ = nameMissingIO(M_, "p", "q");
    
    
    % Construct repeated VCOs for daisy chaining
    M_list = [{Pr}, repmat({M_}, 1, N)];
    
    % Build chain interconnection
    Mchain = chain(M_list{:});
    
    % Network performance signal
    Mchain = lft([1, -1], Mchain, 2, 0);
    Mchain.OutputName{1} = 'z';
    
    Mchain = connect(Mchain, ["p", "w", "u"], ["q", "z", "e"]); % Reorder inputs/outputs
    
    pidx = startsWith(Mchain.InputName, 'p'); np = sum(pidx);
    widx = startsWith(Mchain.InputName, 'w'); nw = sum(widx);
    uidx = startsWith(Mchain.InputName, 'u'); nu = sum(uidx);
    qidx = startsWith(Mchain.OutputName, 'q'); nq = sum(qidx);
    zidx = startsWith(Mchain.OutputName, 'z'); nz = sum(zidx);
    yidx = startsWith(Mchain.OutputName, 'e'); ny = sum(yidx);
    nx = order(Mchain);
    
    Mchain = prescale(Mchain);
    Mchain = truncate_coeff(Mchain, 1e-9);
    %Mchain = balreal(Mchain);
    
    Delta = repmat({Delta}, [1,N]); Delta = blkdiag(Delta{:});
    BlkStruct = repmat(BlkStruct, [1,N]);
    
    % Get extremal uncertainty values and random samples in between
    unc_hull_vrtx = arrayfun(@(x) ones(x, 2) * diag([-1, 1]), [BlkStruct.Occurrences], 'UniformOutput', false);
    unc_hull_vrtx = combvec(unc_hull_vrtx{:});
    
    % Controller
    MchainCL = lft(Mchain, K);

    % Direct Optimization
    ns = size(Sigma, 1);
    sys = sect(-[phi0 * MchainCL + Psi, zeros(np+nw, ns); ...
                 Sigma * MchainCL, -eye(ns) / 2]);
    sys.InputName = 'v'; sys.OutputName = 'c';
    
    [sol, fsoft, ghard, info] = systune(append(sys, PsiY), h2_goal, [sigma_max_goal, pos_real_goal], opt);
    softhard = [[info.fSoft]', reshape([info.gHard], 2, [])'];
    %softhard = [[info.fSoft]', [info.gHard]'];
    [softhardsorted, softhardsortedidx] = sortrows(softhard);

    % sys = replaceBlock(sys, sol.Blocks);
    MchainCL = replaceBlock(MchainCL, sol.Blocks);
    D_ = double(replaceBlock(D, sol.Blocks));
    PsiY_ = ss(replaceBlock(PsiY, sol.Blocks));
    K_ = ss(replaceBlock(K,sol.Blocks));

    gamma{i}.sol = sol; gamma{i}.fsoft = fsoft; gamma{i}.ghard = ghard; gamma{i}.info = info;
end; clear ii;

%% Utility
function sys = nameMissingIO(sys, in_name, out_name)
    sys.InputName(ismissing(sys.InputName)) = cellstr(compose("%s(%d)", in_name, 1:sum(ismissing(sys.InputName))));
    sys.OutputName(ismissing(sys.OutputName)) = cellstr(compose("%s(%d)", out_name, 1:sum(ismissing(sys.OutputName))));
end