function [SM,Info,IRD] = wcdiskmargin(L,e,JOB,Opt)
% [SM,Info,IRData] = WCDISKMARGIN(L,e,JOB,Opt)
%     D       lftdataSS object
%     JOB     'siso' or 'mimo' margins
%     OPT     rctoptions.wcAnalysis object

%   Copyright 2010-2015 The MathWorks, Inc.

% Form X = S+(e-1)/2*I = lft([(1+e)/2 -1;1 -1],L)
[nL,~] = iosize(L);
M = kron([(1+e)/2 -1;1 -1],eye(nL));
X = lft(createGain(L,M),L,nL+1:2*nL,nL+1:2*nL,1:nL,1:nL); % lftdataFRD

% Decompose uncertain model
[X,B,~,muB] = ulftdata(X);
if ~muB.ismuStandard
   % Structures with repeated, non-scalar blocks are not supported
   error(message('Robust:analysis:OneIllegalRSRP'))  % message OK for gain too
end
X = frd(X); % X has no blocks, but is still LFT
Ts = X.Ts;

% Compute frequency response (absorb all delays but do not interpolate).
w = X.Frequency;
H = fresp(X,w);
% Eliminate singularities (e.g., w=0 for L=1/s*(1+DELTA))
ixf = find(all(isfinite(H),[1 2]));
w = w(ixf); H = H(:,:,ixf);


% Compute margins
% DGP = gain/phase pertubation
% IRD = report data
DGP = ltipack.ssdata([],zeros(0,nL),zeros(nL,0),zeros(nL),[],Ts);
IRD = struct('StableFlag',true,'hasID',false,'muB',muB,'WCU',[]);
hw = ctrlMsgUtils.SuspendWarnings; %#ok<NASGU>
switch JOB
   case 'siso'
      % Perform nL loop-at-a-time disk margin calculations
      sz = iosize(X)-nL; % Size of uncertainty channels of X
      IRD = repmat(IRD,nL,1);
      for j=nL:-1:1
         Hj = H([1:sz(1), sz(1)+j],[1:sz(2), sz(2)+j],:);
         [SMj,IRD(j).WCU,Info(j,1)] = localComputeDM(Hj,w,Ts,B,muB,1,Opt);
         SMj.WorstPerturbation = setsubsys(DGP,j,j,SMj.WorstPerturbation);
         SM(j,1) = SMj;
      end
   case 'mimo'
      [SM,IRD.WCU,Info] = localComputeDM(H,w,Ts,B,muB,nL,Opt);
end
% Zero out Info.Bounds when not robustly stable
for j=1:numel(SM)
   Info(j).Bounds(:,[SM(j).LowerBound SM(j).UpperBound]==0) = 0;
end
   
% Transform normalized worst perturbation back to factor multiplying L
M = kron([(1+e)/2 1;1 1],eye(nL));
for ct=1:numel(SM)
   % WCP -> lft(WCP,[(1+e)/2 1;1 1])
   WCP = SM(ct).WorstPerturbation; % ltipack.ssdata
   SM(ct).WorstPerturbation = lft(WCP,createGain(WCP,M),1:nL,1:nL,1:nL,1:nL);
end

%---------- Local subroutine ------------------------------%
function [SM,WCU,Info] = localComputeDM(H,w,Ts,B,muB,nL,Opt)

BlockNames = {muB.BlkInfo.BlockName}';
nblk = numel(BlockNames);
ulevel = Opt.ULevel;
usqrt = sqrt(ulevel);
[ny,nu,~] = size(H);
ny = ny-nL;  nu = nu-nL;

% H is (nu+nL)-by-(ny+nL)
H(:,1:ny,:) = H(:,1:ny,:)*usqrt;
H(1:nu,:,:) = H(1:nu,:,:)*usqrt;
dmBlk = [muB.muBlkNby2; ones(nL,2)];
fixedBlkIdx = 1:nblk;  % uncertain blocks are "fixed" in WCG
[bnds,tinfo] = mussv(H,dmBlk,Opt.MussvOptions,fixedBlkIdx);

% Compute peak (based on MU lower bound, so WCU maximizes SM.UpperBound)
[pkl,WCIndex] = max(bnds(1,2,:));
pku = max(bnds(1,1,:));
wCritical = w(WCIndex);
SM = struct('LowerBound',1/pku,...
   'UpperBound',1/pkl,...
   'CriticalFrequency',wCritical,...
   'WorstPerturbation',[]);

% Populate INFO
PertM = mussvunwrap(tinfo.pvec,tinfo.blk);
% WP = Worst values of uncertain elements
WP = getWorstCasePerturbation(B,muB,ulevel*PertM(1:ny,1:nu,:),w);
Info = struct(...
   'Model',[],...
   'Frequency',w,...
   'Bounds',permute(1./bnds,[3 2 1]),...
   'WorstPerturbation',WP,...
   'Sensitivity',[]);

% Worst case perturbations
WCU = WP(WCIndex);
BLK = ultidyn('DELTA',[1 1],'Ts',Ts);
A = []; B = []; C = []; D = [];
for j=1:nL
   GP = getWorstCasePerturbation(BLK,PertM(ny+j,nu+j,WCIndex),wCritical);
   [A,B,C,D] = ltipack.ssops('append',A,B,C,D,[],GP.a,GP.b,GP.c,GP.d,[]);
end
SM.WorstPerturbation = ltipack.ssdata(A,B,C,D,[],Ts);

% Compute sensitivity data
if strcmp(Opt.Sensitivity,'on') && max(bnds(1,1,:))<Inf
   SensValues = localComputeSensitivity(nL,H,dmBlk,bnds,Opt);
else
   SensValues = NaN(nblk,1);
end
Info.Sensitivity = cell2struct(num2cell(SensValues),BlockNames,1);


%---------- Local subroutine ------------------------------%
function SensValues = localComputeSensitivity(nL,H,dmBlk,bnds,Opt)
% Computes sensitivity of MUSSV peak
gamma = Opt.SensitivityPercent/100;
nblk = size(dmBlk,1)-nL;  % number of uncertain blocks
blkData = rctutil.mkBlkData(dmBlk);
pku = max(bnds(1,1,:));
idx = find(bnds(1,1,:) >= pku*0.85); %1 x <=npts
if length(idx) == 1 && size(H,3)>1
   if idx == 1
      idx = [idx idx+1];
   else
      idx = [idx-1 idx];
   end
end
matg = H(:,:,idx);
sfactor = zeros(nblk,1);
% Options for Sensitivity should be Silent, and take lowerBound down
% to only 1 try (so multipleTimes = 0).
SensMuOpt = [Opt.MussvOptions 's'];
mloc = find(SensMuOpt=='m');
if ~isempty(mloc)
   SensMuOpt(mloc+1) = '0';
end
for k = 1:nblk  % don't compute sensitivity wrt performance blocks
   tmpmat = matg;
   affectedRows = blkData.ridxm(k):blkData.ridxm(k+1)-1;
   tmpmat(affectedRows,:,:) = (1+gamma)*tmpmat(affectedRows,:,:);
   sbnds = mussv(tmpmat,dmBlk,SensMuOpt,1:nblk);
   pkus = sbnds(1,1,:); % 1-by-1-by-(1,2 or more)
   oldpeak = bnds(1,1,idx);
   oldpeak = oldpeak(:); %col vector
   pkus = pkus(:);
   if any(oldpeak==0 & pkus>0)
      sfactor(k) = inf;
   elseif all(oldpeak==0 & pkus==0)
      sfactor(k) = 1;
   else
      keepgz = find(oldpeak>0);
      sfactor(k) = max(abs( (pkus(keepgz)-oldpeak(keepgz)) ./ (gamma*oldpeak(keepgz)) ));
   end
end
SensValues = round(100*sfactor);
