
% This script is used for the tuning of the Master controller defined via
% self-optimizing control.
% It utilizes the function "AmmonReac_NewDist.m" for set-up of the model.
% Hence, both controllers are incorporated into the model.

% 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'])

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

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

% Definition of version
dec.Ksave = false;
dec.H     = 4;
dec.plot  = true;
dec.dys   = -5;


% Choosing of the investigated controller
m = 1;

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

% Calculation of the integral gain
tauC.S = 10*ones(3,1);
KS.I = 1./k_s./tauC.S;

% Predefinition of the controller gain
KM.p = zeros(3,1);
KM.I = zeros(3,1);

%% Definition of the controller for master control

KM.p = zeros(3,4);
KM.I = zeros(3,4);
% load([dirmain '\Data\Tun_Mas_dist1234_MS.mat'])
% KM.p(1:2,dec.H) = Mas.Kp(1:2,dec.H);
% KM.I(1:2,dec.H) = Mas.KI(1:2,dec.H);

%% Open loop step response for the Master controller

% Definition of the function, the DAE, and the inlet conditions
[func,dae,~,~,~,n] = AmmonReac_NewDist(t.int,KS);
N = size(dae.x,1)-n.u;

% Definition of the initial values
x0 = [x0; zeros(n.u,1)];
z0 = [z0; p0(1:n.u)];

% Definition of the variation
p.sim = [H{1}*y0; zeros(n.d,1)]*ones(1,t.N+1);
p.sim(m,t.step/t.int+1:t.N+1) = H{1}(m,:)*y0+dec.dys;

% Predefinition of the structures
x.sim = zeros(n.x,t.N+1);           x.sim(:,1) = x0;
z.sim = zeros(n.z,t.N+1);           z.sim(:,1) = z0;
y.sim = zeros(n.y,t.N+1);           y.sim(:,1) = y0;

time.in.sim = tic;
% Integration of the system.
for j = 2:t.N+1
    func_eval = func('x0',x.sim(:,j-1),'z0',z.sim(:,j-1),'p',p.sim(:,j-1));

    % Redefinition of the meassurements and the integrated error
    x.sim(:,j) = full(func_eval.xf);
    z.sim(:,j) = full(func_eval.zf);
    aux        = [x.sim([10 20 30],j); z.sim(2*N+1:2*N+4,j)];
    y.sim(:,j) = aux([7 4 1 5 2 6 3]);

end
%%
% Reassignment of values
c.M.sim = H{dec.H}*y.sim;
c.S.sim = H{1}*y.sim;
time.sim = toc(time.in.sim);

% Calculation of the difference
c.M.der = c.M.sim(m,:)-c.M.sim(m,1);

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

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

% Plotting of the values
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]);

subplot(3,1,1)
fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
fig.dat(itg.fda) = plot(t.samp,z.sim(2*N+3*n.u+1+m,:)*100,'k'); itg.fda = itg.fda+1;
fig.label(itg.lab) = ylabel(['Splitratio u$_' num2str(m) '$ [\%]']); itg.lab = itg.lab+1;

subplot(3,1,2)
fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
fig.dat(itg.fda) = plot(t.samp,c.S.sim(m,:),'k'); itg.fda = itg.fda+1;
fig.label(itg.lab) = ylabel(['Inlet T$_' num2str(m) '$ [K]']); itg.lab = itg.lab+1;
subplot(3,1,3)
fig.axes(itg.axe) = gca; itg.axe = itg.axe+1;
fig.dat(itg.fda) = plot(t.samp,c.M.der,'k'); itg.fda = itg.fda+1;
fig.label(itg.lab) = ylabel(['$\Delta$ c$_' num2str(m) '$[K]']); itg.lab = itg.lab+1;
fig.label(itg.lab) = xlabel('Time t [min]'); itg.lab = itg.lab+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', 1);
