% 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'])
import casadi.*

% Definition of decision variables
dec.slave = true;
dec.loss  = true;
dec.save  = false;
dec.plot  = true;
dec.red   = false;
dec.new_y = false;
dec.redH  = true*dec.red;

% Definition of the chosen H and the measured disturbances
H_var = [4];          % Calculated and considered H matrices
dist_var = [1 2 3 4];       % Considered disturbances for SOC
if numel(dist_var) > 4
    H_var = H_var(H_var ~= 2);
end

if ~dec.loss
    dec.plot = false;
end

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

% Definition of the save name
if dec.save
    savename = 'SS_opt_SOC_dist';
    savename_H = 'H_dist';
    for k = 1:6
        if any(dist_var == k)
            savename = [savename num2str(k)];
            savename_H = [savename_H num2str(k)];
        end
    end
    if dec.slave
        savename   = [savename '_MS'];
        savename_H = [savename_H '_MS'];
    end
    if dec.red
        savename   = [savename '_red'];
    end
    if dec.redH
        savename   = [savename '_redH'];
        savename_H = [savename_H '_redH'];
    end
    savename   = [savename '.mat'];
    savename_H = [savename_H '.mat'];
end    

% Definition of feed variables
in.nom = zeros(4,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 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,[dec.redH*(-8.73); zeros(n.d-1,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));

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

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.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))];

% Substitution of the inlet values
g = substitute(gadd,d,[dec.redH*(-8.73); zeros(n.d-1,1)]);
J = substitute(dae.quad,d,[dec.redH*(-8.73); zeros(n.d-1,1)]);

% Definition of the NLP function
nlp = struct('x',w_opt,'f',J,'g',g);
solver = nlpsol('solver','ipopt',nlp);
nom_opt.comb = solver('x0',w0,'lbx',lbw,'ubx',ubw,'lbg',lbg,'ubg',ubg);

% Reassignment of variables
nom_opt.x = full(nom_opt.comb.x(ind.x));
nom_opt.z = full(nom_opt.comb.x(ind.z));
nom_opt.y = full(nom_opt.comb.x(ind.y));

nom_opt.T = full(nom_opt.comb.x(ind.T));
nom_opt.w = full(nom_opt.comb.x(ind.w));
nom_opt.R = full(nom_opt.comb.x(ind.R));

nom_opt.u = full(nom_opt.comb.x(ind.u));
nom_opt.p = [full(nom_opt.u); zeros(4,1)];
nom_opt.f = full(nom_opt.comb.f);

fprintf('\n\n\nThe steady-state optimization is completed. \n\n\n')

%% Part c) Calculation of the sensitivity matrix F

% Definition of the variation
pert_opt.dist = [-1 -1 -1 -0.001 -0.05 -0.05]*1;
n.pd = numel(dist_var);

% Predefinition of the solution vectors
pert_opt.x = zeros(n.x,n.pd);
pert_opt.z = zeros(n.z,n.pd);
pert_opt.y = zeros(n.y,n.pd);
pert_opt.u = zeros(n.u,n.pd);

% Definition of the varying vector
in.sens = diag(pert_opt.dist)-[8.73*dec.redH*ones(1,n.d);zeros(n.d-1,n.d)];
pert_opt.count = 1:n.d;
k = 1;

% Solving of the system in the case of disturbances
for j = pert_opt.count(dist_var)
    
    % Substitution of the inlet values
    g = substitute(gadd,d,in.sens(:,j));
    J = substitute(dae.quad,d,in.sens(:,j));

    % Definition of the NLP function
    nlp = struct('x',w_opt,'f',J,'g',g);
    solver = nlpsol('solver','ipopt',nlp);
    pert_opt.comb = solver('x0',w0,'lbx',lbw,'ubx',ubw,'lbg',lbg,'ubg',ubg);

    % Reassignment of variables
    pert_opt.x(:,k) = full(pert_opt.comb.x(ind.x));
    pert_opt.y(:,k) = full(pert_opt.comb.x(ind.y));
    pert_opt.z(:,k) = full(pert_opt.comb.x(ind.z));
    pert_opt.u(:,k) = full(pert_opt.comb.x(ind.u));
    pert_opt.f(:,k) = full(pert_opt.comb.f);
    k = k+1;
end

% Calculation of the disturbance
pert_opt.dd = ones(n.y,1)*pert_opt.dist(dist_var);

% Calculation of the sensitivity matrix
F = (pert_opt.y - nom_opt.y*ones(1,n.pd))./pert_opt.dd;

%% Part d) Calculation of the plant gain matrix Gy by manipulating one DOF

% Predefinition of the solution vectors
pert_con.x = zeros(n.x,n.u);
pert_con.y = zeros(n.y,n.u);
pert_con.z = zeros(n.z,n.u);
pert_con.u = zeros(n.u,n.u);

w0 = full(nom_opt.comb.x);
aux = 1:n.u;

% Solving of the system in the case of disturbances
for j = 1:n.u
    
    % Switch in betwee having a low level slave controller or not
    if dec.slave
        g = vertcat(gadd, (dae.z(2*N+j) - (nom_opt.z(2*N+j)-1)));
        g = vertcat(g,    (dae.z(2*N+aux(aux~=j)) - (nom_opt.z(2*N++aux(aux~=j)))));
    else
        g = vertcat(gadd, (dae.p(j) - nom_opt.u(j)*.99));
        g = vertcat(g, (dae.p(aux(aux~=j)) - nom_opt.u(aux(aux~=j))));
    end
    
    % Substitution of the inlet values
    g = substitute(g,d,[dec.redH*(-8.73); zeros(n.d-1,1)]);
    J = substitute(dae.quad,d,[dec.redH*(-8.73); zeros(n.d-1,1)]);

    % Definition of the NLP function
    nlp = struct('x',w_opt,'f',J,'g',g);
    solver = nlpsol('solver','ipopt',nlp);
    pert_con.comb = solver('x0',w0,'lbx',lbw,'ubx',ubw,'lbg',[lbg;0;0;0],'ubg',[ubg;0;0;0]);

    % Reassignment of variables
    pert_con.x(:,j) = full(pert_con.comb.x(ind.x));
    pert_con.y(:,j) = full(pert_con.comb.x(ind.y));
    pert_con.z(:,j) = full(pert_con.comb.x(ind.z));
    pert_con.u(:,j) = full(pert_con.comb.x(ind.u));
    pert_con.f(:,j) = full(pert_con.comb.f);
end

% Calculation of the setpoint change
if dec.slave
    pert_con.du = pert_con.y(2:2:end,:)-nom_opt.y(2:2:end)*ones(1,n.u);
else
    pert_con.du = pert_con.u-nom_opt.u*ones(1,n.u);
end
pert_con.du = ones(n.y,1)*diag(pert_con.du)';

% Calculation of the plant gain matrix
Gy = (pert_con.y - nom_opt.y*ones(1,n.u))./pert_con.du;

% Calculation of the relative gain array
RGA.nom = Gy.*pinv(Gy)';

%% Part e) Application of the exact local method on the system

% Definition of the scaling
noise.dist = [7 20 25 0.008 0.3 0.1];
% noise.meas = ones(1,n.y)*4;
% noise.meas = [ones(1,n.x+n.u)*4 ones(1,n.x+n.u)*0.01];
noise.meas = ones(1,numel(ind.y))*4;
Wd = diag(noise.dist(dist_var));
Wn = diag(noise.meas);

% Adjustment of the measured disturbances
% mod_d = [2:4];
% mod_y = 1:7;
% F = F(mod_y,mod_d);
% Wd = Wd(mod_d,mod_d);
% Wn = Wn(mod_y,mod_y);
% Gy = Gy(mod_y,:);

% Definition of the measurement indices
ind.m1 = [1  11];
ind.m2 = [12 22];
ind.m3 = [23 33];

% Self-optimizing control based on direct selection
H{1} = zeros(1,7);
H{1}(1,2) = 1;
H{1}(2,4) = 1;
H{1}(3,6) = 1;

% SOC using the nullspace method
H{2} = null(F')';

% SOC using the Exact local method for all inputs
Y  = [F*Wd Wn];
H{3} = ((Y*Y')\Gy)';

% SOC using the exact local method for different inputs
Y1 = [F(ind.m1,:)*Wd Wn(ind.m1,ind.m1)];
Y2 = [F(ind.m2,:)*Wd Wn(ind.m2,ind.m2)];
Y3 = [F(ind.m3,:)*Wd Wn(ind.m3,ind.m3)];
H1 = ((Y1*Y1')\Gy(ind.m1,1))';   
H2 = ((Y2*Y2')\Gy(ind.m2,2))';
H3 = ((Y3*Y3')\Gy(ind.m3,3))';
H{4}  = [zeros(3,1) blkdiag(H1,H2,H3)];

% Scaling of the H matrix
for l = H_var
    H{l}(1,:) = H{l}(1,:)/max(abs(H{l}(1,:)));
    H{l}(2,:) = H{l}(2,:)/max(abs(H{l}(2,:)));
    H{l}(3,:) = H{l}(3,:)/max(abs(H{l}(3,:)));
end

% Calculation of the RGA for the control structures
for j = H_var
    RGA.H{j} = (H{j}*Gy).*pinv(H{j}*Gy)';
end

% Definition of the number of different H matrices
n.H = numel(H);

% Save of the results
if dec.save
    save([dirmain '\Data\' savename_H],'H','noise')
end

%% Part f) Root finding for the new system

if dec.loss

% Definition of the variation
Var.n = zeros(6,1);
Var.n(1) = 31;
Var.n(2) = 101;
Var.n(3) = 51;
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.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  35  0.04  0.2  0.2];
% Var.n(1) = 41;
% Var.n(2) = 41;
% Var.n(3) = 41;
% Var.n(4) = 21;
% Var.n(5) = 31;
% Var.n(6) = 61;
% Var.lub = [-5 -20 -20  0.00 -0.1 -0.4;...
%             5  20  20  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];


% Combination of the variables
w   = [dae.x; dae.z; dae.p(1:n.u)];
w0  = full(nom_opt.comb.x);

% Predefinition of variables and load of the optimal values
SS_SOC = cell(n.d,n.H);
if dec.red
    in.var = -[8.73;zeros(n.d-1,1)];
    load([dirmain '\Data\SS_opt_6dist_red.mat'])
else
    in.var = zeros(n.d,1);
    load([dirmain '\Data\SS_opt_6dist.mat'])
%     load([dirmain '\Data\SS_opt_6dist_rspace.mat'])
end

% Recalculation of the setpoints based on new optimum or not
if dec.new_y
    % Definition of the new inlet
    din = [-5; -10; 0; 0; 0; -0.1];
    
    % Substitution of the inlet values
    g = substitute(gadd,d,din);
    J = substitute(dae.quad,d,din);
    
    % Definition of the NLP function
    nlp = struct('x',w_opt,'f',J,'g',g);
    solver = nlpsol('solver','ipopt',nlp);
    nom_opt.comb = solver('x0',w0,'lbx',lbw,'ubx',ubw,'lbg',lbg,'ubg',ubg);

    nom_opt.y = full(nom_opt.comb.x(ind.y));
end

% Clearing of the auxiliary variable
clear aux

for l = H_var
    
if l == 2 && dec.red
    Var.n(3) = 86;
    Var.lub(2,3) = 35;
elseif l == 2 && ~dec.red
    Var.n(3) = 96;
    Var.lub(2,3) = 45;
else
    Var.n(3) = 101;
    Var.lub(2,3) = 50;
end
        
% Combination of the equality constraints to keep the setpoint constant
g_org = vertcat(dae.ode,dae.alg,H{l}*w(ind.y)-H{l}*nom_opt.y);

for j = 1:n.d
    in.SOC{j,1} = linspace(Var.lub(1,j),Var.lub(2,j),Var.n(j));
    time.in.loss(j,l) = tic;
    for k = 1:Var.n(j)
        % Substitution of the inlet values
        in.dist = in.var;
        in.dist(j) = in.dist(j)+in.SOC{j}(k);
        g = substitute(g_org,d,in.dist);

        % Definition and solving of the system
        g_fun = Function('g_fun',{w},{g});
        G = rootfinder('G','newton',g_fun);
        aux.comb = G('i0',w0);
        aux.x(:,k) = full(aux.comb.o0(ind.x));
        aux.y(:,k) = full(aux.comb.o0(ind.y));
        aux.z(:,k) = full(aux.comb.o0(ind.z));
        aux.u(:,k) = full(aux.comb.o0(ind.u));
        aux.f(k)   = -(aux.z(N,k)-(in.dist(4)+in.nom(4)))*(in.nom(1)+in.dist(1));
        w0 = full(aux.comb.o0);
    end
    w0  = full(nom_opt.comb.x);
    
    % Combination into a structure
    SS_SOC{j,l} = struct('Var',in.SOC{j}+in.nom(j)+dec.red*in.var(j),'z',aux.z,...
                         'u',aux.u,'x',aux.x,'y',aux.y,'f',aux.f);
                     
    % Calculation of the loss
    if numel(SS_opt{j}.Var) == numel(SS_SOC{j,l}.Var)
    if sum(abs(diff([SS_opt{j}.Var;SS_SOC{j,l}.Var]))) < 1e-6
        SS_SOC{j,l}.Var = SS_opt{j}.Var;
    end
    end
    SS_SOC{j,l}.loss = (SS_opt{j}.f(end,ismember(SS_opt{j}.Var,SS_SOC{j,l}.Var))-SS_SOC{j,l}.f(end,:))'...
                        ./SS_opt{j}.f(end,ismember(SS_opt{j}.Var,SS_SOC{j,l}.Var))'*100;
%     SS_SOC{j,l}.loss = (SS_opt{j}.f(end,:)-SS_SOC{j,l}.f(end,:))'...
%                         ./SS_opt{j}.f(end,:)'*100;

    % Sampling of the time
    time.loss(j,l) = toc(time.in.loss(j,l));
    clear aux
    fprintf('The steady-state simulation for the disturbance %1i and the H matrix %1i is finished. \n',j,l)
end
end

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


%% Part g) Plotting of the results

if dec.plot

    % Calculation of the loss for each of the cases
    if ~isfield(SS_SOC{1,1},'loss')
    for j = 1:n.d
        for l = H_var
            if numel(SS_opt{j}.Var) == numel(SS_SOC{j,l}.Var)
                if sum(abs(diff([SS_opt{j}.Var;SS_SOC{j,l}.Var]))) < 1e-6
                    SS_SOC{j,l}.Var = SS_opt{j}.Var;
                end
            end
            SS_SOC{j,l}.loss = (SS_opt{j}.f(end,ismember(SS_opt{j}.Var,SS_SOC{j,l}.Var))-SS_SOC{j,l}.f(end,:))'...
                                ./SS_opt{j}.f(end,ismember(SS_opt{j}.Var,SS_SOC{j,l}.Var))'*100;
        end
    end
    end

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

    % Definition of figure and values
    fig.xlab = {'Flowrate [kg/s]','Pressure [bar]','Temperature [C]',...
                'Mass fraction [-]','Ratio H$_2$/N$_2$ [-]','Catalyst activity [-]'};
    fig.xlim = [];
    fig.d     = [5 25 25 0.02 0.1 0.2];
    for k = 1:n.d
        fig.xlim = [fig.xlim;
                    floor(round(SS_opt{k}.Var(1)/fig.d(k)))*fig.d(k) ...
                    fig.d(k) ...
                    ceil(round(SS_opt{k}.Var(end)/fig.d(k)))*fig.d(k)];
    end
    fig.dimw  = 12;
    fig.dimh  = 7;
    fig.label = [];
    fig.Font  = [10 8];
    fig.ann   = [];
    fig.sub   = [];

    % Definition of the colours
    rk = {'k','k--','r','r--'};
    H_cell = {'T','NSM','ELM$_1$','ELM$_2$'};

    % Initialization of the figure
    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]);

    % Plotting of the loss with flowrate change
    for k = 1:n.d
        fig.sub(k) = subplot(2,ceil(n.d/2),k);
        fig.axes(k) = gca;
        xlim(fig.xlim(k,[1 3]))
        hold on
        for l = H_var
            plot(SS_SOC{k,l}.Var,SS_SOC{k,l}.loss,rk{l})
        end
        fig.axes(k).XTick = fig.xlim(k,1):fig.xlim(k,2):fig.xlim(k,3);
        fig.label(itg.lab) = xlabel(fig.xlab{k}); itg.lab = itg.lab+1;
        fig.axes(k).LineWidth = 1;
        if mod(k-1,n.d/2) == 0
            fig.label(itg.lab) = ylabel('Loss [\%]'); itg.lab = itg.lab+1;
        end
        switch k
            case 1
                fig.label(itg.lab) = legend(H_cell{H_var},'Location','best'); itg.lab = itg.lab+1;
                legend BOXOFF 
            case 4
                fig.axes(k).XTick = [0.04 0.06 0.08 0.10 0.12];
                fig.axes(k).XTickLabel = {'0.04', '0.06', '0.08', '0.10', '0.12'};
            case 5
                ylim([0 0.02])
                fig.axes(k).YTick = 0:0.005:0.02;
                fig.axes(k).YTickLabel = {'0.000','0.005','0.010','0.015','0.020'};
        end
    end

    % Modificiation of the plotting properties
    set(fig.ann, 'Interpreter', 'Latex');
    set(fig.ann, 'LineStyle', 'none');
    set(fig.label, 'Interpreter', 'Latex');
    set(fig.axes,'TickLabelInterpreter', 'Latex');
    set(fig.axes,'XMinorTick', 'on');
    set(fig.ann, 'Fontsize', fig.Font(1));
    set(fig.label, 'Fontsize', fig.Font(1));
    set(fig.label(3), 'Fontsize', fig.Font(2));
    set(fig.axes,'Fontsize', fig.Font(2));
end