function [SS,SSp,SSpp,CS,CSp,CSpp] =splinematrix(xsource,xtarget,xnodes,order,varargin);
%function [SS,SSp,SSpp,CS,CSp,CSpp] =splinematrix(xsource,xtarget,xnodes,order{,constraints});
% Constructs matrices which reduce the spline intepolation problem to a
% matrix multiplication plus vector addition.
% given data (xsource,y), we want to interpolate the data using splines
% and then compute the output on a different grid xtarget
% Possibly want some constraints on the solution or its 1st or 2nd
% derivative.
%
% INPUTS
%   xsource: x grid on which the data to be interpolated will be given
%   xtarget: x grid on which the interpolated data will be returned
%   xnodes:  x grid of spline nodes
%               or number of nodes if scalar
%               or number of nodes as fraction of length(xsource) if between 0 and 1    
%   order:   order of the splines
%   constraints: constraints on the interpolated data (optional) 
%       specified as follows:
%       mycon = [der,pos,val], where 
%           der = derivative on which the constraint is placed 
%           pos = position where the constraint is placed
%           val = value of the given derivative at given position
%       constraints are then given either individually, or as a matrix with
%       a constraint on each row. see examples for details.
%
% OUTPUTS
%   Matrices which can be used to compute interpolated data for any input,
%   see example below.
%   Note that CS,CSp,CSpp are zero if constraints can be written as Cp=0;
%   This is typically the case when specifying zero values or derivatives.
%
% EXAMPLE OF RHS of CALLS
%   splinematrix(xs,xt,11,3); % no constraints
%   splinematrix(xs,xt,11,3,[1 0 0]); % first derivative 0 at x=0
%   splinematrix(xs,xt,11,3,[1 0 0],[0 0 1]); % also x(0)=1;
%   splinematrix(xs,xt,11,3,[1 0 0],[0 0 1],[2,1,0]); % also x(1)''=0;
%   splinematrix(xs,xt,11,3,[[1 0 0];[0 0 1]]); % give constr as matrix 
%
% EXAMPLE OF USE
% xsource = linspace(0,1,51); xtarget = linspace(0,1,101); 
% nnodes  = 15; order = 3;
% constraints_in = [[0 0 2];[2 0 0];[1 1 0]]; 
% % yfit(0) = 2, yfit''(0)=0, yfit'(1) = 0;
%
% [SS,SSp,SSpp,CS,CSp,CSpp]=splinematrix(xsource,xtarget,nnodes,order,constraints_in);
%
% %now for any y, for example
% y = sin(2*pi*4*xsource)';
%
% % the least squares fit with constraints is
% yfit = SS*y + CS;
% yfitp = SSp*y + CSp;
% yfitpp = SSpp*y + CSpp;
%
% figure; clf; 
% subplot(311); plot(xsource,y,'xr-'); hold on; plot(xtarget,yfit,'b.-'); ylabel('y')
% subplot(312); plot(xtarget,yfitp,'b.-'); ylabel('y''');
% subplot(313); plot(xtarget,yfitpp,'b.-'); ylabel('y''''');

% F. Felici CRPP 2010

if nargin == 5 % constraints are given as matrix [1 0 0;2 0 1]);
    constraints_in = varargin{1};
    ncon = size(constraints_in,1);
    if ~isempty(constraints_in) && (size(constraints_in,2) ~= 3); error('constraint matrix must have 3 columns'); end
elseif nargin>6; % constraints are given as [1,0,0],[2 0 1]!
    ncon = nargin-3;
    for icon = 1:ncon;
        thiscon = varargin{icon};
        if ~all(size(thiscon)==[1 3]); 
            error('all constraints must be 1x3'); 
        end
        constraints_in(icon,:) = thiscon
    end
        if any(constraints_in(:,1) > order); error('Constraint order too high for spline order'); end
else ncon=0; % no constraints
end



%% check grids
if any(diff(xsource)<=0); error('xsource must be increasing'); end
if any(diff(xtarget)<=0); error('xtarget must be increasing'); end
if any(diff(xnodes)<=0); error('xnodes must be increasing'); end

%% check if xnodes is appropriate?
xmin = min(xsource(1),xtarget(1));
xmax = max(xsource(end),xtarget(end));
if numel(xnodes) == 1;  % then either number of nodes, or ratio between length(xsource) and nnodes
    if xnodes<=1
        nnodes = round(xnodes*length(xsource)); %fraction of length(xsource)
    else
        nnodes = xnodes; % specified number of nodes
    end
    if nnodes==0; error('xnodes must be >0'); end
    xnodes = linspace(xmin,xmax,nnodes);
else
    if xmin<xnodes(1); error('xnodes must cover xsource and xtarget'); end
    if xmax>xnodes(end); error('xnodes must cover xsource and xtarget'); end
end
   

%% create splines and derivatives
switch order
    case 0
        [S] = splineval(xsource,xnodes,order);
        A=S';  % least squares fit on this basis
        [S] = splineval(xtarget,xnodes,order);
        Aeval=S'; % for evaluation of result
    case 1
        [S,Sp] = splineval(xsource,xnodes,order);
        A=S'; Ap=Sp'; % least squares fit on this basis

        [S,Sp] = splineval(xtarget,xnodes,order);
        Aeval=S'; Aevalp=Sp';  % for evaluation of result
    otherwise
        [S,Sp,Spp] = splineval(xsource,xnodes,order);
        A=S'; Ap=Sp'; App=Spp'; % least squares fit on this basis

        [S,Sp,Spp] = splineval(xtarget,xnodes,order);
        Aeval=S'; Aevalp=Sp'; Aevalpp=Spp'; % for evaluation of result

end

nsp = size(A,2);% number of splines, and of spline coefficients

%% Boundary conditions
CC = zeros(ncon,nsp);
VAL = zeros(ncon,1);
for icon = 1:ncon
    der = constraints_in(icon,1);
    xcon = constraints_in(icon,2);
    val = constraints_in(icon,3);

    % p is the vector of spline parameters, 
    % ysource=A*p; ytarget=Aeval*p;
    
    % constraint has the form cmat*p = val
    ixcon = find(xsource==xcon); % where on xsource is the constraint
    if ~isempty(ixcon); % constraint is on xsource
        switch der
            case 0
                cmat = A(ixcon,:);
            case 1 % p is the vector of spline parameters, ysource=A*p;
                cmat = Ap(ixcon,:); % constraint has the form cmat*p = val
            case 2
                cmat = App(ixcon,:); % constraint has the form cmat*p = val
            otherwise
                 error('higher than second order not implemented');
        end
    else
       ixcon = find(xtarget==xcon); % then try where on xtarget is the constraint?
        if ~isempty(ixcon); % constraint is on xsource, set constraints from Aeval
        switch der
            case 0
                cmat = Aeval(ixcon,:);
            case 1 
                cmat = Aevalp(ixcon,:);
            case 2
                cmat = Aevalpp(ixcon,:); 
             otherwise
                error('higher than second order not implemented');
        end
        else % can not find on xtarget nor on xcon...
            % new spline value on required position
            % check if required position is within interval of xnodes
            if xcon<xnodes(1) || xcon>xnodes(end); error('constraint location outside node interval'); end
            [S,Sp,Spp] = splineval(xcon,xnodes,order);
            switch der
                case 0
                    cmat = S';
                case 1 % p is the vector of spline parameters, ysource=A*p;
                    cmat = Sp'; % constraint has the form cmat*p = val
                case 2
                    cmat = Spp'; % constraint has the form cmat*p = val
                otherwise
                    error('higher than second order not implemented');
            end

        end
    end

    CC(icon,:) = cmat;
    VAL(icon,:) = val; 
    % Constraint is now formulated as CC*p=VAL;
end
nr = rank(CC); % check constraints are linearly independent!
if (nr ~= ncon) 
    error('Constraints are linearly dependent!!'); 
end;

cval = CC\VAL; % particular p=cval: solution of CC*p = VAL

% QR decomposition to get null space of constraints
[Q,R]= qr(CC');  nc = size(R,2);
Q2 = Q(:,nc+1:end); % Q2 is basis for null(C)-> C*Q2*f = 0 for all f
% now for all CC*(cval+Q2*f)=val for all f
% least squares problem becomes
% min_f ||b-A*(cval+Q2*f)|| = min_f ||(b-A*cval)-A*Q2*f)||

if size(A,1)<size(Q2,2); 
    warning('Underdetermined linear system! add more constraints or reduce degees of freedom')
end

P = (A*Q2)\eye(length(xsource));
% P is such that P*(b-A*cval) = f

% p = cval+Q2*f = cval + Q2*(P*(b-A*cval)) = (I-Q2*P*A)cval + Q2*P*b
CY = (Q2*P); 
CV = (eye(nsp)-Q2*P*A); 
%CV = eye(nsp);

% Assign output arguments depending on order..
SSp=[]; CSp = []; SSpp=[]; CSpp=[]; 
if order >=0
    SS = Aeval*CY;
    CS = Aeval*CV*cval;
end
if order >=1
    SSp = Aevalp*CY;
    CSp = Aevalp*CV*cval;
end
if order >=2
    SSpp = Aevalpp*CY;
    CSpp = Aevalpp*CV*cval;
end

% if the constraints are can be written as C*p=0, 
% where p are spline parameters, then CS=0;
% this is typically the case for edge derivatives = 0 etc;


