
% 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,'\Util3. Splinter 3.0']));
setup
import casadi.*

% Definition of decision variables
dec.plot  = true;
dec.H     = 1;
dec.Htype = 'In_Out'; % Norm_2, Norm_1, Norm_inf, Max_Gain, In_Out
dec.QP    = true;
dec.H_Set = true;

% 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 options for the grid
Var.lbmub = [-10.5 -2.2 -2.2 -0.01 -0.2  0 -0.0  0.0 -0.1;...
               0    0    0    0     0    0  0    0    0;...
               0    2.2  2.2  0.01  0.2  0  0.0  0.0  0.0];
Var.lbmub(:,2) = [-5 0 5]';
Var.traind = [2 9];
opt.Np     = 21;
opt.Ns     = 3;
opt.val    = 'n';
opt.type   = 'CG';

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

% Definition of the number of measurements in the QP approach
n.y_used = 2;

% Definition of the save and load name
if dec.QP
    savename_H = ['H_dist12345_QP_n' num2str(n.y_used) '.mat'];
    loadname_H = ['H_dist12345_QP_n' num2str(n.y_used) '.mat'];
else
    savename_H = ['H_dist12345_' dec.Htype '.mat'];
    loadname_H = ['H_dist12345_' dec.Htype '.mat'];
end

% Load of the initial guesses and the H matrices
load([dirmain '\Data\Init_In.mat'])
load(['Local Data\' loadname_H])

% Definition of the options for casadi
opts = struct('warn_initial_bounds',false, ...
              'print_time',false, ...
              'ipopt',struct('print_level',1) ...
              );

%% 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});
cost = Function('cost',{[w_root;dae.p]},{dae.quad});
G = rootfinder('G','newton',g_fun);
[root.comb] = G('i0',w0);

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

if ~exist('ind','var')
    % 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);

    ind.y_used = ind.y;
    ind.y_red = [34 1 11 12 22 23 33];
end

% 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.f = -full(cost([root.comb.o0;p0;zeros(6,1)]));
x0 = root.x;
z0 = root.z;

% Print of the found results
fprintf('The initialization is performed.\n\n')

%% Part b) Calculation of the training set

% Load of the optimal values
load([dirmain '\Data\Init_Opt.mat'])
y0 = [y0(2); x0(1:10); y0(4); x0(11:20); y0(6); x0(21:30); y0(1)]; 

% Combination of the variables
w_opt = [dae.x; dae.z];
lbw   = [lb.x;  lb.z];
ubw   = [ub.x;  ub.z];
w0    = [x0;    z0];

% Definition of the varying matrix
in_tra = GridDef(Var.lbmub,Var.traind,opt);
n.tra = length(in_tra.out);

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

tra.T = zeros(N+3,n.tra);
tra.w = zeros(N+3,n.tra);
tra.R = zeros(N+3,n.tra);

tra.p = zeros(n.p,n.tra);
tra.u = zeros(n.u,n.tra);

tra.f    = zeros(1,n.tra);
tra.Tout = zeros(1,n.tra);

time.in.tra = tic;
for k = 1:size(in_tra.out,1)
    % Substitution of the disturbance values
    g_sol = substitute(g,d,in_tra.out(k,1:n.d));
    g_sol = substitute(g_sol,dae.p(1:n.u),p0(1:n.u)+in_tra.out(k,n.d+1:end)');

    % Solving of the non-linear systems of equations
    g_fun = Function('g_fun',{w_opt},{g_sol});
    time.in.trait = tic;
    G = rootfinder('G','newton',g_fun);
    tra.comb = G('i0',w0);
    time.trait(k) = toc(time.in.trait);

    % Reassignment of the solution vector
    tra.a(:,k) = full(tra.comb.o0);
    tra.f(k)   = -full(cost([tra.a(:,k); in_tra.out(k,:)']));
    
    % Reassignment of variables
    tra.x(:,k) = tra.a(ind.x,k);
    tra.z(:,k) = tra.a(ind.z,k);
    
    tra.T(:,k) = tra.a(ind.T,k);
    tra.w(:,k) = tra.a(ind.w,k);
    tra.R(:,k) = tra.a(ind.R,k);
    
    tra.u(:,k) = p0(1:n.u)'+in_tra.out(k,n.d+1:end);
    tra.p(1:n.u,k) = tra.u(:,k);
    
    tra.Tout(k) = full(ToutFun([tra.a(:,k);in_tra.out(k,n.d+1:end)'+p0(1:n.u);in_tra.out(k,1:n.d)']));
    
%     w0 = nom_tra.a(:,k);
    % Print of the progession
    if mod(k,50) == 0
        fprintf('%4i iterations out of %4i in the training space are completed.\n',k,n.tra)
    end
end
time.tra = toc(time.in.tra);

% Print of the found results
fprintf('The training space is sampled.\n\n')

%% Part c) Calculation of the values using a fixed H

if dec.H_Set
   
    w_SOC = [w_opt; dae.p(1:n.u)];
    w0    = [w0; p0(1:n.u)];
    
    % Adjustment of the options
    Var.H_setind = 2;
    optH_set = opt;
    optH_set.Np = n.tra;
    
    % Definition of the varying matrix
    in_H_set = GridDef(Var.lbmub,Var.H_setind,optH_set);
    n.H_set = length(in_H_set.out);
    
    % Combination of the self-optimizing control variable
    g_SOC = [g; H{dec.H}(Var.traind(2)-n.d,:)*w_SOC(ind.y_used)-H{dec.H}(Var.traind(2)-n.d,:)*y0(ind.y_red)];
    aux = 1:n.u;
    g_SOC = [g_SOC; dae.p(aux(aux ~= Var.traind(2)-n.d)) - p0(aux(aux ~= Var.traind(2)-n.d))];

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

    H_set.T = zeros(N+3,n.H_set);
    H_set.w = zeros(N+3,n.H_set);
    H_set.R = zeros(N+3,n.H_set);

    H_set.p = zeros(n.p,n.H_set);
    H_set.u = zeros(n.u,n.H_set);

    H_set.f    = zeros(1,n.H_set);
    H_set.Tout = zeros(1,n.H_set);

    time.in.H_set = tic;
    for k = 1:size(in_H_set.out,1)
        % Substitution of the disturbance values
        g_sol = substitute(g_SOC,d,in_H_set.out(k,1:n.d));

        % Solving of the non-linear systems of equations
        g_fun = Function('g_fun',{w_SOC},{g_sol});
        time.in.trait = tic;
        G = rootfinder('G','newton',g_fun);
        H_set.comb = G('i0',w0);
        time.trait(k) = toc(time.in.trait);

        % Reassignment of the solution vector
        H_set.a(:,k) = full(H_set.comb.o0);
        H_set.f(k)   = -full(cost([H_set.a(:,k); in_H_set.out(k,1:n.d)']));

        % Reassignment of variables
        H_set.x(:,k) = H_set.a(ind.x,k);
        H_set.z(:,k) = H_set.a(ind.z,k);

        H_set.T(:,k) = H_set.a(ind.T,k);
        H_set.w(:,k) = H_set.a(ind.w,k);
        H_set.R(:,k) = H_set.a(ind.R,k);

        H_set.u(:,k) = H_set.a(ind.u,k);
        H_set.p(1:n.u,k) = H_set.u(:,k);

        H_set.Tout(k) = full(ToutFun([H_set.a(:,k);in_H_set.out(k,1:n.d)']));

    %     w0 = nom_tra.a(:,k);
        % Print of the progession
        if mod(k,50) == 0
            fprintf('%4i iterations out of %4i in the training space are completed.\n',k,n.H_set)
        end
    end
    time.H_set = toc(time.in.H_set);

    % Print of the found results
    fprintf('The H space is sampled.\n\n') 
    
end


%% Part d) Plot of the results

close all
f_plot = reshape(tra.f,       [opt.Np opt.Np]);
T_plot = reshape(tra.Tout,    [opt.Np opt.Np]);
A_plot = reshape(in_tra.grid(:,1),[opt.Np opt.Np])+in_nom(Var.traind(1));
B_plot = reshape(in_tra.grid(:,2),[opt.Np opt.Np])+p0(Var.traind(2)-n.d,:);

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

% Definition of figure and values
fig.dimw = 16;
fig.dimh = 12;
fig.Font = [18 18];
fig.label = [];
fig.ann   = [];

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;
surfc(A_plot,B_plot,-f_plot,'EdgeColor','none')
fig.label(itg.lab) = xlabel('Pressure \itp_{in}\rm'); itg.lab = itg.lab+1;
fig.label(itg.lab) = ylabel('Parameter \itu_{3}\rm'); itg.lab = itg.lab+1;
fig.label(itg.lab) = zlabel('-\it\xi\rm'); itg.lab = itg.lab+1;
ylim([0.2 0.3])
hold on
itg.fig = itg.fig+1;
itg.axe = itg.axe+1;
% line(in_H_set.grid+in_nom(Var.traind(1)),...
%      H_set.u(Var.traind(2)-n.d,:),...
%      -H_set.f,'linewidth',2);
% zlim([16 20])


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;
contour(A_plot,B_plot,T_plot)%,'EdgeColor','interp')
fig.label(itg.lab) = xlabel('Pressure \itp_{in}\rm'); itg.lab = itg.lab+1;
fig.label(itg.lab) = ylabel('Parameter \itu_{3}\rm'); itg.lab = itg.lab+1;
% fig.label(itg.lab) = zlabel('\itT_{out}\rm'); itg.lab = itg.lab+1;
ylim([0.2 0.3])
hold on
% line(in_H_set.grid+in_nom(Var.traind(1)),...
%      H_set.u(Var.traind(2)-n.d,:),...
%      H_set.Tout,'linewidth',2);
% zlim([420 460])
itg.fig = itg.fig+1;
itg.axe = itg.axe+1;

set(fig.label, 'Fontsize',      fig.Font(1));
set(fig.axes,  'FontSize',      fig.Font(2));
set(fig.axes,  'Box',           'on');
set(fig.axes,  'LineWidth',     1.5);