function test = Test_NearestPt_2D(NumRuns,MaxOffset,epslon,SplineSoln)

% This will test the program 'NearestPt_2D'.  It generates a random set of
% points (alpha,beta) around the centerline of the road.  It then uses 
% 'NearestPt_2D' to find the corresponding points on the centerline.
% Various data are collected in order to better understand the accuracy and
% efficiency of 'NearestPt_2D'.  Information about the centerline
% approximation by spline functions is generated by 'RoadSpline_2D'.
%
% As a demo, use the following.
%      SplineSoln = Test_RoadSpline_2D(4,10,200,40);
%      TestData = Test_NearestPt_2D(100,.1,1.0e-8,SplineSoln);

% INPUT:
% NumRuns: The number of random points s and (alpha,beta)=(x(s),y(s)) 
%        to be generated.
% MaxOffset: The maximum possible offset of the test points (alpha,beta)
%        from the centerline.
% epslon:  The error tolerance to be used in the function minimization
%        program 'fminbnd', called from 'NearestPt_2D'.
% SplineSoln: This contains information on the curve as generated by
%        'RoadSpline_2D'.  

% A description of the contents of the output structure 'test' is given 
% below in the program. Look for the statements of the form
% test.Something = SomethingElse.

% Obtain from SplineSoln the curve data needed in generating the data for
% testing 'NearestPt_2D'.
S = SplineSoln.S;
xscoefs = SplineSoln.xscoefs;   yscoefs = SplineSoln.yscoefs; 
xspp = SplineSoln.xspp;         yspp = SplineSoln.yspp; 
L = SplineSoln.arclength;

n=length(S)-1;
h = S(2);
rand('twister',sum(100*clock));

% Initialization.
save_initial_interval = zeros(NumRuns,1);
save_trial_interval = zeros(NumRuns,1);
save_Original_s = zeros(NumRuns,1);
save_offset = zeros(NumRuns,1);
save_OriginalPt = zeros(NumRuns,2);
save_GivenPt = zeros(NumRuns,2);
save_test_arclength = zeros(NumRuns,1);
save_NumIter = zeros(NumRuns,1);
save_FoundPt = zeros(NumRuns,2);
save_Found_s = zeros(NumRuns,1);
save_ComputedOffset = zeros(NumRuns,1);

% The test value 'test_s' is to be in the interval #'initial_interval'.
% The interval index to be given to the program 'NearestPt_2D' is 
% 'trial_interval', a randomly chosen slight change to 'initial_interval'. 
% The variable 'test_offset' is to be the distance from the point at
% parameter test_s to the point GivenPt=(alpha,beta) measured on the
% perpendicular from the centerline. 

for i=1:NumRuns
    initial_interval = ceil(n*rand(1));         % Random integer in [1,n].
    perturb_interval = floor(3*rand(1)-1);      % Random integer in [-1,1].
    % 'trial_interval' is the initial interval given to 'NearestPt_2D' as
    % an initial guess at the location of the closest point to the given
    % offline point (alpha,beta).
    trial_interval = initial_interval + perturb_interval; 
    trial_interval = max(1,trial_interval); 
    trial_interval = min(n,trial_interval);
    
    test_offset = MaxOffset*(2*rand(1) - 1);    % Test offset from curve.
    test_s = h*rand(1) + S(initial_interval);   % Test point on curve.
    
    % Find the point on the curve corresponding to an arc-length of test_s.
    [xs,dxs,d2xs] =...
        EvalCubic(xscoefs(initial_interval,:),test_s - S(initial_interval));
    [ys,dys,d2ys] =...
        EvalCubic(yscoefs(initial_interval,:),test_s - S(initial_interval));
    % Test of spline curve arc-length parameterization. The entries
    % should all be approximately 1.
    test_arclength = sqrt(dxs^2 + dys^2);   % This should equal 1.
    save_test_arclength(i) = test_arclength;
    
    v = [-dys,dxs]/test_arclength;      % Unit normal to curve at 'test_s'.
    GivenPt = [xs,ys] + test_offset*v;  % Test point off of curve.
    
    save_initial_interval(i) = initial_interval;
    save_trial_interval(i) = trial_interval;
    save_Original_s(i) = test_s;
    save_offset(i) = test_offset;
    save_OriginalPt(i,:) = [xs ys];
    save_GivenPt(i,:) = GivenPt;
    
    % Find the point on the centerline that is closest to 'GivenPt'.
    NearData = NearestPt_2D(GivenPt(1),GivenPt(2),trial_interval,...
        epslon,SplineSoln);
    
    save_FoundPt(i,:) = [NearData.XNear,NearData.YNear];
    save_Found_s(i) = NearData.SNear;
    save_NumIter(i) = NearData.NumIter;
    save_ComputedOffset(i) = NearData.ClosestDistance;
end 

% Create an output structure that contains the experimental data for the
% testing of 'NearestPt_2D'.  
test.test_arclength = save_test_arclength;  % This should equal 1.
test.initial_interval = save_initial_interval;  % Index of interval containing a test value of s.
test.trial_interval = save_trial_interval;  % Index of interval being given to 'NearestPt_2D'.
test.original_s = save_Original_s;  % The randomly chosen values of s.
test.offset = save_offset;  % The randomly chosen values of the offset from centerline.
test.original_pt = save_OriginalPt;  % The (x,y) for centerpoint line associated with given s.
test.given_pt = save_GivenPt;  % The given point (alpha,beta) off of centerline given to 'NearestPt_2D'.
test.found_pt = save_FoundPt;  % The point on the centerline found by 'NearestPt_2D'.
test.found_s = save_Found_s;  % The arclength associated with found_pt.
test.num_iter = save_NumIter;  % The number of iterations needed by 'fminbnd'.
test.computed_offset = save_ComputedOffset;  % The offset distance calculated by 'fminbnd' in 'NearestPt_2D'.

% Plot the initial test points (alpha,beta) along with the centerline.
NumPlot = 100;
h = L/NumPlot;
t = 0:h:L;
xp = ppval(xspp,t); yp = ppval(yspp,t); 
figure
plot(xp,yp) 
hold on
plot(save_GivenPt(:,1),save_GivenPt(:,2),'.')
axis equal
hold off
title('The centerline and the given test points (\alpha,\beta)')
xlabel(['NumRuns = ',num2str(NumRuns),',  MaxOffset = ',...
    num2str(MaxOffset),',  epslon = ',num2str(epslon)])

% Calculate and print some summary statistics associated with the test
% data.
max1nrm = norm(test.original_pt-test.found_pt,inf);
maxdev = norm(test.original_s-test.found_s,inf);
disp(['max error in found s values = ',num2str(maxdev)])
disp(['max 1-norm of error in found pts (x,y) = ',num2str(max1nrm)])
maxiter = norm(test.num_iter,inf);
disp(['max number of iterations needed per point = ',num2str(maxiter)])
disp(['Average number of iterations = ',...
    num2str(sum(save_NumIter)/NumRuns)]) 

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

function [p, dp, d2p]=EvalCubic(cf,z)

% Let p(x)=cf(4)+cf(3)*x+cf(2)*x^2+cf(1)*x^3.
% Evaluate p(z),p'(z),p''(z).
% z can be an array.

d=cf(1); zd=z*d;
b2=cf(2)+zd; b1=cf(3)+z.*b2; b0=cf(4)+z.*b1;
p=b0; dp=b1+z.*(b2+zd);  
d2p=2*(b2+2*zd);
end % EvalCubic

end % TestNearestPt_2D