function [PNear,SNear,ClosestDistance,NumIter,NumFcnEval] ...
                         = NearestPt_3D(cv,P,InitInt,epslon)

% Search for the point PNear on the centerline curve that is closest to 
% P, beginning with points near to [S(InitInt),S(InitInt+1)]. Note that 
% we are loading and using variables obtained previously in the routine 
% "RoadSpline_3D", including the node points in S, which are equally 
% spaced and based on arc length. These variables are stored in the 
% structure cv. We use the Matlab function 'fminbnd' for minimizing 
% functions of one variable. This program assumes that the point P is not
% "relatively close" to the road bed described by the data in the structure
% cv.  Otherwise, there is not necessarily a unique closest point PNear.
%
% The parameter InitInt is to be an integer in the interval [1,n],
% where n is the number of subintervals for the spline produced by
% RoadSpline_3D (the value of n is the value of n_final given in the
% call on RoadSpline_3D). The parameter 'epslon' is the error tolerance
% used when calling 'fminbnd'.
%
% In this program, we need to choose an upper and lower bound for the 
% minimizing point's arc length, SNear, which we are seeking. We use, 
% somewhat arbitrarily, the interval 
%       [S(InitInt-1),S(InitInt+2)].
% This guarantees a solution will be found; but it is valid only if the 
% true closest point PNear has an arc length parameter SNear that is 
% within this subinterval. A modification is needed for the end intervals 
% (InitInt = 1 and n). We do a minimization using the quadratic 
% minimization method as implemented in the Matlab function 'fminbnd'.

% Using the structure cv, define the needed parameters for the spline
% parameterization. This loads the variables which were calculated in the
% function RoadSpline_3D.
S = cv.S; xspp = cv.xspp; yspp = cv.yspp; zspp = cv.zspp; 
% The following variables from cv are not used in this test program.
%Ts = cv.Ts; arclength = cv.arclength;
%xsval = cv.xsval; ysval = cv.ysval; zsval = cv.zsval; 
%xsprime = cv.xsprime; ysprime = cv.ysprime; zsprime = cv.zsprime; 
%xsprime2 = cv.xsprime2; ysprime2 = cv.ysprime2; zsprime2 = cv.zsprime2; 
%xscoefs = cv.xscoefs; yscoefs = cv.yscoefs; zscoefs = cv.zscoefs; 
%thetascoefs = cv.thetascoefs; thetasval = cv.thetasval; 
%thetaspp = cv.thetaspp; eta = cv.eta; 

% Initialization.
alpha = P(1); beta = P(2); gamma = P(3);
n=length(S)-1;

if InitInt==1		% Selecting the indices of initial search.
   s_lower = S(1); s_upper = S(4);
elseif InitInt==n
   s_lower = S(n-2); 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);

% Define the output variables.
XNear = ppval(xspp,SNear); 
YNear = ppval(yspp,SNear); 
ZNear = ppval(zspp,SNear);
PNear = [XNear,YNear,ZNear]; 
NumIter = output.iterations;
NumFcnEval = output.funcCount;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function d = fcn_to_min(s)

% This is used with NearestPt_3D

x=ppval(xspp,s); y=ppval(yspp,s); z=ppval(zspp,s);
d=sqrt((x-alpha).^2 + (y-beta).^2 + (z-gamma).^2);
end % fcn_to_min

end % NearestPt_3D
