
disp('this demo is not functional and needs to be updated')
return
%% RAPTOR RT demo: Simple test of real-time observer
clear;close all hidden;
run('../RAPTOR_path.m');

% check MATLAB version
versionstr=version; assert(eval(versionstr(1:3))>=8,'Does not work for MATLAB version < 8.0')

addpath ../code/
addpath ../RT/TCV/

%% RAPTOR config
dt = 2e-3; % time step

fprintf('Configuring RAPTOR_TCV for dt=%2.1fms\n',dt*1e3)
[config] = RAPTOR_config('TCV'); % load default params

config.grid.tgrid = [0:dt:0.5];
config.realtime = true;

% Create RAPTOR model and params with new parameters

[RAPTORmodel,RAPTORparams,init,g,v,U] = build_RAPTOR_model(config);

init.te0 = 200;
% Construct kinetic profiles according to default settings.
v = build_kinetic_profiles(RAPTORmodel,init);

RAPTORparams.debug.iterdisp = 0;
RAPTORparams.debug.iterplot = 0;
RAPTORparams.numerics.nmax = 1;
% initial plasma current
init.Ip0 = 80e3;

% initial conditions
x0 = RAPTOR_initial_conditions(RAPTORmodel,init,g(:,1),v(:,1)); 

%% Kalman Filter configuration
nx = RAPTORmodel.psi.nsp + RAPTORmodel.te.nsp;
KFparams = RAPTOR_EKF_config; % init
KFparams.ndx = 2; % Number of state disturbance terms
KFparams.Lgain = 1; % zero observer gain
KFparams.Ipstart = init.Ip0; % threshold for starting filter
KFparams.Ipstop = init.Ip0-20e3; % threshold for stopping filter

KFparams.x_cov = [1e-3,1e-5,1e3,1e2,1e-3]; % covariances j0,je,te0,tee,psie
KFparams.d_cov = 0.01*[1e-4,1e-5,1e3,1e3,1e-3];
KFparams.x_sm = [0.6,0.6]; % smoothness
KFparams.d_sm = [10,10]; % smoothness
KFparams.s0 = 1e-1; % another scaling

KFparams.iterdisp = 0;
KFparams.iterplot = 0;

u0 = U(:,1); % init u0 size;
[KFmodel,KFparams] = RAPTOR_EKF_config(x0,RAPTORmodel,RAPTORparams,KFparams);

%% Input signals: fake diagnostic measurements
Te0noise = 100; % noise in central Te measurement
Ipflattop = 200e3; % flat-top current

rampfun = @(t,tmin,ymin,tmax,ymax) ...
    max(ymin,min((ymax-ymin)/(tmax-tmin)*(t-tmin),ymax-ymin)+ymin); % anonymous function for ramps

% time grid
t = RAPTORparams.tgrid;


% density profile
ne_profile = 2e19*exp(-RAPTORmodel.rgrid.rhogauss.^2/0.8^2)*ones(1,numel(t));

Ip = rampfun(t,  0,   0,  0.1,  Ipflattop) - rampfun(t,  0.4,   0,  0.5,  Ipflattop);
Te0 = (Ip/Ipflattop)*2e3 + Te0noise*randn(size(t));
Psib = 1.5*t;
P_EC = [1;1]*rampfun(t,  0, 0,  0.1, 0e3);

ts_Ip   = timeseries(Ip',t);
ts_Te0  = timeseries(Te0',t);
ts_ne_profile = timeseries(ne_profile',t);
ts_Psib = timeseries(Psib',t);
ts_P_EC  = timeseries(P_EC',t);

%% Configure and run simulink
clear simParams
simParams.LoadExternalInput = 'on';
simParams.ExternalInput = 'ts_Ip,ts_Te0,ts_ne_profile,ts_Psib,ts_P_EC';
simParams.StartTime = num2str(RAPTORparams.tgrid(1));
simParams.StopTime  = num2str(RAPTORparams.tgrid(end));
simParams.Fixedstep = num2str(dt);

numerics = RAPTORparams.numerics;

configurationSettings
modelname = 'RAPTOR_TCV_demo';
fprintf('Running simulink model %s, please wait...\n',modelname);
simOut = sim(modelname,simParams);

%% plot results
logsout = simOut.get('logsout');

figure(1); clf;
subplot(411);
plot(logsout.get('Ip').Values);
subplot(412);
plot(logsout.get('iota').Values);
subplot(413);
plot(logsout.get('Te0').Values,'k.'); hold on;
plot(logsout.get('te').Values);
title('Te0 and simulated Te');
subplot(414);
plot(logsout.get('dte').Values);
