%% OLGA 7 Simulation
clear all;  clc; %close all;
%% ****** Connect to OLGA *********
da=OLGA_connect(); %Create Data Access Object from OPC server
%% ******** Set up variables in Matlab from OLGA Server *******
[OLGA,var]=OLGA_variables_setup(da);

%% ****** Saves and Loads Initial OLGA OPC Server State *****
%OLGA_load_initial_server_state(var);

%% ****** Simulation Variables and Logging Variables *******
U=[]; Y=[]; X=[]; %Log Control data (U), and Measurement Data (Y)          
simTime=300*60;%600*60; %To to Simulate OLGA [Seconds]
controlStart=simTime/3;%simTime/3;      % Control start-time [s], needed for                         
controlEnd=simTime;        % Time for turning off controller.
X_est=[];               % Estimated State
Y_est=[];               % Esimated Measurement
U_inner=[];

%% **** Parameters for simplified model ******** %%
par = v1_gaslift_parameters();
z1=0.4;    % production choke opening (u1) 
z2=0.8;    % injection valve opening(u2) 
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
uss=u0;
%[A B C D]=central_difference(x0',u,par);
%%
linsys=load('linsys');
A=linsys.A; B=linsys.B; C=linsys.C; D=linsys.D;
%% ******* PI Controller Parameters **********
Kp=-1/10; %-1/120; %Proportional Gain PI-controller
Ti=100;%200;   %Integral Time PI-Controller
e_past=0;
e_past2=0;

%% LQR parameters
K=[];
%% ********* UKF Filter Parameters **************** %%
noise = wgn(simTime,1,1,'linear'); %Measurement white noise
v=cov(noise);
% P=[0.020 -0.001   0.010;
%   -0.001   0.020  -0.040;
%    0.010  -0.040  100];
% 
% P=5*diag([2 1 3]);
% x0_hat=[3100;220;8000];

P=diag([1 1 1]);
e_est=0;
estimationOn=1;

%% ********* PI Filter Parameters **************** %%
y_est=0;

%% ********* START OLGA SIMULATION ********
startedControl=0;
Ts=1; %Sampling Time [s]
disp('OLGA SIMULATION STARTET');
for t=1:Ts:simTime      
    %% Get measurments from OLGA
    [y u x]=OLGA_get_measurement(var);
    X=[X;x'];
    U=[U;u]; %Log control signal
    Y=[Y;y];  %Log outputs
    if t==1
        x_sp=x;
        y0=y;
    end
    
    if estimationOn
        u0(1)=u;        
        if t==1            
            x0_hat=x;            
        end 
        y(7)=y(7)+noise(t); % Add white noise to Pbh measurement
        %[x_hat y_est P] = augmented_UKF_weights(x0_hat,y',P,u0,par);  %real Pbh measurement
        [x_hat y_est P] = augmented_UKF_weights(x0_hat,y',P,u0,par,v);  %noisy Pbh measurment
        x0_hat=x_hat;    
        
        Y_est=[Y_est;y_est'];
        X_est=[X_est;x_hat'];
    end
    %% Send Control Signal to OLGA
    if t>controlStart && t<controlEnd 
        if startedControl==0
            startedControl=t;
        end
        
        %% SISO CONTROL
        saturation='on';
        %[u0(1) e_past]=PIcontroller(Kp,Ti,y0(7),Y(:,7),u,e_past,saturation); % real measurement  of BH olga
        [u0(1) e_past]=PIcontroller(Kp,Ti,y0(7),Y_est(:,5),u,e_past,saturation); % estimated  BH olga  
        
        %% LQR CONTROL
        %[u0(1) e_past] =LQR_controller_Pat_int(A,B,C,D,x,x_sp,uss,y0(5),y(5),e_past,u0(1)); %Measured state 
        %[u0(1) e_past] =LQR_controller_Pat_int(A,B,C,D,x,x_sp,uss,y0(5),y_est(2),e_past,u0(1)); %Estimated state
        %% Cascade Control
        if (0)
          % Outer loop annulus top pressure
            Kp=-3;%-5;
            Ti=450;%50;%450;%500;%20000;
            saturation='off';
            [u_inner_setpoint e_past]=PIcontroller(Kp,Ti,y0(5),Y(:,5),u0(1),e_past,saturation);
            U_inner=[U_inner; u_inner_setpoint];
          %Inner Loop tubing top pressure
            Kp=0.02;%0.012;%0.017;
            Ti=1e10;%50;
            saturation='on';
            [u0(1) e_past2]=PIcontroller(Kp,Ti,u_inner_setpoint,Y(:,6),u0(1),e_past2,saturation); 
        end
        %% Apply control signal to olga
        OLGA_update_control(var.prod_choke,u0(1));
    end
    
    %% Simulate OLGA 1 second    
    OLGA_simulate(var,t,simTime,Ts);      
end
disp('OLGA SIMULATION ENDET');

%% ****** Delete, Disconnect and Close OLGA Server Connection *****
OLGA_finished(da);
%% ******** Plotting *******
slutt=simTime;%60*300;
estimationON=1;
%OLGA_plotting(1:Ts:slutt,y0,Y(1:slutt,:),U(1:slutt,:),startedControl)
OLGA_new_plotting(1:Ts:slutt,y0,Y(1:slutt,:),U(1:slutt,:),startedControl,estimationON,Y_est,U_inner,X,X_est,noise)
if estimationOn
    %OLGA_plotting_estimation(1:Ts:slutt,Y(1:slutt,:),Y_est(1:slutt,:),X_est(1:slutt,:),X(1:slutt,:),y0);
end
%%
%save('PI_Pbhest_meat_Ptt_Pat_Pbhnoisy_Ts_1')

%%
%slutt=60*300;
%if estimationOn
%    noisy_plot(Y(1:slutt,:),Y_est(1:slutt,:),noise(1:slutt,:),1:slutt)
%end

