function [RGM,Info,IRD] = robgain(D,gamma,Opt)
% [StabMarg,Info,IRData] = ROBGAIN(D,OPT)
%     D      Scalar lftdataFRD object
%     OPT    rctoptions.robAnalysis object

%   Copyright 2010-2015 The MathWorks, Inc.
[M,B,~,muB] = ulftdata(D);
if ~muB.ismuStandard
   % Structures with repeated, non-scalar blocks are not supported
   error(message('Robust:analysis:OneIllegalRSRP'))  % message OK for gain too
end
M = frd(M); % M has no blocks, but is still LFT
BlockNames = {muB.BlkInfo.BlockName}';
nblk = numel(BlockNames);

% Absorb all Delays, but do not interpolate.  It is true that FRD-like
% objects never have internal delays.   It is also true that the InputDelay
% and OutputDelay will always be identically zero on "Block" channels.
% Nevertheless, FRESP is the simplest manner in which to obtain the 3-d
% DOUBLE needed for the MUSSV computation.
w = M.Frequency;
Focus = Opt.Focus;
if ~isempty(Focus)
   w = w(w>=Focus(1) & w<=Focus(2));
end
H = fresp(M,w);
[ny,nu] = iosize(B);
[ne,nd] = iosize(D);
if allfinite(H)
   gpeak = 0;
   for ct=1:numel(w)
      gpeak = max(gpeak,norm(H(nu+1:nu+ne,ny+1:ny+nd,ct)));
   end
else
   gpeak = Inf;  % response singular at some frequency
end
OutOfRangeFlag = false;

if gpeak>=gamma
   % Nominal performance >=gamma
   [RGM,WCU,Info] = rctutil.robUnstable(muB,0);
elseif nblk==0
   [RGM,WCU,Info] = rctutil.robNoBlocks();
else
   gamsq = sqrt(gamma);
   H(:,ny+1:ny+nd,:) = H(:,ny+1:ny+nd,:)/gamsq;
   H(nu+1:nu+ne,:,:) = H(nu+1:nu+ne,:,:)/gamsq;
   if nd>0 && ne>0
      % Add fixed performance block
      robgainBlk = [muB.muBlkNby2 ; nd ne];
      fixedBlkIdx = nblk+1;
   else
      % Equivalent to ROBSTAB problem when no I/Os
      robgainBlk = muB.muBlkNby2;
      fixedBlkIdx = [];
   end
   [bnds,tinfo] = mussv(H,robgainBlk,Opt.MussvOptions,fixedBlkIdx);
   PertM = mussvunwrap(tinfo.pvec,tinfo.blk);
   PertM = PertM(1:ny,1:nu,:); % keep only actual perturbations
   [NL,NR] = rctutil.boundIP(getBlockList(B));
   [bnds,PertM,OutOfRangeFlag] = ...
      rctutil.checkUREALRangeFRD(bnds,PertM,NL,NR,muB.muBlkNby2);
   
   % 1/muupperbound how much uncertainty tolerate with gain<=GAMMA
   % 1/mulowerbound = size of pert that makes gain>=GAMMA
   % Compute peak (based on MU lower bound)
   [pkl,WCIndex] = max(bnds(1,2,:));
   OutOfRangeFlag = OutOfRangeFlag(WCIndex);
   pku = max(bnds(1,1,:));
   lb = 1/pku;   ub = 1/pkl;
   RGM = struct(...
      'LowerBound',lb,...
      'UpperBound',ub,...
      'CriticalFrequency',w(WCIndex));
   
   % Populate INFO
   WP = getWorstCasePerturbation(B,muB,PertM,w);
   Info = struct(...
      'Model',[],...
      'Frequency',w,...
      'Bounds',1./permute(bnds(1,[1 2],:),[3 2 1]),...
      'WorstPerturbation',WP,...
      'Sensitivity',[]);
   WCU = WP(WCIndex);
   
   % Compute sensitivity data
   if strcmp(Opt.Sensitivity,'on')
      SensValues = localComputeSensitivity(nblk,H,robgainBlk,bnds,Opt);
   else
      SensValues = NaN(nblk,1);
   end
   Info.Sensitivity = cell2struct(num2cell(SensValues),BlockNames,1);
end

% Report data
IRD = struct('StableFlag',(gpeak<inf),'NominalPerf',gpeak,...
   'hasID',false,'muB',muB,'WCU',WCU,'OutOfRangeUREAL',OutOfRangeFlag);


%---------- Local subroutine ------------------------------%


function SensValues = localComputeSensitivity(nblk,H,robgainBlk,bnds,Opt)
% Computes sensitivity of MUSSV peak
gamma = Opt.SensitivityPercent/100;
blkData = rctutil.mkBlkData(robgainBlk);
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
fixedBlk = nblk+1:size(robgainBlk,1);  % could be empty if there are no I/Os
for k = 1:nblk 
   tmpmat = matg;
   affectedRows = blkData.ridxm(k):blkData.ridxm(k+1)-1;
   tmpmat(affectedRows,:,:) = (1+gamma)*tmpmat(affectedRows,:,:);
   sbnds = mussv(tmpmat,robgainBlk,SensMuOpt,fixedBlk);
   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);
