function [newnum,resid] = fdedmut(w,sam,A,B,Y,num,den,order,weights)
%FDEDMUT Utility function for FDEDMUND.
%
%        [newnum,resid] = fdedmut(w,sam,A,B,Y,num,den,order,weights)
%
% Utility function for fdedmund. Does the least squares optimization.
% No checking is done - it should be done by calling function.
% Notation follows chapter 7 of 'Multivariable Feedback Design',
% except that generalisation to different denominators is implemented.

% Input parameters:
%       w = frequency list
%     sam = sampling interval
%       A = (I-Tt)*G  (MVFR)
%       B = Part of (I-Tt)    (MVFR)
%       Y = Part of B*G*Kt*B  (MVFR)
%     num = matrix of numerators to be optimized
%     den = matrix of denominator polynomials   
%   order = highest degree of polynomials in den
% weights = Part of weights on achievement of Tt  (MVFR)

% J.M.Maciejowski, 1 August 1988. Modified Jan 1989.
% Copyright (C) 1988,1989 Cambridge Control Ltd.
% History:
%     `ones(weights)' replaced by `ones(size(weights))', 9.7.93,JMM.
%     `powers' calculation correcter, 9.7.93, JMM.
%     MRN0038

[gm,gl]=fsize(w,A); [tm,tl]=fsize(w,B);
if tm>1, B = conj(ftrn(w,B)); end;     % B transposed

numsigns = abs(sign(num))'; % 1's indicate coefficients to be tuned
index = find(numsigns(:));  % Contains positions of coeffts to be tuned,
			    % looking along row1, row2, ... row(tm).
if length(index)==0,  % No parameters to be tuned in this block
  newnum = num;       % Return matrix of zeros
  resid = 1;
  return
end
sigcols = (order+1)*tm; % Maximum # columns in sigma 

clear X eta     % Just to make sure 
X=[]; eta=[]; sigma=[]; % To get round bug on HP machines
jT = sqrt(-1)*sam;  % Discrete time only
for i = 1:length(w),
  Ai = fgetf(w,A,i);  Bi = fgetf(w,B,i);
  X = [X;kron(Bi,Ai)]; % not multiplied by sigma yet

  Yi = fgetf(w,Y,i);
  ystack = Yi(:);
  eta = [eta;ystack];
  powers = 1;
  for j = 1:order,
    powers = [exp(jT*w(i))*powers,1]; % Change this for continuous time
	     %% Previous line corrected 9.7.93, JMM.
  end 
  sigcomp = zeros(gl*tl,length(index));
  deni = dmv2fr(den,[zeros(1,order),1],w(i),sam); % Same size as K
  for j = 1:length(index),   % Put elements of sigma into place:
    position = index(j);
    output = ceil(position/sigcols); % Controller output
    col = rem(position,sigcols);
    if col == 0, col = sigcols; end;
    input = ceil(col/(order+1));  % Controller input
    degree = rem(col,order+1);
    if degree == 0, degree = order+1; end; % really order+1-degree
				     % ie counting from left of 'powers'
    % Now build a column of a component of sigma :
    sigcomp((input-1)*gl+output,j) = powers(degree)/deni(output,input);
  end % for j = 1:length(index)
  % Now build up sigma :
  sigma = [sigma ; sigcomp];  % sigma holds info for all frequencies
end % for i = 1:length(w)

% Diagnostics :
[Xr,Xc]=fsize(w,X); [sigr,sigc]=fsize(w,sigma); [etar,etac]=fsize(w,eta);
if Xc~=sigr | etar~=Xr ,
  disp('      Incompatible dimensions :')
  disp(['      X has fsize ',int2str(Xr),' x ',int2str(Xc)']);
  disp(['      sigma has fsize ',int2str(sigr),' x ',int2str(sigc)']);
  disp(['      eta has fsize ',int2str(etar),' x ',int2str(etac)']);
  error(' ')
end

% Now apply weighting, if necessary:    (Start of mods, 8 Jan 89, JMM) ***
if ~all(all(weights==weights(1,1)*ones(size(weights)))),
		%% Previous line changed 9.7.93 by JMM: 
		%% ones(size(...)) replaces ones(weights)
  weights = sqrt(abs(weights));  % Element by element square root
  weights = fdiag(w,fget(w,weights)); % Diagonal MVFR matrix
  % Diagnostics:
  [wtr,wtc] = fsize(w,weights);
  if wtc ~= etar,
    disp('      Incompatible dimensions of (diagonalized) weights :')
    disp(['      weights has fsize ',int2str(wtr),' x ',int2str(wtc)']);
    disp(['      eta has fsize ',int2str(etar),' x ',int2str(etac)']);
    error('  ')
  end
  eta = fmulf(w,weights,eta); % Premultiplying each row of eta and X by the
  X = fmulf(w,weights,X);     % same weight has the effect of solving a
			      % weighted least-squares problem.
end  % if ~all(...)                      (End of mods, 8 Jan 89, JMM) ****

X = fmulf(w,X,sigma);

% Now the optimization :
nu = [real(X);imag(X)] \ [real(eta);imag(eta)];   % That's it !

% Now put parameters into newnum :
newnum = zeros(tm*(order+1),gl); % Shape of K transposed
fullnu = zeros(gl*tm*(order+1),1);
fullnu(index) = nu;
newnum(:) = fullnu;
newnum = newnum';

% Now compute residual :
resid = eta - X*nu;     % Weighted eta and X
resid = norm(resid)/norm(eta);   % Modified 13.1.89, JMM.

% End of fdedmut