function sys = RAPTORout2LTVsys(out,CC,varargin)
% function sys = RAPTORout2LTVsys(out,CC)
% function sys = RAPTORout2LTVsys(out,CC,itout,iu)
% sys: structure with time-varying system matrices (discrete time)
% 
% out: RAPTOR out structure
% CC: output matrix, or cell of output matrices for each time.
% itout: subset of time indices for which to return the linearization.
% iu: subset of input indices for which to return the linearization.

assert(isstruct(out),'out must be a RAPTOR output structure');

nt = numel(out.time);
nu = size(out.df_du{1},2);

if numel(varargin)>=1 && ~isempty(varargin{1})
    itout = varargin{1};
else
    itout = 1:nt;
end

if numel(varargin)>=2 && ~isempty(varargin{2});
  iu = varargin{2};
else
  iu = nu;
end

assert(numel(itout)==1 || all(diff(itout)==1),...
  'itout must either be scalar or contiguous');


% output matrix Cd = dh/dx % if h(x) is nonlinear output function
if ~iscell(CC)
    % must have one output function per time step...
    Ccd = cell(1,nt);
    [Ccd{:}] = deal(CC); % same in all fields
elseif numel(CC) == nt
        Ccd = CC; % output function given at all times directly
        
else
        error('CC has wrong number if elements');
end
ny = size(Ccd{1},1);

DD = zeros(ny,nu); % no direct feedthrough
for ikk = 1:numel(itout)
    it = itout(ikk);
    iA = out.df_dxk{it};
    Ad{ikk} = -iA \ out.df_dxkm1{it};
    Bd{ikk} = -iA \ out.df_du{it}(:,iu);
    Cd{ikk} = Ccd{it};
    Dd{ikk} = DD(:,iu); 
end

sys.A = Ad;
sys.B = Bd;
sys.C = Cd;
sys.D = Dd;
