function sol = RoadSpline_2D(curve,L,n_initial,n_final)

% We begin with a curve described parametrically by
%			curve(t)=(xfcn(t),yfcn(t))), 0 <= t <= L
% We produce a cubic spline interpolant using an even spacing
% with n_initial subintervals of [0,L].  This spline function is used to 
% calculate approximate derivatives for xfcn'(t) and yfcn'(t). We use 
% these to approximate the arc-length of the given curve using Simpson's
% rule; call it AL.  Use a large value for n_initial to obtain good 
% accuracy. This interpolating spline is then used to produce the mesh 
% for the final spline curve, which will have the equal spacing
%     h = AL/n_final
%
% This routine has as interpolation points for the arc-length parameterized 
% spline the points (x,y) which correspond to the parameter values
%     t = 0,h,2h,...,AL,
% and also the points with parameter values h/2 and AL-h/2. This improves
% on the error near to the endpoints when using the not-a-knot endpoint 
% conditions, the boundary conditions are implicit in the Matlab 
% program 'spline'.
%
% Using these spline fits of the x and y coordinates, 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 s, by solving the differential equation
%			dt/ds = 1/sqrt(xfcn'(t)^2 + yfcn'(t)^2)
% based on the derivative from the interpolating spline function. We do 
% this for evenly spaced values of s (stored below in S), obtaining the 
% corresponding values of t (stored below in T_s).
%
% We re-evaluate xfcn and yfcn to obtain values at equally spaced
% values of arclength, using nf 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 and ysprime 
% for the nf+1 evenly spaced values in S).

% Calculate initial spline approximation of curve based on the
% original parameterization variable t.
nf = n_final; n = n_initial;
h = L/n;
t = h*[0:n];
t = [0,h/2,t(2:n),L-h/2,L]';  % Contains endpoint correction nodes
temp = feval(curve,t);
xval = temp(:,1);
yval = temp(:,2);

% Produce the cubic spline interpolants to the x and y components of the
% given curve.
xpp = spline(t,xval);
ypp = spline(t,yval);
[xbreaks,xcoefs,xn,xk] = unmkpp(xpp);
[ybreaks,ycoefs,yn,yk] = unmkpp(ypp);

% Calculate the derivatives of the parameterization functions based
% on the parameterization variable t and using the spline interplant.
t = h*[0:n]';
ind = [1,3:n+1];
xcoefs = xcoefs(ind,:);
ycoefs = ycoefs(ind,:);
xprime = xcoefs(:,3);
yprime = ycoefs(:,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))];

% Calculate the arclength of the curve using Simpson's rule.
v = sqrt(xprime.^2 + yprime.^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 nf+1 evenly
% spaced points in [0,arclength] with the addition of the points
% at ds/2 and arclength-ds/2.
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-13,'AbsTol',1.0E-13);
[S,T_s] = ode45(@FSpline,S_Span,Y0,options);

% Produce the cubic spline to the curve, this time based on an 
% arclength parameterization of the curve.
temp = feval(curve,T_s);
xsval = temp(:,1);
ysval = temp(:,2);
xspp = spline(S,xsval);
yspp = spline(S,ysval);
xspp = remove_two_pts(xspp);
yspp = remove_two_pts(yspp);
[xsbreaks,xscoefs,xsn,xsk] = unmkpp(xspp);
[ysbreaks,yscoefs,ysn,ysk] = unmkpp(yspp);
ind = [1,3:nf+1,nf+3];
S_orig = S; Ts_orig = T_s;
S = S(ind);
T_s = T_s(ind);
xsval = xsval(ind); ysval = ysval(ind);

% Calculate the derivatives of the parameterization functions based
% on the parameterization variable s and using the spline interplant.
xsprime = xscoefs(:,3);
ysprime = yscoefs(:,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))];
xsprime2 = 2*xscoefs(:,2);
ysprime2 = 2*yscoefs(:,2);
xsprime2 = [xsprime2
   2*xscoefs(nf,2)+6*ds*xscoefs(nf,1)];
ysprime2 = [ysprime2
   2*yscoefs(nf,2)+6*ds*yscoefs(nf,1)];

% Set up the solution object for return to calling routine.
sol.S = S; sol.T_s = T_s;
sol.S_orig = S_orig; sol.Ts_orig = Ts_orig;
sol.xsval = xsval; sol.ysval = ysval; 
sol.xsprime = xsprime; sol.ysprime = ysprime; 
sol.xsprime2 = xsprime2; sol.ysprime2 = ysprime2; 
sol.xscoefs = xscoefs; sol.yscoefs = yscoefs; 
sol.xspp = xspp; sol.yspp = yspp; 
sol.arclength = arclength;

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

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 ans = FSpline(sz,tz)

i = floor(tz/h) + 1;
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));
ans = 1/sqrt(xsp^2 + ysp^2);

end % FSpline

end % RoadSpline_2D
