function Test_NearestPt_3D(cv,NumRuns,MaxOffset,MaxLoft,epslon)

% This is a test program for 
%      function [PNear,SNear,ClosestDistance,NumIter,NumFcnEval] ...
%                                  = NearestPt_3D(cv,P,InitInt,epslon)
% See NearestPt_3D for a description of the input and output variables.
% The variable cv is a structure that contains information needed in the
% program 'NearestPt_3D'.
%
% As an example, use the following.
%       cv = Test_RoadSpline_3D(1,2,4*pi,[1 2 0.2],200,50);
%       Test_NearestPt_3D(cv,100,0.1,0.01,1.0e-12)
%
% In the test, we generate randomly NumRuns points along the road bed
% specified by the structure cv produced by 'Test_RoadSpline_3D'. To
% generate a random point P, we first choose a random arc length s along 
% the centerline using the arc length parameterization nodes given in S. 
% We then generate a random point in the roadbed at maximum distance of
% MaxOffset from the centerline in an orthogonal direction. We then
% "lift" it from the roadbed at a random distance bounded by MaxLoft. 
% The variable "epslon" is used in the Matlab function 'fminbnd' that is
% used to perform a function minimization in 'NearestPt_3D'.
%
% Statistics are generated and printed on the efficiency and accuracy of
% the solution obtained by 'NearestPt_3D'.

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

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

save_initial_interval = zeros(NumRuns,1);
save_trial_interval = zeros(NumRuns,1);
save_Original_s = zeros(NumRuns,1);
save_offset = zeros(NumRuns,1);
save_loft = zeros(NumRuns,1);
save_OriginalPt = zeros(NumRuns,3);
save_GivenPt = zeros(NumRuns,3);
save_test_arclength = zeros(NumRuns,1);
save_NumIter = zeros(NumRuns,1);
save_NumFcnEval = zeros(NumRuns,1);
save_FoundPt = zeros(NumRuns,3);
save_Found_s = zeros(NumRuns,1);
save_ComputedOffset = zeros(NumRuns,1);
save_ComputedLoft = zeros(NumRuns,1);

% The test value test_s is to be in the interval #initial_interval. The
% interval index to be given to the NearestPt program is trial_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 = floor(n*rand(1)+1);
    perturb_interval = floor(3*rand(1)-1);
    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_loft = MaxLoft*(2*rand(1) - 1);
    test_s = h*rand(1) + S(initial_interval);
    
    [xs,dxs,d2xs] = EvalCubic(xscoefs(initial_interval,:),...
        test_s - S(initial_interval));
    [ys,dys,d2ys] = EvalCubic(yscoefs(initial_interval,:),...
        test_s - S(initial_interval));
    [zs,dzs,d2zs] = EvalCubic(zscoefs(initial_interval,:),...
        test_s - S(initial_interval));
    [theta_s,dt_s,d2t_s] = EvalCubic(thetascoefs(initial_interval,:),...
        test_s - S(initial_interval));
    test_arclength = sqrt(dxs^2 + dys^2 + dzs^2);
    save_test_arclength(i) = test_arclength;
    
    v = [dxs,dys,dzs]/norm([dxs,dys,dzs]);
    eta = eta_vector(test_s,theta_s,v);
    vert_normal = cross(v,eta);
    GivenPt = [xs,ys,zs] + test_offset*eta + test_loft*vert_normal;
    
    save_initial_interval(i) = initial_interval;
    save_trial_interval(i) = trial_interval;
    save_Original_s(i) = test_s;
    save_offset(i) = test_offset;
    save_loft(i) = test_loft;
    save_OriginalPt(i,:) = [xs ys zs];
    save_GivenPt(i,:) = GivenPt;
    
    [PNear,SNear,ClosestDistance,NumLoop,NumFcnEval] = ...
                NearestPt_3D(cv,GivenPt,trial_interval,epslon);
    save_FoundPt(i,:) = PNear;
    save_Found_s(i) = SNear;
    save_NumIter(i) = NumLoop;
    save_NumFcnEval(i) = NumFcnEval;
    diff_vector = GivenPt - [xs,ys,zs];
    save_ComputedOffset(i) = eta*diff_vector';
    save_ComputedLoft(i) = vert_normal*diff_vector';
end 

disp(['MaxError: s = ', num2str(max(abs(save_Original_s-save_Found_s)))])
disp(['MaxError: offset = ', ...
    num2str(max(abs(save_offset-save_ComputedOffset)))])
disp(['MaxError: loft = ', ...
    num2str(max(abs(save_loft-save_ComputedLoft)))])
disp(['Average number of iterations = ',...
    num2str(sum(save_NumIter)/NumRuns)]) 
disp(['Maximum number of iterations = ',...
    num2str(max(save_NumIter))]) 

hs = arclength/n; 
ss = (hs/20)*(0:20*n)';
xssval = spline(S,xsval,ss);
yssval = spline(S,ysval,ss);
zssval = spline(S,zsval,ss);
figure
plot3(xssval,yssval,zssval)
xlabel('x')
ylabel('y')
zlabel('z')
axis('equal')
hold on
plot3(save_GivenPt(:,1),save_GivenPt(:,2),save_GivenPt(:,3),'.')

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

  function eta = eta_vector(r,theta,v)

% Have r be a column vector.
% The vector v has as its columns the unit vectors directed
% in the direction of motion along the midline of the road,
% as specified by 'position'.  This routine produces a unit
% vector for each point of the road centerline specified in
% the vector r.  This unit vector lines in the road bed and is
% orthogonal to the centerline, pointing to the left as the 
% driver is moving in the direction of the centerline.

% Initialization
if is_row(r)
    r = r';
end
Lr = length(r); 
eta = zeros(Lr,3);
swtch = 1;
test = zeros(size(r));       % This stores a test as to whether the vector
                             % eta is directed correctly to the left.
eta(:,3) = sin(theta)';

for ij=1:Lr
   c = sqrt(v(ij,1)^2 + v(ij,2)^2);
   if eta(ij,3) == 0
      eta(ij,1:2) = [-v(ij,2)/c,v(ij,1)/c]';
   else
      psi = find_angle(v(ij,1)/c,v(ij,2)/c);
      omega = acos(-v(ij,3)*tan(theta(ij))/c);
      phi = psi + swtch*omega;
      eta(ij,1:2)=cos(theta(ij))*[cos(phi),sin(phi)]';
      test(ij) = [-v(ij,2),v(ij,1)]*eta(ij,1:2)';
      if test(ij) < 0
         swtch = -swtch;
         phi = psi + swtch*omega;
         eta(ij,1:2) = cos(theta(ij))*[cos(phi),sin(phi)]';
      end
   end
end         
end % eta_vector

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

function fa_ans=find_angle(x,y)

% It is assumed that (x,y) = (cos t, sin t) is located on the unit
% circle, with t in (-pi,pi]. 

if x == 0
   if y > 0
      fa_ans = pi/2;
   else
      fa_ans = -pi/2;
   end
   return
end

fa_ans = atan(y/x);
if x > 0 
   return
else
   if y > 0
      fa_ans = fa_ans + pi;
   else
      fa_ans = fa_ans - pi;
   end
   return
end
end % find_angle

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

function truth_value = is_row(v)

% Return TRUE if v is a row vector. Otherwise, return FALSE.  
% If v is a column vector, then FALSE is returned.

[n1,n2] = size(v);
if n1 == 1  % v is a row vector
    truth_value = true;
else        % v is a column vector
    truth_value = false;
end
end % is_row

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

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).

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 % Test_NearestPt_3D