% Accuracy of Augmented Reality-guided Needle Placement for Pulsed Radiofrequency Treatment of Pudendal Neuralgia: A Pilot Study on a Phantom Model
% Lars L. Boogaard, Kim J.B. Notten, Kirsten B. Kluivers, Selina E.I. van der Wal, Thomas J.J. Maal, Luc M. Verhamme
% MATLAB Code for analysis

% Clear workspace
clear all; close all;

% Load segmented data in STL format
% Load simulated nerve. This is the nerve which is also visualized in AR
Nerve_ctrl = stlread('[]');

% Load folder path for all matched (based on ICP) STL files of the needles
folderPath = '[]';

% Get a list of all STL files in the folder
stlFiles = dir(fullfile(folderPath, '*.stl'));

stlStruct = struct([]);
stl_needles = struct([]);
num = numel(stlFiles);

% Loop through each STL file and load it into a struct
for i = 1:num
    % Get file name and full file path
    fileName = stlFiles(i).name;
    filePath = fullfile(folderPath, fileName);
    
    % Load STL file into a struct using stlread function
    [vertices, faces] = stlread(filePath);
    stlStruct(i).Name = fileName;
    stlStruct(i).Vertices = vertices;
%     stlStruct(i).Faces = faces;
end

% Iterate through each STL object in the stlStruct array
for i = 1:length(stlStruct)
     % Extract the vertex points of the current STL object
    stl_needles(i).points = stlStruct(i).Vertices.Points;
    
     % Check if the object needs its vertex points flipped
    if stl_needles(i).points(end,3) > stl_needles(i).points(1,3)
        % If the object is upside down, reverse the order of vertex points
        stl_needles(i).points = flip(stl_needles(i).points);
    end
end

% Detect the most outer tip of the needle.
PP_Data = struct([]);
for i = 1:num
   PP_Data(i).line_fit = line_fit_3d(stl_needles(i).points');
end

% Interpolate 3D line of needle with a step size of 0.01 mm.
step_size = 0.01;
% Active tip of needle is 5 mm
l = 5;

% The total amount of points used for the function interparc
t = 5000;

for i = 1:num
    PP_Data(i).needle_line = interparc(t, PP_Data(i).line_fit(1,:), PP_Data(i).line_fit(2,:), PP_Data(i).line_fit(3,:),'linear');

    % Calculate the direction vector of the needle line
    needle_dir_vec = (PP_Data(i).line_fit(:,2) - PP_Data(i).line_fit(:,1)) / norm(PP_Data(i).line_fit(:,2) - PP_Data(i).line_fit(:,1));
    PP_Data(i).tip_needle_line(1,:) = PP_Data(i).line_fit(:,1)';

    for j = 2:l/step_size + 1
        % Calculate the next point along the needle line by adding the scaled direction vector to the previous point
        PP_Data(i).tip_needle_line(j,:) = PP_Data(i).tip_needle_line(j-1,:) + needle_dir_vec'.*step_size;    
    end
end


% Define the increment for extending the needle lines
l_incr = 5000;

for i = 1:num
    % Calculate the delta vector representing the change per step along the needle line
    PP_Data(i).delta = (PP_Data(i).needle_line(end,:) - PP_Data(i).needle_line(1,:))/length(PP_Data(i).needle_line);
    
    % Check if the length of the needle line is equal to t + l_incr
    if length(PP_Data(i).needle_line) == t+l_incr
        continue
    end
    for j = 1:l_incr
        % Calculate a new point to prepend to the needle line
        tmp = PP_Data(i).needle_line(1,:) - PP_Data(i).delta;
        % Prepend the new point to the needle line
        PP_Data(i).needle_line = [tmp; PP_Data(i).needle_line];
    end
end

% Find distances between tip of the needle and nerve in three directions
distance_tip = zeros(1,num);    % Euclidean
distance_dir = zeros(1,num);    % Lateral
distance_depth = zeros(1,num);  % depth

for i = 1:num   
    distance_tip(i) = shortest_distance_cloud_point(Nerve_ctrl.Points, PP_Data(i).tip_needle_line); 
    [PP_Data(i).nerve_P, PP_Data(i).needle_P, distance_dir(i)] = closest_point(Nerve_ctrl.Points, PP_Data(i).needle_line);
    distance_depth(i) = min( pdist2(PP_Data(i).needle_P, PP_Data(i).tip_needle_line)); 
end

% Check direction of distance depth. Positive is defined as 'to deep'. Change accordingly
for i = 1:num
    % integrer of CloseP
    dir_tmp(1) = find(PP_Data(i).needle_line(:,1) == PP_Data(i).needle_P(1));
    % integrer of tip
    dir_tmp(2) = find(PP_Data(i).needle_line(:,1) == PP_Data(i).line_fit(:,1)');
    
    if dir_tmp(2) < dir_tmp(1)
        distance_depth(i) = -abs(distance_depth(i));
    end
end

% Check direction of distance lateral. Positive is defined as 'to the right, from top view'. Change accordingly

for i = 1:num
    [~, nerve_P_idx] = ismember(PP_Data(i).nerve_P, Nerve_ctrl.Points, 'rows');
    line_nerve_dir = Nerve_ctrl.Points(nerve_P_idx,:) - Nerve_ctrl.Points(nerve_P_idx-1,:);    
    line_needle_dir = (PP_Data(i).line_fit(:,2) - PP_Data(i).line_fit(:,1))';

% Calculate the cross product
    cross_prod = cross(line_nerve_dir, line_needle_dir);

% Calculate the vector from needle to nerve
    v = PP_Data(i).nerve_P - PP_Data(i).needle_P;

% Calculate the dot product
    dot_prod(i) = dot(v,cross_prod);
    
    if dot_prod(i) > 0
        distance_dir_sign(i) = -abs(distance_dir(i));
    else
        distance_dir_sign(i) = abs(distance_dir(i));
    end
end

    
%% All used functions
function [short_dist] = shortest_distance_cloud_point(cloud, point)

% Calculate pairwise Euclidean distances between points in 'cloud' and 'point'
D = pdist2(cloud, point, 'euclidean');

% Find the minimum distance value among all pairwise distances
short_dist = min(D(:));

end


function [CloseP_nerve, CloseP_needle, close_dist] = closest_point(nerve, needle)
% CloseP_nerve = closest point on nerve
% CloseP_needle = closest point on needle
% clost_dist = distance between CloseP_nerve and CloseP_needle

D = pdist2(nerve, needle, 'euclidean');

close_dist = min(D(:));
[S_nerve, S_needle] = find(D==close_dist);
CloseP_needle(1,1:3) = needle(S_needle,:);
CloseP_nerve(1,1:3) = nerve(S_nerve,:);


end


function xyz_fit = line_fit_3d(xyz)
% xyz size 3 x n

[m, n] = size(xyz);
if m > 3 && n > 3
    error('xyz must be 3 x n matrix');
end
if m> 3 && n == 3
    xyz = xyz';
end

% Engine  
xyz0 = mean(xyz,2);
A = xyz-xyz0;
[U,S,~] = svd(A);
d = U(:,1);
t = d'*A;
t1 = min(t);
t2 = max(t);
xyz_fit = xyz0 + [t1,t2].*d; % size 3x2

end


function line_interpl = Best_fit_line_3D_nerve(nerve, p, t, varargin)

[m, n] = size(nerve);
if m > 3 && n > 3
    error('input must be 3 x n matrix');
end
if m> 3 && n == 3
    nerve = nerve';
end

method = 'linear';

if nargin > 3
    if ischar(varargin{end})
    method = varargin{end};
    varargin(end) = [];
    
    % method may be any of {'linear', 'pchip', 'spline', 'csape'.}
    % any any simple contraction thereof.
    valid = {'linear', 'spline'};
    [method,errstr] = validstring(method,valid);
        if ~isempty(errstr)
        error('INTERPARC:incorrectmethod',errstr)
        end
    end
end


% All intermediate points as average of certain range
min_z = min(nerve(3,:));
max_z = max(nerve(3,:));
z_interpl = [min_z:(max_z-min_z)/p:max_z];

% Split nerve matrix into small segments
for i = 1:length(z_interpl)-1
   k = find( nerve(3,:) >= z_interpl(i) & nerve(3,:) <= z_interpl(i+1) );
   
   x_tmp(i) = mean(nerve(1,k));
   y_tmp(i) = mean(nerve(2,k));
   z_tmp(i) = (z_interpl(i)+z_interpl(i+1)) / 2;
end

line_interpl = interparc(t, x_tmp', y_tmp', z_tmp', method);

end


function [str,errorclass] = validstring(arg,valid)
% validstring: compares a string against a set of valid options
% usage: [str,errorclass] = validstring(arg,valid)
%
% If a direct hit, or any unambiguous shortening is found, that
% string is returned. Capitalization is ignored.
%
% arguments: (input)
%  arg - character string, to be tested against a list
%        of valid choices. Capitalization is ignored.
%
%  valid - cellstring array of alternative choices
%
% Arguments: (output)
%  str - string - resulting choice resolved from the
%        list of valid arguments. If no unambiguous
%        choice can be resolved, then str will be empty.
%
%  errorclass - string - A string argument that explains
%        the error. It will be one of the following
%        possibilities:
%
%        ''  --> No error. An unambiguous match for arg
%                was found among the choices.
%
%        'No match found' --> No match was found among 
%                the choices provided in valid.
%
%        'Ambiguous argument' --> At least two ambiguous
%                matches were found among those provided
%                in valid.
%        
%
% Example:
%  valid = {'off' 'on' 'The sky is falling'}
%  
%
% See also: parse_pv_pairs, strmatch, strcmpi
%
% Author: John D'Errico
% e-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 3/25/2010

ind = find(strncmpi(lower(arg),valid,numel(arg)));
if isempty(ind)
  % No hit found
  errorclass = 'No match found';
  str = '';
elseif (length(ind) > 1)
  % Ambiguous arg, hitting more than one of the valid options
  errorclass = 'Ambiguous argument';
  str = '';
  return
else
  errorclass = '';
  str = valid{ind};
end

end % function validstring

function [pt,dudt,fofthandle] = interparc(t,px,py,varargin)
% interparc: interpolate points along a curve in 2 or more dimensions
% usage: pt = interparc(t,px,py)    % a 2-d curve
% usage: pt = interparc(t,px,py,pz) % a 3-d curve
% usage: pt = interparc(t,px,py,pz,pw,...) % a 4-d or higher dimensional curve
% usage: pt = interparc(t,px,py,method) % a 2-d curve, method is specified
% usage: [pt,dudt,fofthandle] = interparc(t,px,py,...) % also returns derivatives, and a function handle
%
% Interpolates new points at any fractional point along
% the curve defined by a list of points in 2 or more
% dimensions. The curve may be defined by any sequence
% of non-replicated points.
%
% arguments: (input)
%  t   - vector of numbers, 0 <= t <= 1, that define
%        the fractional distance along the curve to
%        interpolate the curve at. t = 0 will generate
%        the very first point in the point list, and
%        t = 1 yields the last point in that list.
%        Similarly, t = 0.5 will yield the mid-point
%        on the curve in terms of arc length as the
%        curve is interpolated by a parametric spline.
%
%        If t is a scalar integer, at least 2, then
%        it specifies the number of equally spaced
%        points in arclength to be generated along
%        the curve.
%
%  px, py, pz, ... - vectors of length n, defining
%        points along the curve. n must be at least 2.
%        Exact Replicate points should not be present
%        in the curve, although there is no constraint
%        that the curve has replicate independent
%        variables.
%
%  method - (OPTIONAL) string flag - denotes the method
%        used to compute the points along the curve.
%
%        method may be any of 'linear', 'spline', or 'pchip',
%        or any simple contraction thereof, such as 'lin',
%        'sp', or even 'p'.
%        
%        method == 'linear' --> Uses a linear chordal
%               approximation to interpolate the curve.
%               This method is the most efficient.
%
%        method == 'pchip' --> Uses a parametric pchip
%               approximation for the interpolation
%               in arc length.
%
%        method == 'spline' --> Uses a parametric spline
%               approximation for the interpolation in
%               arc length. Generally for a smooth curve,
%               this method may be most accurate.
%
%        method = 'csape' --> if available, this tool will
%               allow a periodic spline fit for closed curves.
%               ONLY use this method if your points should
%               represent a closed curve.
%               
%               If the last point is NOT the same as the
%               first point on the curve, then the curve
%               will be forced to be periodic by this option.
%               That is, the first point will be replicated
%               onto the end.
%
%               If csape is not present in your matlab release,
%               then an error will result.
%
%        DEFAULT: 'spline'
%
%
% arguments: (output)
%  pt - Interpolated points at the specified fractional
%        distance (in arc length) along the curve.
%
%  dudt - when a second return argument is required,
%       interparc will return the parametric derivatives
%       (dx/dt, dy/dt, dz/dt, ...) as an array.
%
%  fofthandle - a function handle, taking numbers in the interval [0,1]
%       and evaluating the function at those points.
%
%       Extrapolation will not be permitted by this call.
%       Any values of t that lie outside of the interval [0,1]
%       will be clipped to the endpoints of the curve.
%
% Example:
% % Interpolate a set of unequally spaced points around
% % the perimeter of a unit circle, generating equally
% % spaced points around the perimeter.
% theta = sort(rand(15,1))*2*pi;
% theta(end+1) = theta(1);
% px = cos(theta);
% py = sin(theta);
%
% % interpolate using parametric splines
% pt = interparc(100,px,py,'spline');
%
% % Plot the result
% plot(px,py,'r*',pt(:,1),pt(:,2),'b-o')
% axis([-1.1 1.1 -1.1 1.1])
% axis equal
% grid on
% xlabel X
% ylabel Y
% title 'Points in blue are uniform in arclength around the circle'
%
%
% Example:
% % For the previous set of points, generate exactly 6
% % points around the parametric splines, verifying
% % the uniformity of the arc length interpolant.
% pt = interparc(6,px,py,'spline');
%
% % Convert back to polar form. See that the radius
% % is indeed 1, quite accurately.
% [TH,R] = cart2pol(pt(:,1),pt(:,2))
% % TH =
% %       0.86005
% %        2.1141
% %       -2.9117
% %        -1.654
% %      -0.39649
% %       0.86005
% % R =
% %             1
% %        0.9997
% %        0.9998
% %       0.99999
% %        1.0001
% %             1
%
% % Unwrap the polar angles, and difference them.
% diff(unwrap(TH))
% % ans =
% %        1.2541
% %        1.2573
% %        1.2577
% %        1.2575
% %        1.2565
%
% % Six points around the circle should be separated by
% % 2*pi/5 radians, if they were perfectly uniform. The
% % slight differences are due to the imperfect accuracy
% % of the parametric splines.
% 2*pi/5
% % ans =
% %        1.2566
%
%
% See also: arclength, spline, pchip, interp1
%
% Author: John D'Errico
% e-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 3/15/2010

% unpack the arguments and check for errors
if nargin < 3
  error('ARCLENGTH:insufficientarguments', ...
    'at least t, px, and py must be supplied')
end

t = t(:);
if (numel(t) == 1) && (t > 1) && (rem(t,1) == 0)
  % t specifies the number of points to be generated
  % equally spaced in arclength
  t = linspace(0,1,t)';
elseif any(t < 0) || any(t > 1)
  error('ARCLENGTH:impropert', ...
    'All elements of t must be 0 <= t <= 1')
end

% how many points will be interpolated?
nt = numel(t);

% the number of points on the curve itself
px = px(:);
py = py(:);
n = numel(px);

% are px and py both vectors of the same length?
if ~isvector(px) || ~isvector(py) || (length(py) ~= n)
  error('ARCLENGTH:improperpxorpy', ...
    'px and py must be vectors of the same length')
elseif n < 2
  error('ARCLENGTH:improperpxorpy', ...
    'px and py must be vectors of length at least 2')
end

% compose px and py into a single array. this way,
% if more dimensions are provided, the extension
% is trivial.
pxy = [px,py];
ndim = 2;

% the default method is 'linear'
method = 'spline';

% are there any other arguments?
if nargin > 3
  % there are. check the last argument. Is it a string?
  if ischar(varargin{end})
    method = varargin{end};
    varargin(end) = [];
    
    % method may be any of {'linear', 'pchip', 'spline', 'csape'.}
    % any any simple contraction thereof.
    valid = {'linear', 'pchip', 'spline', 'csape'};
    [method,errstr] = validstring(method,valid);
    if ~isempty(errstr)
      error('INTERPARC:incorrectmethod',errstr)
    end
  end
  
  % anything that remains in varargin must add
  % an additional dimension on the curve/polygon
  for i = 1:numel(varargin)
    pz = varargin{i};
    pz = pz(:);
    if numel(pz) ~= n
      error('ARCLENGTH:improperpxorpy', ...
        'pz must be of the same size as px and py')
    end
    pxy = [pxy,pz]; %#ok
  end
  
  % the final number of dimensions provided
  ndim = size(pxy,2);
end

% if csape, then make sure the first point is replicated at the end.
% also test to see if csape is available
if method(1) == 'c'
  if exist('csape','file') == 0
    error('CSAPE was requested, but you lack the necessary toolbox.')
  end
  
  p1 = pxy(1,:);
  pend = pxy(end,:);
  
  % get a tolerance on whether the first point is replicated.
  if norm(p1 - pend) > 10*eps(norm(max(abs(pxy),[],1)))
    % the two end points were not identical, so wrap the curve
    pxy(end+1,:) = p1;
    nt = nt + 1;
  end
end

% preallocate the result, pt
pt = NaN(nt,ndim);

% Compute the chordal (linear) arclength
% of each segment. This will be needed for
% any of the methods.
chordlen = sqrt(sum(diff(pxy,[],1).^2,2));

% Normalize the arclengths to a unit total
chordlen = chordlen/sum(chordlen);

% cumulative arclength
cumarc = [0;cumsum(chordlen)];

% The linear interpolant is trivial. do it as a special case
if method(1) == 'l'
  % The linear method.
  
  % which interval did each point fall in, in
  % terms of t?
  [junk,tbins] = histc(t,cumarc); %#ok
  
  % catch any problems at the ends
  tbins((tbins <= 0) | (t <= 0)) = 1;
  tbins((tbins >= n) | (t >= 1)) = n - 1;
  
  % interpolate
  s = (t - cumarc(tbins))./chordlen(tbins);
  % be nice, and allow the code to work on older releases
  % that don't have bsxfun
  pt = pxy(tbins,:) + (pxy(tbins+1,:) - pxy(tbins,:)).*repmat(s,1,ndim);
  
  % do we need to compute derivatives here?
  if nargout > 1
    dudt = (pxy(tbins+1,:) - pxy(tbins,:))./repmat(chordlen(tbins),1,ndim);
  end
  
  % do we need to create the spline as a piecewise linear function?
  if nargout > 2
    spl = cell(1,ndim);
    for i = 1:ndim
      coefs = [diff(pxy(:,i))./diff(cumarc),pxy(1:(end-1),i)];
      spl{i} = mkpp(cumarc.',coefs);
    end
    
    %create a function handle for evaluation, passing in the splines
    fofthandle = @(t) foft(t,spl);
  end
  
  % we are done at this point
  return
end

% If we drop down to here, we have either a spline
% or csape or pchip interpolant to work with.

% compute parametric splines
spl = cell(1,ndim);
spld = spl;
diffarray = [3 0 0;0 2 0;0 0 1;0 0 0];
for i = 1:ndim
  switch method
    case 'pchip'
      spl{i} = pchip(cumarc,pxy(:,i));
    case 'spline'
      spl{i} = spline(cumarc,pxy(:,i));
      nc = numel(spl{i}.coefs);
      if nc < 4
        % just pretend it has cubic segments
        spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs];
        spl{i}.order = 4;
      end
    case 'csape'
      % csape was specified, so the curve is presumed closed,
      % therefore periodic
      spl{i} = csape(cumarc,pxy(:,i),'periodic');
      nc = numel(spl{i}.coefs);
      if nc < 4
        % just pretend it has cubic segments
        spl{i}.coefs = [zeros(1,4-nc),spl{i}.coefs];
        spl{i}.order = 4;
      end
  end
  
  % and now differentiate them
  xp = spl{i};
  xp.coefs = xp.coefs*diffarray;
  xp.order = 3;
  spld{i} = xp;
end

% catch the case where there were exactly three points
% in the curve, and spline was used to generate the
% interpolant. In this case, spline creates a curve with
% only one piece, not two.
if (numel(cumarc) == 3) && (method(1) == 's')
  cumarc = spl{1}.breaks;
  n = numel(cumarc);
  chordlen = sum(chordlen);
end

% Generate the total arclength along the curve
% by integrating each segment and summing the
% results. The integration scheme does its job
% using an ode solver.

% polyarray here contains the derivative polynomials
% for each spline in a given segment
polyarray = zeros(ndim,3);
seglen = zeros(n-1,1);

% options for ode45
opts = odeset('reltol',1.e-9);
for i = 1:spl{1}.pieces
  % extract polynomials for the derivatives
  for j = 1:ndim
    polyarray(j,:) = spld{j}.coefs(i,:);
  end
  
  % integrate the arclength for the i'th segment
  % using ode45 for the integral. I could have
  % done this part with quad too, but then it
  % would not have been perfectly (numerically)
  % consistent with the next operation in this tool.
  [tout,yout] = ode45(@(t,y) segkernel(t,y),[0,chordlen(i)],0,opts); %#ok
  seglen(i) = yout(end);
end

% and normalize the segments to have unit total length
totalsplinelength = sum(seglen);
cumseglen = [0;cumsum(seglen)];

% which interval did each point fall into, in
% terms of t, but relative to the cumulative
% arc lengths along the parametric spline?
[junk,tbins] = histc(t*totalsplinelength,cumseglen); %#ok

% catch any problems at the ends
tbins((tbins <= 0) | (t <= 0)) = 1;
tbins((tbins >= n) | (t >= 1)) = n - 1;

% Do the fractional integration within each segment
% for the interpolated points. t is the parameter
% used to define the splines. It is defined in terms
% of a linear chordal arclength. This works nicely when
% a linear piecewise interpolant was used. However,
% what is asked for is an arclength interpolation
% in terms of arclength of the spline itself. Call s
% the arclength traveled along the spline.
s = totalsplinelength*t;

% the ode45 options will now include an events property
% so we can catch zero crossings.
opts = odeset('reltol',1.e-9,'events',@ode_events);

ti = t;
for i = 1:nt
  % si is the piece of arc length that we will look
  % for in this spline segment.
  si = s(i) - cumseglen(tbins(i));
  
  % extract polynomials for the derivatives
  % in the interval the point lies in
  for j = 1:ndim
    polyarray(j,:) = spld{j}.coefs(tbins(i),:);
  end
  
  % we need to integrate in t, until the integral
  % crosses the specified value of si. Because we
  % have defined totalsplinelength, the lengths will
  % be normalized at this point to a unit length.
  %
  % Start the ode solver at -si, so we will just
  % look for an event where y crosses zero.
  [tout,yout,te,ye] = ode45(@(t,y) segkernel(t,y),[0,chordlen(tbins(i))],-si,opts); %#ok
  
  % we only need that point where a zero crossing occurred
  % if no crossing was found, then we can look at each end.
  if ~isempty(te)
    ti(i) = te(1) + cumarc(tbins(i));
  else
    % a crossing must have happened at the very
    % beginning or the end, and the ode solver
    % missed it, not trapping that event.
    if abs(yout(1)) < abs(yout(end))
      % the event must have been at the start.
      ti(i) = tout(1) + cumarc(tbins(i));
    else
      % the event must have been at the end.
      ti(i) = tout(end) + cumarc(tbins(i));
    end
  end
end

% Interpolate the parametric splines at ti to get
% our interpolated value.
for L = 1:ndim
  pt(:,L) = ppval(spl{L},ti);
end

% do we need to compute first derivatives here at each point?
if nargout > 1
  dudt = zeros(nt,ndim);
  for L = 1:ndim
    dudt(:,L) = ppval(spld{L},ti);
  end
end

% create a function handle for evaluation, passing in the splines
if nargout > 2
  fofthandle = @(t) foft(t,spl);
end

% ===============================================
%  nested function for the integration kernel
% ===============================================
  function val = segkernel(t,y) %#ok
    % sqrt((dx/dt)^2 + (dy/dt)^2 + ...)
    val = zeros(size(t));
    for k = 1:ndim
      val = val + polyval(polyarray(k,:),t).^2;
    end
    val = sqrt(val);
    
  end % function segkernel

% ===============================================
%  nested function for ode45 integration events
% ===============================================
  function [value,isterminal,direction] = ode_events(t,y) %#ok
    % ode event trap, looking for zero crossings of y.
    value = y;
    isterminal = ones(size(y));
    direction = ones(size(y));
  end % function ode_events

end % mainline - interparc


% ===============================================
%       end mainline - interparc
% ===============================================
%       begin subfunctions
% ===============================================

% ===============================================
%  subfunction for evaluation at any point externally
% ===============================================
function f_t = foft(t,spl)
% tool allowing the user to evaluate the interpolant at any given point for any values t in [0,1]
pdim = numel(spl);
f_t = zeros(numel(t),pdim);

% convert t to a column vector, clipping it to [0,1] as we do.
t = max(0,min(1,t(:)));

% just loop over the splines in the cell array of splines
for i = 1:pdim
  f_t(:,i) = ppval(spl{i},t);
end
end % function foft