%LIUZ0  LIUQE vertical stabilisation function based on ZA-Z0
% DZ = LIUZ0(P,RST,Z0,ZA) returns the stabilising vertical shift fedback on
% ZA-Z0 with PID gains P.stab*[1 P.stabint P.stabder] and
% Z0=Z0-P.stabcint*DZ.
%
% For details, see: [MEQ-redbook] 
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
function dz = liuz0(P,rst,z0,zA,~)
 persistent K S Z0
 if nargin
  if rst
   K = P.stab*[1 P.stabint P.stabder]; % PID gains
   S = [0 ; 0; zA-z0];                 % PID states
   Z0 = z0;
  end
  S = (zA - Z0) + [0 ; S(2) ; -S(1)];  % update states
  dz = K*S;
  Z0 = Z0 - dz*P.stabcint;
 else
  dz = false;
 end
end
