% [Pd, Pr, Pu, G11, G12, G21, G22, Gd1, Gd2, R1, R2] =...
%                              partcont( G, Gd, R, op, ip, dip )
% 
% Computation of frequency dependent partial gains.
% 
% Outputs: Pd   - Partial disturbance gain.
%          Pr   - Partial reference gain.
%          Pu   - Partial control gain of unused inputs.
%          G11  - Subsystem of uncontrolled outputs and unused inputs.
%          G12  - Subsystem of uncontrolled outputs and used contol inputs.
%          G21  - Subsystem of controlled outputs and unused inputs.
%          G22  - Subsystem of controlled outputs and used inputs.
%          Gd1  - Disturbance model for uncontrolled outputs.
%          Gd2  - Disturbance model for ontrolled outputs.
%          R1   - Part of R matrix corrosponding to uncontrolled outputs.
%          R2   - Part of R matrix corrosponding to controlled outputs.
%
% Inputs:  G    - System as a varying matrix,
%          Gd   - Disturbance system as a varying matrix or constant matrix,
%          R    - Reference matrix as a varying matrix or constant matrix,
%          op   - Vector specifying the outputs,
%          ip   - Vector specifying the inputs,
%          dinp - Vector specifying the disturbance inputs.
%
% The function uses Mu-tools and the following three functions:
%      outcomp, inpcomp and selc.
%
% Written by: Kjetil Havre, e-mail: kjetil@ife.no
%
%

function [Pd, Pr, Pu, G11, G12, G21, G22, Gd1, Gd2, R1, R2] = partcont( G, Gd, R, op, ip, dip )
   [mt, rd, cd, n]     = minfo( G );
   [mtd, rdd, cdd, nd] = minfo( Gd );
   [mtr, rdr, cdr, nr] = minfo( R );
   if rd ~= rdd
      disp( 'Number of rows in G and Gd does not agree.' );
      return
   end

   
   if strcmp( mt, 'vary' ) ~= 1
       disp( 'Only implemented for varying matrices.' );
       return;
   end
   
   if nargin < 6
      dip = ':';
   end
   if nargin < 5
     ip = ':';
   end
   if nargin < 4           %  Perfect control 
      op = ':';
   end
      
   if nargin < 2
      disp( 'Usage: [Pd, Pr, Pu] = partcont( G, Gd, op, inp, dip )' )
      return
   end
      
   opc  = outcomp( G, op );   % Find the complement of the outputset.
   ipc  = inpcomp( G, ip );   % Find the complement of the input set.
   nipc = max(size(ipc));
   dipc = inpcomp( Gd, dip ); % Find the complement of the dist. set.
   if strcmp(mtd, 'cons') == 1
     Gd   = selc( Gd, ':', dip );
     Gd1  = selc( Gd, opc, dip );
     Gd2  = selc( Gd,  op, dip );
   else
     Gd   = sel( Gd, ':', dip );
     Gd1  = sel( Gd, opc, dip );
     Gd2  = sel( Gd,  op, dip );
   end
   if strcmp(mtr, 'cons') == 1
      R1 = selc(R, opc, opc);
      R2 = selc(R, op, op );
   else
      R1 = sel(R, opc, opc);
      R2 = sel(R, op, op );
   end
   G22  = sel(  G,  op, ip   );
   G12  = sel(  G, opc,  ip  );
   if nipc > 0
      G11  = sel(  G, opc, ipc  );
      G21  = sel(  G,  op, ipc  );
   else
      G11 = [];
      G21 = [];
   end
	 
   [mt, nr22, nc22, nx22] = minfo( G22 );
   if nr22 < rd
      if nr22 == nc22
         G22i = minv(G22);
      else
	 disp('WARNING: Not implemented for G22 non-square.' )
	 return
      end
      Pd = msub( Gd1, mmult(G12,G22i,Gd2) );
      if nipc > 0
         Pu = msub( G11, mmult(G12,G22i,G21) );
      else
         Pu = [];
      end
      Pr = mmult(G12,G22i,R2);
   else                                    %  Perfect control
      disp(' Not implemented.' );
      return
   end      
end
