%% This simulates a PLL with input delay and verifies stability via IQC delay multipliers.

% VCO plant
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
G = zpk([], [0, -2*pi*drv_bw], (2*pi*drv_bw) * drv_k * vco_k);
G = chgTimeUnit(G, "milliseconds");

tau = ureal("tau", 0);
dly_ph = tf(tau, 1, TimeUnit="milliseconds");

% Controller
p_gain = 250e-3;  % These gains yield approx. delay margin of 500us
i_gain = 150;
C = pid(p_gain, i_gain);
C = chgTimeUnit(C, "milliseconds");

CLnom = feedback(G*C, 1);
assert(isstable(CLnom));

% Get LFR of closed-loop system
[lfr.M, lfr.Delta, lfr.BlkStruct] = lftdata(feedback(G*dly_ph*C, 1));

qidx = 1; pidx = 1;

% manual creation
%M = truncate_coeff(balreal(minreal(ss([-C*G, C; G, 0]))), 1e-9);
%Delta = umat(tau);
%BlkStruct = struct(Name='tau', Size=[1, 1], Type='ureal', Occurrences=1, Simplify=2);

% Delay Multiplier from Pfifer, Seiler
tau.NominalValue = 1e-3; tau.Range=[0,1];
phi2 = 2 * tf([tau*tau, 3.5*tau, 1e-6], [tau*tau, 4.5*tau, 7.1], TimeUnit="milliseconds");
Pi2 = blkdiag(phi2*phi2', -1);
phi3 = tf([-2.19*tau*tau, 9.02*tau, 89e-3], [1*tau*tau, -5.64*tau, -17], TimeUnit="milliseconds");
Pi3 = [0, phi3';
       phi3, -1];


% figure(); bode(Pi2.NominalValue, Pi3.NominalValue);

%%
% Before trying KYP lemma, verify time delay margin using frequency gridding to verify multiplier(s)
Mqp = lfr.M(qidx, pidx);
Mqp = feedback(Mqp, -1); % transform Dtau to Stau

for t = [0.1, 0.4, 0.6, 0.9]
    figure(); bode([Mqp; 1]' * usubs(Pi2, "tau", t) * [Mqp; 1], [Mqp; 1]' * usubs(Pi3, "tau", t) * [Mqp; 1]);
end

%%
% KYP Lemma
sdp_opts = sdpsettings('solver', 'mosek', 'debug', 1, 'verbose', 2);
for t = [0.1, 0.4, 0.6, 0.9]
    [Psi, M] = iqc_factorization(usubs(Pi2, "tau", t));
    
    tmp = Psi * [Mqp; 1];
    n = order(tmp);
    l = length(M);

    yalmip('clear');
    X = sdpvar(n);
    p = sdpvar();

    cnst = [[eye(n), zeros(n, 1);
             tmp.A, tmp.B;
             tmp.C, tmp.D]' * blkdiag(fOblkdiag(X), p * M) * [eye(n), zeros(n, 1);
                                                             tmp.A, tmp.B;
                                                             tmp.C, tmp.D] <= 1e-9 * eye(n+1);
             p >= 1e-6];
    sol_diag = optimize(cnst, [], sdp_opts);
end


%% IQCLab
dlt = iqcdelta('delta', InputChannel=1, OutputChannel=1, StaticDynamic='D', DelayType=1, DelayTime=0.4);
udlt = iqcassign(dlt, 'udel', Length=3, AddIQC='yes');

iqcanalysis(Mqp, udlt, Parser="Yalmip", Solver='mosek')