function [sys,x0,str,ts] = separator_3phases_w_weir(t,x,u,flag,R,rho_gasS,MW_gas,rho_oilS,...
    rho_oil,rho_waterS,rho_water,R_sep,L_sep,n,hweir,Lweir,Tgas,h_oil_meas,hw0,ho0,p0,PAHH,PALL,Oil_LAHH,Oil_LALL,W_LAHH,W_LALL,Ts,Tag)
% For simplicity, this model assumes that oil is always filling up the
% water side (up to the weir height)


% Parameters:
% R                   Gas constant [J/mol K]
% rho_gasS            Gas density at standard conditions [kg/Sm3]
% MW_gas              Molar weight of gas [kg/mol]
% rho_oilS            Oil density at standard conditions [kg/Sm3]
% rho_oil             Oil density [kg/m3]
% rho_waterS          Water density at standard conditions [kg/Sm3]
% rho_water           Water density [kg/m3]
% R_sep               Separator radius [m]
% L_sep               Separator length [m]
% n                   Radii_ratio = n:1. Must be a positive number
% hweir               Weir height [m]
% Lweir               Length to weir [m]
% Tgas                Gas temperature [K]
% h_oil_meas          Height of oil outlet [m]
% hw0                 Initial water level [m]
% ho0                 Initial oil level [m]
% p0                  Initial gas pressure [bara]
% PAHH                High gas pressure trip level [bara]
% PALL                High gas pressure trip level [bara]
% Oil_LAHH            High level trip, oil [m]
% Oil_LALL            Low level trip, oil [m]
% W_LAHH              High level trip, water [m]
% W_LALL              Low level trip, water [m]
% Ts                  Sampling time
%
% Inputs:
% 1. Gas rate in [Sm3/s]
% 2. Oil rate in [Sm3/s]
% 3. Water rate in [Sm3/s]
% 4. Gas rate out [Sm3/s]
% 5. Oil rate out [Sm3/s]
% 6. Water rate out [Sm3/s]
%
% Outputs:
% 1. Gas pressure [bara]
% 2. Oil level [m]
% 3. Water level [m]
% 4. Pressure @ oil output [bara]
% 5. Pressure @ water output [bara]



switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
    [sys,x0,str,ts] = mdlInitializeSizes(R_sep,L_sep,n,hweir,Lweir,hw0,ho0,p0);
  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2,                                                
    sys = mdlUpdate(t,x,u,R,rho_gasS,MW_gas,rho_oilS,rho_oil,rho_waterS,rho_water,R_sep,L_sep,n,hweir,Lweir,Tgas,h_oil_meas,PAHH,PALL,Oil_LAHH,Oil_LALL,W_LAHH,W_LALL,Ts,Tag); 
    
  %%%%%%%%%%
  % Output %
  %%%%%%%%%%
  case 3,                                                
    sys = mdlOutputs(t,x,u,rho_oil,rho_water,h_oil_meas);

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case {1,4,9}, % unused flags                  
    sys = []; % do nothing

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    error(['unhandled flag = ',num2str(flag)]);
end

%
%=======================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=======================================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes(R_sep,L_sep,n,hweir,Lweir,hw0,ho0,p0)

sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 6;
sizes.NumOutputs     = 5;
sizes.NumInputs      = 6; %Changed Espen 030305
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;

sys = simsizes(sizes);

%caluclate initial water volume

if hw0 <= hweir
    Vw0 = WaterVol(hw0,R_sep,Lweir,L_sep,n,hweir);
else
    errordlg('Water level above weir plate');
end
    
    
%caluclate initial oil volume
VwWeir = cyl(hweir,R_sep,Lweir) + one_ellip(hweir,R_sep,n);
Vo0 = OilVol(ho0,R_sep,Lweir,L_sep,n,hweir,Vw0);
V_sep=R_sep^2*pi*L_sep+(pi/4)*(4/3)*R_sep^3;
Vg0 = V_sep - Vo0 - Vw0;

if ((Vg0 < 0) | (Vg0 > V_sep))
   errordlg('Something wrong with initial gas volume');
end 
x0  = [hw0 ho0 Vg0 p0*1e5 Vw0 Vo0]; % initialize states
str = [];
ts  = [-1 0]; 

% end mdlInitializeSizes
function sys = mdlUpdate(t,x,u,R,rho_gasS,MW_gas,rho_oilS,rho_oil,rho_waterS,rho_water,R_sep,L_sep,n,hweir,Lweir,Tgas,h_oil_meas,PAHH,PALL,Oil_LAHH,Oil_LALL,W_LAHH,W_LALL,Ts,Tag)
% Convert inputs
persistent errorStatusOil errorStatusWater errorStatusPressure
if ~isfield(errorStatusOil,Tag)
    errorStatusOil=setfield(errorStatusOil,Tag,0)
    errorStatusWater=setfield(errorStatusWater,Tag,0)
    errorStatusPressure=setfield(errorStatusPressure,Tag,0)
end
n_gas_in = u(1)*rho_gasS/MW_gas; % Sm3/s -> mol/s
n_gas_out = u(4)*rho_gasS/MW_gas; % Sm3/s -> mol/s
Q_oil_in = u(2)*rho_oilS/rho_oil; % Sm3/s -> m3/s
Q_oil_out = u(5)*rho_oilS/rho_oil; % Sm3/s -> m3/s
Q_water_in = u(3)*rho_waterS/rho_water; % Sm3/s -> m3/s
Q_water_out = u(6)*rho_waterS/rho_water; % Sm3/s -> m3/

% calculate water volume if water level is equal to weir height:
VwWeir = cyl(hweir,R_sep,Lweir) + one_ellip(hweir,R_sep,n);
% calculate oil volume if oil level is equal to weir height:
VoWeir = cyl(hweir,R_sep,L_sep-Lweir) + one_ellip(hweir,R_sep,n);


% options = optimset('LargeScale','off');
options = optimset('LargeScale','off','Display','off');

% water volume
DeltaVw = (Q_water_in-Q_water_out)*Ts;
if (x(5)+DeltaVw) <= VwWeir, % water level below weir
    sys(1) = fzero(@hwBelowWeir,[x(1)],options,R_sep,Lweir,n,x(5)+DeltaVw);
    if isreal(sys(1)) == 0
        disp('Water volume: Complex solution');
    end
else % water level above weir
    errordlg('WATER LEVEL ABOVE WEIR');
end
if sys(1) <= W_LALL & ~getfield(errorStatusWater,Tag),
    errordlg('Trip on water level (LALL)');
    errorStatusWater=setfield(errorStatusWater,Tag,1);
elseif sys(1) >= W_LAHH & ~getfield(errorStatusWater,Tag),
    errordlg('Trip on water level (LAHH)');
    errorStatusWater=setfield(errorStatusWater,Tag,1);
elseif getfield(errorStatusWater,Tag) & (sys(1) < W_LAHH) & (sys(1) > W_LALL)
    errorStatusWater=setfield(errorStatusWater,Tag,0);
end
% oil level
DeltaVo = (Q_oil_in-Q_oil_out)*Ts;

if (x(6)+DeltaVo) <= VoWeir, % oil level below weir
    sys(2) = fzero(@hoBelowWeir,[x(2)],options,R_sep,L_sep-Lweir,n,x(6)+DeltaVo);
    if isreal(sys(1)) == 0
        disp('Oil volume: Complex solution');
    end
else % oil level possibly above weir
        sys(2) = fzero(@hoAboveWeir,[x(2)],options,R_sep,L_sep,n,x(6)+DeltaVo,x(5)+DeltaVw,hweir,Lweir,VoWeir);
    if isreal(sys(1)) == 0
        disp('Oil volume: Complex solution');
    end
end

if sys(2) <= Oil_LALL & ~getfield(errorStatusOil,Tag),
    errordlg('Trip on oil level (LALL)');
    errorStatusOil=setfield(errorStatusOil,Tag,1);
elseif sys(2) >= Oil_LAHH & ~getfield(errorStatusOil,Tag),
    errordlg('Trip on oil level (LAHH)');
    errorStatusOil=setfield(errorStatusOil,Tag,1);
elseif getfield(errorStatusOil,Tag) & (sys(2) < Oil_LAHH) & (sys(2) > Oil_LALL)
    errorStatusOil=setfield(errorStatusOil,Tag,0);
end
% Gas volume
sys(5) = x(5)+DeltaVw;
sys(6) = x(6)+DeltaVo;
V_sep=R_sep^2*pi*L_sep+(pi/4)*(4/3)*R_sep^3;
sys(3) = V_sep-(x(5)+DeltaVw)-(x(6)+DeltaVo);
if ((sys(3) < 0) | (sys(3) > V_sep))
    errordlg('Something wrong with calculated gas volume');
end
% Gas pressure
sys(4) = x(4) + ((n_gas_in-n_gas_out)*R*Tgas*Ts)/x(3)...
    -(Ts*x(4)/x(3))*(-Q_oil_in+Q_oil_out-Q_water_in+Q_water_out);
if sys(4) <= PALL*1e5 & ~getfield(errorStatusPressure,Tag),
    errordlg('Trip on gas pressure (PALL)');
    errorStatusPressure=setfield(errorStatusPressure,Tag,1);
elseif sys(4) >= PAHH*1e5 & ~getfield(errorStatusPressure,Tag),
    errordlg('Trip on gas pressure (PAHH)');
    errorStatusPressure=setfield(errorStatusPressure,Tag,1);
elseif getfield(errorStatusPressure,Tag) & (sys(4) < PAHH*1e5) & (sys(4) > PALL*1e5)
    errorStatusPressure=setfield(errorStatusPressure,Tag,0);
end
% end mdlUpdate
%
%=======================================================================
% mdlOutputs
% Return the output vector for the S-function
%=======================================================================
%
function sys = mdlOutputs(t,x,u,rho_oil,rho_water,h_oil_meas)
sys(1) = x(4)/(1e5); % Pa -> bara/bar
sys(2) = x(2); % Oil level above bottom
sys(3) = x(1);
sys(4) = (x(4) + rho_oil*9.81*(x(2)-h_oil_meas))/(1e5);
sys(5) = (x(4) + rho_oil*9.81*(x(2)-x(1)) + rho_water*9.81*x(1))/(1e5);
% end mdlOutputs

function WaterVol = WaterVol(hw,R,Lweir,L,n,hweir)
if hw <= hweir % water below weir
    WaterVol = cyl(hw,R,Lweir) + one_ellip(hw,R,n);
else % water above weir -> This is not valid
    errordlg('Water above weir');
    WaterVol = -1;
end


function OilVol = OilVol(ho,R,Lweir,L,n,hweir,Vw0) % Exluding oil on water side
if ho <= hweir % oil below weir
    OilVol = cyl(ho,R,L-Lweir) + one_ellip(ho,R,n);
else 
    OilVol = cyl(ho,R,L) + 2*one_ellip(ho,R,n) ... % as if only oil is in the separator
            - Vw0; % Subtract water
%    OilVol = cyl(ho,R,L)-cyl(hweir,R,Lweir) + 2*one_ellip(ho,R,n) - one_ellip(hweir,R,n); % Disregard water side
end

function cyl=cyl(h,R,L)
if h<0
    h=0;
elseif h>2*R
    h=2*R;
end;
root = 2*R*h-h^2;
cyl  = L*((h-R)*sqrt(root)+R^2*(asin((h-R)/R)+pi/2));

function one_ellip=one_ellip(h,R,n)
one_ellip = (pi/(2*n*n))*(R*h*h-(h*h*h)/3);

function f = hoBelowWeir(ho,R,Ldiff,n,Vo)
f = cyl(ho,R,Ldiff)+one_ellip(ho,R,n)-Vo;
f = (f/Vo);

function f = hoAboveWeir(ho,R,L,n,Vo,Vw,hweir,Lweir,VoWeir)
f = cyl(ho,R,L)+2*one_ellip(ho,R,n)-Vo-Vw;
f = (f/(Vo+Vw));


function f = hwBelowWeir(hw,R,Lweir,n,Vw)
f = cyl(hw,R,Lweir)+(pi/(2*n*n))*(R*hw*hw-(hw*hw*hw)/3)-Vw;
f = (f/Vw);

function f = hwAboveWeir(hw,R,L,n,Vw,VoWeir)
f = cyl(hw,R,L)+2*one_ellip(hw,R,n)-VoWeir-Vw;
f = (f/(Vw+VoWeir));


