function [a,b,c,d]=mvcon(ap,bp,cp,dp,q,iu,iy,sz)
%MVCON Connects multivariable state space systems.
%       [A,B,C,D]=MVCON(AP,BP,CP,DP,Q,IU,IY,SZ)
%       Pure gain blocks that do not connect an input directly
%       to an output are eliminated by block reductions.
%
%       AP,BP,CP,DP are the state space system of all the blocks
%       appended together. Q is the matrix specifying the connections
%       between blocks.
%       IU is a vector of input blocks. IY is a vector of output blocks.
%
%       SZ is a matrix containing in row 1 the number of states in the
%       A matrices that make up AP, in row 2 the number of inputs and
%       in row 3 the number of outputs. The columns of SZ are the order
%       the A matrices appear in AP.
%       MVBLKB produces the required matrix SZ
%
%       See the User's Guide for a fuller explanation.

%       Dr M.P. Ford 7th September 1987
% Copyright (c) 1987 by GEC Engineering Research Centre & Cambridge Control Ltd
%       MRN0013
%       MRN0019

error(abcdchk(a,b,c,d));
%  Check that inputs or outputs are not repeated
liu=length(iu);
for m=1:(liu-1)
  f=find(iu==m);
  if length(find(iu(m+1:liu)==iu(m)))
    error(['Input ',int2str(iu(m)),' repeated']);
  end
end
liy=length(iy);
for m=1:(liy-1)
  f=find(iy==m);
  if length(find(iy(m+1:liy)==iy(m)))
    error(['Output ',int2str(iy(m)),' repeated']);
    end
end

[msz,nsz]=size(sz);
sza=sz(1,:);
szb=sz(2,:);
szc=sz(3,:);
csza=cumsum(sza);  %  cumulative rows/colums of A versus input/outputs
cszb=cumsum(szb);  %  cumulative columns of B
cszc=cumsum(szc);  %  cumulative rows of C
[ma,na]=size(ap);
[mb,nb]=size(bp);
[mc,nc]=size(cp);
if (ma~=csza(nsz))|(nb~=cszb(nsz))|(mc~=cszc(nsz))
   error('Totals of rows of SZ vector ~= sizes of A,B,C matrices')
end
[mq,nq] = size(q);
[md,nd] = size(dp);

% Form k from q, the feedback matrix such that u = k*y forms the
% desired connected system.
% k starts out as a square matrix of zeros and plus or minus ones.

k = zeros(nsz);
% Go through rows of Q
for i=1:mq
   % Remove zero elements from each row of Q
   qi = q(i,find(q(i,:)));
   [m,n] = size(qi);
   % Put the +-ones in K
   if n ~= 1
      k(qi(1),abs(qi(2:n))) = sign(qi(2:n));
   end
end

% Check for hanging blocks and issue warning
for m=1:nsz
    if (~any(k(:,m)))&(~any(iy==m))   % all zeros in column
       msg=['Warning in MVCON the output of block ',int2str(m),...
	    ' is not used.'];
       disp(msg)
    end
    if (~any(k(m,:)))&(~any(iu==m))  % all zeros in row
       msg=['Warning in MVCON the input to block ',int2str(m),...
	    ' is not used'];
       disp(msg)
    end
end

% expand k to include identity matrices
kid=zeros(cszb(nsz),cszc(nsz));
asz(1,:)=csza-sza+1;
asz(2,:)=csza;
bsz(1,:)=cszb-szb+1;
bsz(2,:)=cszb;
csz(1,:)=cszc-szc+1;
csz(2,:)=cszc;
for m=1:nsz
    for n=1:nsz
	if k(m,n)~=0  % check for i/o consistancy
	   if szb(m)~=szc(n)
	      error('The number of outputs of block ',int2str(n),...
		    ' does not match the inputs of block ',int2str(m),...
		    '. You can not connect them.');
	   else
	     bsm=bsz(1,m):bsz(2,m);
	     csm=csz(1,n):csz(2,n);
	     kid(bsm,csm)=eye(szb(m)).*k(m,n);
	   end   % if szb(m) ...
	end  % if k(m,n)
    end   % for n
end   % for m

i=find(sza==1);   % look for possible gains
%  I holds the indices of possible gains in terms of Block Numbers
j=csza(i);    %  row/columns in AP of possible gains
%  J holds the indices of possible gains in terms of the A,B,C,D matrices

% make one big matrix
at=[ap bp;cp dp];

li=length(i);
l=ones(1,li);
for m=1:li      % look at each possibility
   l(m)=(all(at(j(m),:)==0) & all(at(:,j(m))==0));  % 1 if gain, 0 otherwise
end       %  depends of gain terms having 0 in A,B,C matrices

j=j(l);    % eliminate the non gain terms
i=i(l);
at=[];     % Delete AT

li=length(i);
disp(['MVCON  Number of Gain blocks found was ',int2str(li)])

for m=1:li
    if any(iu==i(m))
       disp(['        Gain block ',int2str(i(m)),' is an input'])
    end
end
for m=1:length(i)
    if any(iy==i(m))
       disp(['        Gain block ',int2str(i(m)),' is an output'])
    end
end

%  Simplify Feedback Matrix KID by substitution for gain blocks that
%          are not inputs or outputs

[mkid,nkid]=size(kid);
iuk=eye(mkid);
iyk=eye(nkid);
iui=[];
iyi=[];
for m=1:liu
    iui=[iui,bsz(1,iu(m)):bsz(2,iu(m))];
end
for m=1:liy
    iyi=[iyi,csz(1,iy(m)):csz(2,iy(m))];
end
iuk=iuk(:,iui);
iyk=iyk(iyi,:);

idel=[];
for m=i  % For each Gain Block
    csm=csz(1,m):csz(2,m);
    bsm=bsz(1,m):bsz(2,m);
  %  First check the D matrix for Non Zero gain
    if ~any(any(dp(csm,bsm)))
      disp(['Warning in MVCON  Gain block ',int2str(m),' identically zero'])
    end
  %  Then  check if the Gain Block refers to its self
   if any(any(kid(bsm,csm)))
	   % Gain Block refers to itself Simplify
      disp(['Warning in MVCON Gain block ',int2str(m),...
	    ' feeds back to itself'])
      l=eye(szc(m))-kid(bsm,csm)*dp(csm,bsm);
      if rcond(l)<1000*norm(l,1)*eps
	 error(['Error in MVCON infinite gain in block '...
		,int2str(m),' when feed back to itself'])
      else
	 dp(csm,bsm) = l\dp(csm,bsm)
      end
      kid(bsm,csm)=zeros(szb(m),szc(m))
	  % Delete self reference after simplification
   end
   iuk=iuk+kid(:,csm)*dp(csm,bsm)*iuk(bsm,:); % up date input gain matrix
   iyk=iyk+iyk(:,csm)*dp(csm,bsm)*kid(bsm,:); % up date output gain matrix

   if ((~any(any(kid(:,csm)))) & any(any(iuk(bsm,:))))...
     | ((~any(any(kid(bsm,:)))) & any(any(iyk(:,csm))))
   %  the block does not feed another block
   %  but has an input      OR
   %  the block is not fed from another block
   %  but supplies an output

   %  Then Note index and don't delete at end
     disp(['MVCON gain block ',int2str(m),' not deleted'])
     if ((~any(any(kid(:,csm)))) & any(any(iuk(bsm,:))))...
       & (~any(any(iyk(:,csm))))
   %  the block does not feed another block
   %  but has an input
   %  AND   No output also
       disp('Warning it has an input but no outputs')

     elseif ((~any(any(kid(bsm,:)))) & any(any(iyk(:,csm))))...
	& (~any(any(iuk(bsm,:))))
   %  the block is not fed form another block
   %  but supplies an output
   %  AND No input also
	disp('Warning it has an output but no inputs')
     else
	disp('   as it connects an input to an output via a pure gain')
     end

   else
   %  Substitute for its output in terms of the gain times its inputs
     idel=[idel,find(i==m)];
     kid=kid+kid(:,csm)*dp(csm,bsm)*kid(bsm,:);
     kid(bsm,:)=zeros(length(bsm),nkid);
     kid(:,csm)=zeros(mkid,length(csm));
     iuk(bsm,:)=zeros(length(bsm),length(iui));
     iyk(:,csm)=zeros(length(iyi),length(csm));
   % Note this row and column will not be referred to again and will
   %   be deleted when K is Reduced
   end   %  if any
end  % For each Gain Block

%        Reduce the state space matrices

% reduce I and J by ISAVE
i=i(idel);
j=j(idel);

ima=ones(1,ma);
inb=ones(1,nb);
imc=ones(1,mc);
for m=1:length(i)
  ima(j(m))=0;
  inb(1,bsz(1,i(m)):bsz(2,i(m)))=zeros(1,szb(i(m))); 
  imc(1,csz(1,i(m)):csz(2,i(m)))=zeros(1,szc(i(m))); 
end
ap=ap(ima,ima);
bp=bp(ima,inb);
cp=cp(imc,ima);
dp=dp(imc,inb);

%       Reduce KID,IUK and IYK
kid=kid(inb,imc);
iuk=iuk(inb,:);
iyk=iyk(:,imc);

% Use output feedback to form closed loop system
%  .
%  x = Ax + Bu
%  y = Cx + Du      where  u = KID*y + Ur
%
[md,nd] = size(dp);
b = bp/(eye(nd) - kid*dp);
a = ap + b*kid*cp;
temp = eye(md) - dp*kid;
c = temp\cp;
d = temp\dp;
b = b*iuk;
c = iyk*c;
d = iyk*d*iuk;