function TestFcn = utCheckSeparability(a,b,c,Ts,AbsTol,RelTol)
% Creates test function to check if modal decomposition error
% exceeds user-defined required accuracy.
%
%   Inputs:
%      A,B,C             Original transfer function H(s).
%      TS                Sample time.
%      ABSTOL, RELTOL    Decomposition error should not exceed 
%                        ABSTOL + RTOL * |H(s)|
%
%   LOW-LEVEL UTILITY.

%   Authors: P. Gahinet
%   Copyright 1986-2004 The MathWorks, Inc. 
%   $Revision: 1.1.10.1 $  $Date: 2004/11/18 23:27:20 $

nx = size(a,1);
ny = size(c,1);
nu = size(b,2);
Ts = abs(Ts);

% Select test frequencies
e = ordeig(a);
if Ts==0
   w = abs(e);   
else
   w = abs(log(e(e~=0))/Ts);
   w = w(w<pi/Ts);
end
w = sort(w(w>0));
% Pick one test freq per decade inside the frequency range
% associated with the modes of A
if isempty(w)
   dmin = 0;
   dmax = 0;
else
   dmin = floor(log10(w(1)))-1;
   if Ts==0
      dmax = ceil(log10(w(end)))+1;
   else
      dmax = floor(log10(pi/Ts));
   end
end
w = 10.^(dmin:dmax).';
nw = length(w);

% Form vector of s/z test points
if Ts==0
   s = 1i * w;
else
   s = exp((1i*Ts)*w);
end
   
% Make A upper triangular
[u,a] = rsf2csf(eye(nx),a);
b = u'*b;
c = c*u;

% Precompute max allowable error for each I/O pair at the 
% test frequencies W
MaxError = zeros(ny,nu,nw);
for ct=1:nw
   Phi = s(ct) * eye(nx) - a;
   % Test for singularity
   if any(diag(Phi)==0)
      % Response is singular at test frequency
      w(ct) = NaN;
   else
      x = Phi\b;  % H(s) = c*x
      % Take into account accuracy on computed H(s)
      AbsAccuracy = (nx * eps) * abs(c/Phi) * abs(Phi) * abs(x);
      %[w(ct) AbsAccuracy RelTol * abs(c*x)]
      MaxError(:,:,ct) = 10 * (AbsAccuracy + AbsTol + RelTol * abs(c*x));
   end
end

% Return test function
TestFcn = @localTestFcn;

   function Pass = localTestFcn(a11,c1,a22,b2,t)
      % Estimate max error resulting from splitting 
      %   H(s) = [c1 c2] * (sI - [a11 a12;0 a22]) \ [b1 ; b2]
      % into H1(s) + H2(s) where 
      %   H1(s) = c1 * (sI-a11) \ (b1-t*b2)
      %   H2(s) = (c2+c1*t) * (sI-a22) \ b2
      %   t is the computed solution of a11 * t - t * a22 + a12 = 0
      nx1 = size(a11,1);
      nx2 = size(a22,1);
      
      % Compute frequencies of modes of A11, A22
      e12 = [ordeig(a11) ; ordeig(a22)];
      if Ts==0
         w12 = abs(e12);
      else
         w12 = abs(log(e12(e12~=0))/Ts);
      end
      
      % Identify nearest test frequencies
      idxtest = min(round(log10(w12(w12>0)))-dmin+1,length(w));
      % Eliminate singular frequencies
      idxtest = unique(idxtest(isfinite(w(idxtest))));
      if isempty(idxtest)
         idxtest = 1;
      end

      % Make A11 and A22 upper triangular
      [u1,a11] = rsf2csf(eye(nx1),a11);  
      c1 = c1 * u1;
      [u2,a22] = rsf2csf(eye(nx2),a22);  
      b2 = u2' * b2;
      
      % Compute residual bound
      abst = abs(u1' * t * u2);
      R = eps * (nx1 * abs(a11) * abst + nx2 * abst * abs(a22));
      
      % Compute error bound for H->H1+H2 decomposition
      Pass = true;
      for ct=1:length(idxtest)
         idxw = idxtest(ct);
         sct = s(idxw);
         wcError = abs(c1 / (sct*eye(nx1)-a11)) * R * abs((sct*eye(nx2)-a22) \ b2);
         % [w(idxw) wcError MaxError(:,:,idxw)]
         TolExceeded = (wcError>MaxError(:,:,idxw));
         if any(TolExceeded(:))
            Pass = false;  break
         end
      end      
   end

end