function [k,poles] = rlocfind(sys,p)
%RLOCFIND  Find root locus gains for a given set of roots.
%
%   [K,POLES] = RLOCFIND(SYS)  is used for interactive gain 
%   selection from the root locus plot of the SISO system SYS 
%   generated by RLOCUS.  RLOCFIND puts up a crosshair cursor 
%   in the graphics window which is used to select a pole location 
%   on an existing root locus.  The root locus gain associated 
%   with this point is returned in K and all the system poles for 
%   this gain are returned in POLES.  
%
%   [K,POLES] = RLOCFIND(SYS,P)  takes a vector P of desired root 
%   locations and computes a root locus gain for each of these 
%   locations (i.e., a gain for which one of the closed-loop roots
%   is near the desired location).  The j-th entry of the vector K
%   gives the computed gain for the location P(j), and the j-th 
%   column of the matrix POLES lists the resulting closed-loop poles.
%
%   See also  RLOCUS.

%   Clay M. Thompson  7-16-90
%   Revised ACWG 8-14-91, 6-21-92
%   P. Gahinet 7-23-96
%   Copyright (c) 1986-98 by The MathWorks, Inc.
%   $Revision: 1.5 $  $Date: 1998/08/26 21:48:32 $

ni = nargin;
error(nargchk(1,2,ni));
if ~issiso(sys),
   error('RLOCFIND must be used with SISO systems.')
end

% Interactive mode
if ni==1,
   disp('Select a point in the graphics window')
   [re,im] = ginput(1);    % Get one point
   p = re + sqrt(-1)*im;
end

% Get Z,P,K data
[Zero,Pole,Gain] = zpkdata(sys,'v');
lz = length(Zero);
lp = length(Pole);

% The gain for a given location p is determined by
%     den(p) + k * num(p) = 0
p = p(:);   
np = length(p);
k = (prod(p(:,ones(1,lp)).' - Pole(:,ones(1,np))) ./ ...
     prod(p(:,ones(1,lz)).' - Zero(:,ones(1,np))) ) / Gain;
k = abs(k);   % Root locus gains are positive (negative feedback)

% Determine all the poles for each gain.
poles = zeros(max(lp,lz),length(k));
for j=1:length(k),
  num = Gain * [zeros(1,lp-lz) poly(Zero)];
  den = [zeros(1,lz-lp) poly(Pole)];
  if isfinite(k(j)),
     poles(:,j) = roots(den+k(j)*num);
  else
     poles(1:lz,j) = Zero;
     poles(lz+1:end,j) = Inf;
  end
end

% If selecting points from root locus, plot all the roots
if ni==1,
    status = ishold;
    hold on
    plot(real(poles),imag(poles),'r+')
    if ~status, hold off, end
    selected_point = p  % Feedback to user
end

