spline1d(x,z,xi,action,options)

function zi = spline1d(x,z,xi,action,options)
%
% 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.
%  06/August/2008
%   Evaluate Derivative: by setting the field 'dorder' of structure
%                        and/or options. The total order is cumulative
%                        of both
% 19/Feb/2009
%    Call new find_idx with mex dichotomy search for speed
%
% 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)) [Default shape, spline along the second
%                                   dimension], OR
%                   z(length(x),:) if options.zndgrid==1
%
%       xi list (or matrix) of the location of the points
%       where the interpolation will performed.
%
% output zi: result of the interpolation, size(zi) is: (length(xi),:)
%
% 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

% Default derivative order
spline_dorder=0;

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');
    
    % Get the derivative order
    if isfield(action,'dorder')
        spline_dorder=action.dorder;
    end

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);
    
    zndgrid = getoption(options,'zndgrid',0);
    
    if ~zndgrid % meshfgrid style
        if size(z,2)~=nx
            z=z.';
        end
    else % ndgrid style
        if size(z,1)==nx
            z = z.';
        end
    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
    
    %
    % 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(z,1),1));
    rightzxx = getoption(options,'rightzxx',zeros(size(z,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 ~periodic
        %
        % 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 = bsxfun2006(@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
    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 = bsxfun2006(@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);
            l = length(dx2);
            I = [1:l     1:l l 1:l-1];
            J = [l 1:l-1 1:l 1:l];
            A = sparse(I, J, [dx(1:end-1) dx2 dx(2:end)], l, l);

            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));
            l = length(dx2);
            I = [2:l   1:l 1:l-1];
            J = [1:l-1 1:l 2:l];
            A = sparse(I, J, [dx(2:end-1) dx2 dx(2:end-1)], l, l);           

            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;
        gamma=zeros(1,nx-2); % n-2
        % 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=0;

if UsingMatlabInterp1
    xi=interp1(xgrid,(1:nx),xi,'linear','extrap');
    %xi=min(max(xi,1),nx);
else
    xi=find_idx(xi, xgrid,'extrap');
end
%
idx = fix(xi);
idx(idx<1) = 1;
idx(idx>nx-1) = nx-1;
% idx=max(min(fix(xi),nx-1),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);

h=reshape(dx(idx),size(idx));
% Order of the derivative
dorder = spline_dorder + getoption(options, 'dorder', 0);
dorder = round(dorder);

switch dorder % Derivative
    case 0, % interpolated spline function
        bx=(xi-idx);
        ax=(1-bx);
        g=-(h.^2).*ax.*bx;
        cx=(ax+1).*g;
        dx=(bx+1).*g;
    case 1, % the first derivative
        b=(xi-idx); a=(1-b);        
        ax=-1./h+zeros(size(xi));
        bx=+1./h+zeros(size(xi));
        cx=h.*(+3*b.*(a+1)-2);
        dx=h.*(-3*a.*(b+1)+2);        
    case 2, % the second derivative
        b=(xi-idx); a=(1-b);        
        ax=zeros(size(xi));
        bx=zeros(size(xi));
        cx=6*a;
        dx=6*b;
    case 3, % the third derivative (discontinuous)
        ax=zeros(size(xi));
        bx=zeros(size(xi));
        cx=-6./h+zeros(size(xi));
        dx=+6./h+zeros(size(xi));
    otherwise
        if dorder>3
            error('spline1d: 4th derivative is ill-defined (Dirac)');
        else
            error('spline1d: not valid derivative');
        end
end
%
% These linear-index will be used to access to two brakets indices
% where the points of interpolation belong.
%
idx0=idx;
idx1=idx0+1;
%

zi = bsxfun2006(@times,ax,zz(idx0,:)) + ...
     bsxfun2006(@times,bx,zz(idx1,:)) + ...
     bsxfun2006(@times,cx,zzxx(idx0,:)) + ...
     bsxfun2006(@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