function zi = spline1d(x,z,xi,action,options)
% Found this on the MathWorks forums, posted by Bruno Luong.
% This spline function can do the natural spline, unlike
% the spline() function that is built into MATLAB and Octave.
% I have not tested it. If you try it, let me know how it works!
%
% function s = spline1d(x,z,[],'InitOnly') OR
% zi = spline1d(x,z,xi) OR
% zi = spline1d(..., 'ForceReset') OR
% zi = spline1d(..., s)
%
% zi = spline1d(..., options); % fifth parameters
%
% bi-cubic-spline interpolation (natural conditions by default)
%
% Author: Bruno Luong
% Update: 01/August/2008
% Optional first/second derivatives at the boundary could be provided
% through fields 'leftzx', 'rightzx', 'leftzxx', and 'rightzxx' of
% the structure options (fifth parameters)
% NOTE: Second derivative will be ignored if first derivative is provided
% 14/July/2008
% Not-a-knot condition (third derivatives match at next-end points)
% 01/August/2008
% Periodic spline: used when the field 'periodic' of options is set to 1
% abscissa x, and xi are assumed to be 2pi-periodic.
%
% input x: vectors (required to be monotonics), OR
% x, could be the an array of same dimension than z
% (as generated by meshgrid)
% data matrix z(length(x)),
%
% xi list (or matrix) of the location of the points
% where the interpolation will performed.
%
% output zi: result of the interpolation.
%
% HOW TO CALL:
% - If the last input parameter is 'InitOnly', spline1d returns the structure
% "s" that contains initialization from the coarse data (x,z)
% - The structure "s" can be passed then as last argument for successive
% interpolation calls (in this case inputs (x, z) are ignored).
% - The initialization structure is remembered by the function spline1d,
% unless until the function is called with 'InitOnly' or 'ForceReset'.
%
%

persistent xgrid nx dx
persistent zz zzxx
persistent periodic

if nargin<5 || isempty(options)
    options = struct();
end

if nargin>=4 && isstruct(action) && isfield(action, 'Tag') && ...
        ischar(action.Tag) &&...
        ~isempty(strfind(action.Tag,'spline1d structure'))

    xgrid=action.xgrid;
    nx=action.nx;
    dx=action.dx;
    zz=action.zz;
    zzxx=action.zzxx;
    periodic = strcmp(action.Tag,'Periodic (radian) spline1d
structure');

elseif isempty(zz) || ...
        (nargin>=4 && strcmpi(action, 'InitOnly')) || ...
        (nargin>=4 && strcmpi(action, 'ForceReset'))

    if all(size(x)>1)
        x=x(1,:);
    elseif size(x,1)>1
        x=x';
    end

    nx=length(x);
    
    if size(z,2)~=nx
        z=z';
    end

    if (nx~=size(z,2))
        error('spline1d: dimensions does not match');
    end

    if min(nx)<3
        error('spline1d: Length of the "x" input must larger than 3');
    end

    %
    % Compute the second derivative z_xx by solving the tridiagonale system
    % of linear equations, This part can be called only once for a fixed set
    % of data (x, z). The array zz, zzxx, x, then can be stored in the memory.
    %
    dx=x(2:end)-x(1:end-1); % n-1
    dx2=2*(x(3:end)-x(1:end-2)); % n-2

    zx = bsxfun(@rdivide,z(:,2:end)-z(:,1:end-1), dx); % n-1
    % zx=(z(:,2:end)-z(:,1:end-1))./repmat(dx,size(z,1),1); % n-1

    zxx=(zx(:,2:end)-zx(:,1:end-1)); % n-2

    gamma=zeros(1,nx-2); % n-2
    
    %
    % Put this flag to 0 (independent solver)
    % or 1 (MATLAB solver)
    %
    UseMATLABSolver = 1;

    % Get the optional left and right second derivatives
    leftzxx = getoption(options,'leftzxx',zeros(size(zxx,1),1));
    rightzxx = getoption(options,'rightzxx',zeros(size(zxx,1),1));
    
    % Acually we solve for 1/6 the second derivative
    leftzxx = leftzxx(:)/6;
    rightzxx = rightzxx(:)/6;

    % Get the optional left and right first derivatives (higher priority)
    leftzx = getoption(options,'leftzx',NaN);
    rightzx = getoption(options,'rightzx',NaN);
    
    % Put it in column
    leftzx = leftzx(:);
    rightzx = rightzx(:);
    
    % Flag for not-a-knot conditions
    leftnotaknot = getoption(options,'leftnotaknot',0);
    rightnotaknot = getoption(options,'rightnotaknot',0);
    
    % periodic of period 2*pi
    periodic = getoption(options,'periodic',0);
    
    if UseMATLABSolver==0 && ...
       (leftnotaknot || rightnotaknot || periodic)
        warning('spline1d:SwitchToMatlabSolver', 'Switch to MATLAB solver');
        UseMATLABSolver = 1;
    end

    if UseMATLABSolver
        
        if periodic % Period spline
            
            % Sort the abcissa, and eliminate repeat data
            [x isort] = unique(mod(x,2*pi));
            z = z(isort); % Careful, we do not check if z is consistent
            
            % First and last points are duplicate
            if abs(diff(unwrap(x([1 end])))) < eps(2*pi)
                x = x([end 1:end]); % n+2
                % Careful, we do not check if z is consistent
                z = z([end 1:end]); % n+2 
            else
                x = x([end 1:end 1]); % n+2
                z = z([end 1:end 1]); % n+2
            end
            
            x = unwrap(x);
            if all(diff(x)<0) % reverse an up-side-down angle array
                x = x(end:-1:1);
                z = z(end:-1:1);
            elseif any(diff(x)<0)
                error('spline1d:KnotNotSorted', 'Spline1D: Knots are not sorted');
            end
            
            dx=x(2:end)-x(1:end-1); % n+1
            dx2=2*(x(3:end)-x(1:end-2)); % n 
            zx = bsxfun(@rdivide,z(:,2:end)-z(:,1:end-1), dx); % n+1
            zxx=(zx(:,2:end)-zx(:,1:end-1)); % n
            rhs = zxx';
            %A = diag(dx2,0) + diag(dx(2:end-1),1) + diag(dx(2:end-1),-1);
            A = spdiags([dx(2:end); % sub-diagonal
                dx2; % diagonal
                dx(1:end-1)]', ... % super-diagonal
                (-1:1),...
                length(dx2),length(dx2));
            A(1,end) = dx(1);
            A(end,1) = dx(end);
            zxx = (A \ rhs)';
 
        else

            rhs = zxx';
            %A = diag(dx2,0) + diag(dx(2:end-1),1) + diag(dx(2:end-1),-1);
            A = spdiags([dx(2:end); % sub-diagonal
                dx2; % diagonal
                dx(1:end-1)]', ... % super-diagonal
                (-1:1),...
                length(dx2),length(dx2));

            if leftnotaknot
                A(1,1) = A(1,1) + dx(1)*(1+dx(1)/dx(2));
                A(1,2) = A(1,2) - dx(1)*dx(1)/dx(2);
            elseif isnan(leftzx)
                rhs(1,:) = rhs(1,:)-dx(1)*leftzxx';
            else
                rhs(1,:) = rhs(1,:) + (leftzx - zx(:,1))'/2;
                A(1,1) = A(1,1) - dx(1)/2;
            end

            if rightnotaknot
                A(end,end) = A(end,end) + dx(end)*(1+dx(end)/dx(end-1));
                A(end,end-1) = A(end,end-1) - dx(end)*dx(end)/dx(end-1);
            elseif isnan(rightzx)
                rhs(end,:) = rhs(end,:)-dx(end)*rightzxx';
            else
                rhs(end,:) = rhs(end,:) - (rightzx - zx(:,end))'/2;
                A(end,end) = A(end,end) - dx(end)/2;
            end
            zxx = (A \ rhs)';

        end

    else % Do not use MATLAB Solver, only natural and clamped splines
         % are supported

        % Modify the rhs for using left second derivative
        if isnan(leftzx)
            zxx(:,1) = zxx(:,1) - dx(1)*leftzxx;
        else % Modify the rhs and matrix for using left first derivative
            zxx(:,1) = zxx(:,1) + (leftzx - zx(:,1))/2;
            dx2(1) = dx2(1) - dx(1)/2;
        end

        % Modify the rhs for using right second derivative
        if isnan(rightzx)
            zxx(:,end) = zxx(:,end) - dx(end)*rightzxx;
        else % Modify the rhs and matrix for using right first derivative
            zxx(:,end) = zxx(:,end) - (rightzx - zx(:,end))/2;
            dx2(end) = dx2(end) - dx(end)/2;
        end

        % Solve tri-diagonal linear system (symmetric)
        beta=dx2(1);
        zxx(:,1)=zxx(:,1)/beta;
        % Forward loop
        for j=2:nx-2
            dxj=dx(j);
            gamma(j)=dxj/beta;
            beta=dx2(j)-dxj*gamma(j);
            zxx(:,j)=(zxx(:,j)-dxj*zxx(:,j-1))/beta;
        end
        % Backward loop
        for j=nx-3:-1:1
            zxx(:,j)=zxx(:,j)-gamma(j+1)*zxx(:,j+1);
        end
    end % solver done
    
    if periodic % Periodic spline
        Tag = 'Periodic (radian) spline1d structure';
        x(end)=[]; % Remove the end points
        z(end)=[];
        zxx = zxx([end 1:end]);
    else
        Tag = 'spline1d structure';
        % Compute the second derivative at the left border
        if leftnotaknot
            leftzxx = zxx(:,1)*(1+dx(1)/dx(2)) - ...
                zxx(:,2)*dx(1)/dx(2);
        elseif ~isnan(leftzx)
            leftzxx = (-(leftzx - zx(:,1))/dx(1) - zxx(:,1))/2;
        end
        % Compute the second derivative at the right border
        if rightnotaknot
            rightzxx = zxx(:,end)*(1+dx(end)/dx(end-1)) - ...
                zxx(:,end-1)*dx(end)/dx(end-1);
        elseif ~isnan(rightzx)
            rightzxx = (+(rightzx - zx(:,end))/dx(end) - zxx(:,end))/2;
        end

        % Solution, 1/6 of the second derivatives
        zxx = [leftzxx zxx rightzxx]; % size n
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    % Reshape these matrix as long linear-vectors
    %
    nx = length(x);
    zz=reshape(z,[],nx)';
    zzxx=reshape(zxx,[],nx)';
    xgrid=x;
    clear z zxx;

    if (nargin>=4 && strcmpi(action, 'InitOnly'))
        zi=struct('Tag', Tag, ...
            'xgrid', xgrid, ...
            'nx', nx, ...
            'dx', dx, ...
            'zz', zz, ...
            'zzxx', zzxx);
        return
    end

end
%
% END OF THE PREPARATION STEP
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INTERPOLATION PROPERLY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

sizeres=size(xi);
xi=xi(:);

if periodic
    % wrap into the knots interval
    x1 = xgrid(1);
    xi = x1 + mod(xi-x1,2*pi);
end
%
% Put this flag to 0 (independent searching for index)
% or 1 (MATLAB searching for index)
%
UsingMatlabInterp1=1;

if UsingMatlabInterp1
    xi=interp1(xgrid,(1:nx),xi,'linear','extrap');
    xi=min(max(xi,1),nx);
else
    xi=find_idx(xi, xgrid);
end
%
idx=min(fix(xi),nx-1);
%
% Basis of the cubic polynome in x-direction evaluate
% at the points of interpolation
%
% sqrdx=dx(idx).^2';
% bx=(xi-idx);
% ax=(1-bx);
% cx=sqrdx.*(ax.^3-ax);
% dx=sqrdx.*(bx.^3-bx);

bx=(xi-idx);
ax=(1-bx);
g=-(dx(idx).^2').*ax.*bx;
cx=(ax+1).*g;
dx=(bx+1).*g;
%
% These linear-index will be used to access to two brakets indices
% where the points of interpolation belong.
%
idx0=idx;
idx1=idx0+1;
%
zi = bsxfun(@times,ax,zz(idx0,:)) + ...
     bsxfun(@times,bx,zz(idx1,:)) + ...
     bsxfun(@times,cx,zzxx(idx0,:)) + ...
     bsxfun(@times,dx,zzxx(idx1,:));
% zi=ax.*zz(idx0,:) + ...
% bx.*zz(idx1,:) + ...
% cx.*zzxx(idx0,:) + ...
% dx.*zzxx(idx1,:);
%
clear ax bx cx dx;
clear idx0 idx1;
clear zz zzxx;
%
if numel(zi)==numel(xi)
    zi=reshape(zi,sizeres);
end
%
return

end
%
% Nested function: Get the optional values from the structure options
%
function val = getoption(options, field, default)
if isfield(options, field)
    val = options.(field);
else
    val = default;
end
end

