% doublet control
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
clear;
D = load('doublet_control_target.mat');
L = D.L; LY=D.LX;

%%
E  = contains(L.G.dima,'E');
F  = contains(L.G.dima,'F');
OH = contains(L.G.dima,'OH');
G  = contains(L.G.dima,'G');
ia = 1:L.G.na;

%% Observer
Lh = lih('tcv',68898,[],'iterh',0,'nelem',12);
  
%%
meqploteig(L,'nmodes',2);

%% Unstable eigenmodes
sys = fgess(L);
[V,D] = eig(sys.A');
[Eig,isort] = esort(diag(D));
nunstab = sum(Eig>0);
assert(nunstab==2,'can design controller only for 2 unstable eigenmodes')

%% Ip control direction
Iaind = fgeIaind(L,LY);
TIp = Iaind / norm(Iaind);

%% 'Fast' Z control direction
hfo=figure(1); set(hfo,'name','Observers')
clf;
for jj=1:nunstab
  fprintf('growth rate %2.2f\n',E(jj));

  qi = V(:,isort(jj)); % Coil current direction in unstable direction
  
  % input pole vector pi = B'*qi
  II = eye(L.ne,L.G.na); MM = L.Mee + L.lin.Xee;
  pi = (MM\II)'*qi;

  dIy = reshape(L.lin.dIydIe*qi,L.nzy,L.nry); % dIy in unstable direction
  
  Tz = zeros(L.G.na,1);
  Tz(E|F) = L.G.Maa(:,E|F)\pi;
  
  Tz=Tz/norm(Tz);
  
  figure(hfo);
  subplot(2,3,3*(jj-1)+1)
  meqploteig(L,'imodes',jj);

  subplot(2,3,3*(jj-1)+2)
  LYp = LY;
  LYp.Ia = 10000*Tz; LYp.Iu(:) = 0; LYp.rX = []; LYp.zX=[];
  meqplotfancy(L,LYp,'vacuum',true,'FxLevels',[-1:0.01:1]);

  subplot(2,6,6*(jj-1)+5);
  imagesc(L.ry,L.zy,dIy); axis xy equal;

  subplot(2,6,6*(jj-1)+6);
  imagesc(L.ry,L.zy,reshape(Lh.Tyh*(Lh.Tyh\dIy(:)),L.nzy,L.nry)); axis xy equal;
  hold on; plot(Lh.rh,Lh.zh,'ok');
  drawnow;
  
  if jj==1, TzS = Tz; else, TzQ = Tz; end
end

%% Tr direction

dIydIa = L.lin.dIydIe(:,ia);
drIpDdIEF_t = sum(L.rry(:).* Lh.mask_t.*dIydIa);
drIpDdIEF_b = sum(L.rry(:).* Lh.mask_b.*dIydIa);
dzIpDdIEF_t = sum(L.zzy(:).* Lh.mask_t.*dIydIa);
dzIpDdIEF_b = sum(L.zzy(:).* Lh.mask_b.*dIydIa);

%% finite difference derivatives in R,Z direction for common/symmetric mode
dIydRQ  = ([zeros(L.nzy,1),diff(LY.Iy,[],2)] + [diff(LY.Iy,[],2),zeros(L.nzy,1)])/2;
dIydRS  = dIydRQ; dIydRS(logical(Lh.mask_b)) = -dIydRS(logical(Lh.mask_b));

dIydZQ  = ([zeros(1,L.nry);diff(LY.Iy,[],1)] + [diff(LY.Iy,[],1);zeros(1,L.nry)])/2;
dIydZS  = dIydZQ; dIydZS(logical(Lh.mask_b)) = -dIydZS(logical(Lh.mask_b));

%% find coil direction that gives mostly displacement in direction dIy/dR
% while driving zero net flux and not using OH coils and not moving Z
Fpa = sum(LY.Iy(:).*L.G.Mxa(L.lxy,:));
I = eye(L.G.na);
SOH = I(OH,:);

N = null([Fpa;SOH;dIydZQ(:)'*dIydIa;dIydZS(:)'*dIydIa]);

TrQ = N*pinv((dIydRQ(:)'*dIydIa)*N); TrQ = TrQ/norm(TrQ);
TrS = N*pinv((dIydRS(:)'*dIydIa)*N); TrS = TrS/norm(TrS);

%% Sign of Tz to move plant in positive direction
TzQ = TzQ*sign((dIydZQ(:)'*(dIydIa*TzQ)));
TzS = TzS*sign((dIydZS(:)'*(dIydIa*TzS)));

%%
clf;
subplot(161)
LYp.Ia = 1e3*TrQ; LYp.Iu(:) = 0; LYp.rX = []; LYp.zX=[];
meqplotfancy(L,LYp,'vacuum',true);
subplot(162);
meqplotfancy(L,LYp,'vacuum',true);
imagesc(L.ry,L.zy,reshape(L.lin.dIydIe(:,ia)*TrQ,L.nzy,L.nry),'AlphaData',~~LY.Iy); axis xy equal;

subplot(163)
LYp.Ia = 1e3*TrS; LYp.Iu(:) = 0; LYp.rX = []; LYp.zX=[];
meqplotfancy(L,LYp,'vacuum',true);
subplot(164);
meqplotfancy(L,LYp,'vacuum',true);
imagesc(L.ry,L.zy,reshape(L.lin.dIydIe(:,ia)*TrS,L.nzy,L.nry),'AlphaData',~~LY.Iy); axis xy equal;

subplot(165)
LYp.Ia = 1e3*TIp; LYp.Iu(:) = 0; LYp.rX = []; LYp.zX=[];
meqplotfancy(L,LYp,'vacuum',true);
subplot(166);
meqplotfancy(L,LYp,'vacuum',true);
imagesc(L.ry,L.zy,reshape(L.lin.dIydIe(:,ia)*TIp,L.nzy,L.nry),'AlphaData',~~LY.Iy); axis xy equal;

%% Observers

%% 5 controllers to tune
% Gains
KzQ.P = 3  ; KzQ.I = 0; KzQ.tD = 0;
KzS.P = 1  ; KzS.I = 0; KzS.tD = 0;

KrQ.P =  -0.05; KrQ.I = 50;
KrS.P =  -0.05; KrS.I = 50;

KIp.P = 0; KIp.I = 0;

s = tf('s');
KKzQ = KzQ.P*(1 + KzQ.I/s + KzQ.tD*s/(KzQ.tD*10*s+1));
KKzS = KzS.P*(1 + KzS.I/s + KzS.tD*s/(KzS.tD*10*s+1));
KKrQ = KrQ.P*(1 + KrQ.I/s);
KKrS = KrS.P*(1 + KrS.I/s);
KKIp = KIp.P*(1 + KIp.I/s);

% figure(1); bode(KKzS)

Kc = [TIp,TzQ,TzS,TrQ,TrS]*blkdiag(KKIp,KKzQ,KKzS,KKrQ,KKrS);

Pc = sys({'Ip','zIpD','rIpD'},:);

OS = [1 -1]/2; OQ = [1 1]/2;
Oc = [eye(1,5);
      0   OQ  0 0
      0   OS  0 0
      0  0 0   OQ
      0  0 0   OS];
      

syscl = feedback(Pc*Kc,Oc);

%
figure(2);
clf;step(syscl(:,[2,3])*(1/(0.2*s+1)),1)

%%
save doublet_control_directions TzQ TzS TIp

%%