% Main_SSAna_RedLoop_Ver2 adds a recycle loop to the model AmmonReac_NewDist.
% This recycle loop is incorporated in the script "RecLoop_Ver2.m" till 
% "RecLoop_Ver4.m". This script utilizes the ratio as algebraic variable.
%
% This function allows the utilizatrion of the inlet ratio as optimization
% variable through dec.Ropt.
%
% Additionally, it can be used for open loop experiments using dec.loop.
%
% The difference compared to _Ver1 is given by a different definition of
% the algebraic variables:
%   RecLoop_Ver2.m      wNH3 and WN2
%   RecLoop_Ver3.m      wNH3
%   RecLoop_Ver4.m      wH2 and wN2
%
% Additionally, the integration from Ver1 is removed. 

% The difference to Ver2 is given that it by can be
% additionally used for plotting the optimal inlet ratio as a function of
% the purge ratio. For this dec.Ropt and dec.ratio have to be set to true.
% Except for this, it is rather similar to Ver2.

% Clearing of the workspace and closing of all windows

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.*

% Definition of decision variables
dec.plot  = true;
dec.scan  = true;
dec.root  = false;
dec.Loop  = true;
dec.dp    = true;
dec.ratio = true;
dec.Ropt  = true;
dec.save  = true;

% Definition of the save name
savename = 'SS_opt_Rea.mat';

% Definition of nominal disturbance variables
in.nom = zeros(7,1);
in.nom(1) = 3.00;            % H2/N2 ratio [-]
in.nom(3) = 200;            % Pressure [bar]
in.nom(4) = 250;            % Temperature [C]
in.nom(6) = 1;              % Catalyst activity 
if dec.Loop
    in.nom(2) = 16.2147;    % Feed flow (kg/s)
    in.nom(5) = 0.00;       % Mass fraction ammonia [-]
    in.nom(7) = 0.76;       % Recovery mass fraction of ammonia 
else
    in.nom(2) = 252/3.6;    % Feed flow (kg/s)
    in.nom(5) = 0.08;       % Mass fraction ammonia [-]
    in.nom(7) = 1;          % Recovery mass fraction of ammonia 
end    

% Definition of the variation
if dec.scan
    Var.n = zeros(numel(in.nom),1);
%     Var.n(1) = 11*~dec.Ropt;
%     Var.n(2) = 31;
%     Var.n(3) = 101;
%     Var.n(4) = 101;
%     Var.n(5) = 33;
%     Var.n(6) = 61;
%     Var.n(7) = 61;
    if dec.ratio
        Var.n(8) = 20;
    end
    dec.int  = false;
    dec.save = true;
else
    Var.n = 1;
    dec.save = false;
end

Var.lub = [-0.00 -15 -50 -50 -0.04 -0.4 -0.05 0;...1e-4;...
            0.10  15  50  50  0.04  0.2  0.05 1e-2];

% Load of the model
if dec.ratio
    [func,dae,d,lb,ub,n,Cineq] = RecLoop_Ver2(1,dec,in.nom([2:5 1 6:7]));
%     [func,dae,d,lb,ub,n,Cineq] = RecLoop_Ver4(1,dec,in.nom([2:5 1 6:7]));
else
    [func,dae,d,lb,ub,n,Cineq] = RecLoop_Ver3(1,dec,in.nom(2:7));
end

% Definition of the initial guesses and redefinition into x and z
if dec.Loop
    load([dirmain '\Data\Init_In_CL.mat'])
else
    load([dirmain '\Data\Init_In_OL.mat'])
end
p0 = [p0(1:3); 5e-3; 0];%[0.05; .05]*dec.Ropt];
% z0 = [z0(1:30); [0.747552850187364;0.736406519295665;0.723836926569576;0.709326311476683;0.692136646049220;0.671302374111662;0.646296927788299;0.620136542985628;0.600373599876942;0.589724955654103;0.655707851403388;0.647543782803476;0.638548842706876;0.628634553799218;0.617774313191536;0.606104850685185;0.594084225013909;0.582591790870017;0.572705210231458;0.565147214265485;0.613063536661777;0.603268239829586;0.592818621926927;0.582067534991598;0.571666462537685;0.562444087317238;0.555054103583811;0.549678943871931;0.546064863333455;0.543768271953811];...
%       z0(61:67);[0.7515; 0.7; 0.6]; [252/3.6; 200; 250; 0.08; 0.75]; dec.Loop*[192/3.6; 188; 250; 0.10; 0.57]; 0];
% z0(end-5:end-1) = [192/3.6; 200; 250; 0.10; 0.57];

% Calculation of the number of CSTRs in total
N = size(dae.x,1);

% Decision whether the ratio is an optimizing variable or not
if dec.Ropt
    dae.u = [dae.p(1:n.u); dae.p(n.u+1)];
    d     = d(2:end);
    n.d   = length(d);
    n.u   = length(dae.u);
else
    dae.u = dae.p(1:4);
end

% Definition of indices used in the solution
if dec.ratio
    ind.T    = [3*N+1 1:N/3       3*N+2 N/3+1:2*N/3   3*N+3  2*N/3+1:N]; % 3*N+4 
    ind.wNH3 = [3*N+5 N+1:4*N/3   3*N+6 4*N/3+1:5*N/3 3*N+7  5*N/3+1:2*N];
    ind.wN2  = [3*N+8 2*N+1:7*N/3 3*N+9 7*N/3+1:8*N/3 3*N+10 8*N/3+1:3*N];

    ind.Rea = 3*N+11:3*N+15;
    ind.Rec = 3*N+16:3*N+20;
    ind.W   = 3*N+21;

    ind.x = 1:N;
    ind.y = [3*N+4 3*N+1 N/3 3*N+2 N*2/3 3*N+3 N];
    ind.z = (N+1):(3*N+21);

    ind.u = (3*N+21+1):(3*N+21+n.u);
else
    ind.T    = [2*N+1 1:N/3       2*N+2 N/3+1:2*N/3   2*N+3  2*N/3+1:N]; % 3*N+4 
    ind.wNH3 = [2*N+5 N+1:4*N/3   2*N+6 4*N/3+1:5*N/3 2*N+7  5*N/3+1:2*N];
    ind.wN2  = [];

    ind.Rea = (2*N+8):(2*N+11);
    ind.Rec = (2*N+12):(2*N+15);
    ind.W   = (2*N+16);

    ind.x = 1:N;
    ind.y = [2*N+4 2*N+1 N/3 2*N+2 N*2/3 2*N+3 N];
    ind.z = (N+1):(2*N+16);

    ind.u = (2*N+16+1):(2*N+16+n.u);
    
    z0 = z0([1:30 61:67 71:74 76:79 81]);
end


%% Part a) Root finding for the modified system

% Combination of the variables
w_root = [dae.x;dae.z];
w0     = [x0;z0];

% Combination of the equality constraints and substitution of inlet values
g = vertcat(dae.ode,dae.alg);
g_root = substitute(g,d         ,zeros(n.d,1)); % [.2; zeros(n.d-1,1)]); % 
g_root = substitute(g_root,dae.u,p0(1:n.u));
J_root = substitute(dae.quad    ,d,zeros(n.d,1));
J_root = substitute(J_root,dae.u,p0(1:n.u));
cost = Function('cost',{w_root},{J_root});

% Definition and solving of the system
g_fun = Function('g_fun',{w_root},{g_root});
G = rootfinder('G','newton',g_fun);
[root.comb] = G('i0',w0);

% Reassignment of variables
root.x = full(root.comb.o0(ind.x));
root.z = full(root.comb.o0(ind.z));

root.T    = full(root.comb.o0(ind.T));
root.wNH3 = full(root.comb.o0(ind.wNH3));
root.wN2  = full(root.comb.o0(ind.wN2));

root.Rea = full(root.comb.o0(ind.Rea));
root.Rec = full(root.comb.o0(ind.Rec));
root.W   = full(root.comb.o0(ind.W));

root.f   = -full(cost(root.comb.o0));

x0 = root.x;
z0 = root.z;

% Print of the found results
fprintf('\n\n\nThe root-finding is completed. \n\n\n')

%% Part b) Steady state optimization of the system

% Combination of the variables
w_opt = [dae.x; dae.z; dae.u];
lbw   = [lb.x;  lb.z;  lb.p(1:n.u)];
ubw   = [ub.x;  ub.z;  ub.p(1:n.u)];
w0    = [x0;    z0;    p0(1:n.u)];

% Combination of the equality and inequality constraints
gadd = [g; Cineq];
lbg  = [zeros(size(gadd,1)-numel(Cineq),1); -inf*ones(numel(Cineq),1)];
ubg  = [zeros(size(gadd))];

% Predefinition of varying variables
SS_opt  = cell(numel(Var.n),1);
SS_root = cell(numel(Var.n),1);

% Definition of an auxilliary vector
aux = 1:8;(n.d+~dec.ratio);

time.in.opt = tic;
for k = aux(Var.n ~= 0)
% Definition of the varying vector
in.SS = zeros(n.d,Var.n(k));

% Predefinition of the solution vector
nom_opt.x = zeros(n.x,Var.n(k));
nom_opt.z = zeros(n.z,Var.n(k));

nom_opt.T   = zeros(N+3,Var.n(k));
nom_opt.wNH3 = zeros(N+3,Var.n(k));
nom_opt.wN2 = zeros(numel(ind.wN2),Var.n(k));

nom_opt.p = zeros(n.p,Var.n(k));
nom_opt.u = zeros(n.u,Var.n(k));
nom_opt.f = zeros(1,Var.n(k));

if dec.root
    root_scan.x = zeros(n.x,Var.n(k));
    root_scan.z = zeros(n.z,Var.n(k));
    root_scan.f = zeros(1,Var.n(k));
end

if Var.n(k) ~= 1 && k ~= 8
    in.SS(k,:) = linspace(Var.lub(1,k),Var.lub(2,k),Var.n(k));
    in.lbw     = zeros(1,Var.n(k));
elseif k == 8
    in.lbw     = linspace(Var.lub(1,k),Var.lub(2,k),Var.n(k));
else
    in.lbw = lbw(ind.u(4));
end

for i = 1:size(in.SS,2)
    % Substitution of the disturbance values
    in.dist = in.SS(:,i);
    g_opt = substitute(gadd,d,in.dist);
    J = substitute(dae.quad,d,in.dist);
    lbw(ind.u(4)) = in.lbw(i);

    % Definition of the NLP function
    nlp = struct('x',w_opt,'f',J,'g',g_opt);
    opts = struct('warn_initial_bounds',false	...
              ,'print_time',false               ...
              );
%               ,'ipopt',struct('print_level',3)  ...
    solver = nlpsol('solver','ipopt',nlp,opts);
    nom_opt.comb = solver('x0',w0,'lbx',lbw,'ubx',ubw,'lbg',lbg,'ubg',ubg);
%     
    % Reassignment of variables
    nom_opt.a(:,i) = full(nom_opt.comb.x);
    nom_opt.x(:,i) = full(nom_opt.comb.x(ind.x));
    nom_opt.z(:,i) = full(nom_opt.comb.x(ind.z));
    
    nom_opt.T(:,i)    = full(nom_opt.comb.x(ind.T));
    nom_opt.wNH3(:,i) = full(nom_opt.comb.x(ind.wNH3));
    nom_opt.wN2(:,i)  = full(nom_opt.comb.x(ind.wN2));
        
    nom_opt.Rea(:,i) = full(nom_opt.comb.x(ind.Rea));
    nom_opt.Rec(:,i) = full(nom_opt.comb.x(ind.Rec));
    nom_opt.W(:,i)   = full(nom_opt.comb.x(ind.W));
    
    nom_opt.u(:,i)     = full(nom_opt.comb.x(ind.u));
    nom_opt.p(1:n.u,i) = full(nom_opt.u(:,i));
    nom_opt.f(i)       = -full(nom_opt.comb.f);
    
    if dec.root
        % Substitution of the disturbance values
        g_root = substitute(g,d,in.dist);
        g_root = substitute(g_root,dae.p(1:n.u),p0);

        % Definition and solving of the system
        g_fun = Function('g_fun',{w_root},{g_root});
        G = rootfinder('G','newton',g_fun);
        [root_scan.comb] = G('i0',w0(1:end-n.u));

        % Reassignment of variables
        root_scan.x(:,i) = full(root_scan.comb.o0(ind.x));
        root_scan.z(:,i) = full(root_scan.comb.o0(ind.z));
        root_scan.f(i)   = -full(cost(root_scan.comb.o0));
    end
    
    % Redefinition of the new initial values
%     w0 = full(nom_opt.comb.x);

end

% Redefinition of the new initial values
% w0 = [full(root.comb.o0);p0];
        
% Combining the solution
if k ~=8
    SS_opt{k} = struct('Var',in.SS(k,:)+in.nom(k),'z',nom_opt.z,...
                         'u',nom_opt.u,'x',nom_opt.x,'f',nom_opt.f);
else
    SS_opt{k} = struct('Var',in.lbw*1e2,'z',nom_opt.z,...
                         'u',nom_opt.u,'x',nom_opt.x,'f',nom_opt.f);
end
if dec.root
    SS_root{k} = struct('Var',in.SS(k,:)+in.nom(k),'z',root_scan.z,...
                          'u',p0(1:2)*ones(1,Var.n(k)),'x',root_scan.x,'f',root_scan.f);
end
end
fprintf('\n\n\nThe steady-state optimization is completed. \n\n\n')
time.opt = toc(time.in.opt);

% Save of the results
if dec.save
    save([dirmain '\Data\' savename],'SS_opt')
end

%% Part c) Calculation and plotting of the results

% Definition of integer values
itg.fig = 1;
itg.lab = 1;
itg.fda = 1;
itg.axe = 1;

% Check of the Jacobian
% Jac = jacobian(nlp.g, nlp.x);
% figure(itg.fig)
% fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
% spy(sparse(DM.ones(Jac.sparsity()))); itg.fig = itg.fig+1;

N1 = N/3+1;
N2 = N/3+1;
N3 = N/3+1;

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

if dec.plot
    
if ~dec.scan
    % Plotting of the steady state reactor profiles
    fig.in(itg.fig) = figure(itg.fig);
    set(fig.in(itg.fig),'units','centimeters');
    set(gcf, 'position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
    hold on
    yyaxis left
    fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
    fig.axes(itg.fig).YAxis(1).Color = 'k';
    fig.axes(itg.fig).YAxis(2).Color = 'r';
    fig.dat(itg.fda) = plot([linspace(0,1,N1) linspace(1,2,N2) linspace(2,3,N3)],...
                            full(root.T),'k'); itg.fda = itg.fda+1;
    fig.dat(itg.fda) = plot([linspace(0,1,N1) linspace(1,2,N2) linspace(2,3,N3)],...
                            full(nom_opt.T),'k--'); itg.fda = itg.fda+1;
    fig.label(itg.lab) = ylabel('Temperature $T$ [$^{\circ}$C]'); itg.lab = itg.lab+1;
    fig.label(itg.lab) = xlabel('Reactor Coordinate [-]'); itg.lab = itg.lab+1;

    yyaxis right
    fig.dat(itg.fda) = plot([linspace(0,1,N1) linspace(1,2,N2) linspace(2,3,N3)],...
                            full(root.wNH3)*100,'r'); itg.fda = itg.fda+1;
    hold on
    fig.dat(itg.fda) = plot([linspace(0,1,N1) linspace(1,2,N2) linspace(2,3,N3)],...
                            full(nom_opt.wNH3)*100,'r--'); itg.fda = itg.fda+1;
    fig.label(itg.lab) = ylabel('Mass fraction NH$_3$ $w$ [wt.\%]'); itg.lab = itg.lab+1;
    fig.label(itg.lab) = legend('Original $\mathbf{u}$','Optimized $\mathbf{u}$','Location','SE'); itg.lab = itg.lab+1;
    legend BOXOFF 
    itg.fig = itg.fig + 1;
    if dec.ratio
        fig.in(itg.fig) = figure(itg.fig);
        set(fig.in(itg.fig),'units','centimeters');
        set(gcf, 'position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
        hold on
        yyaxis left
        fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
        fig.axes(itg.fig).YAxis(1).Color = 'k';
        fig.axes(itg.fig).YAxis(2).Color = 'r';
        fig.dat(itg.fda) = plot([linspace(0,1,N1) linspace(1,2,N2) linspace(2,3,N3)],...
                                full(root.wN2)*100,'k'); itg.fda = itg.fda+1;
        fig.dat(itg.fda) = plot([linspace(0,1,N1) linspace(1,2,N2) linspace(2,3,N3)],...
                                full(nom_opt.wN2)*100,'k--'); itg.fda = itg.fda+1;
        fig.label(itg.lab) = ylabel('Mass fraction N$_2$ $w$ [wt.\%]'); itg.lab = itg.lab+1;
        yyaxis right
        fig.dat(itg.fda) = plot([linspace(0,1,N1) linspace(1,2,N2) linspace(2,3,N3)],...
                                full(root.wNH3)*100,'r'); itg.fda = itg.fda+1;
        hold on
        fig.dat(itg.fda) = plot([linspace(0,1,N1) linspace(1,2,N2) linspace(2,3,N3)],...
                                full(nom_opt.wNH3)*100,'r--'); itg.fda = itg.fda+1;
        fig.label(itg.lab) = ylabel('Mass fraction H$_2$ $w$ [wt.\%]'); itg.lab = itg.lab+1;
        fig.label(itg.lab) = xlabel('Reactor Coordinate [-]'); itg.lab = itg.lab+1;
    end
end

% Plotting of the change in the decision variables
if dec.scan
    for k = aux(Var.n ~= 0)
        fig.in(itg.fig) = figure(itg.fig);
        set(fig.in(itg.fig),'units','centimeters');
        set(gcf, 'position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
        fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
        fig.dat(itg.fda:itg.fda+2) = plot(SS_opt{k}.Var, SS_opt{k}.u(1:3,:)); itg.fda = itg.fda+n.u;
        fig.label(itg.lab) = ylabel('Split ratio'); itg.lab = itg.lab+1;
        switch k
            case 1
                fig.label(itg.lab) = xlabel('Mass flow $\dot{m}$ [kg/s]'); itg.lab = itg.lab+1;
            case 2
                fig.label(itg.lab) = xlabel('Pressure $p$ [bar]'); itg.lab = itg.lab+1;
            case 3
                fig.label(itg.lab) = xlabel('Temperature $T$ [$^{\circ}$C]'); itg.lab = itg.lab+1;
            case 4
                fig.label(itg.lab) = xlabel('Inlet mass fraction NH$_3$ [-]'); itg.lab = itg.lab+1;
            case 5
                fig.label(itg.lab) = xlabel('H$_2$/N$_2$ ratio [-]'); itg.lab = itg.lab+1;
            case 6
                fig.label(itg.lab) = xlabel('Catalyst activity [-]'); itg.lab = itg.lab+1;
            case 8
                fig.label(itg.lab) = xlabel('Purge ratio [\%]'); itg.lab = itg.lab+1;
        end
        if k ~=8
            xlim(Var.lub(:,k)+in.nom(k))
        else
            xlim(Var.lub(:,k)*1e2)
        end
        fig.label(itg.lab) = legend('$u_1$','$u_2$','$u_3$','Location','SW'); itg.lab = itg.lab+1;
        legend BOXOFF 
        itg.fig = itg.fig + 1;
        
        fig.in(itg.fig) = figure(itg.fig);
        set(fig.in(itg.fig),'units','centimeters');
        set(gcf, 'position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
        fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
        fig.dat(itg.fda) = plot(SS_opt{k}.Var,SS_opt{k}.f); itg.fda = itg.fda+1;
        fig.label(itg.lab) = ylabel('Cost function J [\$/s]'); itg.lab = itg.lab+1;
        switch k
            case 1
                fig.label(itg.lab) = xlabel('Mass flow $\dot{m}$ [kg/s]'); itg.lab = itg.lab+1;
            case 2
                fig.label(itg.lab) = xlabel('Pressure $p$ [bar]'); itg.lab = itg.lab+1;
            case 3
                fig.label(itg.lab) = xlabel('Temperature $T$ [$^{\circ}$C]'); itg.lab = itg.lab+1;
            case 4
                fig.label(itg.lab) = xlabel('Inlet mass fraction NH$_3$ [-]'); itg.lab = itg.lab+1;
            case 5
                fig.label(itg.lab) = xlabel('H$_2$/N$_2$ ratio [-]'); itg.lab = itg.lab+1;
            case 6
                fig.label(itg.lab) = xlabel('Catalyst activity [-]'); itg.lab = itg.lab+1;
            case 8
                fig.label(itg.lab) = xlabel('Purge ratio [\%]'); itg.lab = itg.lab+1;
        end
        if k ~=8
            xlim(Var.lub(:,k)+in.nom(k))
        else
            xlim(Var.lub(:,k)*1e2)
        end
        legend BOXOFF 
        itg.fig = itg.fig + 1;
        if k == 8
            if dec.Ropt
                fig.in(itg.fig) = figure(itg.fig);
                set(fig.in(itg.fig),'units','centimeters');
                set(gcf, 'position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
                fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
                fig.dat(itg.fda) = plot(SS_opt{k}.Var,SS_opt{k}.u(5,:)+3); itg.fda = itg.fda+1;
                fig.label(itg.lab) = ylabel('H$_2$/N$_2$ ratio [-]'); itg.lab = itg.lab+1;
                fig.label(itg.lab) = xlabel('Purge ratio [\%]'); itg.lab = itg.lab+1;
                xlim(Var.lub(:,k)*1e2)
                legend BOXOFF 
                itg.fig = itg.fig + 1;
            end
        end
        
    end
    if dec.root
        fig.in(itg.fig) = figure(itg.fig);
        set(fig.in(itg.fig),'units','centimeters');
        set(gcf, 'position', [5+itg.fig,5+itg.fig, fig.dimw, fig.dimh]);
        fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
        for k = 1:numel(Var.n)
            subplot(2,2,k)
            fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
            fig.dat(itg.fda) = plot(SS_opt{k}.Var, (SS_opt{k}.f-SS_root{k}.f)./SS_opt{k}.f*100,'r'); itg.fda = itg.fda+1;
            fig.label(itg.lab) = ylabel('Loss [\%]'); itg.lab = itg.lab+1;
            switch k
                case 1
                    fig.label(itg.lab) = xlabel('Mass flow $\dot{m}$ [kg/s]'); itg.lab = itg.lab+1;
                case 2
                    fig.label(itg.lab) = xlabel('Pressure $p$ [bar]'); itg.lab = itg.lab+1;
                case 3
                    fig.label(itg.lab) = xlabel('Temperature $T$ [$^{\circ}$C]'); itg.lab = itg.lab+1;
                case 4
                    fig.label(itg.lab) = xlabel('Inlet mass fraction NH$_3$ [-]'); itg.lab = itg.lab+1;
                case 5
                    fig.label(itg.lab) = xlabel('H$_2$/N$_2$ ratio [-]'); itg.lab = itg.lab+1;
                case 6
                    fig.label(itg.lab) = xlabel('Catalyst activity [-]'); itg.lab = itg.lab+1;
            end
            itg.fig = itg.fig+1;
            xlim(Var.lub(:,k)+in.nom(k))
        end
    end
end

% Redefinition of the labels
set(fig.label, 'Interpreter', 'Latex');
set(fig.label, 'Fontsize', fig.Font(1));
set(fig.axes,'TickLabelInterpreter', 'Latex');
set(fig.axes,'Fontsize', fig.Font(2));
end
