%% Riser-Annulus Model Simulation
clear all; clc; warning('off');
format ('long');
global u0;

%% Inputs to model 
% Manipulated variables
z1=0.4;    % production choke opening (u1) 
z2=0.8;    % gas injection valve opening(u2) 
% Disturbances
GOR = 0;                     % Mass GOR from reservoir, NB: MASS, increasing gives: lower BH, stable
P_r = 160;                      % Rservoir pressure [bar]
P_gs = 150;                     % Gass source pressure [bar]
P0 = 15;                        % Separator Pressure [bar]
d = [GOR;P_r;P_gs;P0];         % Disturbance Vector
u0=[z1;z2;d]; %Only u(1) and u(2) are MVs

%% ***** Parameter Constants*******
par = v1_gaslift_parameters();

%% ******* Initial States and Equilibrium Point ******** 
tic                             % Start Clock
disp('************ Start search for equilibrium point *****************');
[x0,flag,y0] = v1_gaslift_initialize(u0,par); 
disp('************ End search for equilibrium point *****************');
toc                             % Stop Clock 

%% *** Linearazation and Transfer Functions *****
disp('************ Linearization and Transfer Function **************');
[A B C D]=central_difference(x0,u0,par); % State Space Model
%save('linsys','A','B','C','D');

%% ***** Simulation Parameters ****
% State Pertubation
dx=[-10;0;0;];          % State Pertuabtion
x_sp=x0;                % State Sepoint
x0=x0+dx;               % Initial Pertubated State 
x0_hat=x0*0.7;          % Initial State for Estimator, UKF

% Options for ODE-solver and Simulation
M=eye(3);               % Mass Matrix for ODE-solver
dT=1;                   % Time for Inegration 
T_span = [0 dT];        % Integrate from time=0 to dT
options=odeset('AbsTol',1e-10,'RelTol',1e-10,'MaxStep',100,'Mass',M);

% Data Log Variables
Y=[];                   % Outputs, Log of Measurements 
U=[];                   % Inputs, Log of Inputs and Disturbances
X=[];                   % States
X_hat=[];               % Estimated State
Y_est=[];               % Esimated Measurement
U_inner=[];             % Casacade Inner Setpoint
u_inner_setpoint=y0(3); % Initial Cascade Inner Setpoint

% Control, Noise and and Disturbance Settings
SimTime=350*60;          %Simulation time in seconds
disturbanceStart=225*60; % Start-time for applying disturbance [s].
disturbanceEnd=SimTime;  % Time for ending Controller Action [s].
controlStart=150*60;     %SimTime/3;%SimTime/3; % Control start-time [s]                      
controlEnd=SimTime;      % Time for turning off controller.
noise = wgn(SimTime,1,1,'linear'); %Measurement white noise 

%% ******* PI Controller Parameters **********
Kp=-1/10; %Proportional Gain PI-controller
Ti=100;   %Integral Time PI-Controller
e_past=0; %Integrated Error   
e_past2=0;%Integrated Error, inner loop cascade 
startedControl=0; %Variable to log when controller starts
%% ******* Uncented Kalman Filter Variables ********
P=5*diag([2 1 3]);
estimationON=0; % 1= ON, 0 =off
e_est=0;

%% ****** Simulation with Control and Disturbances **************
disp('********** Start Simulation of nonlinear model *******************');
u=u0; % Control Signal = Initial Control Signal
tic   % Start Clock
for i=1:1:SimTime  
    disp(['Percent:', num2str(100*i/SimTime)]);   
    
    %Integration
    x = ode4(@v1_gaslift_model,T_span,x0,u,'derivatives',par);    
    %[t,x]=ode15s(@v1_gaslift_model,T_span,x0,options,u,'derivatives',par); 
    y = v1_gaslift_model([],x(end,:),u,'measurements',par);
    x0=x(end,:);   
    X=[X;x0];              % Log State Vector
    Y=[Y;y'];              % Log Measurement 
    U=[U;u'];              % Log Control Signal   
    U_inner=[U_inner;u_inner_setpoint];
    
    % Estimation
    if (estimationON) %&& (i>=controlStart)              
        [x_hat y_est P] = augmented_UKF_weights(x0_hat,y+noise(i)*ones(8,1),P,u,par);  %ym must be a row vector 
        x0_hat=x_hat; 
        X_hat=[X_hat;x_hat'];  % Log Esimated State
        Y_est=[Y_est;y_est'];  % Log Measurement Estimate 
    end
    
    %Control
    if (i>controlStart) %&& i<=controlEnd 
        if startedControl==0
            startedControl=i;
        end
        %% SISO PI-Controllers        
        %saturation='on';
        %[u(1) e_past]=PIcontroller(Kp,Ti,y0(5),Y_est(:,5),u,e_past,saturation); %estimated PBH measurement       
        %[u(1) e_past]=PIcontroller(Kp,Ti,y0(5),Y(:,5),u,e_past,saturation);     % real PBH measurement       

        %% Cascade controllers
        if (1)
         % Outer loop annulus top pressure
            Kp=-2;%-5;
            Ti=450;%500;%20000;
            saturation='off';
            [u_inner_setpoint e_past]=PIcontroller(Kp,Ti,y0(2),Y(:,2),u,e_past,saturation); 
         %Inner Loop tubing top pressure
            Kp=0.02;%0.012;%0.017;
            Ti=1e10;%100;
            saturation='on';
            [u(1) e_past2]=PIcontroller(Kp,Ti,u_inner_setpoint,Y(:,3),u,e_past2,saturation);
        end
        
        %% LQR-Controllers     
        %[u(1) e_past] =LQR_controller(A,B,C,D,x0',x_sp,u0,y0(2),y(2),e_past,u(1)); %Measured state  
        %[u(1) e_past]=LQR_controller(A,B,C,D,x0_hat,x_sp,u0,y0(2),y(2),e_past,u(1)); % Estimated state --> LQG controller
    end
    
    %Disturbance
    if i>disturbanceStart && i<disturbanceEnd
        d=disturbance(i,SimTime);  % Disturbance vector      
        u(4)=u0(4)+d.P_res;        % Reservoir Pressure Disturbance
        %u(3)=u0(3)+d.GOR;         % Mass Gas-Oil Rato Disturbance
        %u(5)=u0(5)+d.P_gs;
    end        
end 
toc %Stop Clock
disp('********** End Simulation of nonlinear model *******************');

%% ****** Plotting ********
plotting(U,Y,1:SimTime,y0,startedControl,disturbanceStart,U_inner,X);
if (estimationON) 
    slutt=SimTime;
    plotting_states(1:slutt,X(1:slutt,:));
    plotting_estimation(1:slutt,X_hat(1:slutt,:),X(1:slutt,:),Y_est(1:slutt,:),Y(1:slutt,:));   
    noisy_plot(Y,Y_est,noise,1:SimTime)
end
