function [dU_dp,dU_dt,nnp] = cvp_setup(tgrid,tknotss,psporder,pscale)
% function [dU_dp,dU_dt,nnp] = cvp_setup(tgrid,tknotss,usporder,uscale)
% control vector parametrization, parametrizes u(t) as B-splines of any order.
%
% INPUTS
% tgrid:    grid of time points
% tknotss:  grid of knot points for spline, can be a cell array, in this
%           case diffent knots for each input
% psporder: array of spline orders for each input
% pscale:   individual scaling factor for each input
% 
% OUTPUTS
% dU_dp: maps parameters to inputs
%        each dUdp(:,:,it) is the 2D-matrix du(it)/dp, of size [nu x np]
%        such that u(:,it) = dUdp(:,:,it)*p !!  (trust me)
% dU_dt: maps parameters to input time derivatives
%        each dU_dt(:,:,iu) is the ntxnu matrix such that dU_dt(:,:,iu)*p
%        is the time derivative of trajectory for actuator iu
% nnp:   array containing the number of parameters parametrizing each input.
%        total number of paramters: np=sum(nnp);
%

% F. Felici CRPP 2010
nu = length(psporder);
ntgrid = numel(tgrid);

for iu = 1:nu
  if ~iscell(tknotss)
    tknots{iu} = tknotss;
  else
    tknots = tknotss;
  end
  nnp(iu) = length(tknots{iu})-1+psporder(iu); % number of cvp segments
end

np = sum(nnp); % total number of parameters
dU_dp = zeros(nu,np,ntgrid); % map from p to U (inputs in time)
dU_dt = zeros(ntgrid,np,nu); % map from p time derivative of U

if nnp(1)>1 % not just a constant input
  for iu = 1:nu
    if psporder(iu) == 0
      % n intervals = nconstu - usporder: ensure all Us are parametrized
      % by the same number of variables
      [SP] =  splineval(tgrid',tknots{iu},0);
      dU_dp(iu,sum(nnp(1:iu-1))+[1:nnp(iu)],:) = pscale(iu)*full(SP);
      dU_dt(:, sum(nnp(1:iu-1))+[1:nnp(iu)],iu) = 0;
    else
      [SP,SPp] =  splineval(tgrid',tknots{iu},psporder(iu));
      dU_dp(iu,sum(nnp(1:iu-1))+[1:nnp(iu)],:) = pscale(iu)*full(SP);
      dU_dt(:, sum(nnp(1:iu-1))+[1:nnp(iu)],iu) = full(SPp');
    end
    % each dUdp(:,:,it) is the 2D-matrix du(it)/dp, of size [nu x np]
    % such that u(:,it) = dUdp(:,:,it)*p !!  (trust me)
    
    % each dU_dt(:,:,iu) is the ntxnu matrix such that dU_dt(:,:,iu)*p
    % is the time derivative of trajectory for actuator iu
  end
else % constant input, trivial dU_dt
  for it = 1:ntgrid
    dU_dp(:,:,it) = diag(pscale);
  end
  % dU_dt = zero!
end