% This script is used for evaluation of control structures with the
% additional application of extremum seeking control
% It utilizes the function "AmmonReac_TCont.m" for set-up of the model.
% Hence, both controllers are incorporated into the model.
%
% Plots to be done for presentation:
% Comparison of SOC4, ESC1, ESC4
% Disturbances: 1 flow rate (+15 == +20%)
%               6 reaction rate (-20%)
% Plots:        Cost function
%               Loss w.r.t. true optimum

% Clear of the command line and the workspace;
clc, clear, close all

% Definition of the current path folder
FileName = mfilename('fullpath');
[directory,~,~] = fileparts(FileName);
[dirmain,~,~] = fileparts(directory);

% Import of CASADI
addpath([dirmain '\Util1. Casadi3.1.0'])
addpath([dirmain '\Util2. Functions'])
addpath(genpath([dirmain '\Util4. MATLAB2tikz']))
import casadi.*

% Load of the slave tunings
load([dirmain '\Data\Tun_Slave.mat'])

% Definition of version
dec.Ver    = {'SOC','','',''}; % 'SOC', 'MESC', or 'eNMPC'
dec.H      = [4 4 4 1]; % 1, 2, 3, or 4
dec.H_comp = 4;
dec.plot   = true;
dec.noise  = false;
dec.mnoise = false;
dec.step   = false;
dec.red    = false;
dec.redH   = false;
dec.dist   = [1];
distH      = [1 2 3 4];

% Definition of the total integration time and the number of time steps for
% the integrator
t.tot  = 100*60;
t.step = 10*60;
t.Noise = 30*60;
t.int = 1;             % MESC requires a step size <= 0.2
t.N  = t.tot/t.int;

for k = 1:4
    if strcmp(dec.Ver(k),'eNMPC')
        dec.H(k) = 0;
    elseif strcmp(dec.Ver(k),'mESC')
        dec.H(k) = 1;
    end
end

if t.tot <= 6000
    t.samp = (0:t.int:t.tot)/60;
elseif  t.tot <= 3600*24
    t.samp = (0:t.int:t.tot)/3600;
else
    t.samp = (0:t.int:t.tot)/3600/24;
end

if any(strcmp(dec.Ver,'eNMPC'))
    % Resetting the H matrix to 0
    dec.H(strcmp(dec.Ver,'eNMPC')) = 0;
    
    % Definition of the total integration time and the number of time steps for
    % the first NMPC
    t_eNMPC{1}.N  = 20;
    t_eNMPC{1}.int = 30;
    t_eNMPC{1}.mul = t_eNMPC{1}.int/t.int;
    t_eNMPC{1}.tot  = t_eNMPC{1}.int*t_eNMPC{1}.N;
    t_eNMPC{1}.Varint = t_eNMPC{1}.int*ones(1,t_eNMPC{1}.N);
    t_eNMPC{1}.samp = [0 cumsum(t_eNMPC{1}.Varint)]/60;
    
    % Definition of the total integration time and the number of time steps for
    % the second NMPC
    t_eNMPC{2}.N  = 20;
    t_eNMPC{2}.int = 30;
    t_eNMPC{2}.mul = t_eNMPC{2}.int/t.int;
    t_eNMPC{2}.tot  = t_eNMPC{2}.int*t_eNMPC{2}.N;
    t_eNMPC{2}.Varint = t_eNMPC{2}.int*ones(1,t_eNMPC{2}.N);
    t_eNMPC{2}.samp = [0 cumsum(t_eNMPC{2}.Varint)]/60;
    
    % Definition of the total integration time and the number of time steps for
    % the third NMPC
    t_eNMPC{3}.N  = 20;
    t_eNMPC{3}.int = 30;
    t_eNMPC{3}.mul = t_eNMPC{3}.int/t.int;
    t_eNMPC{3}.tot  = t_eNMPC{3}.int*t_eNMPC{3}.N;
    t_eNMPC{3}.Varint = t_eNMPC{3}.int*ones(1,t_eNMPC{3}.N);
    t_eNMPC{3}.samp = [0 cumsum(t_eNMPC{3}.Varint)]/60;
    
    % Definition of the total integration time and the number of time steps for
    % the NMPC
    t_eNMPC{4}.N  = 20;
    t_eNMPC{4}.int = 30;
    t_eNMPC{4}.mul = t_eNMPC{4}.int/t.int;
    t_eNMPC{4}.tot  = t_eNMPC{4}.int*t_eNMPC{4}.N;
    t_eNMPC{4}.Varint = t_eNMPC{4}.int*ones(1,t_eNMPC{4}.N);
    t_eNMPC{4}.samp = [0 cumsum(t_eNMPC{4}.Varint)]/60;
end
if any(strcmp(dec.Ver,'MESC'))
    % Resetting the H matrix to 1
    dec.H(strcmp(dec.Ver,'MESC')) = 1;
end

% Definition of the load files
if true
    loadname.SOC = [dirmain '\Data\SS_opt_SOC_dist'];
    loadname.opt = [dirmain '\Data\SS_opt_6dist'];
    loadname.H   = [dirmain '\Data\H_dist'];
    loadname.tun = [dirmain '\Data\Tun_Mas_dist'];
    for k = 1:6
        if any(distH == k)
            loadname.SOC = [loadname.SOC num2str(k)];
            loadname.H   = [loadname.H   num2str(k)];
            loadname.tun = [loadname.tun num2str(k)];
        end
    end
    loadname.SOC = [loadname.SOC '_MS'];
    loadname.H   = [loadname.H   '_MS'];
    loadname.tun = [loadname.tun '_MS'];
    if dec.red
        loadname.SOC = [loadname.SOC '_red'];
        loadname.opt = [loadname.opt '_red'];
    end
    if dec.redH 
        loadname.H   = [loadname.H   '_redH'];
        loadname.tun = [loadname.tun '_redH'];
    end
    loadname.SOC = [loadname.SOC '.mat'];
    loadname.opt = [loadname.opt '.mat'];
    loadname.H   = [loadname.H   '.mat'];
    loadname.tun = [loadname.tun '.mat'];
end

% Load of the H matrices and the respective tunings
load(loadname.H)
load(loadname.tun)
    
%% Definition of the controller gains

% Rearrangement of H{2}
H{2} = H{2}([3 1 2],:);

% Definition of the tuning parameters
tauC.S = 10*ones(3,1);
tauC.M = ones(2,4).*Mas.theta;

% Calculation of the controller gains for the Slave controller
K_S.I = 1./k_s./tauC.S;

% Calculation of the controller gains for the Master controller
K_M.p = Mas.Kp;
K_M.I = Mas.KI;
K_M.p(1:2,:) = 1./Mas.k.*Mas.tau_one./(tauC.M+Mas.theta);
K_M.I(1:2,:) = K_M.p(1:2,:)./min(Mas.tau_one,4*(tauC.M+Mas.theta));
K_M.p(3,2) = 1;
K_M.I(3,2) = 0.05;

% Definition of the ESC controller parameters
if any(strcmp(dec.Ver,'ESC'))
    % Multiplicator of step implementation
    ESC.step = [1 1 1 1];
    
    % Multiplicators of the sinusoidals [K]
    ESC.mul = [...
               5*[0.2; 0.2; 0.2] ...            ESC 4
               5*[0.2; 0.2; 0.2] ...            ESC 4
               5*[0.2; 0.2; 0.2] ...            ESC 4
               5*[0.2; 0.2; 0.2] ...            ESC 4
               ];
    % Period of the sinisuodals [s]
    ESC.sin = [...
               [120; 150; 180] ...              ESC 4
               [120; 150; 180] ...              ESC 4
               [120; 150; 180] ...              ESC 1
               [120; 150; 180] ...              ESC 1
               ]*60;
    ESC.t = [60 60 60 60]'*60;                  % Time, ESC starts
    
    % Integrator gain [s]
    K_ESC.I = [...
               .4*[0.2; 0.1; 0.1] ...           ESC 4
               .2*[0.4; 0.4; 0.2] ...           ESC 4 Steady state detection
               .2*[0.4; 0.05; 0.1] ...          ESC 4 unbounded
               [0.25; 0.1; 0.1] ...             ESC 4 using measurement
               ];
%                [0.05; 0.05; 0.05] ...           ESC 1
%                .2*[0.05; 0.08; 0.04] ...        ESC 1 unbounded

    % Bound for changes in theta
    K_ESC.bound = [...
                   [0.02; 0.03; 0.03] ...       ESC 4
                   2e4*[0.01; 0.015; 0.015] ...	ESC 4 Steady state detection
                   2e4*[0.01; 0.015; 0.015] ... ESC 4 unbounded
                   [0.015; 0.015; 0.015] ...    ESC 4 using measurement
                   ]*t.int;
%                    [0.02; 0.015; 0.015] ...     ESC 1
%                    1e4*[0.02; 0.015; 0.015] ... ESC 1 unbounded
end

% Definition of the MESC controller parameters
if any(strcmp(dec.Ver,'MESC'))
    % Proportional gain
    K_MESC.P = [[65; 61; 80] ...
                [65; 61; 80] ...
                [65; 61; 80] ...
                [65; 61; 80]];
    % Integrator gain 
    K_MESC.I = [[1.6; 3.4; 4.45] ...
                [1.6; 3.4; 4.45] ...
                [1.6; 3.4; 4.45] ...
                [1.6; 3.4; 4.45]];
end

% Definition of the eNMPC controller parameters
if any(strcmp(dec.Ver,'eNMPC'))
    % Definition of the tuning parameters for Version 1
    eNMPC{1}.Du  = 100;
    eNMPC{1}.z   = 1;
    eNMPC{1}.arr = [1:4 6 8 10 15];
    % Definition of the tuning parameters for Version 2
    eNMPC{2}.Du  = 100;
    eNMPC{2}.z   = 1;
    eNMPC{2}.arr = [1:4 6 8 10 15];
    % Definition of the tuning parameters for Version 3
    eNMPC{3}.Du  = 100;
    eNMPC{3}.z   = 1;
    eNMPC{3}.arr = [1:4 6 8 10 15];
    % Definition of the tuning parameters for Version 4
    eNMPC{4}.Du  = 100;
    eNMPC{4}.z   = 1;
    eNMPC{4}.arr = [1:4 6 8 10 15];
end

% Turn of the singularity warning
warning('off','MATLAB:nearlySingularMatrix')

%% Evaluation of the control structures

% H{3} is only able to maintain -46 in temperature and pressure as
% otherwise the controller 1 wants to go to 0 all the time

% Choosing of the disturbance
d.val   = [15 -50 -50 0.04 0.2 -0.2]';

% Definition of feed variables
d.nom = zeros(4,1);
d.nom(1) = 252/3.6;         % Feed flow (kg/s)
d.nom(2) = 200;             % Pressure [bar]
d.nom(3) = 250;             % Temperature [C]
d.nom(4) = 0.08;            % Mass fraction ammonia [-]
d.nom(5) = 3;               % Hydrogen/nitrogen molar ratio [-]
d.nom(6) = 1;               % Catalyst activity 

% Definition of the function, the DAE, and the inlet conditions for a
% generic case to get the number of disturbances
[func,dae,~,~,~,n] = AmmonReac_NewDist(t.int,K_S,K_M,H{4});

% Differentiation between step change, disturbances or nothing 
p.sim = zeros(n.p,t.N+1);
if dec.step
    p.sim(dec.dist,t.step/t.int+1:t.N+1) = p.sim(dec.dist,t.step/t.int+1:t.N+1)+5;
else
    if length(dec.dist) > 1
        p.sim(dec.dist(1)+n.u,t.step(1)/t.int+1:t.step(2)/t.int) = d.val(dec.dist(1))*ones(1,size(t.step(1)/t.int+1:t.step(2)/t.int,2));
        p.sim(dec.dist(2)+n.u,t.step(2)/t.int+1:t.N+1)           = d.val(dec.dist(2))*ones(1,size(t.step(2)/t.int+1:t.N+1,2));
    elseif length(dec.dist) == 1
        p.sim(dec.dist+n.u,t.step/t.int+1:t.N+1) = d.val(dec.dist);
    end
end
p.sim(n.u+1,:) = p.sim(n.u+1,:)-8.73*dec.red;
int.sim = p.sim(n.u+1:end,:);
% p.sim(n.u+1:end,2:end) = p.sim(n.u+1:end,2:end)+dec.noise*kron(wgn(n.d,t.tot/t.Noise,-50),ones(1,t.Noise/t.int)).*(d.nom*ones(1,t.N));

% Predefinition of the structure for the first simulation
if ~isempty(dec.Ver{1})
    load([dirmain '\Data\Init_Opt.mat'])
    if dec.H(1) == 0
        [func_Ver1,dae_Ver1,~,~,~,n] = AmmonReac_NewDist(t.int);
        N_Ver1 = size(dae_Ver1.x,1);
    elseif dec.H(1) == 1
        [func_Ver1,dae_Ver1,~,~,~,n] = AmmonReac_NewDist(t.int,K_S);
        N_Ver1 = size(dae_Ver1.x,1)-n.u;
        x0 = [x0; zeros(n.u,1)];
        z0 = [z0; p0(1:n.u)];
    elseif dec.H(1) > 1
        [func_Ver1,dae_Ver1,~,~,~,n] = AmmonReac_NewDist(t.int,K_S,K_M,H,dec.H(1));
        N_Ver1 = size(dae_Ver1.x,1)-2*n.u;
        x0 = [x0; zeros(2*n.u,1)];
        z0 = [z0; p0(1:n.u); z0(2*N_Ver1+1:2*N_Ver1+n.u); H{dec.H(1)}*y0];
    end
    if strcmp(dec.Ver{1},'MESC')
%         [func_Ver1_EKF,ode_Ver1_EKF,~,~,par] = AmmonReac_ODE_Ver2(t.int,K_S);
        [func_Ver1_EKF,ode_Ver1_EKF,~,~,par] = AmmonReac_ODE_Ver3(t.int,K_S);
    end

    Ver1.x.sim   = zeros(n.x,t.N+1);            Ver1.x.sim(:,1) = x0;
    Ver1.z.sim   = zeros(n.z,t.N+1);            Ver1.z.sim(:,1) = z0;
    Ver1.y.sim   = zeros(numel(y0),t.N+1);   	Ver1.y.sim(:,1) = y0; 
    Ver1.y.noise = zeros(numel(y0),t.N+1);   	Ver1.y.noise(:,1) = y0; 
    Ver1.EoR.sim = zeros(1,t.N+1);
    Ver1.EoRint.sim = zeros(1,t.N+1);
    
    if dec.H > 0
        Ver1.cS.sim = zeros(n.u,t.N+1);         Ver1.cS.sim(:,1) = H{1}*y0;
        Ver1.cS.set = Ver1.cS.sim(:,1)*ones(1,t.N+1);

        Ver1.cM.sim = zeros(n.u,t.N+1);         Ver1.cM.sim(:,1) = H{dec.H(1)}*y0*~(strcmp(dec.Ver{1},'MESC'));
        Ver1.cM.set = Ver1.cM.sim(:,1)*ones(1,t.N+1);

        Ver1.p.sim = p.sim;                     Ver1.p.sim(1:n.u,:) = H{dec.H(1)}*y0*ones(1,t.N+1)+Ver1.p.sim(1:n.u,:);
    else
        Ver1.p.sim = p.sim;                     Ver1.p.sim(1:n.u,1) = p0(1:n.u);
    end
    
    % Additional parameters for least-square-based extremum seeking control
    if strcmp(dec.Ver{1},'ESC')
        Ver1.theta_est = zeros(n.u,t.N+1);
        Ver1.theta_int = zeros(n.u,t.N+1);
        Ver1.theta_inte = zeros(n.u,t.N+1);
        Ver1.del_u = zeros(n.u,t.N+1);
        if dec.H(1) > 1
            Ver1.Jest = Ver1.cM.sim;
        else
            Ver1.Jest = Ver1.cS.sim;
        end
        
    % Additional parameters for Model-based extremum seeking control
    elseif strcmp(dec.Ver{1},'MESC')
        % Configure EKF
        ddt_EKF = ode_Ver1_EKF.x + t.int*ode_Ver1_EKF.ode;

        % EKF System/linearizied System around current estimate JacFx / JacHx
        % Definition of the system states
        Ver1.f_EKF = Function('f_EKF',{ode_Ver1_EKF.x,ode_Ver1_EKF.p},{ddt_EKF},{'x','p'},{'xdot'});
        Ver1.JacFx = Function('JacFx',{ode_Ver1_EKF.x,ode_Ver1_EKF.p},{jacobian(ddt_EKF,ode_Ver1_EKF.x)});

        % Definition of model for measurements
        if dec.H(1) < 1
            u_slave = MX.sym('u_slave',3);
        else
            u_slave = [];
        end
%         [~, y_model] = Measurement_MESC_Ver2(ode_Ver1_EKF.x,y0,p0,d.nom,u_slave,ode_Ver1_EKF.p,par,N_Ver1,K_S);
        [~, y_model] = Measurement_MESC_Ver3(ode_Ver1_EKF.x,y0,p0,d.nom(1:5),ode_Ver1_EKF.p(n.u+1:end),u_slave,ode_Ver1_EKF.p,par,N_Ver1,K_S);

        % With Ver1.h_EKF we estimate the measurements
        Ver1.h_EKF = Function('h_EKF',{ode_Ver1_EKF.x,[ode_Ver1_EKF.p;u_slave]},{y_model});
        Ver1.JacHx = Function('JacHx',{ode_Ver1_EKF.x,[ode_Ver1_EKF.p;u_slave]},{jacobian(y_model,ode_Ver1_EKF.x)});

        % Using estimated states, cost function model to obtain linear model from u
        % to J
        Jacx = [ode_Ver1_EKF.x(1:N_Ver1); ode_Ver1_EKF.x(N_Ver1+1:N_Ver1+n.u); ode_Ver1_EKF.x(N_Ver1+n.u+1:N_Ver1*2+n.u)];
        Jacp = [ode_Ver1_EKF.p; ode_Ver1_EKF.x(end)];
%         Jacp = [ode_Ver1_EKF.p; ode_Ver1_EKF.x(end-n.d+1:end)];
        Ver1.JacAx = Function('JacAx',{Jacx,Jacp},{jacobian(ode_Ver1_EKF.ode(1:2*N_Ver1+n.u), Jacx)});
        Ver1.JacBu = Function('JacBu',{Jacx,Jacp},{jacobian(ode_Ver1_EKF.ode(1:2*N_Ver1+n.u), ode_Ver1_EKF.p(1:n.u))});
        Ver1.JacJx = Function('JacJx',{Jacx,Jacp},{jacobian(ode_Ver1_EKF.quad,Jacx)});
        Ver1.JacJu = Function('JacJu',{Jacx,Jacp},{jacobian(ode_Ver1_EKF.quad,ode_Ver1_EKF.p(1:n.u))});
        
        Ver1.cM.set = zeros(n.u,t.N+1);
        Ver1.err    = zeros(n.u,t.N+1);
        Ver1.errint = zeros(n.u,t.N+1);
        Ver1.ek.sim = zeros(n.y,t.N+1);
%         Ver1.xEstErr.sim = zeros(N_Ver1*2+n.u+n.d,t.N+1);
        Ver1.xEstErr.sim = zeros(N_Ver1*2+n.u+1,t.N+1);
%         Ver1.est_x.sim = [Ver1.x.sim; Ver1.z.sim(1:N_Ver1,:); zeros(n.d,t.N+1)];
        Ver1.est_x.sim = [Ver1.x.sim; Ver1.z.sim(1:N_Ver1,:); zeros(1,t.N+1)];

        % Initialization of various parameters for the EKF 
        Ver1.diag_EKF = speye(length(ode_Ver1_EKF.x));
        Ver1.Pk = 1.*Ver1.diag_EKF;           % Covarianzmatrix for estimation
        Ver1.Qk = 0.01*diag([Ver1.est_x.sim(1:30,1); [5 5 5]'; Ver1.est_x.sim(34:63,1); d.nom(end)]);   % Covarianzmatrix for process noise
        Ver1.Rk = 1.*eye(n.y);                % Covarianzmatrix for observation noise
        
    % Additional parameters for economic non-linear model predictive
    % control
    elseif strcmp(dec.Ver{1},'eNMPC')
        [Ver1.eNMPC,Ver1_reA] = eNMPC_Setup(t,t_eNMPC{1},eNMPC{1});
        Ver1.eNMPC.N_sub = N_Ver1+length(Ver1.eNMPC.ind_z);
    end
end

% Predefinition of the structures for the second simulation
if ~isempty(dec.Ver{2})
    load([dirmain '\Data\Init_Opt.mat'])
    if dec.H(2) == 0
        [func_Ver2,dae_Ver2,~,~,~,n] = AmmonReac_NewDist(t.int);
        N_Ver2 = size(dae_Ver2.x,1);
    elseif dec.H(2) == 1
        [func_Ver2,dae_Ver2,~,~,~,n] = AmmonReac_NewDist(t.int,K_S);
        N_Ver2 = size(dae_Ver2.x,1)-n.u;
        x0 = [x0; zeros(n.u,1)];
        z0 = [z0; p0(1:n.u)];
    elseif dec.H(2) > 1
        [func_Ver2,dae_Ver2,~,~,~,n] = AmmonReac_NewDist(t.int,K_S,K_M,H,dec.H(2));
        N_Ver2 = size(dae_Ver2.x,1)-2*n.u;
        x0 = [x0; zeros(2*n.u,1)];
        z0 = [z0; p0(1:n.u); z0(2*N_Ver2+1:2*N_Ver2+n.u); H{dec.H(2)}*y0];
    end
    if strcmp(dec.Ver{2},'MESC')
%         [func_Ver2_EKF,ode_Ver2_EKF,~,~,par] = AmmonReac_ODE_Ver1(t.int,K_S);
        [func_Ver2_EKF,ode_Ver2_EKF,~,~,par] = AmmonReac_ODE_Ver3(t.int,K_S);
    end

    Ver2.x.sim = zeros(n.x,t.N+1);          Ver2.x.sim(:,1) = x0;
    Ver2.z.sim = zeros(n.z,t.N+1);          Ver2.z.sim(:,1) = z0;
    Ver2.y.sim = zeros(numel(y0),t.N+1);   	Ver2.y.sim(:,1) = y0; 
    Ver2.EoR.sim = zeros(1,t.N+1);
    Ver2.EoRint.sim = zeros(1,t.N+1);
    
    if dec.H > 0
        Ver2.cS.sim = zeros(n.u,t.N+1);         Ver2.cS.sim(:,1) = H{1}*y0;
        Ver2.cS.set = Ver2.cS.sim(:,1)*ones(1,t.N+1);

        Ver2.cM.sim = zeros(n.u,t.N+1);         Ver2.cM.sim(:,1) = H{dec.H(2)}*y0*~(strcmp(dec.Ver{2},'MESC'));
        Ver2.cM.set = Ver2.cM.sim(:,1)*ones(1,t.N+1);

        Ver2.p.sim = p.sim;                     Ver2.p.sim(1:n.u,:) = H{dec.H(2)}*y0*ones(1,t.N+1)+Ver2.p.sim(1:n.u,:);
    else
        Ver2.p.sim = p.sim;                     Ver2.p.sim(1:n.u,1) = p0(1:n.u);
    end
    
    % Additional parameters for least-square-based extremum seeking control
    if strcmp(dec.Ver{2},'ESC')
        Ver2.theta_est = zeros(n.u,t.N+1);
        Ver2.theta_int = zeros(n.u,t.N+1);
        Ver2.theta_inte = zeros(n.u,t.N+1);
        Ver2.del_u = zeros(n.u,t.N+1);
        if dec.H(2) > 1
            Ver2.Jest = Ver2.cM.sim;
        else
            Ver2.Jest = Ver2.cS.sim;
        end
        
    % Additional parameters for Model-based extremum seeking control
    elseif strcmp(dec.Ver{2},'MESC')
        % Configure EKF
        ddt_EKF = vertcat(ode_Ver2_EKF.x + t.int*ode_Ver2_EKF.ode);

        % EKF System/linearizied System around current estimate JacFx / JacHx
        % Definition of the system states
        Ver2.f_EKF = Function('f_EKF',{ode_Ver2_EKF.x,ode_Ver2_EKF.p},{ddt_EKF},{'x','p'},{'xdot'});
        Ver2.JacFx = Function('JacFx',{ode_Ver2_EKF.x,ode_Ver2_EKF.p},{jacobian(ddt_EKF,ode_Ver2_EKF.x)});

        % Definition of model for measurements
        if dec.H(2) < 1
            u_slave = MX.sym('u_slave',3);
        else
            u_slave = [];
        end
%         [~, y_model] = Measurement_MESC_Ver1(ode_Ver2_EKF.x,y0,p0,d.nom,ode_Ver2_EKF.p(n.u+1:end),u_slave,ode_Ver2_EKF.p,par,N_Ver2,K_S);
        [~, y_model] = Measurement_MESC_Ver3(ode_Ver2_EKF.x,y0,p0,d.nom(1:5),ode_Ver2_EKF.p(n.u+1:end),u_slave,ode_Ver1_EKF.p,par,N_Ver1,K_S);

        % With Ver2.h_EKF we estimate the measurements 
        Ver2.h_EKF = Function('h_EKF',{ode_Ver2_EKF.x,[ode_Ver2_EKF.p;u_slave]},{y_model});
        Ver2.JacHx = Function('JacHx',{ode_Ver2_EKF.x,[ode_Ver2_EKF.p;u_slave]},{jacobian(y_model,ode_Ver2_EKF.x)});

        % Using estimated states, cost function model to obtain linear model from u
        % to J
%         Ver2.JacAx = Function('JacAx',{ode_Ver2_EKF.x,ode_Ver2_EKF.p},{jacobian(ode_Ver2_EKF.ode, ode_Ver2_EKF.x)});
%         Ver2.JacBu = Function('JacBu',{ode_Ver2_EKF.x,ode_Ver2_EKF.p},{jacobian(ode_Ver2_EKF.ode, ode_Ver2_EKF.p(1:n.u))});
%         Ver2.JacJx = Function('JacJx',{ode_Ver2_EKF.x,ode_Ver2_EKF.p},{jacobian(ode_Ver2_EKF.quad,ode_Ver2_EKF.x)});
%         Ver2.JacJu = Function('JacJu',{ode_Ver2_EKF.x,ode_Ver2_EKF.p},{jacobian(ode_Ver2_EKF.quad,ode_Ver2_EKF.p(1:n.u))});
        Jacx = [ode_Ver2_EKF.x(1:N_Ver1); ode_Ver2_EKF.x(N_Ver1+1:N_Ver1+n.u); ode_Ver2_EKF.x(N_Ver1+n.u+1:N_Ver1*2+n.u)];
        Jacp = [ode_Ver2_EKF.p; ode_Ver2_EKF.x(end)];
        Ver2.JacAx = Function('JacAx',{Jacx,Jacp},{jacobian(ode_Ver2_EKF.ode(1:2*N_Ver2+n.u), Jacx)});
        Ver2.JacBu = Function('JacBu',{Jacx,Jacp},{jacobian(ode_Ver2_EKF.ode(1:2*N_Ver2+n.u), ode_Ver2_EKF.p(1:n.u))});
        Ver2.JacJx = Function('JacJx',{Jacx,Jacp},{jacobian(ode_Ver2_EKF.quad,Jacx)});
        Ver2.JacJu = Function('JacJu',{Jacx,Jacp},{jacobian(ode_Ver2_EKF.quad,ode_Ver2_EKF.p(1:n.u))});
       
        Ver2.cM.set = zeros(n.u,t.N+1);
        Ver2.err    = zeros(n.u,t.N+1);
        Ver2.errint = zeros(n.u,t.N+1);
        Ver2.ek.sim = zeros(n.y,t.N+1);
%         Ver2.xEstErr.sim = zeros(N_Ver2*2+n.u,t.N+1);
        Ver2.xEstErr.sim = zeros(N_Ver2*2+n.u+1,t.N+1);
%         Ver2.est_x.sim = [Ver2.x.sim;  Ver2.z.sim(1:N_Ver2,:); zeros(n.d,t.N+1)];
        Ver2.est_x.sim = [Ver2.x.sim; Ver2.z.sim(1:N_Ver2,:); zeros(1,t.N+1)];

        % Initialization of various parameters for the EKF 
        Ver2.diag_EKF = speye(length(ode_Ver2_EKF.x));
        Ver2.Pk = 0.1.*Ver2.diag_EKF;           % Covarianzmatrix for estimation
        Ver2.Qk = 0.01*diag([Ver2.est_x.sim(1:30,1); [5 5 5]'; Ver2.est_x.sim(34:63,1); d.nom(end)]);      % Covarianzmatrix for process noise
        Ver2.Rk = 4.*eye(n.y);                  % Covarianzmatrix for observation noise
        
    % Additional parameters for economic non-linear model predictive
    % control
    elseif strcmp(dec.Ver{2},'eNMPC')
        [Ver2.eNMPC,Ver2_reA] = eNMPC_Setup(t,t_eNMPC{2},eNMPC{2});
        Ver2.eNMPC.N_sub = N_Ver2+length(Ver2.eNMPC.ind_z);
    end
end

% Predefinition of the structures for the third simulation
if ~isempty(dec.Ver{3})
    load([dirmain '\Data\Init_Opt.mat'])
    if dec.H(3) == 0
        [func_Ver3,dae_Ver3,~,~,~,n] = AmmonReac_NewDist(t.int);
        N_Ver3 = size(dae_Ver3.x,1);
    elseif dec.H(3) == 1
        [func_Ver3,dae_Ver3,~,~,~,n] = AmmonReac_NewDist(t.int,K_S);
        N_Ver3 = size(dae_Ver3.x,1)-n.u;
        x0 = [x0; zeros(n.u,1)];
        z0 = [z0; p0(1:n.u)];
    elseif dec.H(3) > 1
        [func_Ver3,dae_Ver3,~,~,~,n] = AmmonReac_NewDist(t.int,K_S,K_M,H,dec.H(3));
        N_Ver3 = size(dae_Ver3.x,1)-2*n.u;
        x0 = [x0; zeros(2*n.u,1)];
        z0 = [z0; p0(1:n.u); z0(2*N_Ver3+1:2*N_Ver3+n.u); H{dec.H(3)}*y0];
    end
    if strcmp(dec.Ver{3},'MESC')
        [func_Ver3_EKF,ode_Ver3_EKF,~,~,par] = AmmonReac_ODE_Ver3(t.int,K_S);
    end

    Ver3.x.sim = zeros(n.x,t.N+1);          Ver3.x.sim(:,1) = x0;
    Ver3.z.sim = zeros(n.z,t.N+1);          Ver3.z.sim(:,1) = z0;
    Ver3.y.sim = zeros(numel(y0),t.N+1);   	Ver3.y.sim(:,1) = y0; 
    Ver3.EoR.sim = zeros(1,t.N+1);
    Ver3.EoRint.sim = zeros(1,t.N+1);
    
    if dec.H > 0
        Ver3.cS.sim = zeros(n.u,t.N+1);         Ver3.cS.sim(:,1) = H{1}*y0;
        Ver3.cS.set = Ver3.cS.sim(:,1)*ones(1,t.N+1);

        Ver3.cM.sim = zeros(n.u,t.N+1);         Ver3.cM.sim(:,1) = H{dec.H(3)}*y0*~(strcmp(dec.Ver{3},'MESC'));
        Ver3.cM.set = Ver3.cM.sim(:,1)*ones(1,t.N+1);

        Ver3.p.sim = p.sim;                     Ver3.p.sim(1:n.u,:) = H{dec.H(3)}*y0*ones(1,t.N+1)+Ver3.p.sim(1:n.u,:);
    else
        Ver3.p.sim = p.sim;                     Ver3.p.sim(1:n.u,1) = p0(1:n.u);
    end
    
    % Additional parameters for least-square-based extremum seeking control
    if strcmp(dec.Ver{3},'ESC')
        Ver3.theta_est = zeros(n.u,t.N+1);
        Ver3.theta_int = zeros(n.u,t.N+1);
        Ver3.theta_inte = zeros(n.u,t.N+1);
        Ver3.del_u = zeros(n.u,t.N+1);
        if dec.H(3) > 1
            Ver3.Jest = Ver3.cM.sim;
        else
            Ver3.Jest = Ver3.cS.sim;
        end
        
    % Additional parameters for Model-based extremum seeking control
    elseif strcmp(dec.Ver{3},'MESC')
        % Configure EKF
        ddt_EKF = ode_Ver3_EKF.x + t.int*ode_Ver3_EKF.ode;

        % EKF System/linearizied System around current estimate JacFx / JacHx
        % Definition of the system states
        Ver3.f_EKF = Function('f_EKF',{ode_Ver3_EKF.x,ode_Ver3_EKF.p},{ddt_EKF},{'x','p'},{'xdot'});
        Ver3.JacFx = Function('JacFx',{ode_Ver3_EKF.x,ode_Ver3_EKF.p},{jacobian(ddt_EKF,ode_Ver3_EKF.x)});

        % Definition of model for measurements
        if dec.H(1) < 1
            u_slave = MX.sym('u_slave',3);
        else
            u_slave = [];
        end
%         [~, y_model] = Measurement_MESC_Ver2(ode_Ver3_EKF.x,y0,p0,d.nom,u_slave,ode_Ver3_EKF.p,par,N_Ver3,K_S);
        [~, y_model] = Measurement_MESC_Ver3(ode_Ver3_EKF.x,y0,p0,d.nom(1:5),ode_Ver3_EKF.p(n.u+1:end),u_slave,ode_Ver3_EKF.p,par,N_Ver3,K_S);

        % With Ver3.h_EKF we estimate the measurements
        Ver3.h_EKF = Function('h_EKF',{ode_Ver3_EKF.x,[ode_Ver3_EKF.p;u_slave]},{y_model});
        Ver3.JacHx = Function('JacHx',{ode_Ver3_EKF.x,[ode_Ver3_EKF.p;u_slave]},{jacobian(y_model,ode_Ver3_EKF.x)});

        % Using estimated states, cost function model to obtain linear model from u
        % to J
        Jacx = [ode_Ver3_EKF.x(1:N_Ver3); ode_Ver3_EKF.x(N_Ver3+1:N_Ver3+n.u); ode_Ver3_EKF.x(N_Ver3+n.u+1:N_Ver3*2+n.u)];
        Jacp = [ode_Ver3_EKF.p; ode_Ver3_EKF.x(end)];
%         Jacp = [ode_Ver3_EKF.p; ode_Ver3_EKF.x(end-n.d+1:end)];
        Ver3.JacAx = Function('JacAx',{Jacx,Jacp},{jacobian(ode_Ver3_EKF.ode(1:2*N_Ver3+n.u), Jacx)});
        Ver3.JacBu = Function('JacBu',{Jacx,Jacp},{jacobian(ode_Ver3_EKF.ode(1:2*N_Ver3+n.u), ode_Ver3_EKF.p(1:n.u))});
        Ver3.JacJx = Function('JacJx',{Jacx,Jacp},{jacobian(ode_Ver3_EKF.quad,Jacx)});
        Ver3.JacJu = Function('JacJu',{Jacx,Jacp},{jacobian(ode_Ver3_EKF.quad,ode_Ver3_EKF.p(1:n.u))});
        
        Ver3.cM.set = zeros(n.u,t.N+1);
        Ver3.err    = zeros(n.u,t.N+1);
        Ver3.errint = zeros(n.u,t.N+1);
        Ver3.ek.sim = zeros(n.y,t.N+1);
%         Ver3.xEstErr.sim = zeros(N_Ver3*2+n.u+n.d,t.N+1);
        Ver3.xEstErr.sim = zeros(N_Ver3*2+n.u+1,t.N+1);
%         Ver3.est_x.sim = [Ver3.x.sim; Ver3.z.sim(1:N_Ver3,:); zeros(n.d,t.N+1)];
        Ver3.est_x.sim = [Ver3.x.sim; Ver3.z.sim(1:N_Ver3,:); zeros(1,t.N+1)];

        % Initialization of various parameters for the EKF 
        Ver3.diag_EKF = speye(length(ode_Ver3_EKF.x));
        Ver3.Pk = 1.*Ver3.diag_EKF;           % Covarianzmatrix for estimation
        Ver3.Qk = 0.01*diag([Ver3.est_x.sim(1:30,1); [5 5 5]'; Ver3.est_x.sim(34:63,1); d.nom(end)]);   % Covarianzmatrix for process noise
        Ver3.Rk = 1.*eye(n.y);                % Covarianzmatrix for observation noise

    % Additional parameters for economic non-linear model predictive
    % control
    elseif strcmp(dec.Ver{3},'eNMPC')
        [Ver3.eNMPC,Ver3_reA] = eNMPC_Setup(t,t_eNMPC{3},eNMPC{3});
        Ver3.eNMPC.N_sub = N_Ver3+length(Ver3.eNMPC.ind_z);
    end
end

% Predefinition of the structures for the forth simulation
if ~isempty(dec.Ver{4})
    load([dirmain '\Data\Init_Opt.mat'])
    if dec.H(4) == 0
        [func_Ver4,dae_Ver4,~,~,~,n] = AmmonReac_NewDist(t.int);
        N_Ver4 = size(dae_Ver4.x,1);
    elseif dec.H(4) == 1
        [func_Ver4,dae_Ver4,~,~,~,n] = AmmonReac_NewDist(t.int,K_S);
        N_Ver4 = size(dae_Ver4.x,1)-n.u;
        x0 = [x0; zeros(n.u,1)];
        z0 = [z0; p0(1:n.u)];
    elseif dec.H(4) > 1
        [func_Ver4,dae_Ver4,~,~,~,n] = AmmonReac_NewDist(t.int,K_S,K_M,H,dec.H(4));
        N_Ver4 = size(dae_Ver4.x,1)-2*n.u;
        x0 = [x0; zeros(2*n.u,1)];
        z0 = [z0; p0(1:n.u); z0(2*N_Ver4+1:2*N_Ver4+n.u); H{dec.H(4)}*y0];
    end
    if strcmp(dec.Ver{4},'MESC')
        [func_Ver4_EKF,ode_Ver4_EKF,~,~,par] = AmmonReac_ODE_Ver3(t.int,K_S);
    end

    Ver4.x.sim = zeros(n.x,t.N+1);          Ver4.x.sim(:,1) = x0;
    Ver4.z.sim = zeros(n.z,t.N+1);          Ver4.z.sim(:,1) = z0;
    Ver4.y.sim = zeros(numel(y0),t.N+1);   	Ver4.y.sim(:,1) = y0; 
    Ver4.EoR.sim = zeros(1,t.N+1);
    Ver4.EoRint.sim = zeros(1,t.N+1);
    
    if dec.H > 0
        Ver4.cS.sim = zeros(n.u,t.N+1);         Ver4.cS.sim(:,1) = H{1}*y0;
        Ver4.cS.set = Ver4.cS.sim(:,1)*ones(1,t.N+1);

        Ver4.cM.sim = zeros(n.u,t.N+1);         Ver4.cM.sim(:,1) = H{dec.H(4)}*y0*~(strcmp(dec.Ver{4},'MESC'));
        Ver4.cM.set = Ver4.cM.sim(:,1)*ones(1,t.N+1);

        Ver4.p.sim = p.sim;                     Ver4.p.sim(1:n.u,:) = H{dec.H(4)}*y0*ones(1,t.N+1)+Ver4.p.sim(1:n.u,:);
    else
        Ver4.p.sim = p.sim;                     Ver4.p.sim(1:n.u,1) = p0(1:n.u);
    end
    
    % Additional parameters for least-square-based extremum seeking control
    if strcmp(dec.Ver{4},'ESC')
        Ver4.theta_est = zeros(n.u,t.N+1);
        Ver4.theta_int = zeros(n.u,t.N+1);
        Ver4.theta_inte = zeros(n.u,t.N+1);
        Ver4.del_u = zeros(n.u,t.N+1);
        if dec.H(4) > 1
            Ver4.Jest = Ver4.cM.sim;
        else
            Ver4.Jest = Ver4.cS.sim;
        end
        
    % Additional parameters for Model-based extremum seeking control
    elseif strcmp(dec.Ver{4},'MESC')
        % Configure EKF
        ddt_EKF = ode_Ver4_EKF.x + t.int*ode_Ver4_EKF.ode;

        % EKF System/linearizied System around current estimate JacFx / JacHx
        % Definition of the system states
        Ver4.f_EKF = Function('f_EKF',{ode_Ver4_EKF.x,ode_Ver4_EKF.p},{ddt_EKF},{'x','p'},{'xdot'});
        Ver4.JacFx = Function('JacFx',{ode_Ver4_EKF.x,ode_Ver4_EKF.p},{jacobian(ddt_EKF,ode_Ver4_EKF.x)});

        % Definition of model for measurements
        if dec.H(1) < 1
            u_slave = MX.sym('u_slave',3);
        else
            u_slave = [];
        end
%         [~, y_model] = Measurement_MESC_Ver2(ode_Ver4_EKF.x,y0,p0,d.nom,u_slave,ode_Ver4_EKF.p,par,N_Ver4,K_S);
        [~, y_model] = Measurement_MESC_Ver3(ode_Ver4_EKF.x,y0,p0,d.nom(1:5),ode_Ver4_EKF.p(n.u+1:end),u_slave,ode_Ver4_EKF.p,par,N_Ver4,K_S);

        % With Ver4.h_EKF we estimate the measurements
        Ver4.h_EKF = Function('h_EKF',{ode_Ver4_EKF.x,[ode_Ver4_EKF.p;u_slave]},{y_model});
        Ver4.JacHx = Function('JacHx',{ode_Ver4_EKF.x,[ode_Ver4_EKF.p;u_slave]},{jacobian(y_model,ode_Ver4_EKF.x)});

        % Using estimated states, cost function model to obtain linear model from u
        % to J
        Jacx = [ode_Ver4_EKF.x(1:N_Ver4); ode_Ver4_EKF.x(N_Ver4+1:N_Ver4+n.u); ode_Ver4_EKF.x(N_Ver4+n.u+1:N_Ver4*2+n.u)];
        Jacp = [ode_Ver4_EKF.p; ode_Ver4_EKF.x(end)];
%         Jacp = [ode_Ver4_EKF.p; ode_Ver4_EKF.x(end-n.d+1:end)];
        Ver4.JacAx = Function('JacAx',{Jacx,Jacp},{jacobian(ode_Ver4_EKF.ode(1:2*N_Ver4+n.u), Jacx)});
        Ver4.JacBu = Function('JacBu',{Jacx,Jacp},{jacobian(ode_Ver4_EKF.ode(1:2*N_Ver4+n.u), ode_Ver4_EKF.p(1:n.u))});
        Ver4.JacJx = Function('JacJx',{Jacx,Jacp},{jacobian(ode_Ver4_EKF.quad,Jacx)});
        Ver4.JacJu = Function('JacJu',{Jacx,Jacp},{jacobian(ode_Ver4_EKF.quad,ode_Ver4_EKF.p(1:n.u))});
        
        Ver4.cM.set = zeros(n.u,t.N+1);
        Ver4.err    = zeros(n.u,t.N+1);
        Ver4.errint = zeros(n.u,t.N+1);
        Ver4.ek.sim = zeros(n.y,t.N+1);
%         Ver4.xEstErr.sim = zeros(N_Ver4*2+n.u+n.d,t.N+1);
        Ver4.xEstErr.sim = zeros(N_Ver4*2+n.u+1,t.N+1);
%         Ver4.est_x.sim = [Ver4.x.sim; Ver4.z.sim(1:N_Ver4,:); zeros(n.d,t.N+1)];
        Ver4.est_x.sim = [Ver4.x.sim; Ver4.z.sim(1:N_Ver4,:); zeros(1,t.N+1)];

        % Initialization of various parameters for the EKF 
        Ver4.diag_EKF = speye(length(ode_Ver4_EKF.x));
        Ver4.Pk = 1.*Ver4.diag_EKF;           % Covarianzmatrix for estimation
        Ver4.Qk = 0.01*diag([Ver4.est_x.sim(1:30,1); [5 5 5]'; Ver4.est_x.sim(34:63,1); d.nom(end)]); % Covarianzmatrix for process noise
        Ver4.Rk = 1.*eye(n.y);                % Covarianzmatrix for observation noise

    % Additional parameters for economic non-linear model predictive
    % control
    elseif strcmp(dec.Ver{4},'eNMPC')
        [Ver4.eNMPC,Ver4_reA] = eNMPC_Setup(t,t_eNMPC{4},eNMPC{4});
        Ver4.eNMPC.N_sub = N_Ver4+length(Ver4.eNMPC.ind_z);
    end
end

% Startup of the waitbar
h = waitbar(0,'Integrating the system(s)');
time.in.sim = tic;

% Integration of the system.
for j = 2:t.N+1
    
    % Control strategy 1
    if ~isempty(dec.Ver{1})
        % Running of the integrator for the proposed time-step
        func_eval = func_Ver1('x0',Ver1.x.sim(:,j-1),'z0',Ver1.z.sim(:,j-1),'p',Ver1.p.sim(:,j-1));

        % Redefinition of the meassurements and the integrated error
        Ver1.x.sim(:,j) = full(func_eval.xf);
        Ver1.z.sim(:,j) = full(func_eval.zf);
        aux             = [Ver1.x.sim([1/3*N_Ver1 2/3*N_Ver1 N_Ver1],j);...
                           Ver1.z.sim(2*N_Ver1+1:2*N_Ver1+n.u+1,j)];
        Ver1.y.sim(:,j) = aux([7 4 1 5 2 6 3]);
        Ver1.EoR.sim(j) = full(func_eval.qf)/t.int;

        % Integrating the loss and rearrangement of the setpoints
        Ver1.EoRint.sim(j)  = full(func_eval.qf);
        if dec.H(1) == 1
            Ver1.cS.sim(:,j) = Ver1.z.sim(2*N_Ver1+1:2*N_Ver1+n.u,j);
        elseif dec.H(1) > 1
            Ver1.cS.sim(:,j) = Ver1.z.sim(2*N_Ver1+1:2*N_Ver1+n.u,j);
            Ver1.cS.set(:,j) = Ver1.z.sim(end-2*n.u+1:end-n.u,j);
            Ver1.cM.sim(:,j) = Ver1.z.sim(end-n.u+1:end,j);
        end

        % Extremum seeking control Algorithm
        if strcmp(dec.Ver{1},'ESC') 
            if dec.H(1) > 1
            	Ver1.Jest(:,j) = Ver1.cM.sim(:,j);
            else
            	Ver1.Jest(:,j) = Ver1.cS.sim(:,j);
            end
            Ver1.Jest(:,j) = Ver1.p.sim(1:n.u,j-1);
            
            % Definition of the number of points for gradient estimation
            Nw = min(j,ESC.t(1)/t.int);

            % Definition of the measurements used for ESC
            if dec.mnoise
               Ver1.EoR.sim(j) = Ver1.EoR.sim(j) + wgn(1,1,-25);
            end
            Y = -Ver1.EoR.sim(j-Nw+1:j)';
            Ys{j} = Y;

            % Calculation of the gradient
            phi = [Ver1.Jest(1:n.u,j-Nw+1:j)',ones(size(Y))]./(ones(size(Y))*[500 500 500 1]);
            Ver1.Varphi(:,j) = var(phi,0,1)';
            Ver1.check(:,j)  = ones(3,1);%*(mod(j,ESC.step(1)) == 0);~(Ver1.Varphi(1:n.u,j)./(H{dec.H(1)}*y0) > 2e-3);

            if (mod(j,ESC.step(1)) == 0)
                k = j;
            end
            
            if any(Ver1.Varphi(1:n.u,j)./(H{dec.H(1)}*y0) > 5e-8)
                % Incorporation of the gradient
                Ver1.del_u(:,j) = Ver1.del_u(:,j-1);
            else
                % Calculation of the gradient
                theta = (phi'*phi)\(phi'*Y);
                Ver1.theta_est(:,j) = theta(1:n.u)./[500 500 500]';
                
                % Print of the gradient
                if mod(j,100) == 0
                    disp('Version 1:')
                    disp(['ESC Active - estimated gradient 1 = ' num2str(Ver1.theta_est(1,j))])
                    disp(['ESC Active - estimated gradient 2 = ' num2str(Ver1.theta_est(2,j))])
                    disp(['ESC Active - estimated gradient 3 = ' num2str(Ver1.theta_est(3,j))])
                    fprintf('\n')
                end
                % Calculation of the change in u
                if j > ESC.t(1)/t.int+1 + 3600/t.int
                    Ver1.theta_int(:,j) = (Ver1.theta_est(:,j)+Ver1.theta_est(:,j-1))/2*t.int;
                    Ver1.theta_inte(:,j) = Ver1.theta_inte(:,j-1) + Ver1.check(:,j).*K_ESC.I(:,1).*sign(Ver1.theta_int(:,j)).*min(abs(Ver1.theta_int(:,j)),K_ESC.bound(:,1)); 
                    Ver1.del_u(:,j) = Ver1.theta_inte(:,k);
                end
            end

            % Update the input for the master controller from the output from ESC 
            Ver1.p.sim(1:n.u,j) = Ver1.p.sim(1:n.u,j) + Ver1.del_u(:,j) + ...
                                  ESC.mul(:,1).*(sin(2*pi*t.int*(1./ESC.sin(:,1))*j));
            
        elseif strcmp(dec.Ver{1},'MESC')
             % EKF    
%              Fj = full(Ver1.JacFx(Ver1.est_x.sim(:,j-1),Ver1.p.sim(:,j-1)));
             Fj = full(Ver1.JacFx(Ver1.est_x.sim(:,j-1),Ver1.p.sim(1:end-1,j-1)));
             if (isnan(Fj))
                 disp('NaN1 in x EKF')
             end

             %Prediction 
             xk_hat_1 =  full(Ver1.f_EKF(Ver1.est_x.sim(:,j-1),Ver1.p.sim(1:end-1,j-1)));
             %Using model for prediction of xk_1 (more exakt but slower)
%              func_xk_hat_1 = func_Ver1_EKF('x0',Ver1.est_x.sim(:,j-1),'p',Ver1.p.sim(1:n.u,j-1));
%              xk_hat_1 = full(func_xk_hat_1.xf) ;
             Pk_1 = Fj*Ver1.Pk*Fj' + Ver1.Qk;

             %Update
             Ver1.y.noise(:,j) = Ver1.y.sim(:,j)+0.2*dec.mnoise*wgn(n.y,1,0);
             Hj = full(Ver1.JacHx(xk_hat_1,[Ver1.p.sim(1:end-1,j-1)]));
             ek = full(Ver1.y.noise(:,j) -...
                        Ver1.h_EKF(xk_hat_1,[Ver1.p.sim(1:end-1,j-1)]));
             Sk = Hj*Pk_1*Hj' + Ver1.Rk;
             Kk = (Pk_1*Hj')/(Sk);
             Ver1.ek.sim(:,j) = ek;

             Ver1.Pk = (Ver1.diag_EKF - Kk*Hj)*Pk_1;
             Ver1.est_x.sim(:,j) = xk_hat_1 + Kk*ek; 
             
             if isnan(Ver1.est_x.sim(:,j))
                 disp('NaN2 in y EKF')
             end
%              Ver1.xEstErr.sim(:,j) = abs(Ver1.est_x.sim(:,j) - [Ver1.x.sim(:,j); Ver1.z.sim(1:N_Ver1,j); Ver1.p.sim(n.u+1:end,j)]);
             Ver1.xEstErr.sim(:,j) = abs(Ver1.est_x.sim(:,j) - [Ver1.x.sim(:,j); Ver1.z.sim(1:N_Ver1,j); Ver1.p.sim(end,j)]);

             % Calculation of the state matrices
%              A = full(Ver1.JacAx(Ver1.est_x.sim(1:end-n.d,j),[Ver1.p.sim(1:n.u,j-1); Ver1.est_x.sim(end-n.d+1:end,j)]));
%              B = full(Ver1.JacBu(Ver1.est_x.sim(1:end-n.d,j),[Ver1.p.sim(1:n.u,j-1); Ver1.est_x.sim(end-n.d+1:end,j)]));
%              C = full(Ver1.JacJx(Ver1.est_x.sim(1:end-n.d,j),[Ver1.p.sim(1:n.u,j-1); Ver1.est_x.sim(end-n.d+1:end,j)]));
%              D = full(Ver1.JacJu(Ver1.est_x.sim(1:end-n.d,j),[Ver1.p.sim(1:n.u,j-1); Ver1.est_x.sim(end-n.d+1:end,j)]));
             A = full(Ver1.JacAx(Ver1.est_x.sim(1:end-1,j),[Ver1.p.sim(1:end-1,j-1); Ver1.est_x.sim(end,j)]));
             B = full(Ver1.JacBu(Ver1.est_x.sim(1:end-1,j),[Ver1.p.sim(1:end-1,j-1); Ver1.est_x.sim(end,j)]));
             C = full(Ver1.JacJx(Ver1.est_x.sim(1:end-1,j),[Ver1.p.sim(1:end-1,j-1); Ver1.est_x.sim(end,j)]));
             D = full(Ver1.JacJu(Ver1.est_x.sim(1:end-1,j),[Ver1.p.sim(1:end-1,j-1); Ver1.est_x.sim(end,j)]));

            % Integrating the loss and rearrangement of the setpoints
            Ver1.cM.sim(:,j)    = (-C*(A\B)+D)';

            % Calculation of the error and the integrated error
            Ver1.err(:,j)    = Ver1.cM.set(:,j)-Ver1.cM.sim(:,j);
            Ver1.errint(:,j) = Ver1.errint(:,j-1) + mean(Ver1.err(:,j-1:j),2)*t.int;

            % Calculation of the controller action
            Ver1.p.sim(1:3,j) = y0(2:2:end) + (K_MESC.P(:,1).*Ver1.err(:,j) + K_MESC.I(:,1).*Ver1.errint(:,j));
            
        elseif strcmp(dec.Ver{1},'eNMPC')
            if rem(j-2,t_eNMPC{1}.mul) == 0
                % Redefinition of the initial guess and the starting values of the nMPC
                % solver
                w0 = full(Ver1.eNMPC.it{Ver1.eNMPC.l}.comb.x);
                Ver1.eNMPC.lbw(1:Ver1.eNMPC.N_sub) = [Ver1.x.sim(:,j-1); Ver1.z.sim(Ver1.eNMPC.ind_z,j-1)];
                Ver1.eNMPC.ubw(1:Ver1.eNMPC.N_sub) = [Ver1.x.sim(:,j-1); Ver1.z.sim(Ver1.eNMPC.ind_z,j-1)];
                if j == 2
                    Ver1.eNMPC.lbw(Ver1_reA.uM(end,:))  =  p0(1:n.u);
                    Ver1.eNMPC.ubw(Ver1_reA.uM(end,:))  =  p0(1:n.u);
                    Ver1.eNMPC.nlp.f = substitute(Ver1.eNMPC.J,Ver1.eNMPC.U_prev_k{1},p0(1:n.u));
                else
                    Ver1.eNMPC.lbw(Ver1_reA.uM(end,:))  =  Ver1.p.sim(1:n.u,j-2);
                    Ver1.eNMPC.ubw(Ver1_reA.uM(end,:))  =  Ver1.p.sim(1:n.u,j-2);
                    Ver1.eNMPC.nlp.f = substitute(Ver1.eNMPC.J,Ver1.eNMPC.U_prev_k{1},Ver1.p.sim(1:n.u,j-2));
                end

                % Substitution of the inlet values and introduction of noise
                Ver1.eNMPC.nlp.g = substitute(Ver1.eNMPC.g_save,Ver1.eNMPC.dae.p(n.u+1:end),Ver1.p.sim(n.u+1:end,j));
                Ver1.eNMPC.nlp.f = substitute(Ver1.eNMPC.nlp.f, Ver1.eNMPC.dae.p(n.u+1:end),Ver1.p.sim(n.u+1:end,j));
                solver = nlpsol('solver','ipopt',Ver1.eNMPC.nlp);

                % Solving of this timeframe
                time.in.NMPC = tic;
                Ver1.eNMPC.it{Ver1.eNMPC.l+1}.comb = ...
                solver('x0',w0,'lbx',Ver1.eNMPC.lbw,'ubx',Ver1.eNMPC.ubw,...
                               'lbg',Ver1.eNMPC.lbg,'ubg',Ver1.eNMPC.ubg);
                time.NMPC(Ver1.eNMPC.l) = toc(time.in.NMPC);

                % Reassignment of the solution values
                Ver1.eNMPC.it{Ver1.eNMPC.l+1}.x = full(Ver1.eNMPC.it{Ver1.eNMPC.l+1}.comb.x(Ver1_reA.xM));
                Ver1.eNMPC.it{Ver1.eNMPC.l+1}.z = full(Ver1.eNMPC.it{Ver1.eNMPC.l+1}.comb.x(Ver1_reA.zM));
                Ver1.eNMPC.it{Ver1.eNMPC.l+1}.u = full(Ver1.eNMPC.it{Ver1.eNMPC.l+1}.comb.x(Ver1_reA.uM));
                Ver1.eNMPC.it{Ver1.eNMPC.l+1}.f = full(Ver1.eNMPC.it{Ver1.eNMPC.l+1}.comb.f);
                Ver1.p.sim(1:n.u,j) = Ver1.eNMPC.it{Ver1.eNMPC.l+1}.u(1,:);

                % Increment of Ver1.eNMPC_l
                Ver1.eNMPC.l = Ver1.eNMPC.l+1;

            else
                Ver1.p.sim(1:n.u,j) = Ver1.p.sim(1:n.u,j-1);
            end
        end
        
        % Rearrangement of the setpoints
        if dec.H(1) == 1
            Ver1.cS.set(:,j) = Ver1.p.sim(1:n.u,j);
        else
            Ver1.cM.set(:,j) = Ver1.p.sim(1:n.u,j);
        end
    end
    
    % Control strategy 2
    if ~isempty(dec.Ver{2})
        
        % Running of the integrator for the proposed time-step
        func_eval = func_Ver2('x0',Ver2.x.sim(:,j-1),'z0',Ver2.z.sim(:,j-1),'p',Ver2.p.sim(:,j-1));

        % Redefinition of the meassurements and the integrated error
        Ver2.x.sim(:,j) = full(func_eval.xf);
        Ver2.z.sim(:,j) = full(func_eval.zf);
        aux             = [Ver2.x.sim([1/3*N_Ver2 2/3*N_Ver2 N_Ver2],j);...
                           Ver2.z.sim(2*N_Ver2+1:2*N_Ver2+n.u+1,j)];
        Ver2.y.sim(:,j) = aux([7 4 1 5 2 6 3]);
        Ver2.EoR.sim(j) = full(func_eval.qf)/t.int;

        % Integrating the loss and rearrangement of the setpoints
        Ver2.EoRint.sim(j)  = full(func_eval.qf);
        if dec.H(2) == 1
            Ver2.cS.sim(:,j) = Ver2.z.sim(2*N_Ver2+1:2*N_Ver2+n.u,j);
        elseif dec.H(2) > 1
            Ver2.cS.sim(:,j) = Ver2.z.sim(2*N_Ver2+1:2*N_Ver2+n.u,j);
            Ver2.cS.set(:,j) = Ver2.z.sim(end-2*n.u+1:end-n.u,j);
            Ver2.cM.sim(:,j) = Ver2.z.sim(end-n.u+1:end,j);
        end

        % Extremum seeking control Algorithm
        if strcmp(dec.Ver{2},'ESC') 
            if dec.H(2) > 1
            	Ver2.Jest(:,j) = Ver2.cM.sim(:,j);
            else
            	Ver2.Jest(:,j) = Ver2.cS.sim(:,j);
            end
            Ver2.Jest(:,j) = Ver2.p.sim(1:n.u,j-1);
            % Definition of the number of points for gradient estimation
            Nw = min(j,ESC.t(2)/t.int);

            % Definition of the measurements used for ESC
            if dec.mnoise
               Ver2.EoR.sim(j) = Ver2.EoR.sim(j) + wgn(1,1,-25);
            end
            Y = -Ver2.EoR.sim(j-Nw+1:j)';

            % Calculation of the gradient
            phi = [Ver2.Jest(1:n.u,j-Nw+1:j)',ones(size(Y))]./(ones(size(Y))*[500 500 500 1]);
            Ver2.Varphi(:,j) = var(phi,0,1)';
            Ver2.check(:,j)  = ones(3,1);%*(mod(j,ESC.step(2)) == 0);~(Ver2.Varphi(1:n.u,j)./(H{dec.H(2)}*y0) > 2e-3);

            if (mod(j,ESC.step(2)) == 0)
                k = j;
            end
            
            if any(Ver2.Varphi(1:n.u,j)./(H{dec.H(2)}*y0) > 5e-8)
                % Incorporation of the gradient
                Ver2.del_u(:,j) = Ver2.del_u(:,j-1);
            else
                % Calculation of the gradient
                theta = (phi'*phi)\(phi'*Y);
                Ver2.theta_est(:,j) = theta(1:n.u)./[500 500 500]';
                
                % Print of the gradient
                if mod(j,100) == 0
                    disp('Version 2:')
                    disp(['ESC Active - estimated gradient 1 = ' num2str(Ver2.theta_est(1,j))])
                    disp(['ESC Active - estimated gradient 2 = ' num2str(Ver2.theta_est(2,j))])
                    disp(['ESC Active - estimated gradient 3 = ' num2str(Ver2.theta_est(3,j))])
                    fprintf('\n')
                end
                % Calculation of the change in u
                if j > ESC.t(2)/t.int+1 + 3600/t.int
                    Ver2.theta_int(:,j) = (Ver2.theta_est(:,j)+Ver2.theta_est(:,j-1))/2*t.int;
                    Ver2.theta_inte(:,j) = Ver2.theta_inte(:,j-1) + Ver2.check(:,j).*K_ESC.I(:,2).*sign(Ver2.theta_int(:,j)).*min(abs(Ver2.theta_int(:,j)),K_ESC.bound(:,2)); 
                    Ver2.del_u(:,j) = Ver2.theta_inte(:,k);
                end
            end

            % Update the input for the master controller from the output from ESC 
            Ver2.p.sim(1:n.u,j) = Ver2.p.sim(1:n.u,j) + Ver2.del_u(:,j) + ...
                                  ESC.mul(:,2).*(sin(2*pi*t.int*(1./ESC.sin(:,2))*j));
            
        elseif strcmp(dec.Ver{2},'MESC')
             % EKF    
%              Fj = full(Ver2.JacFx(Ver2.est_x.sim(:,max(1,j-1)),Ver2.p.sim(:,max(1,j-1))));
             Fj = full(Ver2.JacFx(Ver2.est_x.sim(:,max(1,j-1)),Ver2.p.sim(1:end-1,max(1,j-1))));
             if (isnan(Fj))
                 disp('NaN1 in x EKF')
             end

             %Prediction 
%              xk_hat_1 =  full(Ver2.f_EKF(Ver2.est_x.sim(:,j-1),Ver2.p.sim(:,j-1)));
             xk_hat_1 =  full(Ver2.f_EKF(Ver2.est_x.sim(:,j-1),Ver2.p.sim(1:end-1,j-1)));
             %Using model for prediction of xk_1 (more exakt but slower)
%              func_xk_hat_1 = func_Ver2_EKF('x0',Ver2.est_x.sim(:,j-1),'p',Ver2.p.sim(:,j-1));
%              xk_hat_1 = full(func_xk_hat_1.xf) ;
             Pk_1 = Fj*Ver2.Pk*Fj' + Ver2.Qk;

             %Update
%              Hj = full(Ver2.JacHx(xk_hat_1,Ver2.p.sim(:,max(1,j-1))));
%              ek = full(Ver2.y.sim(:,j) - Ver2.h_EKF(xk_hat_1,Ver2.p.sim(:,j-1)));
             Hj = full(Ver2.JacHx(xk_hat_1,Ver2.p.sim(1:end-1,max(1,j-1))));
             ek = full(Ver2.y.sim(:,j) - Ver2.h_EKF(xk_hat_1,Ver2.p.sim(1:end-1,j-1)));
             Sk = Hj*Pk_1*Hj' + Ver2.Rk;
             Kk = (Pk_1*Hj')/(Sk);
             Ver2.ek.sim(:,j-1) = ek;

             Ver2.Pk = (Ver2.diag_EKF - Kk*Hj)*Pk_1;
             Ver2.est_x.sim(:,j) = xk_hat_1 + Kk*ek; 
             
             if isnan(Ver2.est_x.sim(:,j))
                 disp('NaN2 in y EKF')
             end
%              Ver2.xEstErr.sim(:,j) = abs(Ver2.est_x.sim(:,j) - [Ver2.x.sim(:,j); Ver2.z.sim(1:N_Ver2,j)]);
             Ver2.xEstErr.sim(:,j) = abs(Ver2.est_x.sim(:,j) - [Ver2.x.sim(:,j); Ver2.z.sim(1:N_Ver2,j); Ver2.p.sim(end,j)]);

             % Calculation of the state matrices
%              A = full(Ver2.JacAx(Ver2.est_x.sim(:,j),Ver2.p.sim(:,j-1)));
%              B = full(Ver2.JacBu(Ver2.est_x.sim(:,j),Ver2.p.sim(:,j-1)));
%              C = full(Ver2.JacJx(Ver2.est_x.sim(:,j),Ver2.p.sim(:,j-1)));
%              D = full(Ver2.JacJu(Ver2.est_x.sim(:,j),Ver2.p.sim(:,j-1)));
             A = full(Ver2.JacAx(Ver2.est_x.sim(1:end-1,j),[Ver2.p.sim(1:end-1,j-1); Ver2.est_x.sim(end,j)]));
             B = full(Ver2.JacBu(Ver2.est_x.sim(1:end-1,j),[Ver2.p.sim(1:end-1,j-1); Ver2.est_x.sim(end,j)]));
             C = full(Ver2.JacJx(Ver2.est_x.sim(1:end-1,j),[Ver2.p.sim(1:end-1,j-1); Ver2.est_x.sim(end,j)]));
             D = full(Ver2.JacJu(Ver2.est_x.sim(1:end-1,j),[Ver2.p.sim(1:end-1,j-1); Ver2.est_x.sim(end,j)]));

            % Integrating the loss and rearrangement of the setpoints
            Ver2.EoRint.sim(j)  = t.int*(Ver2.EoR.sim(j)+Ver2.EoR.sim(j-1))/2;
            Ver2.cM.sim(:,j)    = (-C*(A\B) + D)';

            % Calculation of the error and the integrated error
            Ver2.err(:,j)    = Ver2.cM.set(:,j)-Ver2.cM.sim(:,j);
            Ver2.errint(:,j) = Ver2.errint(:,j-1) + mean(Ver2.err(:,j-1:j),2)*t.int;

            % Calculation of the controller action
            Ver2.p.sim(1:3,j) = y0(2:2:end) + (K_MESC.P(:,2).*Ver2.err(:,j) + K_MESC.I(:,2).*Ver2.errint(:,j));
            
        elseif strcmp(dec.Ver{2},'eNMPC')
            if rem(j-2,t_eNMPC{2}.mul) == 0
                % Redefinition of the initial guess and the starting values of the nMPC
                % solver
                w0 = full(Ver2.eNMPC.it{Ver2.eNMPC.l}.comb.x);
                Ver2.eNMPC.lbw(1:Ver2.eNMPC.N_sub) = [Ver2.x.sim(:,j-1); Ver2.z.sim(Ver2.eNMPC.ind_z,j-1)];
                Ver2.eNMPC.ubw(1:Ver2.eNMPC.N_sub) = [Ver2.x.sim(:,j-1); Ver2.z.sim(Ver2.eNMPC.ind_z,j-1)];
                if j == 2
                    Ver2.eNMPC.lbw(Ver2_reA.uM(end,:))  =  p0(1:n.u);
                    Ver2.eNMPC.ubw(Ver2_reA.uM(end,:))  =  p0(1:n.u);
                    Ver2.eNMPC.nlp.f = substitute(Ver2.eNMPC.J,Ver2.eNMPC.U_prev_k{1},p0(1:n.u));
                else
                    Ver2.eNMPC.lbw(Ver2_reA.uM(end,:))  =  Ver2.p.sim(1:n.u,j-2);
                    Ver2.eNMPC.ubw(Ver2_reA.uM(end,:))  =  Ver2.p.sim(1:n.u,j-2);
                    Ver2.eNMPC.nlp.f = substitute(Ver2.eNMPC.J,Ver2.eNMPC.U_prev_k{1},Ver2.p.sim(1:n.u,j-2));
                end

                % Substitution of the inlet values and introduction of noise
                Ver2.eNMPC.nlp.g = substitute(Ver2.eNMPC.g_save,Ver2.eNMPC.dae.p(n.u+1:end),Ver2.p.sim(n.u+1:end,j));
                Ver2.eNMPC.nlp.f = substitute(Ver2.eNMPC.nlp.f, Ver2.eNMPC.dae.p(n.u+1:end),Ver2.p.sim(n.u+1:end,j));
                solver = nlpsol('solver','ipopt',Ver2.eNMPC.nlp);

                % Solving of this timeframe
                time.in.NMPC = tic;
                Ver2.eNMPC.it{Ver2.eNMPC.l+1}.comb = ...
                solver('x0',w0,'lbx',Ver2.eNMPC.lbw,'ubx',Ver2.eNMPC.ubw,...
                               'lbg',Ver2.eNMPC.lbg,'ubg',Ver2.eNMPC.ubg);
                time.NMPC(Ver2.eNMPC.l) = toc(time.in.NMPC);

                % Reassignment of the solution values
                Ver2.eNMPC.it{Ver2.eNMPC.l+1}.x = full(Ver2.eNMPC.it{Ver2.eNMPC.l+1}.comb.x(Ver2_reA.xM));
                Ver2.eNMPC.it{Ver2.eNMPC.l+1}.z = full(Ver2.eNMPC.it{Ver2.eNMPC.l+1}.comb.x(Ver2_reA.zM));
                Ver2.eNMPC.it{Ver2.eNMPC.l+1}.u = full(Ver2.eNMPC.it{Ver2.eNMPC.l+1}.comb.x(Ver2_reA.uM));
                Ver2.eNMPC.it{Ver2.eNMPC.l+1}.f = full(Ver2.eNMPC.it{Ver2.eNMPC.l+1}.comb.f);
                Ver2.p.sim(1:n.u,j) = Ver2.eNMPC.it{Ver2.eNMPC.l+1}.u(1,:);

                % Increment of Ver2.eNMPC_l
                Ver2.eNMPC.l = Ver2.eNMPC.l+1;

            else
                Ver2.p.sim(1:n.u,j) = Ver2.p.sim(1:n.u,j-1);
            end
        end

        % Rearrangement of the setpoints
        if dec.H(2) == 1
            Ver2.cS.set(:,j) = Ver2.p.sim(1:n.u,j);
        else
            Ver2.cM.set(:,j) = Ver2.p.sim(1:n.u,j);
        end
    end

    % Control strategy 3
    if ~isempty(dec.Ver{3})
        % Running of the integrator for the proposed time-step
        func_eval = func_Ver3('x0',Ver3.x.sim(:,j-1),'z0',Ver3.z.sim(:,j-1),'p',Ver3.p.sim(:,j-1));

        % Redefinition of the meassurements and the integrated error
        Ver3.x.sim(:,j) = full(func_eval.xf);
        Ver3.z.sim(:,j) = full(func_eval.zf);
        aux             = [Ver3.x.sim([1/3*N_Ver3 2/3*N_Ver3 N_Ver3],j);...
                           Ver3.z.sim(2*N_Ver3+1:2*N_Ver3+n.u+1,j)];
        Ver3.y.sim(:,j) = aux([7 4 1 5 2 6 3]);
        Ver3.EoR.sim(j) = full(func_eval.qf)/t.int;
        
        % Integrating the loss and rearrangement of the setpoints
        Ver3.EoRint.sim(j)  = full(func_eval.qf);
        if dec.H(3) == 1
            Ver3.cS.sim(:,j) = Ver3.z.sim(2*N_Ver3+1:2*N_Ver3+n.u,j);
        elseif dec.H(3) > 1
            Ver3.cS.sim(:,j) = Ver3.z.sim(2*N_Ver3+1:2*N_Ver3+n.u,j);
            Ver3.cS.set(:,j) = Ver3.z.sim(end-2*n.u+1:end-n.u,j);
            Ver3.cM.sim(:,j) = Ver3.z.sim(end-n.u+1:end,j);
        end

        % Extremum seeking control Algorithm
        if strcmp(dec.Ver{3},'ESC') 
            if dec.H(3) > 1
            	Ver3.Jest(:,j) = Ver3.cM.sim(:,j);
            else
            	Ver3.Jest(:,j) = Ver3.cS.sim(:,j);
            end
            Ver3.Jest(:,j) = Ver3.p.sim(1:n.u,j-1);
            % Definition of the number of points for gradient estimation
            Nw = min(j,ESC.t(3)/t.int);

            % Definition of the measurements used for ESC
            if dec.mnoise
               Ver3.EoR.sim(j) = Ver3.EoR.sim(j) + wgn(1,1,-25);
            end
            Y = -Ver3.EoR.sim(j-Nw+1:j)';

            % Calculation of the gradient
            phi = [Ver3.Jest(1:n.u,j-Nw+1:j)',ones(size(Y))]./(ones(size(Y))*[500 500 500 1]);
            Ver3.Varphi(:,j) = var(phi,0,1)';
            Ver3.check(:,j)  = ones(3,1);%*(mod(j,ESC.step(3)) == 0);~(Ver3.Varphi(1:n.u,j)./(H{dec.H(3)}*y0) > 2e-3);

            if (mod(j,ESC.step(3)) == 0)
                k = j;
            end
            
%             if any(Ver3.Varphi(1:n.u,j)./(H{dec.H(3)}*y0) > 5e-3)
%                 % Incorporation of the gradient
% 
%             else
                % Calculation of the gradient
                theta = (phi'*phi)\(phi'*Y);
                Ver3.theta_est(:,j) = theta(1:n.u)./[500 500 500]';
                
                % Print of the gradient
                if mod(j,100) == 0
                    disp('Version 3:')
                    disp(['ESC Active - estimated gradient 1 = ' num2str(Ver3.theta_est(1,j))])
                    disp(['ESC Active - estimated gradient 2 = ' num2str(Ver3.theta_est(2,j))])
                    disp(['ESC Active - estimated gradient 3 = ' num2str(Ver3.theta_est(3,j))])
                    fprintf('\n')
                end
                % Calculation of the change in u
                if j > ESC.t(3)/t.int+1 + 3600/t.int
                    Ver3.theta_int(:,j) = (Ver3.theta_est(:,j)+Ver3.theta_est(:,j-1))/2*t.int;
                    Ver3.theta_inte(:,j) = Ver3.theta_inte(:,j-1) + Ver3.check(:,j).*K_ESC.I(:,3).*sign(Ver3.theta_int(:,j)).*min(abs(Ver3.theta_int(:,j)),K_ESC.bound(:,3)); 
                    Ver3.del_u(:,j) = Ver3.theta_inte(:,k);
                end
%             end

            % Update the input for the master controller from the output from ESC 
            Ver3.p.sim(1:n.u,j) = Ver3.p.sim(1:n.u,j) + Ver3.del_u(:,j) + ...
                                  ESC.mul(:,3).*(sin(2*pi*t.int*(1./ESC.sin(:,3))*j));
            
        elseif strcmp(dec.Ver{3},'MESC')
             % EKF    
%              Fj = full(Ver3.JacFx(Ver3.est_x.sim(:,j-1),Ver3.p.sim(:,j-1)));
             Fj = full(Ver3.JacFx(Ver3.est_x.sim(:,j-1),Ver3.p.sim(1:end-1,j-1)));
             if (isnan(Fj))
                 disp('NaN1 in x EKF')
             end

             %Prediction 
             xk_hat_1 =  full(Ver3.f_EKF(Ver3.est_x.sim(:,j-1),Ver3.p.sim(1:end-1,j-1)));
             %Using model for prediction of xk_1 (more exakt but slower)
%              func_xk_hat_1 = func_Ver3_EKF('x0',Ver3.est_x.sim(:,j-1),'p',Ver3.p.sim(1:n.u,j-1));
%              xk_hat_1 = full(func_xk_hat_1.xf) ;
             Pk_1 = Fj*Ver3.Pk*Fj' + Ver3.Qk;

             %Update
             Ver3.y.noise(:,j) = Ver3.y.sim(:,j)+0.2*dec.mnoise*wgn(n.y,1,0);
             Hj = full(Ver3.JacHx(xk_hat_1,[Ver3.p.sim(1:end-1,j-1)]));
             ek = full(Ver3.y.noise(:,j) -...
                        Ver3.h_EKF(xk_hat_1,[Ver3.p.sim(1:end-1,j-1)]));
             Sk = Hj*Pk_1*Hj' + Ver3.Rk;
             Kk = (Pk_1*Hj')/(Sk);
             Ver3.ek.sim(:,j) = ek;

             Ver3.Pk = (Ver3.diag_EKF - Kk*Hj)*Pk_1;
             Ver3.est_x.sim(:,j) = xk_hat_1 + Kk*ek; 
             
             if isnan(Ver3.est_x.sim(:,j))
                 disp('NaN2 in y EKF')
             end
%              Ver3.xEstErr.sim(:,j) = abs(Ver3.est_x.sim(:,j) - [Ver3.x.sim(:,j); Ver3.z.sim(1:N_Ver3,j); Ver3.p.sim(n.u+1:end,j)]);
             Ver3.xEstErr.sim(:,j) = abs(Ver3.est_x.sim(:,j) - [Ver3.x.sim(:,j); Ver3.z.sim(1:N_Ver3,j); Ver3.p.sim(end,j)]);

             % Calculation of the state matrices
%              A = full(Ver3.JacAx(Ver3.est_x.sim(1:end-n.d,j),[Ver3.p.sim(1:n.u,j-1); Ver3.est_x.sim(end-n.d+1:end,j)]));
%              B = full(Ver3.JacBu(Ver3.est_x.sim(1:end-n.d,j),[Ver3.p.sim(1:n.u,j-1); Ver3.est_x.sim(end-n.d+1:end,j)]));
%              C = full(Ver3.JacJx(Ver3.est_x.sim(1:end-n.d,j),[Ver3.p.sim(1:n.u,j-1); Ver3.est_x.sim(end-n.d+1:end,j)]));
%              D = full(Ver3.JacJu(Ver3.est_x.sim(1:end-n.d,j),[Ver3.p.sim(1:n.u,j-1); Ver3.est_x.sim(end-n.d+1:end,j)]));
             A = full(Ver3.JacAx(Ver3.est_x.sim(1:end-1,j),[Ver3.p.sim(1:end-1,j-1); Ver3.est_x.sim(end,j)]));
             B = full(Ver3.JacBu(Ver3.est_x.sim(1:end-1,j),[Ver3.p.sim(1:end-1,j-1); Ver3.est_x.sim(end,j)]));
             C = full(Ver3.JacJx(Ver3.est_x.sim(1:end-1,j),[Ver3.p.sim(1:end-1,j-1); Ver3.est_x.sim(end,j)]));
             D = full(Ver3.JacJu(Ver3.est_x.sim(1:end-1,j),[Ver3.p.sim(1:end-1,j-1); Ver3.est_x.sim(end,j)]));

            % Integrating the loss and rearrangement of the setpoints
            Ver3.cM.sim(:,j)    = (-C*(A\B)+D)';

            % Calculation of the error and the integrated error
            Ver3.err(:,j)    = Ver3.cM.set(:,j)-Ver3.cM.sim(:,j);
            Ver3.errint(:,j) = Ver3.errint(:,j-1) + mean(Ver3.err(:,j-1:j),2)*t.int;

            % Calculation of the controller action
            Ver3.p.sim(1:3,j) = y0(2:2:end) + (K_MESC.P(:,1).*Ver3.err(:,j) + K_MESC.I(:,1).*Ver3.errint(:,j));
                        
        elseif strcmp(dec.Ver{3},'eNMPC')
            if rem(j-2,t_eNMPC{3}.mul) == 0
                % Redefinition of the initial guess and the starting values of the nMPC
                % solver
                w0 = full(Ver3.eNMPC.it{Ver3.eNMPC.l}.comb.x);
                Ver3.eNMPC.lbw(1:Ver3.eNMPC.N_sub) = [Ver3.x.sim(:,j-1); Ver3.z.sim(Ver3.eNMPC.ind_z,j-1)];
                Ver3.eNMPC.ubw(1:Ver3.eNMPC.N_sub) = [Ver3.x.sim(:,j-1); Ver3.z.sim(Ver3.eNMPC.ind_z,j-1)];
                if j == 2
                    Ver3.eNMPC.lbw(Ver3_reA.uM(end,:))  =  p0(1:n.u);
                    Ver3.eNMPC.ubw(Ver3_reA.uM(end,:))  =  p0(1:n.u);
                    Ver3.eNMPC.nlp.f = substitute(Ver3.eNMPC.J,Ver3.eNMPC.U_prev_k{1},p0(1:n.u));
                else
                    Ver3.eNMPC.lbw(Ver3_reA.uM(end,:))  =  Ver3.p.sim(1:n.u,j-2);
                    Ver3.eNMPC.ubw(Ver3_reA.uM(end,:))  =  Ver3.p.sim(1:n.u,j-2);
                    Ver3.eNMPC.nlp.f = substitute(Ver3.eNMPC.J,Ver3.eNMPC.U_prev_k{1},Ver3.p.sim(1:n.u,j-2));
                end

                % Substitution of the inlet values and introduction of noise
                Ver3.eNMPC.nlp.g = substitute(Ver3.eNMPC.g_save,Ver3.eNMPC.dae.p(n.u+1:end),Ver3.p.sim(n.u+1:end,j));
                Ver3.eNMPC.nlp.f = substitute(Ver3.eNMPC.nlp.f, Ver3.eNMPC.dae.p(n.u+1:end),Ver3.p.sim(n.u+1:end,j));
                solver = nlpsol('solver','ipopt',Ver3.eNMPC.nlp);

                % Solving of this timeframe
                time.in.NMPC = tic;
                Ver3.eNMPC.it{Ver3.eNMPC.l+1}.comb = ...
                solver('x0',w0,'lbx',Ver3.eNMPC.lbw,'ubx',Ver3.eNMPC.ubw,...
                               'lbg',Ver3.eNMPC.lbg,'ubg',Ver3.eNMPC.ubg);
                time.NMPC(Ver3.eNMPC.l) = toc(time.in.NMPC);

                % Reassignment of the solution values
                Ver3.eNMPC.it{Ver3.eNMPC.l+1}.x = full(Ver3.eNMPC.it{Ver3.eNMPC.l+1}.comb.x(Ver3_reA.xM));
                Ver3.eNMPC.it{Ver3.eNMPC.l+1}.z = full(Ver3.eNMPC.it{Ver3.eNMPC.l+1}.comb.x(Ver3_reA.zM));
                Ver3.eNMPC.it{Ver3.eNMPC.l+1}.u = full(Ver3.eNMPC.it{Ver3.eNMPC.l+1}.comb.x(Ver3_reA.uM));
                Ver3.eNMPC.it{Ver3.eNMPC.l+1}.f = full(Ver3.eNMPC.it{Ver3.eNMPC.l+1}.comb.f);
                Ver3.p.sim(1:n.u,j) = Ver3.eNMPC.it{Ver3.eNMPC.l+1}.u(1,:);

                % Increment of Ver3.eNMPC_l
                Ver3.eNMPC.l = Ver3.eNMPC.l+1;

            else
                Ver3.p.sim(1:n.u,j) = Ver3.p.sim(1:n.u,j-1);
            end
        end

        % Rearrangement of the setpoints
        if dec.H(3) == 1
            Ver3.cS.set(:,j) = Ver3.p.sim(1:n.u,j);
        else
            Ver3.cM.set(:,j) = Ver3.p.sim(1:n.u,j);
        end
    end
    
    % Control strategy 4
    if ~isempty(dec.Ver{4})
        
        % Running of the integrator for the proposed time-step
        func_eval = func_Ver4('x0',Ver4.x.sim(:,j-1),'z0',Ver4.z.sim(:,j-1),'p',Ver4.p.sim(:,j-1));

        % Redefinition of the meassurements and the integrated error
        Ver4.x.sim(:,j) = full(func_eval.xf);
        Ver4.z.sim(:,j) = full(func_eval.zf);
        aux             = [Ver4.x.sim([1/3*N_Ver4 2/3*N_Ver4 N_Ver4],j);...
                           Ver4.z.sim(2*N_Ver4+1:2*N_Ver4+n.u+1,j)];
        Ver4.y.sim(:,j) = aux([7 4 1 5 2 6 3]);
        Ver4.EoR.sim(j) = full(func_eval.qf)/t.int;

        % Integrating the loss and rearrangement of the setpoints
        Ver4.EoRint.sim(j)  = full(func_eval.qf);
        if dec.H(3) == 1
            Ver4.cS.sim(:,j) = Ver4.z.sim(2*N_Ver4+1:2*N_Ver4+n.u,j);
        elseif dec.H(3) > 1
            Ver4.cS.sim(:,j) = Ver4.z.sim(2*N_Ver4+1:2*N_Ver4+n.u,j);
            Ver4.cS.set(:,j) = Ver4.z.sim(end-2*n.u+1:end-n.u,j);
            Ver4.cM.sim(:,j) = Ver4.z.sim(end-n.u+1:end,j);
        end
        
        % Extremum seeking control Algorithm
        if strcmp(dec.Ver{4},'ESC') 
            if dec.H(4) > 1
            	Ver4.Jest(:,j) = Ver4.cM.sim(:,j);
            else
            	Ver4.Jest(:,j) = Ver4.cS.sim(:,j);
            end
            % Definition of the number of points for gradient estimation
            Nw = min(j,ESC.t(4)/t.int);

            % Definition of the measurements used for ESC
            Y = -Ver4.EoR.sim(j-Nw+1:j)';

            % Calculation of the gradient
            phi = [Ver4.Jest(1:n.u,j-Nw+1:j)',ones(size(Y))]./(ones(size(Y))*[500 500 500 1]);
            Ver4.Varphi(:,j) = var(phi,0,1)';
            Ver4.check(:,j)  = ones(3,1);%*(mod(j,ESC.step(4)) == 0);~(Ver4.Varphi(1:n.u,j)./(H{dec.H(4)}*y0) > 2e-3);

            if (mod(j,ESC.step(4)) == 0)
                k = j;
            end
            
%             if any(Ver4.Varphi(1:n.u,j)./(H{dec.H(4)}*y0) > 5e-3)
%                 % Incorporation of the gradient
% 
%             else
                % Calculation of the gradient
                theta = (phi'*phi)\(phi'*Y);
                Ver4.theta_est(:,j) = theta(1:n.u)./[500 500 500]';
                
                % Print of the gradient
                if mod(j,100) == 0
                    disp('Version 4:')
                    disp(['ESC Active - estimated gradient 1 = ' num2str(Ver4.theta_est(1,j))])
                    disp(['ESC Active - estimated gradient 2 = ' num2str(Ver4.theta_est(2,j))])
                    disp(['ESC Active - estimated gradient 3 = ' num2str(Ver4.theta_est(3,j))])
                    fprintf('\n')
                end
                % Calculation of the change in u
                if j > ESC.t(4)/t.int+1 + 3600/t.int
                    Ver4.theta_int(:,j) = (Ver4.theta_est(:,j)+Ver4.theta_est(:,j-1))/2*t.int;
                    Ver4.theta_inte(:,j) = Ver4.theta_inte(:,j-1) + Ver4.check(:,j).*K_ESC.I(:,4).*sign(Ver4.theta_int(:,j)).*min(abs(Ver4.theta_int(:,j)),K_ESC.bound(:,4)); 
                    Ver4.del_u(:,j) = Ver4.theta_inte(:,k);
                end
%             end

            % Update the input for the master controller from the output from ESC 
            Ver4.p.sim(1:n.u,j) = Ver4.p.sim(1:n.u,j) + Ver4.del_u(:,j) + ...
                                  ESC.mul(:,4).*(sin(2*pi*t.int*(1./ESC.sin(:,4))*j));
            
        elseif strcmp(dec.Ver{4},'MESC')
             % EKF    
%              Fj = full(Ver4.JacFx(Ver4.est_x.sim(:,j-1),Ver4.p.sim(:,j-1)));
             Fj = full(Ver4.JacFx(Ver4.est_x.sim(:,j-1),Ver4.p.sim(1:end-1,j-1)));
             if (isnan(Fj))
                 disp('NaN1 in x EKF')
             end

             %Prediction 
             xk_hat_1 =  full(Ver4.f_EKF(Ver4.est_x.sim(:,j-1),Ver4.p.sim(1:end-1,j-1)));
             %Using model for prediction of xk_1 (more exakt but slower)
%              func_xk_hat_1 = func_Ver4_EKF('x0',Ver4.est_x.sim(:,j-1),'p',Ver4.p.sim(1:n.u,j-1));
%              xk_hat_1 = full(func_xk_hat_1.xf) ;
             Pk_1 = Fj*Ver4.Pk*Fj' + Ver4.Qk;

             %Update
             Ver4.y.noise(:,j) = Ver4.y.sim(:,j)+0.2*dec.mnoise*wgn(n.y,1,0);
             Hj = full(Ver4.JacHx(xk_hat_1,[Ver4.p.sim(1:end-1,j-1)]));
             ek = full(Ver4.y.noise(:,j) -...
                        Ver4.h_EKF(xk_hat_1,[Ver4.p.sim(1:end-1,j-1)]));
             Sk = Hj*Pk_1*Hj' + Ver4.Rk;
             Kk = (Pk_1*Hj')/(Sk);
             Ver4.ek.sim(:,j) = ek;

             Ver4.Pk = (Ver4.diag_EKF - Kk*Hj)*Pk_1;
             Ver4.est_x.sim(:,j) = xk_hat_1 + Kk*ek; 
             
             if isnan(Ver4.est_x.sim(:,j))
                 disp('NaN2 in y EKF')
             end
%              Ver4.xEstErr.sim(:,j) = abs(Ver4.est_x.sim(:,j) - [Ver4.x.sim(:,j); Ver4.z.sim(1:N_Ver4,j); Ver4.p.sim(n.u+1:end,j)]);
             Ver4.xEstErr.sim(:,j) = abs(Ver4.est_x.sim(:,j) - [Ver4.x.sim(:,j); Ver4.z.sim(1:N_Ver4,j); Ver4.p.sim(end,j)]);

             % Calculation of the state matrices
%              A = full(Ver4.JacAx(Ver4.est_x.sim(1:end-n.d,j),[Ver4.p.sim(1:n.u,j-1); Ver4.est_x.sim(end-n.d+1:end,j)]));
%              B = full(Ver4.JacBu(Ver4.est_x.sim(1:end-n.d,j),[Ver4.p.sim(1:n.u,j-1); Ver4.est_x.sim(end-n.d+1:end,j)]));
%              C = full(Ver4.JacJx(Ver4.est_x.sim(1:end-n.d,j),[Ver4.p.sim(1:n.u,j-1); Ver4.est_x.sim(end-n.d+1:end,j)]));
%              D = full(Ver4.JacJu(Ver4.est_x.sim(1:end-n.d,j),[Ver4.p.sim(1:n.u,j-1); Ver4.est_x.sim(end-n.d+1:end,j)]));
             A = full(Ver4.JacAx(Ver4.est_x.sim(1:end-1,j),[Ver4.p.sim(1:end-1,j-1); Ver4.est_x.sim(end,j)]));
             B = full(Ver4.JacBu(Ver4.est_x.sim(1:end-1,j),[Ver4.p.sim(1:end-1,j-1); Ver4.est_x.sim(end,j)]));
             C = full(Ver4.JacJx(Ver4.est_x.sim(1:end-1,j),[Ver4.p.sim(1:end-1,j-1); Ver4.est_x.sim(end,j)]));
             D = full(Ver4.JacJu(Ver4.est_x.sim(1:end-1,j),[Ver4.p.sim(1:end-1,j-1); Ver4.est_x.sim(end,j)]));

            % Integrating the loss and rearrangement of the setpoints
            Ver4.cM.sim(:,j)    = (-C*(A\B)+D)';

            % Calculation of the error and the integrated error
            Ver4.err(:,j)    = Ver4.cM.set(:,j)-Ver4.cM.sim(:,j);
            Ver4.errint(:,j) = Ver4.errint(:,j-1) + mean(Ver4.err(:,j-1:j),2)*t.int;

            % Calculation of the controller action
            Ver4.p.sim(1:3,j) = y0(2:2:end) + (K_MESC.P(:,1).*Ver4.err(:,j) + K_MESC.I(:,1).*Ver4.errint(:,j));
            
        elseif strcmp(dec.Ver{4},'eNMPC')
            if rem(j-2,t_eNMPC{4}.mul) == 0
                % Redefinition of the initial guess and the starting values of the nMPC
                % solver
                w0 = full(Ver4.eNMPC.it{Ver4.eNMPC.l}.comb.x);
                Ver4.eNMPC.lbw(1:Ver4.eNMPC.N_sub) = [Ver4.x.sim(:,j-1); Ver4.z.sim(Ver4.eNMPC.ind_z,j-1)];
                Ver4.eNMPC.ubw(1:Ver4.eNMPC.N_sub) = [Ver4.x.sim(:,j-1); Ver4.z.sim(Ver4.eNMPC.ind_z,j-1)];
                if j == 2
                    Ver4.eNMPC.lbw(Ver4_reA.uM(end,:))  =  p0(1:n.u);
                    Ver4.eNMPC.ubw(Ver4_reA.uM(end,:))  =  p0(1:n.u);
                    Ver4.eNMPC.nlp.f = substitute(Ver4.eNMPC.J,Ver4.eNMPC.U_prev_k{1},p0(1:n.u));
                else
                    Ver4.eNMPC.lbw(Ver4_reA.uM(end,:))  =  Ver4.p.sim(1:n.u,j-2);
                    Ver4.eNMPC.ubw(Ver4_reA.uM(end,:))  =  Ver4.p.sim(1:n.u,j-2);
                    Ver4.eNMPC.nlp.f = substitute(Ver4.eNMPC.J,Ver4.eNMPC.U_prev_k{1},Ver4.p.sim(1:n.u,j-2));
                end

                % Substitution of the inlet values and introduction of noise
                Ver4.eNMPC.nlp.g = substitute(Ver4.eNMPC.g_save,Ver4.eNMPC.dae.p(n.u+1:end),Ver4.p.sim(n.u+1:end,j));
                Ver4.eNMPC.nlp.f = substitute(Ver4.eNMPC.nlp.f, Ver4.eNMPC.dae.p(n.u+1:end),Ver4.p.sim(n.u+1:end,j));
                solver = nlpsol('solver','ipopt',Ver4.eNMPC.nlp);

                % Solving of this timeframe
                time.in.NMPC = tic;
                Ver4.eNMPC.it{Ver4.eNMPC.l+1}.comb = ...
                solver('x0',w0,'lbx',Ver4.eNMPC.lbw,'ubx',Ver4.eNMPC.ubw,...
                               'lbg',Ver4.eNMPC.lbg,'ubg',Ver4.eNMPC.ubg);
                time.NMPC(Ver4.eNMPC.l) = toc(time.in.NMPC);

                % Reassignment of the solution values
                Ver4.eNMPC.it{Ver4.eNMPC.l+1}.x = full(Ver4.eNMPC.it{Ver4.eNMPC.l+1}.comb.x(Ver4_reA.xM));
                Ver4.eNMPC.it{Ver4.eNMPC.l+1}.z = full(Ver4.eNMPC.it{Ver4.eNMPC.l+1}.comb.x(Ver4_reA.zM));
                Ver4.eNMPC.it{Ver4.eNMPC.l+1}.u = full(Ver4.eNMPC.it{Ver4.eNMPC.l+1}.comb.x(Ver4_reA.uM));
                Ver4.eNMPC.it{Ver4.eNMPC.l+1}.f = full(Ver4.eNMPC.it{Ver4.eNMPC.l+1}.comb.f);
                Ver4.p.sim(1:n.u,j) = Ver4.eNMPC.it{Ver4.eNMPC.l+1}.u(1,:);

                % Increment of Ver4.eNMPC_l
                Ver4.eNMPC.l = Ver4.eNMPC.l+1;

            else
                Ver4.p.sim(1:n.u,j) = Ver4.p.sim(1:n.u,j-1);
            end
        end
        
        % Rearrangement of the setpoints
        if dec.H(4) == 1
            Ver4.cS.set(:,j) = Ver4.p.sim(1:n.u,j);
        else
            Ver4.cM.set(:,j) = Ver4.p.sim(1:n.u,j);
        end
    end
    
    % Updating the waitbar
    if mod(j,100) == 0
        waitbar(j/(t.N+1))
        fprintf('\n')
    end
end

% Close of the waitbar
close(h)
time.sim = toc(time.in.sim);

%% Plotting of the values

if dec.plot

% Load of the values
load(loadname.SOC)
load(loadname.opt)
% Initialization of the variables
if true
    opt.u = SS_opt{1}.u(:,16);
    opt.x = SS_opt{1}.x(:,16);
    opt.z = SS_opt{1}.z(:,16);
    opt.y = SS_opt{1}.y(:,16);
    opt.f = SS_opt{1}.f(:,16);
    SOC.u = SS_SOC{1,dec.H_comp}.u(:,16);
    SOC.x = SS_SOC{1,dec.H_comp}.x(:,16);
    SOC.z = SS_SOC{1,dec.H_comp}.z(:,16);
    SOC.f = SS_SOC{1,dec.H_comp}.f(:,16);
    opt.t = 0;
    opt.loss_int = 0;
    SOC.loss_int = 0;
    Ver1.opt.loss_int = 0;
    Ver2.opt.loss_int = 0;
    Ver3.opt.loss_int = 0;
    Ver4.opt.loss_int = 0;
    Ver1.SOC.loss_int = 0;
    Ver2.SOC.loss_int = 0;
    Ver3.SOC.loss_int = 0;
    Ver4.SOC.loss_int = 0;
end

for k = 2:t.N+1
    if any(int.sim(:,k)-int.sim(:,k-1))
        opt.t = [opt.t; t.samp(k); t.samp(k)];
        for l = 1:n.d
            if int.sim(l,k)-int.sim(l,k-1) ~= 0
                opt.u = [opt.u opt.u(:,end) SS_opt{l}.u(:,SS_opt{l}.Var == int.sim(l,k)+d.nom(l))];
                opt.x = [opt.x opt.x(:,end) SS_opt{l}.x(:,SS_opt{l}.Var == int.sim(l,k)+d.nom(l))];
                opt.y = [opt.y opt.y(:,end) SS_opt{l}.y(:,SS_opt{l}.Var == int.sim(l,k)+d.nom(l))];
                opt.z = [opt.z opt.z(:,end) SS_opt{l}.z(:,SS_opt{l}.Var == int.sim(l,k)+d.nom(l))];
                opt.f = [opt.f opt.f(:,end) SS_opt{l}.f(:,SS_opt{l}.Var == int.sim(l,k)+d.nom(l))];

                SOC.u = [SOC.u SOC.u(:,end) SS_SOC{l,dec.H_comp}.u(:,SS_SOC{l,dec.H_comp}.Var == int.sim(l,k)+d.nom(l))];
                SOC.x = [SOC.x SOC.x(:,end) SS_SOC{l,dec.H_comp}.x(:,SS_SOC{l,dec.H_comp}.Var == int.sim(l,k)+d.nom(l))];
                SOC.z = [SOC.z SOC.z(:,end) SS_SOC{l,dec.H_comp}.z(:,SS_SOC{l,dec.H_comp}.Var == int.sim(l,k)+d.nom(l))];
                SOC.f = [SOC.f SOC.f(:,end) SS_SOC{l,dec.H_comp}.f(:,SS_SOC{l,dec.H_comp}.Var == int.sim(l,k)+d.nom(l))];
            end
        end
    end
    % Calculation and integration of the loss for Version 1
    if ~isempty(dec.Ver{1})
        Ver1.SOC.loss(k) = Ver1.EoR.sim(k)-SOC.f(end);
        Ver1.opt.loss(k) = Ver1.EoR.sim(k)-opt.f(end);
        Ver1.SOC.loss_int(k) = Ver1.SOC.loss_int(k-1)+t.int*(Ver1.SOC.loss(k)+Ver1.SOC.loss(k-1))/2;
        Ver1.opt.loss_int(k) = Ver1.opt.loss_int(k-1)+t.int*(Ver1.opt.loss(k)+Ver1.opt.loss(k-1))/2;
    end

    % Calculation and integration of the loss for Version 2
    if ~isempty(dec.Ver{2})
        Ver2.SOC.loss(k) = Ver2.EoR.sim(k)-SOC.f(end);
        Ver2.opt.loss(k) = Ver2.EoR.sim(k)-opt.f(end);
        Ver2.SOC.loss_int(k) = Ver2.SOC.loss_int(k-1)+t.int*(Ver2.SOC.loss(k)+Ver2.SOC.loss(k-1))/2;
        Ver2.opt.loss_int(k) = Ver2.opt.loss_int(k-1)+t.int*(Ver2.opt.loss(k)+Ver2.opt.loss(k-1))/2;
    end

    % Calculation and integration of the loss for Version 3
    if ~isempty(dec.Ver{3})
        Ver3.SOC.loss(k) = Ver3.EoR.sim(k)-SOC.f(end);
        Ver3.opt.loss(k) = Ver3.EoR.sim(k)-opt.f(end);
        Ver3.SOC.loss_int(k) = Ver3.SOC.loss_int(k-1)+t.int*(Ver3.SOC.loss(k)+Ver3.SOC.loss(k-1))/2;
        Ver3.opt.loss_int(k) = Ver3.opt.loss_int(k-1)+t.int*(Ver3.opt.loss(k)+Ver3.opt.loss(k-1))/2;
    end

    % Calculation and integration of the loss for self-optimizing control
    if ~isempty(dec.Ver{4})
        Ver4.SOC.loss(k) = Ver4.EoR.sim(k)-SOC.f(end);
        Ver4.opt.loss(k) = Ver4.EoR.sim(k)-opt.f(end);
        Ver4.SOC.loss_int(k) = Ver4.SOC.loss_int(k-1)+t.int*(Ver4.SOC.loss(k)+Ver4.SOC.loss(k-1))/2;
        Ver4.opt.loss_int(k) = Ver4.opt.loss_int(k-1)+t.int*(Ver4.opt.loss(k)+Ver4.opt.loss(k-1))/2;
    end
end

% Addition of the final time step
opt.t = [opt.t; t.samp(end)];
opt.u = [opt.u opt.u(:,end)];
opt.x = [opt.x opt.x(:,end)];
opt.y = [opt.y opt.y(:,end)];
opt.z = [opt.z opt.z(:,end)];
opt.f = [opt.f opt.f(:,end)];
opt.dy = opt.y-opt.y(:,1)*ones(1,size(opt.y,2));

SOC.u = [SOC.u SOC.u(:,end)];
SOC.x = [SOC.x SOC.x(:,end)];
SOC.z = [SOC.z SOC.z(:,end)];
SOC.f = [SOC.f SOC.f(:,end)];

% Definition of integer values
itg.fig = 1;
itg.lab = 1;
itg.fda = 1;
itg.axe = 1;
if t.tot <= 6000
    itg.xlab = 'Time t [min]';
elseif  t.tot <= 3600*24
    itg.xlab = 'Time t [h]';
else
    itg.xlab = 'Time t [d]';
end

% Definition of figure and values
fig.dimw = 16;
fig.dimh = 14;
fig.Font = [10 8];
fig.label = [];

% Plot of the responses for a setpoint change or a disturbance
if ~isempty(dec.Ver{1})
    if dec.H(1) < 1
        subpl = 1;
    elseif dec.H(1) == 1
        subpl = 2;
    else
        subpl = 3;
    end
    fig.in(itg.fig) = figure(itg.fig);
    set(fig.in(itg.fig),'units','centimeters','position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
    for k = 1:n.u
        
        if  dec.H(1) == 0
            subplot(subpl,n.u,k)
            fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
            hold on
            fig.dat(itg.fda) = plot(t.samp,Ver1.p.sim(k,:),'k'); itg.fda = itg.fda+1;
            fig.dat(itg.fda) = plot(opt.t,SOC.u(k,:),'k--'); itg.fda = itg.fda+1;
            fig.dat(itg.fda) = plot(opt.t,opt.u(k,:),'r'); itg.fda = itg.fda+1;
            if k == 1
                fig.label(itg.lab) = ylabel('Input $u_i$ [\%]'); itg.lab = itg.lab+1;
            end
        
        else
            subplot(subpl,n.u,k)
            fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
            hold on
            fig.dat(itg.fda) = plot(t.samp,Ver1.z.sim(2*N_Ver1+3*n.u+1+k,:),'k'); itg.fda = itg.fda+1;
            fig.dat(itg.fda) = plot(opt.t,SOC.u(k,:),'k--'); itg.fda = itg.fda+1;
            fig.dat(itg.fda) = plot(opt.t,opt.u(k,:),'r'); itg.fda = itg.fda+1;
            if k == 1
                fig.label(itg.lab) = ylabel('Input $u_i$ [\%]'); itg.lab = itg.lab+1;
            end
            
            subplot(subpl,n.u,n.u+k)
            fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
            hold on
            fig.dat(itg.fda) = plot(t.samp,Ver1.cS.sim(k,:),'k'); itg.fda = itg.fda+1;
            fig.dat(itg.fda) = plot(opt.t,opt.z(2*N_Ver1+k,:),'r'); itg.fda = itg.fda+1;
            if dec.H(1) > 0 
                fig.dat(itg.fda) = plot(t.samp,Ver1.cS.set(k,:),'k-.'); itg.fda = itg.fda+1;
            end
            if k == 1
                fig.label(itg.lab) = ylabel('Slave [C]'); itg.lab = itg.lab+1;
            end
            if dec.H(1) > 1
                if strcmp(dec.Ver{1},'MESC')
                    fig.dat(itg.fda) = plot(opt.t,SOC.z(2*N_Ver1+k,:),'k--'); itg.fda = itg.fda+1;
                else
                    fig.dat(itg.fda) = plot(opt.t,SOC.z(2*N_Ver1+k,:),'k--'); itg.fda = itg.fda+1;
                end

                subplot(subpl,n.u,2*n.u+k)
                fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
                hold on
                fig.dat(itg.fda) = plot(t.samp,Ver1.cM.sim(k,:)-Ver1.cM.sim(k,1),'k'); itg.fda = itg.fda+1;
                fig.dat(itg.fda) = plot(t.samp,Ver1.cM.set(k,:)-Ver1.cM.set(k,1),'k--'); itg.fda = itg.fda+1;
                if strcmp(dec.Ver{1},'ESC')
                    fig.dat(itg.fda) = plot(opt.t,H{dec.H(1)}(k,:)*opt.dy,'r'); itg.fda = itg.fda+1;
                end
                if k == 1
                    fig.label(itg.lab) = ylabel('Master [C]'); itg.lab = itg.lab+1;
                end
            end
        end
        fig.label(itg.lab) = xlabel(itg.xlab); itg.lab = itg.lab+1;
    end
    itg.fig = itg.fig + 1;
end
if ~isempty(dec.Ver{2})
    if dec.H(2) == 1
        subpl = 2;
    else
        subpl = 3;
    end
    fig.in(itg.fig) = figure(itg.fig);
    set(fig.in(itg.fig),'units','centimeters','position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
    for k = 1:n.u
        subplot(subpl,n.u,k)
        fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
        hold on
        fig.dat(itg.fda) = plot(t.samp,Ver2.z.sim(2*N_Ver2+3*n.u+1+k,:),'k'); itg.fda = itg.fda+1;
        fig.dat(itg.fda) = plot(opt.t,SOC.u(k,:),'k--'); itg.fda = itg.fda+1;
        fig.dat(itg.fda) = plot(opt.t,opt.u(k,:),'r'); itg.fda = itg.fda+1;
        if k == 1
            fig.label(itg.lab) = ylabel('Input $u_i$ [\%]'); itg.lab = itg.lab+1;
        end

        subplot(subpl,n.u,n.u+k)
        fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
        hold on
        fig.dat(itg.fda) = plot(t.samp,Ver2.cS.sim(k,:),'k'); itg.fda = itg.fda+1;
        fig.dat(itg.fda) = plot(opt.t,opt.z(2*N_Ver2+k,:),'r'); itg.fda = itg.fda+1;
        if dec.H(2) > 0 
            fig.dat(itg.fda) = plot(t.samp,Ver2.cS.set(k,:),'k-.'); itg.fda = itg.fda+1;
        end
        if k == 1
            fig.label(itg.lab) = ylabel('Slave [C]'); itg.lab = itg.lab+1;
        end
        if dec.H(2) > 1
            fig.dat(itg.fda) = plot(opt.t,SOC.z(2*N_Ver2+k,:),'k--'); itg.fda = itg.fda+1;

            subplot(subpl,n.u,2*n.u+k)
            fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
            hold on
            fig.dat(itg.fda) = plot(t.samp,Ver2.cM.sim(k,:)-Ver2.cM.sim(k,1),'k'); itg.fda = itg.fda+1;
            fig.dat(itg.fda) = plot(t.samp,Ver2.cM.set(k,:)-Ver2.cM.set(k,1),'k--'); itg.fda = itg.fda+1;
            if strcmp(dec.Ver{2},'ESC')
                fig.dat(itg.fda) = plot(opt.t,H{dec.H(2)}(k,:)*opt.dy,'r'); itg.fda = itg.fda+1;
            end
            if k == 1
                fig.label(itg.lab) = ylabel('Master [C]'); itg.lab = itg.lab+1;
            end
        end
        fig.label(itg.lab) = xlabel(itg.xlab); itg.lab = itg.lab+1;
    end
    itg.fig = itg.fig + 1;
end
if ~isempty(dec.Ver{3})
    if dec.H(3) == 1
        subpl = 2;
    else
        subpl = 3;
    end
    fig.in(itg.fig) = figure(itg.fig);
    set(fig.in(itg.fig),'units','centimeters','position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
    for k = 1:n.u
        subplot(subpl,n.u,k)
        fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
        hold on
        fig.dat(itg.fda) = plot(t.samp,Ver3.z.sim(2*N_Ver3+3*n.u+1+k,:),'k'); itg.fda = itg.fda+1;
        fig.dat(itg.fda) = plot(opt.t,SOC.u(k,:),'k--'); itg.fda = itg.fda+1;
        fig.dat(itg.fda) = plot(opt.t,opt.u(k,:),'r'); itg.fda = itg.fda+1;
        if k == 1
            fig.label(itg.lab) = ylabel('Input $u_i$ [\%]'); itg.lab = itg.lab+1;
        end

        subplot(subpl,n.u,n.u+k)
        fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
        hold on
        fig.dat(itg.fda) = plot(t.samp,Ver3.cS.sim(k,:),'k'); itg.fda = itg.fda+1;
        fig.dat(itg.fda) = plot(opt.t,opt.z(2*N_Ver3+k,:),'r'); itg.fda = itg.fda+1;
        if dec.H(3) > 0 
            fig.dat(itg.fda) = plot(t.samp,Ver3.cS.set(k,:),'k-.'); itg.fda = itg.fda+1;
        end
        if k == 1
            fig.label(itg.lab) = ylabel('Slave [C]'); itg.lab = itg.lab+1;
        end
        if dec.H(3) > 1
            fig.dat(itg.fda) = plot(opt.t,SOC.z(2*N_Ver3+k,:),'k--'); itg.fda = itg.fda+1;

            subplot(subpl,n.u,2*n.u+k)
            fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
            hold on
            fig.dat(itg.fda) = plot(t.samp,Ver3.cM.sim(k,:)-Ver3.cM.sim(k,1),'k'); itg.fda = itg.fda+1;
            fig.dat(itg.fda) = plot(t.samp,Ver3.cM.set(k,:)-Ver3.cM.set(k,1),'k--'); itg.fda = itg.fda+1;
            if strcmp(dec.Ver{3},'ESC')
                fig.dat(itg.fda) = plot(opt.t,H{dec.H(3)}(k,:)*opt.dy,'r'); itg.fda = itg.fda+1;
            end
            if k == 1
                fig.label(itg.lab) = ylabel('Master [C]'); itg.lab = itg.lab+1;
            end
        end
        fig.label(itg.lab) = xlabel(itg.xlab); itg.lab = itg.lab+1;
    end
    itg.fig = itg.fig + 1;
end
if ~isempty(dec.Ver{4})
    if dec.H(4) == 1
        subpl = 2;
    else
        subpl = 3;
    end
    fig.in(itg.fig) = figure(itg.fig);
    set(fig.in(itg.fig),'units','centimeters','position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
    for k = 1:n.u
        subplot(subpl,n.u,k)
        fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
        hold on
        fig.dat(itg.fda) = plot(t.samp,Ver4.z.sim(2*N_Ver4+3*n.u+1+k,:),'k'); itg.fda = itg.fda+1;
        fig.dat(itg.fda) = plot(opt.t,SOC.u(k,:),'k--'); itg.fda = itg.fda+1;
        fig.dat(itg.fda) = plot(opt.t,opt.u(k,:),'r'); itg.fda = itg.fda+1;
        if k == 1
            fig.label(itg.lab) = ylabel('Input $u_i$ [\%]'); itg.lab = itg.lab+1;
        end

        subplot(subpl,n.u,n.u+k)
        fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
        hold on
        fig.dat(itg.fda) = plot(t.samp,Ver4.cS.sim(k,:),'k'); itg.fda = itg.fda+1;
        fig.dat(itg.fda) = plot(opt.t,opt.z(2*N_Ver4+k,:),'r'); itg.fda = itg.fda+1;
        if dec.H(4) > 0 
            fig.dat(itg.fda) = plot(t.samp,Ver4.cS.set(k,:),'k-.'); itg.fda = itg.fda+1;
        end
        if k == 1
            fig.label(itg.lab) = ylabel('Slave [C]'); itg.lab = itg.lab+1;
        end
        if dec.H(4) > 1
            fig.dat(itg.fda) = plot(opt.t,SOC.z(2*N_Ver4+k,:),'k--'); itg.fda = itg.fda+1;

            subplot(subpl,n.u,2*n.u+k)
            fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
            hold on
            fig.dat(itg.fda) = plot(t.samp,Ver4.cM.sim(k,:)-Ver4.cM.sim(k,1),'k'); itg.fda = itg.fda+1;
            fig.dat(itg.fda) = plot(t.samp,Ver4.cM.set(k,:)-Ver4.cM.set(k,1),'k--'); itg.fda = itg.fda+1;
            if strcmp(dec.Ver{4},'ESC')
                fig.dat(itg.fda) = plot(opt.t,H{dec.H(4)}(k,:)*opt.dy,'r'); itg.fda = itg.fda+1;
            end
            if k == 1
                fig.label(itg.lab) = ylabel('Master [C]'); itg.lab = itg.lab+1;
            end
        end
        fig.label(itg.lab) = xlabel(itg.xlab); itg.lab = itg.lab+1;
    end
    itg.fig = itg.fig + 1;
end

% Plot of the total extent of reaction with the optimal given as well
fig.in(itg.fig) = figure(itg.fig);
set(fig.in(itg.fig),'units','centimeters','position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
hold on
if ~isempty(dec.Ver{1})
    fig.dat(itg.fda) = plot(t.samp(2:end),-Ver1.EoR.sim(2:end)*3.6,'k'); itg.fda = itg.fda+1;
end
if ~isempty(dec.Ver{2})
    fig.dat(itg.fda) = plot(t.samp(2:end),-Ver2.EoR.sim(2:end)*3.6,'k--'); itg.fda = itg.fda+1;
end
if ~isempty(dec.Ver{3})
    fig.dat(itg.fda) = plot(t.samp(2:end),-Ver3.EoR.sim(2:end)*3.6,'b'); itg.fda = itg.fda+1;
end
if ~isempty(dec.Ver{4})
    fig.dat(itg.fda) = plot(t.samp(2:end),-Ver4.EoR.sim(2:end)*3.6,'b--'); itg.fda = itg.fda+1;
end
fig.dat(itg.fda) = plot(opt.t,-opt.f*3.6,'r'); itg.fda = itg.fda+1;
fig.dat(itg.fda) = plot(opt.t,-SOC.f*3.6,'r--'); itg.fda = itg.fda+1;
fig.label(itg.lab) = xlabel(itg.xlab); itg.lab = itg.lab+1;
fig.label(itg.lab) = ylabel('Extent of reaction $\xi$ [t/h]'); itg.lab = itg.lab+1;
itg.fig = itg.fig + 1;  

% Plot of the integrated loss
fig.in(itg.fig) = figure(itg.fig);
set(fig.in(itg.fig),'units','centimeters','position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
hold on
if ~isempty(dec.Ver{1})
    fig.dat(itg.fda) = plot(t.samp,Ver1.opt.loss_int/1e3,'k'); itg.fda = itg.fda+1;
end
if ~isempty(dec.Ver{2})
    fig.dat(itg.fda) = plot(t.samp,Ver2.opt.loss_int/1e3,'k--'); itg.fda = itg.fda+1;
end
if ~isempty(dec.Ver{3})
    fig.dat(itg.fda) = plot(t.samp,Ver3.opt.loss_int/1e3,'b'); itg.fda = itg.fda+1;
end
if ~isempty(dec.Ver{4})
    fig.dat(itg.fda) = plot(t.samp,Ver4.opt.loss_int/1e3,'b--'); itg.fda = itg.fda+1;
end
fig.dat(itg.fda) = plot([0 max(t.samp)],[0 0],'k--'); itg.fda = itg.fda+1;
fig.label(itg.lab) = xlabel(itg.xlab); itg.lab = itg.lab+1;
fig.label(itg.lab) = ylabel('Loss L [t]'); itg.lab = itg.lab+1;
itg.fig = itg.fig + 1;  


set(fig.label, 'Fontsize', fig.Font(1));
set(fig.label, 'Interpreter', 'Latex');
set(fig.axes,'Fontsize', fig.Font(2));
set(fig.axes,'TickLabelInterpreter', 'Latex');
set(fig.axes,'LineWidth',.75);
set(fig.dat,'LineWidth',.75)

end
