function [varargout] = splineval(xgrid,xknots,order,varargin)%#codegen
% [S] = splineval(xgrid,xknots,order);
% [S,Sp] = splineval(xgrid,xknots,order);
% [S,Sp,Spp] = splineval(xgrid,xknots,order);
% [S,Sp,Spp] = splineval(xgrid,xknots,order,constraint);
% [S,Sp,Spp] = splineval(xgrid,xknots,order,constraint1,constr2,..);
%
%
%  constraints are given in the format [der,pos,val] i.e. derivative, position, value
%
% Returns matrices with values of splines of order=order
% with knots at xknots on xgrid
% Optionally returns derivatives as well
% Splines have finite support
% Works for irregular grid and arbitrary order

% Based on M.Tran & S.Brunner's notes on bsplines
% F. Felici 2010 CRPP

derreq = nargout-1; % number of derivatives required
if derreq>order; error('too high derivatives requested for this spline order'); end
%if any(diff(xgrid)<=0); error('xgrid must be monotonically increasing'); end
if any(diff(xknots)<=0); error('xknots must be monotonically increasing'); end
if order<0; error('order can not be negative'); end;

assert(iscolumn(xgrid),'xgrid must be a column vector');
assert(iscolumn(xknots),'xknots must be a column vector');

% if numel(xgrid) ~= length(xgrid); error('xgrid must be vector');  end
% if size(xgrid,2) == 1; xgrid = xgrid'; end
%
% if numel(xknots) ~= length(xknots); error('xknots must be vector');  end
% if size(xknots,2) == 1; xknots = xknots'; end


% %
% xgrid = linspace(0,1,1001);
% xknots = linspace(0,1,11);
% order = 3;
nk = numel(xknots);

%xknots = [ones(1,p)*xknots(1) xknots xknots(end)*ones(1,p)];

LL = zeros(nk-1+order,numel(xgrid),order+1);
% order 0 spline
for ik = ((1:nk-1))
    LL(ik,:,1) = round(0.5*(sign(xgrid'-xknots(ik))+sign(xknots(ik+1)-xgrid')));
    if ik~=nk-1 % unless last spline
        LL(ik,xgrid'==xknots(ik+1),1) = 0; % rightmost point 0 if at knot
    end
    
end

%% CONSTRUCT SPLINES UP TO NECESSARY ORDER
for ip = 1:order
    %ip = 1;
    ispgrid = -ip:nk-2;
    for isp = -ip:nk-2% index of spline
        
        if isp==ispgrid(1) % first spline
            ww1 = zeros(size(xgrid'));
        else
            iw = min(isp,nk-ip-1);
            ileft = max(isp,0);
            ww1= (xgrid'-xknots(ileft+1))/(xknots(iw+ip+1)-xknots(ileft+1));
        end
        if isp==ispgrid(end) % last splines
            ww2 = zeros(size(xgrid'));
        else
            iw = max(isp,-1);
            iright = min(isp+ip+2,nk);
            ww2 = (xknots(iright)-xgrid')/(xknots(iright)-xknots(iw+2));
        end
        
        istore = isp + ip + 1; % index of storage for w_(isp)
        
        % istore = min(istore,nk-2);
        %disp(istore)
        if isp==ispgrid(1) % first
            LL(istore,:,ip+1) =ww2.*LL(istore,:,ip);
        elseif isp==ispgrid(end) % last
            LL(istore,:,ip+1) =ww1.*LL(istore-1,:,ip);
        else
            LL(istore,:,ip+1) =ww1.*LL(istore-1,:,ip) + ww2.*LL(istore,:,ip);
        end
    end
end

%% Assign spline value to output variable
splinevalue = squeeze(LL(:,:,order+1));
varargout{1} = collapse_for_all_constraints(splinevalue,varargin{:});

%% CONSTRUCT DERIVATIVES
if derreq > 0 % if derivatives required
    oLL = LL;% start with 'zeroth' derivative, i.e. spline itself
    for ider = 1:derreq;
        dLL = zeros(nk-1+order,numel(xgrid),order+1);
        for ip = order:-1:order-derreq+1;
            ispgrid = -ip:nk-2;
            for isp = -ip:nk-2% index of spline
                
                if isp==ispgrid(1) % first spline
                    dw1 = 0;
                else
                    iw = min(isp,nk-ip-1);
                    ileft = max(isp,0);
                    dw1= 1/(xknots(iw+ip+1)-xknots(ileft+1));
                end
                if isp==ispgrid(end) % last splines
                    dw2 = 0;
                else
                    iw = max(isp,-1);
                    iright = min(isp+ip+2,nk);
                    dw2 = 1/(xknots(iright)-xknots(iw+2));
                end
                
                istore = isp + ip + 1;% index of storage for w_(isp)
                if isp==ispgrid(1) % first
                    dLL(istore,:,ip+1) =-ip*dw2.*oLL(istore,:,ip);
                elseif isp==ispgrid(end) % last
                    dLL(istore,:,ip+1) = ip*dw1.*oLL(istore-1,:,ip);
                else
                    dLL(istore,:,ip+1) = ip*(dw1.*oLL(istore-1,:,ip) - dw2.*oLL(istore,:,ip));
                end
            end
        end
        oLL = dLL;
        thisder = squeeze(dLL(:,:,order+1));% matrix of spline derivatives
        varargout{ider+1} = collapse_for_all_constraints(thisder,varargin{:});
    end
end
return

function collapsed_matrix = collapse_for_all_constraints(matrix,varargin)
if ~isempty(varargin)
    % each varargin contains constraint in the format
    % [der,pos,val] i.e. derivative, position, value
    assert(numel(varargin)==1,'not implemented for multiple constraints yet')
    constraint = varargin{1};
    collapsed_matrix = collapse_spline_matrix(matrix,constraint);
else
    collapsed_matrix = matrix;
end
return

function collapsed_matrix = collapse_spline_matrix(matrix,constraint)
if ~isempty(constraint)
    if all(constraint == [1 0 0]);  %only implemented this case for now
        % optional: return bases such that derivative at 0 = 0
        cc = eye(size(matrix,1)-1);
        collapsematrix = [cc(:,1),cc];
    else
        error('not implemented for this constraint case')
    end
    collapsed_matrix = collapsematrix*matrix;
else
    collapsed_matrix = matrix;
end
return
