function cv = Test_RoadSpline_3D(problem,angle_fcn,curve_length,...
         prob_params,n_initial_spline,n_final_spline)

% As an example case, try the command
%       CurveObject = Test_RoadSpline_3D(1,2,4*pi,[1 2 0.2],200,50);
% When the program pauses, merely touch the space bar to have it continue.
% The output variable 'cv' is a structure that contains information
% needed in the program NearestPt_3D.

% This is a test program for 
%     sol = RoadSpline_3D(position,road_angle,L,n_initial,n_final)
% Various test problems can be accommodated by modifying the functions
% 'position' and 'road_angle' given below at the end of this file, and
% they are indexed using the  input parameters 'problem' and 'angle_fcn',
% respectively. The other input parameters are defined as follows.
%
% curve_length:     The interval of the original parameterization variable
%                   is [0,curve_length].
% prob_params:      This is a vector giving parameters needed in defining
%                   centerline given below in function 'position'.
% n_initial_spline: The first spline fit is based on the initial
%                   parameterization variable, and it uses n_initial_spline
%                   subdivisions of [0,curve_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.
%

% Initialization.
n = n_initial_spline; nf = n_final_spline; L = curve_length;
format long

% Plot the original curve using the original parameterization variable.
h=L/n;
t=h*(0:n)';
% Calculate the midline of the road and the unit vector
% in the direction of motion along that midline.
p = position(t); v = velocity(t); theta = road_angle(t);
% Calculate the unit vector in the roadway orthogonal to the centerline
% and pointing to the left.
eta = eta_vector(t,theta,v);
trial(length(t)-1,p,eta);
title('The curve using the given parameterization over [0,L]')
pause

% Produce the spline approximations of 'position' and 'road_angle'.
cv = RoadSpline_3D(@position,@road_angle,L,n,nf);

% Using the structure cv, define the needed parameters for the spline
% parameterization. 
S = cv.S; Ts = cv.Ts; 
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; 
arclength = cv.arclength;
xscoefs = cv.xscoefs; yscoefs = cv.yscoefs; zscoefs = cv.zscoefs; 
thetascoefs = cv.thetascoefs;
% The following are not used in this test program.
%xspp = cv.xspp; yspp = cv.yspp; zspp = cv.zspp; 
%thetasval = cv.thetasval; thetaspp = cv.thetaspp; eta = cv.eta; 

figure
plot(S,Ts)
xlabel(['n_{init} = ',num2str(n),'   n_{final} = ',num2str(nf)])
axis('auto')
title('Arclength s vs. given parameter t')
xlabel('s')
ylabel('t')
disp(['Arc length = ',num2str(arclength)])
pause

hs = arclength/nf; 
ss = (hs/20)*(0:20*nf)';
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')
title(['The centerline with given parameterization : n_{init} = ',... 
                  num2str(n),'   n_{final} = ',num2str(nf),'     '])
axis('equal')
hold on
plot3(xsval,ysval,zsval,'o')
grid on
hold off
pause

hf = arclength/n;
ss=hf*(0:n)';
[p,v,theta] = EvalSpline(ss);
eta = eta_vector(ss,theta,v);
trial(length(ss)-1,p,eta);
title(['The road with arclength parameterization of the centerline : ',...
    'n_{init} = ',num2str(n),'   n_{final} = ',num2str(nf),'     '])
pause

% Do a test of the arc-length parameterization.  Ideally we have the
% identity
%   sqrt([x'(s)^2 + y'(s)^2 + z'(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+zsprime.^2);
figure
plot(S,test-1)
axis('auto')
title('Arclength parameterization test for spline')
xlabel(['Initial n = ',num2str(n),'   Final n = ',num2str(nf)])
format short e
testmax = max(abs(test-1));
testmax2 = max(abs(test(3:nf-1)-1));
disp(['max of arclength parameterization test = ',num2str(testmax)])
disp('max of arclength parameterization test absent the two left-most')
disp(['           and two right-most node points = ',num2str(testmax2)])
format short
pause

% Calculate curvature at each point of S
p1 = ysprime2.*zsprime - zsprime2.*ysprime;
p2 = zsprime2.*xsprime - xsprime2.*zsprime;
p3 = xsprime2.*ysprime - ysprime2.*xsprime;
numerator = sqrt(p1.*p1 + p2.*p2 + p3.*p3);
denominator = sqrt(xsprime.^2 + ysprime.^2 + zsprime.^2).^3;
curvature = numerator./denominator;
curvmax = max(curvature);
curvmin = min(curvature);
disp(['max of curvature = ',num2str(curvmax)])
disp(['min of curvature = ',num2str(curvmin)])
format short

figure
plot(S,curvature)
title(['Curvature of spline function, regarded',...
    ' as a function of arclength'])
xlabel(['n_{init} = ',num2str(n),'   n_{final} = ',num2str(nf)])


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

function trial(m,p,eta)

% Draw the roadway using the positions in p and the road bed vectors 
% in eta. The rows of v are the unit vectors along the centerline. The
% number of points at which p and eta is specified is m.

% Initialize
road_width = .20;   % Needs to be more nuanced and problem independent.

% Plot the midline.
figure
plot3(p(:,1),p(:,2),p(:,3),'k')
xlabel('x')
ylabel('y')
zlabel('z')
hold on

edge_left = zeros(size(eta));
edge_right = zeros(size(eta));
% Plot the orthogonal lines to the midline
for i=1:m+1
   x = p(i,1)+road_width*eta(i,1);
   y = p(i,2)+road_width*eta(i,2);
   z = p(i,3)+road_width*eta(i,3);
   plot3([p(i,1),x],[p(i,2),y],[p(i,3),z],'k')
   edge_left(i,:)=[x,y,z];
   x = p(i,1)-road_width*eta(i,1);
   y = p(i,2)-road_width*eta(i,2);
   z = p(i,3)-road_width*eta(i,3);
   plot3([p(i,1),x],[p(i,2),y],[p(i,3),z],'k')
   edge_right(i,:) = [x,y,z];
end

% Plot the edges of the road bed, with green on the right side
% and red on the left side of the roadbed.
plot3(edge_left(:,1),edge_left(:,2),edge_left(:,3),'r')
plot3(edge_right(:,1),edge_right(:,2),edge_right(:,3),'g')

axis equal
hold off

end % trial

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

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

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

function [p,v,theta] = EvalSpline(t)

% Evaluate the interpolating spline which uses the arc-length
% parameterization, along with its first and second derivatives, at the
% node points given in t.

m = length(t);
p = zeros(m,3); theta = zeros(m,1); v = zeros(m,3);
for ik=1:m
    ind = floor(t(ik)/hs);
    if ind == nf
        ind = nf-1;
    end
    ind = ind + 1;
    % The point t(ik) is in [S(ind),S(ind+1)].
    z = t(ik) - S(ind);
    [px,dpx,d2px] = EvalCubic(xscoefs(ind,:),z);
    [py,dpy,d2py] = EvalCubic(yscoefs(ind,:),z);
    [pz,dpz,d2pz] = EvalCubic(zscoefs(ind,:),z);
    [theta_s,dtheta_s,d2theta_s]=EvalCubic(thetascoefs(ind,:),z);
    p(ik,:) = [px,py,pz]; theta(ik) = theta_s;
    v(ik,:) = [dpx,dpy,dpz]/norm([dpx,dpy,dpz]);
end  
end % EvalSpline

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

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

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

function pos_ans = position(t)

% Make t a column vector.
% Each row of ans will represent a position for
% the corresponding component of the parameter t.

switch problem
    case 1
        a = prob_params(1); b = prob_params(2); c = prob_params(3); 
        pos_ans = [a*cos(t),b*sin(t),c*t]; 
    case 2
        a = prob_params(1); b = prob_params(2); c = prob_params(3); 
        pos_ans = [t - a*sin(t),1-b*cos(t),c*t];
end
end % position

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

function vel_ans=velocity(t)

% Make t a column vector.
% Each column of ans will represent the unit vector
% in the direction of the motion specified by position(t)
% for the corresponding component of the parameter t.

switch problem
    case 1
        a = prob_params(1); b = prob_params(2); c = prob_params(3); 
        vel_ans = [-a*sin(t),b*cos(t),c*ones(size(t))]; 
    case 2
        a = prob_params(1); b = prob_params(2); c = prob_params(3); 
        vel_ans = [1 - a*cos(t),b*sin(t),c*ones(size(t))];
end

% Normalize to length 1.

for i=1:length(t)
   absv = norm(vel_ans(i,:));
   vel_ans(i,:)=vel_ans(i,:)./absv;
end
end % velocity

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

function theta = road_angle(t)

% This defines the function theta(t), used in determining the slope of the 
% road in the direction perpendicular to and to the left of the mid-line, in
% the direction of motion.  In short, this gives the angle of elevation  
% from the horizontal for the road when looking to your left as driving.
% An angle of theta=0 denotes a flat road as regards looking to your left
% in the direction perpendicular to the mid-line.  The angle is measured in
% radian measure.

switch angle_fcn
    case 1
        theta = zeros(size(t));
    case 2
        theta = -(pi/20)*(sin(t).^2)/2; 
    case 3
        theta = -(pi/20)*((1+sin(t))/2);
end
end % road_angle

end % SCC_mod3
