% Main file to simulate gas lift optimization using feedback controllers
% and selector logic, published in Control Engineering Practice.
%
% Author: Dinesh Krishnamoorthy, dineshk@ntnu.no
% Feb 2019, NTNU

close all
clear
clc

% addpath ('casadi-osx-matlabR2015a-v3.4.5')
import casadi.*

% plant model
par = GasLiftRiser_Param();
[dx0,z0,u0,lbx,lbz,lbu,ubx,ubz,ubu] = GasLiftRiser_Initialization_bounds(par);
par.tSim = 1;

par.GOR_val_SS = par.GOR + 1*[0;0;1;1.5;0;0].*par.GOR_var; % nominal GOR
par.qGLmax = 10;
par.QgMax = 30;
[F_plant,w_gl_SP,w_opt_SS] = WellSimulator(par);

% Display Nominal optimal conditions
disp('Optial Gas Lift Allocation:')
disp(w_gl_SP)
disp('Optial Gas Lift constraint:')
disp(sum(w_gl_SP))
disp('Optial gas capacity constraint:')
disp(sum(w_opt_SS(69:74)))
disp('Optial objective:')
disp(w_opt_SS(97))


xf = dx0; zf = z0;


%% Configure EKF

[f_EKF,JacFx,h_EKF,JacHx,z_EKF,yIndex,JacAx,JacBu,JacJx,JacJu,J_EKF] = EKF_integrating(par);

nxEKF =26;
nyEKF   = length(yIndex);
xk_hat  = [dx0;par.GOR];
uEKF    = u0;

Pk = 1e3.*eye(nxEKF);
Qk = 1e3.*eye(nxEKF);
Rk = 1e0.*eye(nyEKF);


%%

% Initialization of the controllers
u0 = 2.*ones(6,1);
GL_in = w_gl_SP;
nIter = 10*3600;
h = waitbar(0,'Simulation in Progress...');

% Error for equal mGOR control 
err0_2 = 0;
err0_3= 0;
err0_4 = 0;
err0_5 = 0;
err0_6 = 0;

err0Qg = 0; % Error for Max gas production constraint control
err0Ju = 0; % Error for Gradient contorller (mGOR = 0)
Ju_hat = [0;0;0;0;0;0];

Cost = 0;
GOR_real = par.GOR;% + 1.5*par.GOR_var;

GL_in1 = u0(1);
GL_in2 = u0(1);
GL_in3 = u0(1);

for sim_k = 1:nIter
    
    waitbar(sim_k/nIter)
    
    % PID controllers are tuned using SIMC tuning rules
    
    % --------- Well 2 ---------
    tauC = 200;
    tauI = min(4*tauC,800);
    Kp1 = 800/(0.29*tauC);
    Ki1 = Kp1/tauI;


    err_2 = -Ju_hat(2)+Ju_hat(1);
    if sim_k >=30
        
        GL_in(2) = max(0.1,min(12, GL_in(2) + (Kp1*err_2 + Ki1*err_2 - Kp1*err0_2)));
        
    end
    err0_2 = err_2;
    
     % --------- Well 3 ---------
    tauC = 200;
    tauI = min(4*tauC,800);
    Kp1 = 800/(0.439*tauC);
    Ki1 = Kp1/tauI;
    err_3= -Ju_hat(3)+Ju_hat(1);
    if sim_k >=30
        
        GL_in(3) = max(0.1,min(12, GL_in(3) + (Kp1*err_3 + Ki1*err_3 - Kp1*err0_3)));
        
    end
    err0_3 = err_3;
    
     % --------- Well 4 ---------
    tauC = 200;
    tauI = min(4*tauC,800);
    Kp1 = 800/(0.31*tauC);
    Ki1 = Kp1/tauI;
    err_4 = -Ju_hat(4)+Ju_hat(1);
    if sim_k >=30
        
        GL_in(4) = max(0.1,min(12, GL_in(4) + (Kp1*err_4 + Ki1*err_4 - Kp1*err0_4)));
        
    end
    err0_4 = err_4;
    
    
     % --------- Well 5 ---------
    tauC = 200;
    tauI = min(4*tauC,800);
    Kp1 = 800/(0.32*tauC);
    Ki1 = Kp1/tauI;
    err_5 = -Ju_hat(5)+Ju_hat(1);
    if sim_k >=30
        
        GL_in(5) = max(0.1,min(12, GL_in(5) + (Kp1*err_5 + Ki1*err_5 - Kp1*err0_5)));
        
    end
    err0_5 = err_5;
    
    
    % --------- Well 6 ---------
    tauC = 300;
    tauI = min(4*tauC,800);
    Kp1 = 800/(0.38*tauC);
    Ki1 = Kp1/tauI;
    err_6 = -Ju_hat(6)+Ju_hat(1);
    if sim_k >=30
        
        GL_in(6) = max(0.1,min(12, GL_in(6) + (Kp1*err_6 + Ki1*err_6 - Kp1*err0_6)));
        
    end
    err0_6 = err_6;
    
    
    % --------- Well 1 ---------
    
    % Max gas lift constraint control 
    if sim_k >=30
        GL_in1 = max(0.1,par.qGLmax - sum(GL_in(2:6)));%max(0.01,min(10, GL_in(2) + ( Kp2*err(2) + Ki2*err(2) - Kp2*err0(2)) ));
    end
    
    % Max gas prodcution constraint control
    tauC=3000;
    tauI = min(4*tauC,800);
    Kp2 = 800/(1*tauC);
    Ki2 = Kp2/tauI;
    if sim_k>1
        errQg = par.QgMax - meas.w_tg(sim_k-1) ;
    end
    if sim_k >=30
        
        GL_in2 = max(0.1,min(12, GL_in2 + (Kp2*errQg + Ki2*errQg - Kp2*err0Qg)));
        
    end
    if sim_k>1
        err0Qg = errQg;
    end
    
    % Unconstrained Optimum: mGOR = 0
    tauC = 200;
    tauI = min(4*tauC,800);
    Kp3 = 800/(0.43*tauC);
    Ki3 = Kp3/tauI;
    errJu = -0.25-Ju_hat(1);
    if sim_k >=30
        
        GL_in3 = max(0.1,min(12, GL_in3 + (Kp3*errJu + Ki3*errJu - Kp3*err0Ju)));
        
    end
    err0Ju = errJu;

    
    % ----------- vary GOR conditions in the wells -----------
    if sim_k >3*3600
        GOR_real(3) = par.GOR(3) + 1*par.GOR_var(3);
    end
    if sim_k >4*3600
        GOR_real(4) = par.GOR(4) + 1.5*par.GOR_var(4);
    end
    
    if sim_k>7*3600 && sim_k<=9*3600
        par.QgMax = 25;
    end
%     
    
    if sim_k >=30
        GL_in(1) =  min(GL_in1,min(GL_in2,GL_in3));
        
    end
    
    
    % Bumpless transfer
    
    if GL_in(1) == GL_in1
        flag(sim_k) =1;
    end
    if GL_in(1) == GL_in2
        flag(sim_k) =2;
    end
    if GL_in(1) == GL_in3
        flag(sim_k) =3;
    end
    
    GL_in1 = GL_in(1);
    GL_in2 = GL_in(1);
    GL_in3 = GL_in(1);
    
    
    %  --------- simulate plant --------- 
    u_in = [GL_in;GOR_real];
    meas.GOR_real(:,sim_k) = GOR_real;
    
    % Simulator using CVODES integrator
    Fk = F_plant('x0',xf,'z0',zf,'p',u_in);
    xf = full(Fk.xf);
    zf = full(Fk.zf);
    J_real(sim_k) = full(Fk.qf);
    
    ymeas = zf(yIndex) + (randn(nyEKF,1).*0.00);
    
    meas.p_wh(:,sim_k)    = ymeas(7:12);
    meas.p_rh(:,sim_k)    = ymeas(13);
    meas.p_m(:,sim_k)     = ymeas(14);
    meas.w_gl(:,sim_k)    = u_in(1:6);
    meas.w_to(sim_k)      = zf(77);
    meas.w_tg(sim_k)      = zf(78);
    
    
   
        % Extended Kalman filter for state and mGOR estimation
        
        Fj = full(JacFx(xk_hat,uEKF));
        
        if max(max(isnan(Fj)))
            
            disp('NaN in x EKF')
        end
        
        xk_hat_1 =  full(f_EKF(xk_hat,uEKF));
        Pk_1 = Fj*Pk*Fj' + Qk;
        
        uEKF    = [u_in(1:6)];
        
        Hj      = full(JacHx(xk_hat_1,uEKF));
        ek      = full(ymeas - h_EKF(xk_hat_1,uEKF));
        Sk      = Hj*Pk_1*Hj' + Rk;
        Kk      = (Pk_1*Hj')/(Sk);
        xk_hat  = xk_hat_1 + Kk*ek;
        Pk      = (eye(nxEKF) - Kk*Hj)*Pk_1;
        zk_hat  = full(z_EKF(xk_hat,uEKF));
        
        x_hat   = xk_hat(1:20);
        GOR_hat = xk_hat(21:26);
        GOR_est(:,sim_k) = GOR_hat;
        
        if isnan(xk_hat)
            disp('NaN in y EKF')
        end
        
        % Estimation Error
        xEstErr = abs(full(x_hat) - xf);
        zEstErr = abs(full(zk_hat) - zf);
        
        % set new initial values for the next iteration
        dx0     =  full(x_hat) + 0.0*randn(1);
        z0      = full(zk_hat) + 0.0*randn(1);
        %         u0      = [u_in(1:6)];
        d_hat = par.GOR;
        A = full(JacAx(x_hat,[uEKF;d_hat]));
        B = full(JacBu(x_hat,[uEKF;d_hat]));
        C = full(JacJx(x_hat,[uEKF;d_hat]));
        D = full(JacJu(x_hat,[uEKF;d_hat]));
        Ju_hat = (-C*(A\B) + D)'; % Estimate mGOR using FRTO method
        Ju(:,sim_k) = Ju_hat;
        
        if max(isnan(A))
            disp('NaN in A EKF')
        end
        
 
end

sim.Ju = Ju;
sim.meas = meas;
sim.t = 1:par.tSim:par.tSim*nIter;
% save('sim_OnSOCLtd.mat','sim')

close(h);
%%
% save('sim_MinSelect_CEP.mat','sim')        
    

figure(56)
clf
subplot(325)
hold all
% plot(sim.t./3600,zeros(size(sim.Ju')),'k:')
plot(sim.t./3600,0.25*ones(size(sim.t)),'k:')
plot(sim.t./3600,-sim.Ju','linewidth',1)
ylim([0,2])
title('marginal GOR [kg/kg]','interpreter','Latex')
grid on

subplot(323)
hold all
% plot(sim.t./3600,sim.meas.w_gl','linewidth',1)
plot(sim.t./3600,10.*ones(size(sim.t)),'k:','linewidth',1)
plot(sim.t./3600,(sim.meas.w_gl(1,:)+sim.meas.w_gl(2,:)+sim.meas.w_gl(3,:)+sim.meas.w_gl(4,:)+sim.meas.w_gl(5,:)+sim.meas.w_gl(6,:)),'k','linewidth',1)
box on
grid on
title( ' Total Gas lift rate [kg/s]','interpreter','Latex')
% legend('well 1','well 2','Total')

subplot(322)
plot(sim.t./3600,sim.meas.w_to','k','linewidth',1)
ylim([150,170])
box on
grid on
title( ' Total  oil [kg/s]','interpreter','Latex')


subplot(321)
hold all
plot(sim.t./3600,sim.QgMax,'k:','linewidth',1)
plot(sim.t./3600,sim.meas.w_tg','k','linewidth',1)
ylim([20,30])
title( 'Total  gas [kg/s]','interpreter','Latex')
ylabel('CV: $\sum w_{pg_i}$','interpreter','Latex')
box on
box on
grid on

subplot(324)
hold all
plot(sim.t./3600,sim.meas.w_gl','linewidth',1)
title( 'Gas lift rate [kg/s]','interpreter','Latex')
box on
grid on
xlabel ('$time $[h]','interpreter','Latex')

subplot(326)
hold all
plot(sim.t./3600,sim.meas.GOR_real','linewidth',1)
% ylim([20,30])
title( 'GOR [kg/kg]','interpreter','Latex')
box on
grid on

xlabel ('$time$ [h]','interpreter','Latex')


