function [newnum,resid] = fedmunds(w,G,Tt,num,den,weights,blocks)
%FEDMUNDS Control design by parameter optimization.
%      [newnum,resid] = fedmunds(w,G,Tt,num,den,weights,blocks)
%
% Inputs:  w - Frequency vector
%          G - Frequency response of plant (open loop)
%         Tt - Target frequency response of closed loop
%        num - Numerator polynomials to be tuned (zero coefficients
%              are not tuned)
%        den - Definition of denominator polynomials of controller
%    weights - MVFR matrix of weights : abs(weights) is used. If
%              size(weights)==fsize(w,Tt) then the same weights 
%              are used at each frequency.
%     blocks - Vector of sizes of diagonal blocks in Tt.
%
% Defaults:  weights = ones(.), blocks = max(fsize(w,Tt))
%
% Outputs:  newnum - Tuned numerator polynomials
%            resid - vector of residuals for each block
%
% Implementation of Edmunds' algorithm for controller tuning.

% This function calls FEDMUTIL. Notation follows that in chapter 7
% of Maciejowski,J.M,'Multivariable Feedback Design'(Addison-Wesley).
%
% J.M.Maciejowski, 29 July 1988. Modified Jan 89, JMM.
% Copyright (C) 1988,1989 by Cambridge Control Ltd.

% ***** First check compatibilities & sort out defaults : *****

nargchk(nargin,5,7);
mu = length(w);      % Number of frequencies
[gm,gl] = fsize(w,G);
disp(['Plant has ',int2str(gl),' inputs and ',int2str(gm),' outputs']);
[tm,tl] = fsize(w,Tt);
if tm~=tl,
  error('Target closed loop must be square');
else
  disp(['Target has ',int2str(tm),' inputs and outputs']);
end;
if tm~=gm,
  error('Target and plant must have same number of outputs'),
end;
if nargin < 6,        % Default weights
  weights = ones(tm);
  disp('Using default weights : 1 on everything')
else
  weights = abs(weights);    % Use magnitudes only
end
[wm,wl] = size(weights);
if wl~=tl,
  error('Weights and Target must have same number of inputs'),
end;
if wm==tm, % Use the same weight at each frequency:
  temp = weights;
  for i=1:mu-1, weights=[weights; temp]; end
  disp('Using same weights at each frequency');
end;
[wm,wl]=fsize(w,weights);
if wm~=tm,
  error('Weights and Target must have same number of outputs'),
end;

% ***** Now sort out controller information : *****

[drows,dcols] = size(den);
[nrows,ncols] = size(num);
if nrows ~= gl,
  error('# controller outputs must equal # plant inputs');
end;
if rem(ncols,gm) ~= 0,
  error('Numerator matrix has # columns incompatible with # plant outputs');
else
  order = fix(ncols/gm)-1;
  disp(['Controller has ',int2str(gm),' inputs and ',int2str(gl),' outputs']);
  disp(['Maximum order of any controller element is ',int2str(order)]);
  if order < 0,
    error('Negative order !! Something is wrong.')
  end
end
if drows == 1 & dcols == order+1,  % Common denominator
  temp=den; for i=1:nrows-1, den=[den;temp]; end;
  temp=den; for i=1:ncols-1, den=[den,temp]; end;
  disp('Using a common denominator for the controller');
elseif drows == nrows & dcols == ncols, % Separate denominators
  disp('Using different denominators for the controller elements');
else
  error('Dimensions of denominator & numerator matrices inconsistent');
end;

% ***** Now compute A, B, and Y (as defined in chapter 7 of *****
% 'Multivariable Feedback Design', but A not divided by denominators):
B = fadd(w,-Tt,eye(tm));  % B = I-Tt
A = fmulf(w,B,G);         % A = (I-Tt)*G
Y = fmulf(w,Tt,B);        % Y = B*G*Kt*B, since A not divided by
			  %   = Tt*B            denominators yet.

% ***** Now split problem into sub-problems, if any : *****

if nargin < 7,       % Default blocks
%%% If you want to split into 1x1 blocks by default when Tt is
%%% diagonal, uncomment the next 5 lines starting with %%%.
%%%  if norm(fcsod(w,Tt)) < mu*tm*eps,  % Diagonal Tt             %%%
%%%    blocks = ones(1,tm);                                       %%%
%%%    disp('Diagonal Target. Optimizing each column separately');%%%
%%%  else                                                         %%%
    blocks = tm;     % Do everything as one big block
%%%  end                                                          %%%
end
if sum(blocks) ~= tm,
  error('Sum of block sizes must equal # Plant outputs');
end

nb = length(blocks);
newnum=num; resid=zeros(1,nb); % Create matrices of correct sizes
disp(['Solving ',int2str(nb),' subproblems :'])

for i = 1:nb,   % ******  Do each subproblem  ******
  if i==1,
   thiscolt = 1:blocks(1); thiscoln = 1:(order+1)*blocks(1);
  else
   thiscolt = (i-1)*blocks(i-1)+1:(i-1)*blocks(i-1)+blocks(i);
   thiscoln = (order+1)*(thiscolt(1)-1)+1:(order+1)*thiscolt(length(thiscolt));
  end;

  % Now the main call which does the optimization :  *** CALLS fedmutil ***
  [newnum(:,thiscoln),resid(i)] = fedmutil(w,A,fpart(w,B,thiscolt,thiscolt),...
    fpart(w,Y,1:tm,thiscolt),num(:,thiscoln),den(:,thiscoln),order,...
    fpart(w,weights,1:tm,thiscolt));
  disp(['     Subproblem ',int2str(i),' finished'])
end % subproblems