function [d,z0] = dist_to_ellipse(z,tol)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [d,z0] = dist_to_ellipse(z,tol)
%
% Given a point  z  (2 coordinates), inside the ellipse
% the ellipse
%         (x/myalpha)^2 + (y/mybeta)^2 = 1,
% find a point on the ellipse that is closest to z, 
% and set d equal to the distance between z and z0.
%
% The parameter tol is a zero tolerance; coordinates with
% absolute value less than tol will be treated as zero, 
% and differences abs(myalpha-mybeta) < tol will indicate a 
% circle.
%
% The problem is solved using the method of Lagrange 
% multipliers, using Matlab's fzero to find the multiplier.
%
% The parameters myalpha and mybeta are passed through 
% global variables.
%
%  dist_to_ellipse.m Dianne P. O'Leary   04/04    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global myalpha mybeta myz

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Return with error if z is outside the ellipse. 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (z(1)/myalpha)^2 + (z(2)/mybeta)^2 - 1 > 0
   d = NaN;
   z0 = [NaN;NaN];
   return
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Determine the orientation of the ellipse.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (myalpha <= mybeta)
   minparam = myalpha;
   perm = [1 2];
else
   minparam = mybeta;
   perm = [2,1];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Treat z=0 as a special case. 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (abs(z(1)) < tol) & (abs(z(2)) < tol)
  s = sign(z(perm(1)));
  if (s==0)
     s=1;
  end
  z0 = [s*minparam;0];
  z0 = z0(perm);
  d = minparam;
  return
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Make z a global variable for use in find_dist.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

myz = z;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Treat the cases in which one coordinate of z is zero.
%
% If the first coordinate is zero, test two possibilities:
% walking along the vertical axis to a point z0, or using 
% the Lagrange multiplier lam = myalpha^2 to get a point z1.
% The second point does not exist if the ellipse is a circle.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (abs(z(1)) < tol) 
   z0 = [0;sign(z(2))*mybeta];
   if(abs(myalpha-mybeta) < tol)
     z1 = z0;
   else   
     lam = myalpha^2;
     z1(2,1) = mybeta^2*z(2)/(mybeta^2-lam);
     s = sign(z(1));
     if (s==0)
       s=1;
     end
     z1(1,1) = s*myalpha*sqrt(1-(z1(2)/mybeta)^2);
   end
   d = norm(z-z0);
   d1 = norm(z-z1);
   if (d1<d)
     z0 = z1;
     d = d1;
   end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The case when the second coordinate is zero is handled 
% in a similar way.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

elseif (abs(z(2))< tol)
   z0 = [sign(z(1))*myalpha;0];
   if(abs(myalpha-mybeta) < tol)
     z1 = z0;
   else 
     lam = mybeta^2;
     z1(1,1) = myalpha^2*z(1)/(myalpha^2-lam);
     s = sign(z(2));
     if (s==0)
       s=1;
     end
     z1(2,1) = s*mybeta*sqrt(1-(z1(1)/myalpha)^2);
   end
   d = norm(z-z0);
   d1 = norm(z-z1);
   if (d1<d)
     z0 = z1;
     d = d1;
   end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Otherwise, both coordinates are nonzero, and the closest
% point is determined by solving for the Lagrange 
% multiplier lam with  0 < lam < min(myalpha^2,mybeta^2).
% Since one z coordinate is infinite at the right endpoint,
% we look for a zero in the interval 
% [0,(1-eps)*min(myalpha^2, mybeta^2)].
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

else 
   lam = fzero('find_dist',[0,(1-eps)*minparam^2]);
   z0 = [myalpha^2*z(1)/(myalpha^2-lam),
         mybeta^2*z(2)/(mybeta^2-lam)];
   d = norm(z-z0);
end