function sol = RoadSpline_3D(position,road_angle,L,n_initial,n_final)

% We begin with a curve (called the centerline of the road) described 
% parametrically by
%			position(t) = (xfcn(t),yfcn(t),zfcn(t)), 0 <= t <= L
% The function road_angle(t) is used in determining the slope of the road
% in the direction perpendicular to and to the left of the centerline, in
% the direction of motion (increasing values of the parameterization 
% variable t).
%
% The purpose of the program is produce cubic spline approximations to both
% the functions 'position' and 'road_angle', regarding them as functions of
% arclength s (i.e. distance along the centerline).  The spline functions
% will have n_final sub-divisions of equal arc length.
%
% We begin by producing a cubic spline interpolant to the function 
% 'position' using an even spacing of n_initial subintervals of [0,L].  
% This spline function is used to calculate approximate derivatives for 
% xfcn'(t), yfcn'(t), and zfcn'(t). Then we calculate an approximation to 
% the arclength of the curve over the parameterization interval of [0,L].  

% We then find the parameterization variable t as a function of 
% arclength,say s, by solving the differential equation
%			dt/ds = 1/sqrt(xfcn'(t)^2 + yfcn'(t)^2+ zfcn'(t)^2)
% based on the derivative from the interpolating spline function. 
% We do this for evenly spaced values of s (stored in S), obtaining 
% the corresponding values of t (stored in Ts).
%
% We re-evaluate 'position' to obtain values of it at equally spaced
% values of arclength, using n_final subdivisions of [0,arclength].
% We construct another cubic spline interpolant, this time based on 
% the arclength parameter s.  We then produce the tangents to the 
% curve from this new cubic spline (stored in xsprime, ysprime, and
% zsprime for the n_final+1 evenly spaced values in S, along with other
% useful quantities involved in the approximation). We also produce a 
% cubic spline approximation of 'road_angle' that uses the nodal values 
% at the node points in S.

% Initialization
n = n_initial; nf = n_final; h = L/n; 

% Calculate initial spline approximation of centerline curve based on the
% original parameterization variable t. It uses not-a-knot interpolation
% conditions, and the node points h/2 and L-h/2 are included for extra
% accuracy.
t = h*(0:n); t = [0,h/2,t(2:n),L-h/2,L]';
vpos = position(t);
xval = vpos(:,1); yval = vpos(:,2); zval = vpos(:,3);
xpp = spline(t,xval); ypp = spline(t,yval); zpp = spline(t,zval);
[xbreaks,xcoefs,xn,xk] = unmkpp(xpp);
[ybreaks,ycoefs,yn,yk] = unmkpp(ypp);
[zbreaks,zcoefs,zn,zk] = unmkpp(zpp);

% Calculate the derivatives of the parameterization functions based
% on the parameterization variable t and using the spline interpolant.
% These correspond to the node points t = h*(0:n).
ind = [1,3:n+1];    % This omits information at h/2 and L-h/2.
xcoefs = xcoefs(ind,:); ycoefs = ycoefs(ind,:); zcoefs = zcoefs(ind,:);
xprime = xcoefs(:,3); yprime = ycoefs(:,3); zprime = zcoefs(:,3);
xprime = [xprime
   xcoefs(n,3)+h*(2*xcoefs(n,2)+3*h*xcoefs(n,1))];
yprime = [yprime
   ycoefs(n,3)+h*(2*ycoefs(n,2)+3*h*ycoefs(n,1))];
zprime = [zprime
   zcoefs(n,3)+h*(2*zcoefs(n,2)+3*h*zcoefs(n,1))];

% Calculate the arclength of the curve using Simpson's rule.
v = sqrt(xprime.^2 + yprime.^2+ zprime.^2);
arclength = (h/3)*(v(1)+v(n+1) + 4*sum(v(2:2:n)) + 2*sum(v(3:2:n-1)));

% Calculate the variable t as a function of s, at the n_final+1 
% evenly spaced points in [0,arclength] with the addition of the
% points at ds/2 and arclength-ds/2.  The latter are included so as 
% use not-a-knot interpolation conditions for construction of the 
% cubic spline.
ds = arclength/nf;
S_Span = ds*(0:nf);
S_Span = [0,ds/2,S_Span(2:nf),arclength-ds/2,arclength]';
Y0 = 0;
options = odeset('RelTol',1.0E-12,'AbsTol',1.0E-12);
[S,Ts] = ode45(@FSpline,S_Span,Y0,options);
if is_row(S) 
    S = S'; Ts = Ts';
end

% Calculate the points on the curve for the arclength nodes in S.
vspos = position(Ts);
thetasval = road_angle(Ts);
xsval = vspos(:,1); ysval = vspos(:,2); zsval = vspos(:,3);

% Produce the cubic spline to the curve, this time based on an 
% arclength parameterization of the curve.
xspp = spline(S,xsval); yspp = spline(S,ysval); zspp = spline(S,zsval);
thetaspp = spline(S,thetasval);

% Remove the points corresponding to ds/2 and arclength-ds/2.
xspp = remove_two_pts(xspp);
yspp = remove_two_pts(yspp);
zspp = remove_two_pts(zspp);
thetaspp = remove_two_pts(thetaspp);
ind = [1,3:nf+1,nf+3]; S = S(ind); Ts = Ts(ind);

xsval = xsval(ind); ysval = ysval(ind); zsval = zsval(ind);
thetasval = thetasval(ind);

[xsbreaks,xscoefs,xsn,xsk] = unmkpp(xspp);
[ysbreaks,yscoefs,ysn,ysk] = unmkpp(yspp);
[zsbreaks,zscoefs,zsn,zsk] = unmkpp(zspp);
[thetasbreaks,thetascoefs,thetasn,thetask] = unmkpp(thetaspp);

% Calculate the derivatives of the parameterization functions based
% on the parameterization variable s and using the spline interpolant.

xsprime = xscoefs(:,3); ysprime = yscoefs(:,3); zsprime = zscoefs(:,3);
xsprime = [xsprime
   xscoefs(nf,3)+ds*(2*xscoefs(nf,2)+3*ds*xscoefs(nf,1))];
ysprime = [ysprime
   yscoefs(nf,3)+ds*(2*yscoefs(nf,2)+3*ds*yscoefs(nf,1))];
zsprime = [zsprime
   zscoefs(nf,3)+ds*(2*zscoefs(nf,2)+3*ds*zscoefs(nf,1))];
xsprime2 = 2*xscoefs(:,2); 
ysprime2 = 2*yscoefs(:,2); 
zsprime2 = 2*zscoefs(:,2);
xsprime2 = [xsprime2
   2*xscoefs(nf,2)+6*ds*xscoefs(nf,1)];
ysprime2 = [ysprime2
   2*yscoefs(nf,2)+6*ds*yscoefs(nf,1)];
zsprime2 = [zsprime2
   2*zscoefs(nf,2)+6*ds*zscoefs(nf,1)];

% At each point in the centerline of the roadbed, corresponding to the 
% points in S, produce the unit vector in the roadbed pointing to the 
% left (with respect to the direction of movement along the road) and 
% perpendicular to the centerline.
v = [xsprime,ysprime,zsprime];
for iv=1:length(S)           % Normalize to length 1.
   absv = norm(v(iv,:));
   v(iv,:) = v(iv,:)/absv;
end

% Produce the unit vectors in the road bed giving the direction (or slope)
% at each node point in S.
eta = eta_vector(S,thetasval,v);

% Set up the solution object for return to calling routine.
sol.S = S; sol.Ts = Ts;  sol.arclength = arclength;
sol.xsval = xsval; sol.ysval = ysval; sol.zsval = zsval; 
sol.thetasval = thetasval; sol.xsprime = xsprime; sol.ysprime = ysprime;
sol.zsprime = zsprime; sol.xsprime2 = xsprime2; sol.ysprime2 = ysprime2;
sol.zsprime2 = zsprime2; sol.xscoefs = xscoefs; sol.yscoefs = yscoefs; 
sol.zscoefs = zscoefs; sol.thetascoefs = thetascoefs; sol.xspp = xspp; 
sol.yspp = yspp; sol.zspp = zspp; sol.thetaspp = thetaspp; sol.eta = eta;

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

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 eta = eta_vector(r,theta,v)

% This routine produces a unit vector for each point of the road 
% centerline as specified according to arc length given in the  
% vector r. The corresponding slope of the road bed is given in the vector
% theta. This unit vector is located in the road bed and is orthogonal 
% to the centerline, pointing to the left as the driver moves along
% the centerline in the direction of increasing arc length.

% Have r be a column vector. The vector v has as its rows the unit 
% vectors directed in the direction of motion along the midline of 
% the road, as specified by 'position'.

% Initialization.
if is_row(r)
    r = r';
end
Lr = length(r); 
eta = zeros(Lr,3);
swtch = 1;
eta(:,3) = sin(theta)';  % The vertical component of the direction vector.

% Find the horizontal components of the direction vector.
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 = [-v(ij,2),v(ij,1)]*eta(ij,1:2)';
      if test < 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 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 pp_new=remove_two_pts(pp)

% This is to be used to remove any reference to the second and final 
% subintervals when using the not-a-knot interpolation condition.  This 
% is because the spline is the same polynomial of degree < 4 on the first 
% two subintervals and again, similarly, on the last two subintervals.

[breaks,coefs,L,k,d] = unmkpp(pp);
coefs = [coefs(1,:)
    coefs(3:L-1,:)];
breaks = [breaks(1), breaks(3:L-1) breaks(L+1)];
pp_new = mkpp(breaks,coefs,d);
end % remove_two_pts

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

function Fval = FSpline(sz,tz)

i = floor(tz/h) + 1; % Recall, n = n_initial and h = L/n
if i == n+1
   i = n;
end

d = tz - (i-1)*h;
xsp = xcoefs(i,3) + d*(2*xcoefs(i,2) + 3*d*xcoefs(i,1));
ysp = ycoefs(i,3) + d*(2*ycoefs(i,2) + 3*d*ycoefs(i,1));
zsp = zcoefs(i,3)+d*(2*zcoefs(i,2)+3*d*zcoefs(i,1));
Fval = 1/sqrt(xsp^2 + ysp^2 + zsp^2);
end % FSpline

end % RoadSpline3D
