function [sys,ind,x0,yo] = fgess(L,Ts,outputlist)
% fgess - Compute state-space model matrices
% [sys,ind,x0,u0,y0]= = fgess(L,Ts{,measlist})
%
%  L: L structure containing L.lin (from fgel)
%  Ts: Sample time (0=continuous time, default)
%  outputlist: cell array with list of outputs to pass. Default = 'all';
%
% state: x  = {Ia,Iu,(Ip)}, inputs: u = {Va, Co, dCoidt, (Ini)}. 
% outputs: defined via outputlist. Default 'all'.
%
% Quantities between parenthesis () are used only when cde is included.
% State space form as
% S dxdt = K x + U u -> dxdt = A x + B u
%                          y = C x + D u + yo
%
%% Check inputs
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
assert(~any(L.icde) || ~L.P.idoublet, 'Linearization with cde not available for multiple domain');
assert(~any(contains(L.agconc(:,3),'ag')), 'Linearization not available for ag constraints');
assert(isfield(L,'lin'), 'fgel needs to be called before fgess to compute the linearization')

%% Chose if continuous/discrete time system output
if nargin==1
  Ts = 0; % continuous-time system
else
  assert(isscalar(Ts),'Ts must be scalar')
end

%% Extract indices of Ip in constraints in case of CDE
if L.icde
  iCoIp = contains(L.agconc(:,3), 'Ip');
  iCoin = ~iCoIp;
else
  iCoin = true(L.nC,1);
end

%% Outputlist
if nargin<=2
  outputlist = 'all';
end

if isequal(outputlist,'all')
  measlist = {'Bm','Ff','Ia','Iu','Ip', 'Ft','rIp','zIp','Fx','FA','FB','ag','rA','zA','bp','qA','li'};
  if L.nn, measlist = [measlist,{'Fn','Brn','Bzn'}]; end % optional
  if L.P.idoublet,  measlist = [measlist,{'IpD','rIpD','zIpD','bpD'}]; end % optional
else
  measlist = outputlist;
end

%% Define state related matrices S,K
% S dxdt = K x + U u 
% S is the matrix formed by the mutual inductances Mee (+ Lp if cde) 
% and the correction coming from plasma motion Xee (+ Xeo, Xpe, Xpo if cde). 
% K is the resistance matrix, formed by the resistances Re (+ Rp if cde)
% U is the actuator matrix, formed the matrices that connect the actuator
% Va (+ Ini if cde) to the state evolution. It also connect the disturbances Co and dCodt 
% through Xeo and Xpo (they vary if Ip is a state or a Co).  

ind = struct;
[ind, nS] = indxgen(ind,{'iSIa',L.G.na,'iSIu',L.G.nu},0); % Gen index structure
ind.iSIe = [ind.iSIa,ind.iSIu];
if L.icde,  [ind, nS] = indxgen(ind,{'iSIp',L.np},nS); end % Update index structure with optional state Ip

% S matrix
S = zeros(nS);
S(ind.iSIe,ind.iSIe) =  L.Mee + L.lin.Xee; % dIedt state
if L.icde
  % Only OhmTor_rigid available at the moment
  S(ind.iSIe,ind.iSIp) = L.lin.Xeo(:,iCoIp);
  switch L.P.cde
    case 'OhmTor_rigid'
      S(ind.iSIp,[ind.iSIe,ind.iSIp]) = [L.lin.Mpe + L.lin.Xpe,L.lin.Xpo(:,iCoIp)];
    otherwise
      error('linearisation not available for the cde selected');
      % placehold for implementation of different CDE linearization
  end
end

% K matrix
K = zeros(nS);
K(ind.iSIe, ind.iSIe) = -diag(L.Re);
if L.icde, K(ind.iSIp, ind.iSIp) = -L.lin.Rp; end % this will need update to support other CDEs

% Labels for state
labelIa = cellstr(strcat('Ia_',L.G.dima, 'S'));
labelIu = cellstr(strcat('Iu_',L.G.dimu, 'S'));
state_lbl = [labelIa;labelIu];
if L.icde, state_lbl = [state_lbl; 'Ip_S']; end

% Pack linearization state
x0 = [L.lin.Ia;L.lin.Iu];
if L.icde, x0 = [x0;L.lin.Ip]; end

%% Define the measurements matrices from state C, y = C x + D u + yo
% where yo = -C*x0  -D*u0 + y0
% Most measurement follow the form:
% meas = dmeasdIe*(Ie-Ie0) + dmeasdCo*(Co-Co0) + meas0
% Except: 
%  Ia,Iu: directly from state
%  Ip: From state or from input depending on CDE

namedimpair = {};
for ii=measlist
  namedimpair = [namedimpair,{['iM' ii{:}],prod(meqsize(L,ii{:}))}]; %#ok<AGROW>
end
% Indexing for measurements
[ind, nM] = indxgen(ind,namedimpair,0); % Gen index structure

C = zeros(nM,nS);
% Part of C related to Ie
for ii = setdiff(measlist,{'Ip'})
  val = L.lin.(['d' ii{:} 'dIe']); nval = size(val,1); 
  C(ind.(['iM' ii{:}])(1:nval),ind.iSIe) = val;
end

if any(L.icde) && ismember('Ip',measlist)
  % Part of C related to Ip state
  % Ip is a state in this case
  C(ind.iMIp,ind.iSIp)  = 1;  % Ip taken from the state if CDE
  for ii= setdiff(measlist,{'Ip','Ia','Iu'})
    C(ind.(['iM' ii{:}]),ind.iSIp) = L.lin.(['d' ii{:} 'dCo'])(:,iCoIp);
  end
end

meas_lbl = {};
for ii = measlist
  mymeas = ii{:};
  switch mymeas
    case {'Bm','Ff'} % use L.G.dim* directly
      mylabel = cellstr(strcat([mymeas,'_'],L.G.(['dim',mymeas(end)])));
    case {'Ia','Iu'} % prefix with I_ or V_
      mylabel = cellstr(strcat([mymeas,'_'],L.G.(['dim',mymeas(end)])));
    otherwise
      prodsize = prod(meqsize(L,mymeas));
      if prodsize==1
        mylabel = mymeas;
      else
        formatstring = sprintf('%%0%dd',1+floor(log10(prodsize))); % correct padding zeros
        mylabel = cellstr(strcat([mymeas,'_'],num2str((1:prodsize)', formatstring)));
      end
  end
  meas_lbl = [meas_lbl;mylabel]; %#ok<AGROW>
end

%% Pack measurements of linearization state
y0 = [];
for ii = measlist
  mymeas = ii{:};
  myval = L.lin.(ii{:});
  val = zeros(prod(meqsize(L,mymeas)),1);
  val(1:numel(myval)) = myval(:); % For A/B-suffixd quantities size(myval,1)=nA/nB while val needs to be of size `L.nD`  
  y0 = [y0;val]; %#ok<AGROW>
end

%% Define indices for input vector u  = {Va,Co, dCodt, (Ini)}
[ind, nU] = indxgen(ind,{'iUVa',L.G.na,'iUCo',sum(iCoin),'iUdCodt',sum(iCoin)},0);
% Optional inputs
if L.icde, [ind, nU] = indxgen(ind,{'iUIni',L.np},nU); end

% U matrix
U = zeros(nS,nU);
U(ind.iSIa,ind.iUVa) = eye(L.G.na);
U(ind.iSIe,ind.iUdCodt) = -L.lin.Xeo(:,iCoin);
if L.icde
  U(ind.iSIp,ind.iUIni) = L.lin.Rp;
  switch L.P.cde
    case 'OhmTor_rigid'
      U(ind.iSIp,ind.iUdCodt) = -L.lin.Xpo(:,iCoin);
 %   otherwise %for now, it will never arrive at this point, because of the
 %   previous otherwise
      % placehold for implementation of different CDE linearization
  end
end

% Labels for u
labelVa    = cellstr(strcat('Va_'    ,L.G.dima));
labelCo    = cellstr(strcat('Co_'    ,L.agconc(iCoin,3)));
labeldCodt = cellstr(strcat('dCodt_' ,L.agconc(iCoin,3)));
input_lbl  = [labelVa;labelCo;labeldCodt];
if L.icde, input_lbl{ind.iUIni} = 'Ini'; end

% Pack linearization initial inputs
u0 = [zeros(L.G.na,1); L.lin.Co(iCoin); zeros(sum(iCoin),1)];
if any(L.icde), u0 = [u0;L.lin.Ini(L.icde)]; end

%% Define the measurement from inputs matrix D, such that y = C x + D u
D = zeros(nM,nU);
% Part of D related to Co
for ii = setdiff(measlist,{'Ia','Iu'})
  switch ii{:}
    case 'Ip', val = zeros(1,sum(iCoin));
      if ~L.icde, val(contains(L.agconc(:,3),'Ip')) = 1; end
    otherwise, val = L.lin.(['d' ii{:} 'dCo'])(:,iCoin);
  end
  nval = size(val,1);
  D(ind.(['iM' ii{:}])(1:nval),ind.iUCo)  = val;
end

%% Compute bias for measurements such that 
yo = -C*x0 - D*u0 + y0;

%% Generate the state space representation
if Ts == 0
  % Continuous time representation
  A = S\K;
  B = S\U;
  sys = ss(A,B,C,D);
else
  % Discrete time representation. Euler Implicit scheme.
  A = (S-Ts*K)\S;
  B = (S-Ts*K)\(Ts*U);
  sys = ss(A,B,C,D,Ts);
  % Warning when using lsim
  % Matlab ss for discrete system considers
  % x[n+1] = A x[n] + B u[n]
  % y[n]   = C x[n] + D u[n]
  % While tipically the fgetk linear stepper considers
  % y[n+1]   = C x[n+1] + D u[n+1]
end
sys = set(sys,'StateName', state_lbl, 'InputName',input_lbl, 'OutputName',  meas_lbl);

assert(size(B,2)==numel(u0),'size mismatch between size(B,2) and u0')

%% Add input delays
if isfield(L.G,'Vadelay')
  if ~all(L.G.Vadelay==0)
    warning('Found nonzero values of L.G.Vadelay. These are not included in the state-space system')
  end
end

%% Group outputs
for ii= measlist
  sys.OutputGroup.(ii{:}) = ind.(['iM' ii{:}]);
end

%% Group inputs
inlist = {'Va','dCodt','Co'};
if L.icde, inlist = [inlist,'Ini']; end 

for ii = inlist
  sys.InputGroup.(ii{:}) = ind.(['iU' ii{:}]);
end

end

function [ind,indend] = indxgen(ind, label_dim_pair, istart)
% Generate subsequent index structure from couple of {label, dimension}
% Extends the ind structure with the new inputs
% istart -> index offset to generate the new index starting from istart + 1

for ii=1:2:numel(label_dim_pair)
  ind.(label_dim_pair{ii}) = istart + (1:label_dim_pair{ii+1});
  istart = ind.(label_dim_pair{ii})(end);
end
indend = istart;
end

