% Simulation file for CV-CV active constraint switching using selectors
% tested on a CSTR model from Economou et al. 
% Simulation results published in Krishnamoorthy & Skogestad, Ind. Eng.
% Chem. Res., Bonvin Festschrift
%
% Author: Dinesh Krishnamoorthy, dineshk@ntnu.no
% Sep 2018, NTNU

clear all
clc

% addpath ('C:\Users\dineshk\CasADi\casadi-matlabR2014b-v3.1.0-rc1')
import casadi.*

%% Load Model
par.dist = [0.75;0.0];
[par,diff,x_var,u_var,p_var,d_var,L,F_integrator,f] = CSTR_model(par);

F_in = 1;
T_in = 422;
xf = par.dx0;

%% Prepare EKF


y_model = x_var;

[f_EKF,JacFx,h_EKF,JacHx,JacAx,JacBu,JacJx,JacJu] = prepareAEKF(diff,x_var,d_var,u_var,y_model,par,L);

ny = length(y_model);
nxEKF = numel(vertcat(x_var,d_var));
xk_hat = [par.dx0;par.dist];
uEKF = par.u0(1:2);
Pk = 1e3.*eye(nxEKF);
Qk = 1e3.*eye(nxEKF);
Rk = 1e3.*eye(ny);
x_hat =par.dx0;
%% Controller setup

Ju_hat = [0;0];

GC.u = 432.6+2;%w_opt_SS(4);
GC.err0 = 0;
GC.tauC = 73;
GC.tau1 = 65;
GC.k = 2.0796e-4;
GC.theta = 2;
GC.Kp =(GC.tau1/GC.k)/(GC.tauC+GC.theta);
GC.Ki = GC.Kp/GC.tau1;

TC.u = 432.6+2;
TC.err0 = 0;
TC.tauC = 60;
TC.Kp =(1/1)/(TC.tauC);
TC.Ki = TC.Kp/1;

FC.u = 1;
FC.err0 = 0;
FC.tauC = 30;
FC.tau1 = 12;
FC.k = 0.115;
FC.theta = 1;
FC.Kp =(FC.tau1/FC.k)/(FC.tauC+FC.theta);
FC.Ki = FC.Kp/FC.tau1;

%%
nIter = 3600;
h = waitbar(0,'Simulation in Progress...');

for sim_k = 1:nIter
    
    waitbar(sim_k/nIter)
    
    if sim_k>=1200 && sim_k<2400
        par.dist = [0.9;0];
    else if sim_k>=2400
            par.dist = [1.1;0];
        else
            par.dist = [0.75;0];
        end
    end
    
    TC.u = T_in;
    GC.u = T_in;
    
    GC.err = 0-Ju_hat(1);
    GC.u = min(500,max(300,GC.u + ( GC.Kp*GC.err + GC.Ki*GC.err - GC.Kp*GC.err0))) ;
    GC.err0 = GC.err;
    
    TC.err = 425-xf(3);
    TC.u = min(500,max(300,TC.u + ( TC.Kp*TC.err + TC.Ki*TC.err - TC.Kp*TC.err0))) ;
    TC.err0 = TC.err;
    
    sim.TC.u(:,sim_k) = TC.u;
    sim.GC.u(:,sim_k) = GC.u;
    
    if F_in<1
        T_in = TC.u;
    else
        T_in = min(GC.u,TC.u);
    end
    
    T_in = min(GC.u,TC.u);
    
    FC.u = F_in;
    
    FC.err = 0.5-xf(1);
    FC.u = min(1,max(0.2,FC.u + ( FC.Kp*FC.err + FC.Ki*FC.err - FC.Kp*FC.err0))) ;
    
    F_in = min(FC.u,1);
    
    sim.FC.u(:,sim_k) = FC.u;
    
    u_in = [T_in;F_in;par.dist];
    
    % Simulator using IDAS integrator
    Fk = F_integrator('x0',xf,'p',u_in);
    xf = full(Fk.xf);
    sim.J(sim_k) = full(Fk.qf);
    sim.y(:,sim_k) = xf;
    sim.u(:,sim_k) = u_in(1:2);
    sim.d(:,sim_k) = par.dist;
    
    ymeas = [sim.y([1:3],sim_k)];
    
    
    uEKF = u_in(1:2);
    tic
    [xk_hat,Pk] = EKF_estimation(f_EKF,JacFx,h_EKF,JacHx,ymeas,xk_hat,uEKF,Pk,Qk,Rk);
    sim.time(sim_k) = toc;
    x_hat = xk_hat(1:3);
    d_hat = xk_hat(4:5);
    est.x(:,sim_k) = x_hat;
    est.d(:,sim_k) = d_hat;
    xEstErr(:,sim_k) = abs(full(x_hat) - xf);
    
    Ju_hat = EstJu(JacAx,JacBu,JacJx,JacJu,x_hat,uEKF,d_hat);
    sim.Ju(:,sim_k) = Ju_hat;
    
    
    
end

close(h);

save('CSTR1_sim0.mat','sim')

%%
load('CSTR1_sim.mat')

sim.t = (1:3600)./60;
figure(15)
clf
subplot(321)
hold all
plot(sim.t,sim.y(1,:),'linewidth',1.5)
plot(sim.t,0.5.*ones(size(sim.y(1,:))),'k:')
ylabel ('$C_A$','Interpreter','Latex')
axs = gca;
axs.TickLabelInterpreter = 'latex';
xlim([0,60])
box on

subplot(323)
hold all
plot(sim.t,sim.y(3,:),'linewidth',1.5)
plot(sim.t,425.*ones(size(sim.y(3,:))),'k:')
ylabel ('$T$','Interpreter','Latex')
axs = gca;
axs.TickLabelInterpreter = 'latex';
xlim([0,60])
box on

subplot(324)
plot(sim.t,sim.u(1,:),'linewidth',1.5,'color',[0.64,0.08,0.18])
ylabel ('$T_{i}$','Interpreter','Latex')
axs = gca;
axs.TickLabelInterpreter = 'latex';
xlim([0,60])
box on

subplot(322)
hold all
plot(sim.t,sim.u(2,:),'linewidth',1.5,'color',[0.64,0.08,0.18])
plot(sim.t,ones(size(sim.y(3,:))),'k:')
ylabel ('$F$','Interpreter','Latex')
axs = gca;
axs.TickLabelInterpreter = 'latex';
xlim([0,60])
ylim([0.6,1.1])
box on

subplot(325)
hold all
plot(sim.t,sim.Ju(1,:),'linewidth',1.5)
plot(sim.t,0.*ones(size(sim.y(1,:))),'k:')
ylabel ('$\nabla_{T_i}J$','Interpreter','Latex')
xlabel('time [min]','Interpreter','latex')
axs = gca;
axs.TickLabelInterpreter = 'latex';
xlim([0,60])
box on

subplot(326)
hold all
plot(sim.t,sim.d(1,:),'k','linewidth',1.5)
ylabel ('$C_{A_i}$','Interpreter','Latex')
xlabel('time [min]','Interpreter','latex')
axs = gca;
axs.TickLabelInterpreter = 'latex';
xlim([0,60])
box on
