resid2rhit(b,a);

function [cs,poles,mk,nail,thumb] = resid2rhit(b,a);
%
% function to compute the residues for repeated pole sets:
% [cs,poles] = resid2rhit(b,a);
%
% Inputs:  b = numerator polynomial, order < n
%          a = denominator polynomial, order = n
%
% Outputs: cs = the residues sorted in the same order as poles
%          poles = the system poles
% System must be at least third order 
% this is an attempt to tackle the repeated roots case:
% 
% Bradley T. Burchett 1/5/2004

% normalize the system s.t. a(1) = 1;
b = b/a(1);
a = a/a(1);

% can first find the number of system poles using roots:
poles = roots(a);
n = length(poles);

eh = ones(n,n)*diag(poles);
bee = eh.';
tol = 0.005; % multiple pole tolerance, adjust if needed.

nail = abs(eh-bee) < tol;  % Pole matching matrix
thumb = sum(nail);         % Pole multiplicities
mk = thumb;

% strip away any lead zeros in numerator poly:
aye = find(b);
if aye(1) ~= 1
    b = b(aye);
end

% pre-pad the numerator coeffs with zeros and transpose:
b = [zeros(n-length(b),1); b(:)];

% now be sure to pre-allocate memory for the A matrix:
A = zeros(n,n);
cs = zeros(n,1);

if any(thumb > 1) % there are repeated poles.
  
  % reset repeated poles to average of repeated poles!!!!
  
  % Easiest to work with repeated poles consecutive, and preferably first:
  [t,aye] = sort(thumb);  % puts repeated poles first
  poles = poles(fliplr(aye));
  nail = nail(:,fliplr(aye));

  thumb = fliplr(t);

  % break into subset of repeated, non-repeated?
  reap = find(thumb-1);
  repoles = poles(reap);
  renail = nail(:,reap);

  rethumb = thumb(reap);
  dispoles = poles(max(reap)+1:length(poles));
  disnail = nail(:,max(reap)+1:length(poles));

  disthumb = thumb(max(reap)+1:length(poles));
  % sort repeated poles by abs
  [r,jay] = sort(abs(repoles));
  repoles = repoles(jay);
  renail = renail(:,jay);

  rethumb = rethumb(jay);

  % sort by angle:
  r = angle(repoles);
  rjay = find(abs(r) < tol);
  r(rjay) = 0;

  [r,jay] = sort(r);
  repoles = repoles(jay);
  renail = renail(:,jay);

  rethumb = rethumb(jay);

  poles = [repoles; dispoles];
  thumb = [rethumb, disthumb];
  nail = [renail, disnail];


    % build the matrix column by column...
    jay = 1;
    while jay <= n
        
        % average the repeated poles 
        if thumb(jay) > 1
            poles(jay:jay+thumb(jay)-1) = ones(size( poles(jay:jay+thumb(jay)-1))).*mean( poles(jay:jay+thumb(jay)-1));
        end % averaging
        
       % for loop, trapezoidal block for repeated pole
        for kay = 1:thumb(jay)  % column counter
            A([1:thumb(jay)-kay],kay+jay-1) = zeros(thumb(jay)-kay,1);
            A(thumb(jay)-kay+1,kay+jay-1) = 1;
            
            A(thumb(jay)-kay+2,kay+jay-1) = sum(-poles) + (thumb(jay)-kay+1)*poles(jay);
            
            % row counter for bottom rows:
            for el = thumb(jay)-kay+3:n
               A(el,kay+jay-1) = sum(prod(combnk(-poles([1:jay-1, jay+thumb(jay)+1-kay:n]),el-(thumb(jay)-kay+1)),2));  % change ,2 to .' for v4.x  
            end
            
        end % kay columns of block for repeated pole
        jay = jay + kay;
    end  % while jay
    
else % no repeated roots
    A(1,:) = ones(1,n);

    for s = 1:n
        A(2,s) = sum(-poles([1:s-1, s+1:n]));
    end % s, inner loop

    % outer loop, row number and k of nchoosek:
    for k = 2:n-1  
    % inner loop
      for s = 1:n
        A(k+1,s) = sum(prod(combnk(-poles([1:s-1, s+1:n]),k),2)); 
      end % s, inner loop
    end % k, outer loop

end % if repeated.
% solve the system:
A = A(1:n,1:n);
cs = A\b;
poles = poles(:);

% END resid2rhit