
% 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.int  = true;
dec.plot = true;
dec.scan = false;
dec.root = false;
dec.red  = false;
dec.save = false*dec.scan;

% Definition of the save name
if dec.red
    savename = 'SS_opt_6dist_red.mat';
else
    savename = 'SS_opt_6dist_rspace.mat';
end

% Definition of the total integration time and the number of time steps for
% the integrator
t.tot  = 100*60;
t.int = 30;
t.N  = t.tot/t.int;
t.samp = (0:t.int:t.tot)/60;

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

% Definition of the variation
if dec.scan
    Var.n = zeros(numel(in.nom),1);
    Var.n(1) = 31;
    Var.n(2) = 101;
    Var.n(3) = 101;
    Var.n(4) = 33;
    Var.n(5) = 31;
    Var.n(6) = 61;
%     Var.lub = [-15 -50 -50 -0.04 -0.1 -0.4;...
%                 15  50  50  0.04  0.2  0.2];
% 
%     Var.lub = [-15 -50 -50 -0.04 -0.1 -0.4;...
%                 15  50  50  0.04  0.2  0.2];
    
    Var.n(1) = 76;
    Var.n(2) = 41;
    Var.n(3) = 41;
    Var.n(4) = 31;
    Var.n(5) = 61;
    Var.n(6) = 61;
    Var.lub = [-7.5 -20 -20 -0.015 -0.3 -0.4;...
                7.5  20  20  0.015  0.3  0.2];
    dec.int  = false;
    dec.save = true;
else
    Var.n = 1;
    dec.save = false;
end


% Load of the model
[func,dae,d,lb,ub,n,Cineq,ToutFun] = AmmonReac_NewDist(t.int);

% Definition of the integrations step-changes
in_int = zeros(n.d,t.N+1);
% in_int(1,10*60/t.int+1:100*60/t.int) = 3.4;
in_int(2,10*60/t.int+1:end) = -2.2;
% in_int(3,10*60/t.int+1:end) = -2.2;
% in_int(4,10*60/t.int+1:75*60/t.int) = -0.2;
% in_int(2,10*60/t.int+1:75*60/t.int) = -20;
% in_int(5,10*60/t.int+1:75*60/t.int) = -0.2;

% Definition of the initial guesses and redefinition into x and z
load([dirmain '\Data\Init_In.mat'])

%% 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));
g_root = substitute(g_root,dae.p(1:n.u),p0(1:n.u));

% 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);

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

% Definition of indices used in the solution
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.w = [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.R = [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.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+10);

ind.u = (3*N+11):(3*N+13);

% 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.w = full(root.comb.o0(ind.w));
root.R = full(root.comb.o0(ind.R));
root.f = -(root.z(N)-(in.nom(4)))*(in.nom(1)+dec.red*(-8.73));
x0 = root.x;
z0 = root.z;

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

if dec.int
    % Predefinition of the outlet vectors and integrating of the DAE system
    sub = in_int;
    var0.old.x = x0;
    var0.old.z = z0;
    var0.old.p = [p0; zeros(n.d,1)];
    sol_int.old = int_sys(func,var0.old,t,sub);
end

%% Part b) Steady state optimization of the system

% Combination of the variables
w_opt = [dae.x; dae.z; dae.p(1:n.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)-1,1); -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:n.d;

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.y = zeros(n.y,Var.n(k));
nom_opt.z = zeros(n.z,Var.n(k));

nom_opt.T = zeros(N+3,Var.n(k));
nom_opt.w = zeros(N+3,Var.n(k));
nom_opt.R = zeros(N+3,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
    in.SS(k,:) = linspace(Var.lub(1,k),Var.lub(2,k),Var.n(k));
end

for i = 1:size(in.SS,2)
    % Substitution of the disturbance values
    in.dist = in.SS(:,i)+dec.red*[-8.73;zeros(n.d-1,1)];
    g_opt = substitute(gadd,d,in.dist);
    J = substitute(dae.quad,d,in.dist);

    % 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.y(:,i) = full(nom_opt.comb.x(ind.y));
    nom_opt.z(:,i) = full(nom_opt.comb.x(ind.z));
    
    nom_opt.T(:,i) = full(nom_opt.comb.x(ind.T));
    nom_opt.w(:,i) = full(nom_opt.comb.x(ind.w));
    nom_opt.R(:,i) = full(nom_opt.comb.x(ind.R));
    
    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)   = -(root_scan.z(N,i)-(in.dist(4)+in.nom(4)))*(in.nom(1)+in.dist(1)+dec.red*(-8.73));
    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
SS_opt{k} = struct('Var',in.SS(k,:)+in.nom(k),'z',nom_opt.z,...
                     'u',nom_opt.u,'x',nom_opt.x,'y',nom_opt.y,'f',nom_opt.f);
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);

% Addition of the disturbance
if isstruct(SS_opt{1})
    SS_opt{1}.Var = SS_opt{1}.Var+dec.red*(-8.73);
end

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

if dec.int
    % Definition of the DAE system and simulation of it
    % Redefinition of the initial guesses
    x0 = nom_opt.x;
    z0 = nom_opt.z;
    p0 = nom_opt.u;

    % Predefinition of the outlet vectors and integrating of the DAE system
    var0.opt.x = x0;
    var0.opt.z = z0;
    var0.opt.p = [p0; zeros(n.d,1)];
    sol_int.opt = int_sys(func,var0.opt,t,sub);
end


%% Part c) Calculation and plotting of the results

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

% Check of the Jacobian
% Jac = jacobian(nlp.g, nlp.x);
% figure(itg.fig)
% 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.fig) = gca;
    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.w)*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.w)*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;
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.fig) = gca;
        fig.dat(itg.fda:itg.fda+n.u-1) = plot(SS_opt{k}.Var, SS_opt{k}.u); 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;
        end
        xlim(Var.lub(:,k)+in.nom(k))
        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.fig) = gca;
        fig.dat(itg.fda) = plot(SS_opt{k}.Var,-SS_opt{k}.f); itg.fda = itg.fda+1;
        fig.label(itg.lab) = ylabel('Extent of reaction $\xi$ [kg/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;
        end
        xlim(Var.lub(:,k)+in.nom(k))
        legend BOXOFF 
        itg.fig = itg.fig + 1;
    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.fig) = gca;
        for k = 1:numel(Var.n)
            subplot(2,2,k)
            fig.axes(itg.fig) = gca;
            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
if dec.int
    % Plotting of the dynamics of the outlet temperature
    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.fig) = gca;
    fig.dat(itg.fda) = plot(t.samp,sol_int.old.x(end,:),'k');
    hold on
    fig.dat(itg.fda) = plot(t.samp,sol_int.opt.x(end,:),'k--');
    fig.label(itg.lab) = ylabel('Temperature $T_{30}$ [$^{\circ}$C]'); itg.lab = itg.lab+1;
    fig.label(itg.lab) = xlabel('Time $t$ [min]'); itg.lab = itg.lab+1;
    fig.label(itg.lab) = legend('Original $\mathbf{u}$','Optimized $\mathbf{u}$','Location','E'); itg.lab = itg.lab+1;
    legend BOXOFF 
    itg.fig = itg.fig + 1;
end

% Redefinition of the labels
set(fig.label, 'Interpreter', 'Latex');
set(fig.label, 'Fontsize', fig.Font(1));
% set(fig.label(itg.lab-1), 'Fontsize', fig.Font(2));
% set(fig.label(7), 'Fontsize', fig.Font(2));
set(fig.axes,'Box',           'on');
set(fig.axes,'TickLabelInterpreter', 'Latex');
set(fig.axes,'Fontsize', fig.Font(2));
set(fig.axes,'LineWidth',     .75);
set(fig.axes,'XMinorTick',    'on');
set(fig.axes,'YMinorTick',    'on');
set(fig.dat, 'LineWidth',     .75)
end