function NearData = ...
               NearestPt_2D(alpha,beta,InitInt,epslon,SplineSoln)

% Search for point on curve closest to the given point (alpha,beta), 
% beginning with points near to [S(InitInt),S(InitInt+1)]. Note that 
% the structure 'SplineSoln' contains data produced in the routine 
% 'RoadSpline2D', including the node points for the spline function. 
% This spline function is based on even spacing in an arclength based 
% parameterization of the curve.
% 
% In this program, we use the Matlab function 'fminbnd' for minimizing 
% functions of one variable.  It is based on a program of Richard Brent.
%
% INPUT:
% alpha,beta: The point (alpha,beta) is a given point close to the road
%      specified by 'SplineCurve'.
% InitInt: An integer in the interval [1,n], where n is the number of 
%      subintervals for the spline produced by 'RoadSpline2D' (n=n_final 
%      in the call on RoadSpline2D). The subinterval 
%      [S(InitInt),S(InitInt+1)] is to be an initial guess as to a
%      subinterval containing the point closest to (alpha,beta).  
% epslon: the error tolerance in finding the closest point to the curve 
%      by using the minimization routine 'fminbnd'.
% SplineCurve: A Matlab structure containing information about the spline
%      approximation to the centerline of the road. The spline is based on
%      an arclength based parameterization of the centerline, with the
%      nodes evenly spaced.
%
% We need to choose an upper and lower bound for the minimizing point we 
% are seeking. We use, somewhat arbitrarily, the three points on S that 
% are closest to the given point (alpha,beta) from among the indices 
% S(InitInt-1) and S(InitInt+2). A modification is needed for the end 
% intervals (InitInt = 1 and n).
% 
% We do a minimization using the quadratic minimization method, which is
% embedded in 'fminbnd'.

% Obtain needed data from previous computed spline fit to the centerline.
S = SplineSoln.S;
xspp = SplineSoln.xspp;         yspp = SplineSoln.yspp; 

n=length(S)-1;
NumLoop = 0;

if InitInt==1		% Selecting the indices of initial search.
   s_lower = 0; s_upper = S(3);
elseif InitInt==n
   s_lower = S(n-1); s_upper = S(n+1);
else
   s_lower = S(InitInt-1); s_upper = S(InitInt+2);
end

% Search for minimizing point s.
options = optimset('TolX',epslon,'MaxIter',200);
[SNear,ClosestDistance,exitflag,output] ...
    = fminbnd(@fcn_to_min,s_lower,s_upper,options);
XNear=ppval(xspp,SNear); YNear=ppval(yspp,SNear);

% Provide an output structure containing results that give the point on
% the centerline that is closest to the given point (alpha,beta).
NearData.XNear = XNear;
NearData.YNear = YNear;
NearData.SNear = SNear;
NearData.ClosestDistance = ClosestDistance;
NearData.NumIter = output.iterations;


function d=fcn_to_min(s)

% This is used with NearestPt_V3.  It is the function being minimized in
% order to find the point on the center-line that is closest to the given
% point (alpha,beta).

x=ppval(xspp,s); y=ppval(yspp,s);
d=sqrt((x-alpha).^2 + (y-beta).^2);

end % fcn_to_min

end % NearestPt_V3
