  function xStar = Closest2E(E,p,tol,itMax)
% E is an ellipsoid and p is a column 3-vector that represents a point 
% outside E. xStar is a column 3-vector that represents that point on E
% that is closest to p. The method of steepest descent with line search
% via quadratic interpolation is used. At most itMax steps are applied.
% Otherwise termination when the gradient of the distance function is
% less than tol in norm.

% Shift the problem to the origin and ``unrotate''
% The axes are the same
a = E.a; b = E.b; c = E.c;
% p gets move and rotated too
p0 = E.Q'*(p-E.r);
% We basically now have the problem with Q = eye(3,3), r = [0;0;0].
% We'll call this the canonical problem.
% This simplifies everything. When we are all done we have to rotate and
% translate the canonical problem solution to get the original problem solution.

% Determine initial guess xc = [phi_c,theta_c] by
% approximating the canonical ellipsoid with a sphere S with radius 
r = (a+b+c)/3;
% Here is the closest point to p that is on S:
q = (p0/norm(p0))*r;
% Now get q's latitude and longitude...
theta_c = asin(q(3)/r);
phi_c   = asin(q(2)/(r*cos(theta_c)));
% and use that for a starting value...
xc = [phi_c; theta_c];
% Initialize for the steepest descent iteration...
fc = f(xc,a,b,c,p0);
gc = grad_f(xc,a,b,c,p0);
its = 0;
% Iterate...
fprintf('  its    phi        theta       sqrt(fc)   norm(gc)     tc\n')
fprintf('------------------------------------------------------------------\n')
fprintf('%3d %10.6f  %10.6f    %10.6f  %10.3e\n',its,xc(1),xc(2),sqrt(fc),norm(gc))
while norm(gc)>tol && its <itMax
    % Compute the steplength.
    fL = fc;                    % ftilde(0)
    fR = f(xc-gc,a,b,c,p0);     % ftilde(1)
    derfL = -gc'*gc;            % ftilde'(0)
    % The quadratic
    %     q(t) = fL + derfL*t + (fR - fL - derfL)*t^2
    % satisfies q(0) = ftilde(0), q'(0) = ftilde'(0), q(1) = ftilde(1).
    % Find tc in [0,1] that minimizes q over [0,1]
    if fR - fL - derfL <=0
        tc = 1; 
    else
        tstar = -derfL/(2*(fR-fL-derfL));
        if tstar >=1
           tc = 1;
        else
           tc = tstar;
        end
    end 
    xc = xc - tc*gc;
    fc = f(xc,a,b,c,p0);
    gc = grad_f(xc,a,b,c,p0);
    its = its+1;
    fprintf('%3d %10.6f  %10.6f    %10.6f  %10.3e   %5.3f\n',its,xc(1),xc(2),sqrt(fc),norm(gc),tc)
end
% Now undo the rotation and the translation to the origin...
phi   = xc(1);
theta = xc(2);
xStar = [a*cos(phi)*cos(theta);b*sin(phi)*cos(theta);c*sin(theta)];
xStar = E.Q*xStar + E.r;  
        


  function fc = f(xc,a,b,c,p0) 
% The square of the distance from xc to the normalized ellipsoid...
cphi   = cos(xc(1)); sphi   = sin(xc(1));
ctheta = cos(xc(2)); stheta = sin(xc(2));
z = [a*cphi*ctheta;...
     b*sphi*ctheta;...
     c*stheta] - p0;
fc = z'*z;
  
  function gc = grad_f(xc,a,b,c,p0)
% The gradient of f at xc...
cphi   = cos(xc(1)); sphi   = sin(xc(1));
ctheta = cos(xc(2)); stheta = sin(xc(2));
partial_f_wrt_phi   = 2*(a*cphi*ctheta-p0(1))*(-a*sphi*ctheta)+...
                      2*(b*sphi*ctheta-p0(2))*( b*cphi*ctheta);
partial_f_wrt_theta = 2*(a*cphi*ctheta-p0(1))*(-a*cphi*stheta)+...
                      2*(b*sphi*ctheta-p0(2))*(-b*sphi*stheta)+...
                      2*(c*stheta-p0(3))*(c*ctheta);
gc = [partial_f_wrt_phi;partial_f_wrt_theta];
%Nearest  3 points for intelligent starting value, 
%        4 pts for correctly getting tc, 
%        3 points for correct  f-evals and gradient evals.   
%        -2 for redundant f -evals, step out of bound
  
  

    
 