function [k,poles] = rlocfind(a,b,c,d,p)
%RLOCFIND Find the root locus gains for a given set of roots.
%	[K,POLES] = RLOCFIND(A,B,C,D) 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.  To use this command, the root locus for the 
%	SISO state-space system (A,B,C,D) must be present in the graphics
%	window.  If the system is MIMO, an error message is produced.  
%	RLOCFIND works with both continuous and discrete linear systems.
%
%	[K,POLES] = RLOCFIND(NUM,DEN) is used to select a point on the 
%	root locus of the polynomial system G = NUM/DEN where NUM and DEN 
%	are polynomials in descending powers of s or z.
%
%	When invoked with an additional right hand argument,
%		[K,POLES] = RLOCFIND(A,B,C,D,P)
%		[K,POLES] = RLOCFIND(NUM,DEN,P)
%	returns a vector K of gains and the matrix of associated poles, 
%	POLES. The vector K contains one element for each desired root 
%	location in P.  The matrix POLES contains one column for each 
%	root location in P and (LENGTH(DEN)-1) or LENGTH(A) rows.
%
%	See also: RLOCUS.

%	Clay M. Thompson  7-16-90
%	Revised ACWG 8-14-91, 6-21-92
%	Copyright (c) 1986-93 by the MathWorks, Inc.

error(nargchk(2,5,nargin));

% --- Determine which syntax is being used ---
if (nargin==2),		% Transfer function without poles
	[num,den] = tfchk(a,b);
	% Normalize transfer function
	num = num/den(1);
	den = den/den(1);
	[ny,nn] = size(num);
	if (ny~=1), error('RLOCFIND must be used with SISO systems.'); end
	disp('Select a point in the graphics window')
	[re,im] = ginput(1);	% Get one point
	p = re + sqrt(-1)*im;

elseif (nargin==3),	% Transfer function with poles
	[num,den] = tfchk(a,b);
	[ny,nn] = size(num);
	if (ny~=1), error('RLOCFIND must be used with SISO systems.'); end
	p=c;

elseif (nargin==4),	% State space system without poles
	error(abcdchk(a,b,c,d));
	[ny,nu] = size(d);
	if (ny*nu~=1), error('RLOCFIND must be used with SISO systems.'); end
	[num,den] = ss2tf(a,b,c,d);
	disp('Select a point in the graphics window')
	[re,im] = ginput(1);	% Get one point
	p = re + sqrt(-1)*im;

else,			% State space system with poles
	error(abcdchk(a,b,c,d));
	[ny,nu] = size(d);
	if (ny*nu~=1), error('RLOCFIND must be used with SISO systems.'); end
	[num,den] = ss2tf(a,b,c,d);

end

% Use root locus magnitude rule to determine gain value.  Assume the
% root locus is based on negative feedback.

z = roots(num); 
e = roots(den);
[ny,ns] = size(num);
for i=1:ny,
	tfgain(i) = num(i,min(find(abs(num(i,:))>eps)));
end
p = p(:);	% Make sure desired roots are a column

np=length(p); ne = length(e); nz = length(z);
k = prod(abs(ones(ne,1)*p.'-e*ones(1,np))) ./ prod(abs(ones(nz,1)*p'-z*ones(1,np))) ./ tfgain;
k = abs(k);

% Determine all the poles for each gain.
for i=1:length(k),
	if ~finite(k(i)),
		poles(:,i)=roots(num);
	else
		poles(:,i)=roots(den+k(i)*num);
	end
end

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


