function FF = fcon(w,F,Q,iu,iy,sz)
%FCON Frequency domain MIMO block connectiions.
%
%        FF = fcon(w,F,Q,iu,iy,sz)
%
% Frequency response of interconnected system from frequency 
%           responses of component (MIMO) subsystems.
%
% Inputs:    w = frequency vector
%            F = frequency responses of component sub-systems
%                as produced by FBLKB
%            Q = interconnection matrix              ) As in MVCON
%           iu = vector identifying external inputs  )    or
%           iy = vector identifying external outputs )  CONNECT
%           sz = matrix of subsystem dimensions (similar to MVCON)
%
% Output:   FF = Frequency response of interconnected system

% J.M.Maciejowski, 2 March 1989
% Copyright (C) 1989, Cambridge Control Ltd.

% Reference: Chapter 8 of Maciejowski,J.M, Multivariable Feedback
%            Design, (Addison-Wesley), 1989.

% ***********  Start of Consistency checks:  ***********
[qr,qc] = size(Q);
[szr,szc] = size(sz);

if szr < 2,
  error('At least 2 rows needed in sz')
elseif szr > 3,
  error('At most 3 rows allowed in sz')
elseif szr == 3,
  disp('Warning: sz has 3 rows. Using only rows 2 and 3. (FCON)')
  sz(1,:)=[];   % Delete row 1 of sz
  [szr,szc] = size(sz);
end

if length(iu) == 0 | max(iu)<=0,
  error('No external inputs specified')
end
if length(iy) == 0 | max(iy)<=0,
  error('No external outputs specified')
end
if max(iu)>szc
  error('External input exceeds number of subsystems')
end
if max(iy)>szc
  error('External output exceeds number of subsystems')
end
if ~all(diff(iu)),
  disp('Warning: Repeated entries in iu (FCON)')
end
if ~all(diff(iy)),
  disp('Warning: Repeated entries in iy (FCON)')
end

[fr,fc] = size(F);    lw = length(w);
if fr ~= lw,
  error('# rows in input freq resp data must equal # frequencies')
end
% Assume sz has a column for each subsystem:
if fc ~= sum(prod(sz)),
  error('# cols in input freq resp data must equal sum(prod(sz))')
end

% Check consistency of connections :
if max(max(abs(Q))) > szc,
  error('Block number in Q exceeds number of subsystems')
end
if min(Q(:,1)) < 1,
  error('First column of Q contains invalid block number')
end
for i=1:szc,  % Check that every block is connected to something:
  if (isempty(find(Q(:,1)==i)) | max(abs(Q(find(Q(:,1)==i),2:qc)))==0) ...
      & isempty(find(iu==i)),
    disp(['Warning: Block ',int2str(i),' has no inputs. (FCON)'])
  end
  if isempty(find(any(abs(Q(:,2:qc))==i))) & isempty(find(iy==i)),
    disp(['Warning: Block ',int2str(i),' has no outputs. (FCON)'])
  end
end
% Check consistency of input & output dimensions:
errflag = 0;  errtable = [];
for block = 1:qr,         % Repeat for each row of Q
  Qb = Q(block,find(Q(block,:))); % Remove zeros from row of Q
  Qb = abs(Qb);  % Remove any negative signs
  ncon = length(Qb) - 1;  % Number of connections to make
  for con = 1:ncon,  % Repeat for each connection
    if sz(2,Qb(con+1)) ~= sz(1,Qb(1)),
      errflag = errflag + 1;
      errtable = [errtable; Qb(con+1), Qb(1)];
    end
  end
end
if errflag > 0,
  disp('Subsystem dimensions inconsistent with connections:')
  for err = 1 : errflag,
    disp(['      # outputs of block ',int2str(errtable(err,1)),...
             ' ~= # inputs of block ',int2str(errtable(err,2)) ]);
  end
  error(' ')
end
% **********  End of consistency checks  ***********

% Form an MVFR matrix of all the subsystems regarded as one big system:
totin = sum(sz(1,:));      % sz(1,:) holds input dimensions
totout = sum(sz(2,:));     % sz(2,:) holds output dimensions
ffr = lw * totout; 
ffc = totin;         
FF = zeros(ffr,ffc);
lastrow = 0;  lastcol = 0;
for block = 1:szc,         % Repeat for each block
  rows = lastrow + 1 : lastrow + sz(2,block); % Rows of each FF component
  cols = lastcol + 1 : lastcol + sz(1,block);
  lastrow = rows(length(rows));  lastcol = cols(length(cols));
  ffrows = rows;
  for i = 1:lw-1,
    ffrows = [ffrows, i*totout+rows]; % Rows of FF to be filled
  end
  fcols = 1:prod(sz(:,block)); % The next columns of F to be handled
  FF(ffrows,cols) = shpf(F(:,fcols),sz(2,block),sz(1,block));
  F(:,fcols) = [];  % Delete columns of F already handled
end

% Form K, the feedback matrix such that u=K*y forms the desired
% connected system. K contains zero and unit matrices only :
K = zeros(totin,totout);
for block = 1:qr,         % Repeat for each row of Q
  Qb = Q(block,find(Q(block,:))); % Remove zeros from row of Q
  ncon = length(Qb) - 1;  % Number of connections to make
  if ncon >0,
    if Qb(1) == 1  % First block
      krows = 1:sz(1,1);
    else          % Other blocks
      krows = sum(sz(1,1:Qb(1)-1))+1 : sum(sz(1,1:Qb(1))) ;
    end
  end
  for con = 1:ncon,  % Repeat for each connection
    I = sign(Qb(con+1)) * eye(sz(1,Qb(1))); % Signed unit matrix
    Qb(con+1) = abs(Qb(con+1));   % Remove possible negative sign
    if Qb(con+1) == 1  % First block
      kcols = 1 : sz(2,1);
    else               % Other blocks:
      kcols = sum(sz(2,1:Qb(con+1)-1))+1 : sum(sz(2,1:Qb(con+1))) ;
    end
    K(krows,kcols) = I;
  end
end    % for each block


% Now put K in (positive) feedback around FF :
if totout < totin,
  FF = fmulf(w,finv(w,fadd(w,-fmul(w,FF,K),eye(totout))),FF) ;
else
  FF = fmulf(w,FF,finv(w,fadd(w,-fmul(w,K,FF),eye(totin)))) ;
end


% Now keep only external inputs and outputs, with the 
% ordering defined by iu and iy :
for input = 1 : length(iu),
  keepcols = [keepcols, sum(sz(1,1:iu(input)-1))+1 : sum(sz(1,1:iu(input)))];
end
for output = 1 : length(iy),
  keeprows = [keeprows, sum(sz(2,1:iy(output)-1))+1 : sum(sz(2,1:iy(output)))];
end
FF = fpart(w,FF,keeprows,keepcols);