function  [cstate,V,ctpar,ctdebug] = ctVSG(mode,cstate,ctpar,L,LY)
% [cstate,V] = ctVSG(cstate,ctpar,L,LX,LY)
% Vertical Stability Controller using G coil
%
% cstate: controller state
% mode: 0: parameters - 1:initialize - 2: step
% V: controller feedback voltage
% ctpar: controller parameters
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.


persistent A B C D % controller matrices

switch mode
  case 'setup'
    % initialize parameters
    ctpar = struct('Kp',0.3,... % P gain
      'dt',1e-3,... % calling time
      'Ti',1e-3,... % integral time constant
       'Tg',double(contains(L.G.dima,'G_001')),... % control direction vector
       'zAref',struct('t',0,'y',0)... % z position reference
       );
    cstate = []; V = [];
  case 'init'
    % initialize state
    s = tf('s');
    sys = ss(ctpar.Kp*(1+1/s/ctpar.Ti)); % PID controller
    sysd = c2d(sys,ctpar.dt);
    A = sysd.A; B=sysd.B; C=sysd.C; D=sysd.D;
    cstate = zeros(size(A,2),1); % controller state init
    V = zeros(L.G.na,1);
  case 'step'
    % Step call
      
    % Controller for vertical stability using G coil only
    
    izAref = find(ctpar.zAref.t<=LY.t,1,'last'); % time index of zA ref
    zAref  = ctpar.zAref.y(izAref);
    
    ezIp = (LY.zA-zAref).*LY.Ip;
    
    % control law - state space form
    xc   = A*cstate + B*ezIp;
    uc   = C*cstate + D*ezIp;
    
    cstate = xc;
  
    % assign control output to G coil
    V = ctpar.Tg*uc;
    
    ctdebug = [];
  case 'reset'
    clear A B C D
  otherwise
    error('unknown mode');
end

end
