function soln = Test_RoadSpline_2D(problem,Param_length,...
    n_initial_spline,n_final_spline)

% As an example case, try the command
%       CurveObject = Test_RoadSpline_2D(4,10,200,40)
% When the program pauses, merely touch the space bar to have it continue.
% The output structure 'CurveObject' contains information needed in the
% program NearestPt_V3.

% This is a test program for 
%     sol = RoadSpline_2D(n_initial,curve,L,n_final)
% Various test problems can be accommodated by modifying the function
% 'curve' given below, and they are indexed using the input parameter
% 'problem'. The other input parameters are defined as follows.
%
% Param_length:     The interval of the original parameterization variable
%                   is [0,Param_length].
% n_initial_spline: The first spline fit is based on the initial
%                   parameterization variable, and it uses n_initial_spline
%                   subdivisions of [0,Param_length], with an additional
%                   point in the first and last subintervals.  Use a large
%                   value, as it will not affect the number of subdivisions
%                   in the final spline produced, but it will affect the
%                   accuracy of it.
% n_final_spline:   This is the number of equal subdivisions for the final
%                   parameterization that is based on arc-length. Larger
%                   values are better, but this may also be affected by
%                   storage limitations for the spline that represents the
%                   midline of the roadway.
%

% Plot the original curve using the original parameterization variable.
L = Param_length;
n = n_initial_spline;
nf = n_final_spline;
h = L/n;
t = h*(0:n);
t = [0,h/2,t(2:n),L-h/2,L]';
temp = curve(t);
xval=temp(:,1);
yval=temp(:,2);
plot(xval,yval)
axis('equal')
title('The curve using the given parameterization over [0,L]')
hold on
pause
plot(xval,yval,'o')
pause

% Calculate the spline fit with endpoint correction.
soln = RoadSpline_2D(@curve,Param_length,n_initial_spline,n_final_spline);

S = soln.S; T_s = soln.T_s;
xsval = soln.xsval; ysval = soln.ysval; 
xsprime = soln.xsprime; ysprime = soln.ysprime; 
xsprime2 = soln.xsprime2; ysprime2 = soln.ysprime2; 
xscoefs = soln.xscoefs; yscoefs = soln.yscoefs; 
xspp = soln.xspp; yspp = soln.yspp; 

disp(['Arc length = ',num2str(soln.arclength)])

% Plot the original parameterization variable t as a function of the
% arc length s.
max_x = max(S);
min_x = min(S);
max_y = max(T_s);
min_y = min(T_s);
figure
axis([min_x,max_x,min_y,max_y])
hold on
plot(S,T_s)
axis('auto')
title('Arclength s vs. given parameter t')
xlabel('s')
ylabel('t')
hold off
pause

% Evaluate and plot the spline curve that uses the arc-length parameterization.
h = soln.arclength/nf;
ss=(h/10)*(0:10*nf)';
tt = spline(soln.S_orig, soln.Ts_orig,ss);
temp = feval(@curve,tt);
xssval=temp(:,1);
yssval=temp(:,2);
max_x = max(xssval);
min_x = min(xssval);
max_y = max(yssval);
min_y = min(yssval);
figure
axis([min_x,max_x,min_y,max_y])
hold on
plot(xssval,yssval,xsval,ysval,'o')
axis('equal')
title('The curve using the arclength parameterization')
xlabel(['Initial n = ',num2str(n),'   Final n = ',num2str(nf)])
hold off
clear ss temp xssval yssval
pause

% Do a test of the arc-length parameterization.  Ideally we have the
% identity
%   sqrt([x'(s)^2 + y'(s)^2]) - 1 = 0
% We check this and plot the computed value of the left side at the
% interpolation node points. 
test = sqrt(xsprime.^2 + ysprime.^2);
max_x = max(S);
min_x = min(S);
max_y = max(test-1);
min_y = min(test-1);
figure
axis([min_x,max_x,min_y,max_y])
hold on
plot(S,test-1)
axis('auto')
title('Parameterization test #1 for spline with endpoint modification')
xlabel(['Initial n = ',num2str(n),'   Final n = ',num2str(nf)])
%gtext('s')
testmax=max(abs(test-1));
testmax2=max(abs(test(3:nf-1)-1));
disp(['max of parameterization test #1 = ',num2str(testmax)])
disp('max of parameterization test #1 absent the two left-most')
disp(['           and two right-most node points = ',num2str(testmax2)])
hold off
pause

% Calculate and plot the curvature at each point of S.
curvature = abs(xsprime.*ysprime2 - ysprime.*xsprime2)...
    ./(xsprime.^2+ysprime.^2).^1.5;
max_x = max(S);
min_x = min(S);
max_y = max(curvature);
min_y = min(curvature);
figure
axis([min_x,max_x,min_y,max_y])
hold on
plot(S,curvature)
title(['Curvature of spline fit as a function',...
               ' of arclength with endpoint modification'])
xlabel(['Initial n = ',num2str(n),'   Final n = ',num2str(nf)])
%gtext('s')
hold off
pause

% Do another test of the arc-length parameterization.  Theoretically 
% we have the identity
%   (x'(s),y'(s)).(x"(s),y"(s)) = 0
% We calculate and plot the computed value at the interpolation node points.
test_orthogonality = xsprime.*xsprime2 + ysprime.*ysprime2;
disp(['Norm(test_orthogonality,inf) = ',num2str(norm(test_orthogonality,inf))])
figure
plot(S,test_orthogonality)
title('    Test #2: the orthogonality of arc-length based parameterization')
xlabel(['Initial n = ',num2str(n),'   Final n = ',num2str(nf)])

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

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
    truth_value = true;
else
    truth_value = false;
end
end % is_row

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

function curve_ans = curve(r)
   
% This is a parameterization of the given planar curve 
% for the center line of the road. 

% If needed, change r to a column vector.
if is_row(r)
    r = r';
end

switch problem
    case 1
        % The curve is a circle.
        curve_ans = [cos(r),sin(r)];
    case 2
        % The curve is a simple curve.
        curve_ans = [r,(2/3)*(r+1).^(3/2)];
    case 3
        % The curve is a trochoid.
        curve_ans = [r - .5*sin(r),1-.5*cos(r)];
    case 4
        % The curve is a logarithmic (or Bernoulli) spiral.
        mult = exp(.1*r);
        curve_ans = [mult.*cos(r), mult.*sin(r)];
end

end % curve

end % Test_RoadSpline_2D

