
% This script is used for the tuning of the Master controller defined via
% self-optimizing control
% It utilizes the function "AmmonReac_TCont.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'])

savename = ['Tun_Mas_' 'dist1234_MS'];

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

% Definition of version
dec.save = true;
dec.Open = false;
dec.redH = false;
dec.H    = [2:4];
dec.Cont = [1:2 1:2];

% Definition of the tuning parameter tauC (in case of the Master, a
% multiplier of theta)
tauC.S = 10*ones(3,1);
tauC.M = 1*ones(3,1);

% Calculation of the integral gain
load([dirmain '\Data\Tun_Slave.mat'])
KS.I = 1./k_s./tauC.S;

        % With others open
%         KM.p0 = -[.3060;   -0.198;      1.25];
%         KM.p = -[0.1892;   -0.1227;    .1];
%         KM.I = -[1.4355-3; -2.0826e-3;  .02];
   
% Load of the number of disturbances and inputs
KM.p = zeros(3,1);
KM.I = zeros(3,1);
[~,dae,~,~,~,n] = AmmonReac_NewDist(1,KS,KM,H{2});
N = size(dae.x,1)-2*n.u;

% Definition of the initial values
x0 = [x0; zeros(n.u*2,1)];
z0 = [z0; p0(1:n.u); z0(2*N+1:2*N+n.u); zeros(n.u,1)];
        
% Definition of the initial guesses for the controller
KM.p0 = [zeros(n.u,1) -[0.28  -0.15  1.25]' [0.05 0.05 1.25]' [0.2  0.3 1.25]'];
    
if dec.Open
    KM.p0(:,2) = -[.30 -0.15 1.25]';
end

%% Definition of the controller for master control

for H_var = dec.H
    
    if H_var == 2
        dec.Open = true;
        dec.Cont = [1:2];
    else
        dec.Open = false;
        dec.Cont = [1:2 1:2];
    end
    
    % Resetting of KM
    KM.p = zeros(n.u,1);
    KM.I = zeros(n.u,1);
    
    % Predefinition of the controller gains
    if dec.Open
        KM.p = zeros(n.u,1);
        KM.I = zeros(n.u,1);
    elseif H_var == 2
        KM.p(3) = 1;
        KM.I(3) = 0.05;
    else
        KM.p(3) = 1;
        KM.I(3) = 0.05;
    end
    
    for m = dec.Cont
        % Predefinition of the controller gains
        if dec.Open
            KM.p = zeros(n.u,1);
        end

        % Readjustment of the tunings
        KM.p(m) = KM.p0(m,H_var);
        KM.I(m) = 0;

        % Definition of the Master inlet values
        z0(end-2:end) = H{H_var}*y0;

        % Predefinition of tuning values
        tun.Nmax  = 100;
        tun.D     = zeros(tun.Nmax,1);
        tun.dyu   = zeros(tun.Nmax,1);
        tun.dyp   = zeros(tun.Nmax,1);
        tun.dyinf = zeros(tun.Nmax,1);
        tun.dys   = 2;
        tun.k     = 2;
        tun.mul   = 0.01;

        % Definition of the integration step and the total time
        t.int = 20;
        if H_var == 2 && m == 2
            t.tot = 100*60;
        else
            t.tot = 50*60;
        end
        t.N  = t.tot/t.int;
        t.samp = (0:t.int:t.tot)/60;
        
            
        % Definition of the variation
        p.sim = [H{H_var}*y0; dec.redH*(-8.73); zeros(n.d-1,1)]*ones(1,t.N+1);
        p.sim(m,5*60/t.int+1:t.N+1) = H{H_var}(m,:)*y0+tun.dys;

        % Predefinition of the figure
        figure(H_var)
        subplot(1,n.u,m)
        plot([0 t.tot/60],[0 0],'k--')
        hold on
        xlabel('Time [min]')
        ylabel('\Delta y')

        while tun.D(tun.k-1) < 0.3 && tun.k <= tun.Nmax

            % Increase and decrease of the multiplier for the increase
            if tun.k > 3
                if tun.D(tun.k-1)-tun.D(tun.k-2) > 0.01
                    tun.mul = tun.mul/5;
                elseif tun.D(tun.k-1)-tun.D(tun.k-2) < 0.01 && 0.3-tun.D(tun.k-1) > 0.02
                    tun.mul = tun.mul*5;
                end
            end
            
            % Loop breakage for too high Kp values
            if abs(KM.p(m)) > abs(10*KM.p0)
                fprintf('\nThe iteration failes to converge to a feasible point.\n')
                break
            end

            % Reduction in the tuning parameter
            KM.p(m) = KM.p(m)+sign(KM.p(m))*tun.mul;

            % Definition of the function, the DAE, and the inlet conditions
            [func,dae] = AmmonReac_NewDist(t.int,KS,KM,H{H_var});

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

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

            end

            c.M.sim = z.sim(end-n.u+1:end,:);
            time.sim(tun.k) = toc(time.in.sim);

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

            % Plotting of the values
            figure(H_var)
            subplot(1,n.u,m)
            plot(t.samp,der)
            pause(0.01)

            % Definition of the gain for the first overshoot and the first undershoot
            if tun.dys < 0
                [tun.dyp(tun.k),tun.l(tun.k)] = min(der);
                tun.dyu(tun.k) = max(der(tun.l(tun.k):end));
            else
                [tun.dyp(tun.k),tun.l(tun.k)] = max(der);
                tun.dyu(tun.k) = min(der(tun.l(tun.k):end));
            end

            % Calculation of the infinite gain
            % tun.dyinf(tun.k) = 0.45*(tun.dyp(tun.k)+tun.dyu(tun.k));
            tun.dyinf(tun.k) = der(end);
%             abs(.45*(tun.dyp(tun.k)+tun.dyu(tun.k))-der(end))./der(end)*100


            % Calculation of the Overshoot D
            tun.D(tun.k) = (tun.dyp(tun.k)-tun.dyinf(tun.k))/tun.dyinf(tun.k);

            % Increase of k by 1
            tun.k = tun.k+1;

            fprintf('The overshoot for K_p = %.2f  is given by %2.2f%%.\n',KM.p(m),tun.D(tun.k-1)*100)

        end

        % Removal of the last value
        tun.k = tun.k-1;

        % Calculation of the tuning parameters according to the SIMC rules
        tun.B = (tun.dys-tun.dyinf(tun.k))/tun.dyinf(tun.k);
        tun.A = 1.152*tun.D(tun.k)^2-1.607*tun.D(tun.k)+1;
        tun.r = 2*tun.A/tun.B;

        tun.Kc0 = KM.p(m);
        tun.tp  = t.samp(tun.l(tun.k))*60-60;
        tun.km = 1/(tun.Kc0*tun.B);

        tun.theta = tun.tp*(0.309+0.209*exp(-0.61*tun.r));
        tau.one   = tun.r*tun.theta;
        tun.tauC  = tauC.M(m)*tun.theta;
        tun.Kc    = 1/tun.km*tau.one/(tun.theta+tun.tauC);
        tun.tauI  = min(tau.one,4*(tun.theta+tun.tauC));
        KM.p(m)   = tun.Kc;
        KM.I(m)   = tun.Kc/tun.tauI;

        % Redefinition of the calculated values for saving
        Mas.Kp0(m,H_var)     = KM.p0(m,H_var);
        Mas.tau_one(m,H_var) = tau.one;
        Mas.theta(m,H_var)   = tun.theta;
        Mas.k(m,H_var)       = tun.km;
        Mas.Kp(m,H_var)      = KM.p(m);
        Mas.KI(m,H_var)      = KM.I(m);

        fprintf('The controller tuning for controller %1i and H matrix %1i is finished.\n\n',m,H_var)
    end
    Mas.Kp(3,H_var)      = KM.p(3);
    Mas.KI(3,H_var)      = KM.I(3);
end

% Save of the results
if dec.save
    KM.p(3,2) = 1;
    KM.I(3,2) = 0.05;
    save([dirmain '\Data\' savename],'Mas')
end
