%%%
% This is just an example on how to provide data
% Not sure what is the linearization point that RZIP is considering so I don't expect a match
%%%
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

close all
clear all
clc

%% 
addpath(genpath('/home/carpanes/meq_GIT'))
addpath(genpath('/home/carpanes/rzip'))

%% 
shot = 40000;
tstart = 0.3;
tend = tstart +  0.005;
dt = 1e-5;
timefge = tstart:dt:tend;
Icoil = 19; % Step on G coil with RZIP ordering

%% Compute linear response FGE
[L,LX,LY] = fge('tcv', shot, tstart, 'ifequilinit', 1); % Get the data for the 1st time step

%%
[M,lin ] = fgelin(L,LY, 'observables' , {'RZIP'});

sys = ss(M.A, M.B, M.C, M.D);
% Simulate linear system with step response to G coils
simtime = timefge;
nt = numel(simtime);
U = zeros(nt,  L.G.na + L.ng + 1 + numel(LX.Fedot));
U(:,lin.Uind.Va(Icoil)) = 1; % Step perturbation in G coil
U(:,lin.Uind.xdot) = 0; % Steady state condition
[YFGE,TFGE,XFGE] = lsim(sys, U , timefge); % Simulate system

%% Compute linear response RZIP
[sys,sysinfo] = make_rzip_model('v',256,'fgs', L, LY,'TCV',1:19, shot,tstart);

% Simulate linear system with step response to G coils
simtime = timefge;
nt = numel(simtime);
U = zeros(nt, L.G.na );
U(:,Icoil) = 1; % Step perturbation in G coil
[Y,T,X] = lsim(sys, U , timefge); % Simulate system


%%
hf = figure;
ax = axes(hf);
hold(ax, 'on')
plot(ax, TFGE, YFGE(:,lin.yind.Ia(19)),'r');
plot(ax, T, Y(:,lin.yind.Ia(19)),'b');
xlabel(ax,'t [s]')
ylabel(ax, ['\delta Ia G coil'])
p(1) = plot(ax,NaN,NaN,'r'); % trick to add single line in legend
p(2) = plot(ax,NaN,NaN,'b'); % Trck to add single line in legend
legend(p, {'fgelin', 'RZIP'})
title(['shot' num2str(shot) ])


%%
% Plot all obervables in observables list
for ii = fieldnames(lin.yind).'
  hf = figure;
  ax = axes(hf);
  hold(ax, 'on')
  plot(ax, TFGE, YFGE(:,lin.yind.(ii{:})),'r');
  plot(ax, T, Y(:,lin.yind.(ii{:})),'b');
  xlabel(ax,'t [s]')
  ylabel(ax, ['\delta' ii{:}])
  p(1) = plot(ax,NaN,NaN,'r'); % trick to add single line in legend
  p(2) = plot(ax,NaN,NaN,'b'); % Trck to add single line in legend
  legend(p, {'fgelin', 'RZIP'})
  title(['shot' num2str(shot)])
end

