function [sys,x0,str,ts] = sPumps(t,x,u,flag,Input1,Input2,nPumps,dPFlow,dPSpeed,s0)
% Simulink S-function for representing n paralell variable speed pumps with
% nonlinear characteristics
%
% Inputs:
% u(1) = Input1
% u(2) = input2
% u(3:2+nPumps) = pump speeds (vector with pump speeds)
% u(3+nPumps:2+2*nPumps) = Run signals (binary)
%
% States:
% No states. (This is a static function.)
%
% Outputs:
% sys(1) = Discharge Pressure [barg]
% sys(2:nPumps+1) = Individual Flows [Sm3/h]
%
% Parameters:
% nPumps = numper of paralell pumps
% dPFlow = dP-Flow polynomial coefficient matrix for flow at referance speed, [order x nPumps]
% dPSpeed= dP-Speed polynomial coefficient matrix, [order x nPumps]
% s0 = referance speeds [1 x nPumps]
%
%
% Parameters:
%
%

switch flag,

    %%%%%%%%%%%%%%%%%%
    % Initialization %
    %%%%%%%%%%%%%%%%%%
    case 0,
        [sys,x0,str,ts]=mdlInitializeSizes(nPumps);

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Derivatives,Update & Terminate %
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    case {1,2,9}
        sys=[]; % Do nothing

        %%%%%%%%%%%
        % Outputs %
        %%%%%%%%%%%
    case 3,
        sys=mdlOutputs(t,x,u,Input1,Input2,nPumps,dPFlow,dPSpeed,s0);

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

end

% end sfuntmpl

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts]=mdlInitializeSizes(nPumps)

%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded.  This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;

sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 1+nPumps;
sizes.NumInputs      = 3*nPumps+2;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);

%
% initialize the initial conditions
%
x0  = [];

%
% str is always an empty matrix
%
str = [];

%
% initialize the array of sample times
%
ts  = [0 0];

% end mdlInitializeSizes


%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,u,Input1,Input2,nPumps,dPFlow,dPSpeed,s0)

%% input assignment
if Input1==1;
    Ps=u(1);
    if Input2==1;
        disp('Please select 2 different inputs')
        return
    elseif Input2==2
        Pd=u(2);
        solvefor='q';
    elseif Input2==3
        Q_tot=u(2)*3600;
        solvefor='Pd';
    else
        disp('Error in input assignment')
        return
    end
elseif Input1==2
    Pd=u(1);
    if Input2==2;
        disp('Please select 2 different inputs')
        return
    elseif Input2==1
        Ps=u(2);
        solvefor='q';
    elseif Input2==3
        Q_tot=u(2)*3600;
        solvefor='Ps';
    else
        disp('Error in input assignment')
        return
    end
elseif Input1==3
    Q_tot=u(1)*3600;
    if Input2==3;
        disp('Please select 2 different inputs')
        return
    elseif Input2==1
        Ps=u(2);
        solvefor='Pd';
    elseif Input2==2
        Pd=u(2);
        solvefor='Ps';
    else
        disp('Error in input assignment')
        return
    end
else
    disp('Error in input assignment')
    return
end
Q_recycle = u(3:2+nPumps)*3600; %dedicated recycle rates for each pump
s = u(3+nPumps:2+2*nPumps); % pump speeds (vector with pump speeds)
RunSignal = u(3+2*nPumps:2+3*nPumps); % trip signals (binary)
%% Pump model

iActivePumps=find(RunSignal);
nActivePumps=length(iActivePumps);
for i=1:nActivePumps
    speedFactor(iActivePumps(i))=dPSpeed(iActivePumps(i))*(s(iActivePumps(i))-s0(iActivePumps(i)))+1;
end
if isequal(solvefor,'Pd')|isequal(solvefor,'Ps')
    q=zeros(nPumps,1);
    if nActivePumps==2
        Q=Q_tot+sum(Q_recycle(iActivePumps));
        S1=speedFactor(iActivePumps(1)); S2=speedFactor(iActivePumps(2));
        a1=dPFlow(iActivePumps(1),1);a2=dPFlow(iActivePumps(2),1);
        b1=dPFlow(iActivePumps(1),2);b2=dPFlow(iActivePumps(2),2);
        c1=dPFlow(iActivePumps(1),3);c2=dPFlow(iActivePumps(2),3);
        if S1==S2&a1==a2&b1==b2&c1==c2
            q(iActivePumps)=Q/2;
        else
            q(iActivePumps(1))=1/(2*(S2*a2-S1*a1))*(2*S2*a2*Q+S1*b1+S2*b2+sqrt(4*S2*a2*Q*S1*b1+S1^2*b1^2+2*S1*b1*S2*b2+S2^2*b2^2-4*S2^2*a2*c2+4*S2*a2*S1*c1+4*S1*a1*S2*a2*Q^2+4*S1*a1*S2*c2+4*S1*a1*S2*b2*Q-4*S1^2*a1*c1));
            q(iActivePumps(2))=Q-q(iActivePumps(1));
        end
    elseif nActivePumps==1
        q(iActivePumps)=Q_tot+Q_recycle(iActivePumps);
    elseif nActivePumps==0
        %flow remains zero
    else
        disp('only one or two active pumps implemented for this input option')
        return
    end
    for i=1:nActivePumps
        dP(i)=speedFactor(iActivePumps(i))*polyval(dPFlow(iActivePumps(i),:),q(iActivePumps(i)));
    end
    if nActivePumps==0
        if isequal(solvefor,'Pd') %suction pressure as input
            sys=[Ps;q/3600];
        elseif isequal(solvefor,'Ps')
            sys=[Pd;q/3600];
        end
    else
        if isequal(solvefor,'Pd') %suction pressure as input
            sys=[Ps+mean(dP);q/3600];
        elseif isequal(solvefor,'Ps')
            sys=[Pd-mean(dP);q/3600];
        end
    end
elseif isequal(solvefor,'q')
    q=zeros(nPumps,1);
    for i=1:nActivePumps
        a=dPFlow(iActivePumps(i),1);
        b=dPFlow(iActivePumps(i),2);
        c=dPFlow(iActivePumps(i),3)-(Pd-Ps)/speedFactor(i);
        q(iActivePumps(i))=(-b-sqrt(b^2-4*a*c))/(2*a);
    end
    if nActivePumps~=0
        sys=[(sum(q(:)-Q_recycle));(q(:))]/3600;
    else
        sys=zeros(1+nPumps,1);
    end;
    if ~isreal(sys)
        sprintf('complex %f + %f i',real(q),imag(q))
    end
end
% end mdlOutputs

