function [Hk,Tk] = ILC_statetransition_matrix(sys,ity,itu)
% function [Hk,Tk] = statetranstitionmatrix(sys)
% State transition matrix
% sys: LTV model with sys.A, sys.B, sys.C, sys.D cells of matrices
% 
% Yk = Hk%x0 + Tk*Uk
% 
% extended by S Van Mulders, 21.05.2021
% allowing to control state derivative dependent quantities
% LTV model
% delta_x_k = A_k*delta_x_{k-1} + B_k*delta_u_k
% delta_y_k = C_k*delta_x_k + E_k*delta_x_{k-1} + D_k*delta_u_k
% if no E_k matrix is provided, zeros are assumed
%%
nt = numel(sys.A); % time points in LTV system

if nargin == 1 || isempty(ity)
  ity = 1:nt;
end

if nargin == 1 || isempty(itu)
  itu = 1:nt;
end

assert(iscell(sys.A),'A not a cell');
assert(iscell(sys.B),'B not a cell');
assert(iscell(sys.C),'C not a cell');
assert(iscell(sys.D),'D not a cell');
nx = size(sys.A{1},1);
nu = size(sys.B{1},2);
ny = size(sys.C{1},1);
if isfield(sys, 'E')
  assert(iscell(sys.E),'E not a cell');
  Ed=sys.E;
else
  Ed = cell(1,nt);
  for ii = 1:nt
    Ed{ii} = zeros(ny,nx);
  end
end

Hk = zeros(nt*ny,nx);
Tk = zeros(nt*ny,nu);

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

for ik=1:nt
    irow = (ik-1)*ny;
    
    % update L (matrix with A products only)    
    for ikk = 1:ik;
        if ik ==ikk
            Lk{ik,ikk} = Ad{ik};
        else
            Lk{ik,ikk} = Ad{ik}*Lk{ik-1,ikk};
        end
    end
    
    if ik == 1
      Hk(irow+(1:ny),:) = Cd{ik}*Lk{ik,1} + Ed{ik};
    else
      Hk(irow+(1:ny),:) = Cd{ik}*Lk{ik,1} + Ed{ik}*Lk{ik-1,1}; % State transition matrix
    end
    for ikk = 1:ik % columns, first to last
        irow = (ik-1)*ny;
        icol = (ikk-1)*nu;
        if ikk==ik
            Tk(irow+(1:ny),icol+(1:nu)) = Dd{ik}+Cd{ik}*Bd{ik};
        elseif ikk+1==ik
            Tk(irow+(1:ny),icol+(1:nu)) = Cd{ik}*(Lk{ik,ikk+1}*Bd{ikk}) + Ed{ik}*Bd{ikk};
        else
            Tk(irow+(1:ny),icol+(1:nu)) = Cd{ik}*(Lk{ik,ikk+1}*Bd{ikk}) + Ed{ik}*(Lk{ik-1,ikk+1}*Bd{ikk});
        end
 
    end
end

%% select desired input and output times from Hk,Tk
assert(numel(ity)==1 || all(diff(ity)==1),...
  'ity must be a scalar, or contiguous');

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

itySel = (ity(1)-1)*ny + (1:(numel(ity)*ny));
ituSel = (itu(1)-1)*nu + (1:(numel(itu)*nu));

Hk = Hk(itySel,:);
Tk = Tk(itySel,ituSel);

end