mpcsfunc(t,z,y,flag,A,B,C,D,P,M,Q,R,Ts)

function [sys,x0,str,ts] = mpcsfunc(t,z,y,flag,A,B,C,D,P,M,Q,R,Ts)

%sfunction for mpc.
%
%By Yi Cao, Cranfield University, UK, (c) 27 January 2009
%

    persistent ssmpc ny

    switch flag,
    
        %%%%%%%%%%%%%%%%%%
        % Initialization %
        %%%%%%%%%%%%%%%%%%
        case 0,
            error(nargchk(13,13,nargin));

            [nx,nu]=size(B);
            ny=size(C,1);
            
            if ~isequal(size(A), [nx nx]) || ~isequal(size(D),[ny nu])
                error('Model matrix dimension does not match.');
            end
            
            if ~isscalar(P) || ~isscalar(M) || P<=0 || M <= 0 || M>P
                error('P and M must be positive scalars and P>=M.')
            end
            
            if ~isequal(size(Q), [ny*P, ny*P]) || ~isequal(size(R), [nu*M, nu*M])
                error('Weight matrix dimension does not match.')
            end
            
            if ~isscalar(Ts) || Ts<=0
                error('dt must be a positive scalar.');
            end
            
            % MPC set-up
            ssmpc=mpcsetup(A,B,C,D,P,M,Q,R);
            
            sizes = simsizes;
            sizes.NumContStates  = 0;
            sizes.NumDiscStates  = 0;
            %      sizes.NumDiscStates  = nx+ny;
            sizes.NumOutputs     = nu;
            sizes.NumInputs      = 2*ny;  % output + setpoint
            sizes.DirFeedthrough = 1;
            sizes.NumSampleTimes = 1;
            sys = simsizes(sizes);
            str = [];
            x0 = [];
            %       offset=zeros(ny,1);
            %      x0 = zeros(nx+ny,1);
            %       du = zeros(nu,1);
            ts  = [Ts 0];
            
            
            %%%%%%%%%%
            % Update %
            %%%%%%%%%%
        case 2,
            sys = [];
            
            %%%%%%%%%%
            % Output %
            %%%%%%%%%%
        case 3,
            sys = ssmpc(y(1:ny),y(ny+1:2*ny));
            
            %%%%%%%%%%%%%
            % Terminate %
            %%%%%%%%%%%%%
        case 9,
            sys = []; % do nothing
            
            %%%%%%%%%%%%%%%%%%%%
            % Unexpected flags %
            %%%%%%%%%%%%%%%%%%%%
        otherwise
            error(['unhandled flag = ',num2str(flag)]);
    end
end

function f=mpcsetup(A,B,C,D,p,m,Q,R,x0,u0)

% Input and output check
error(nargchk(8,10,nargin));
error(nargoutchk(1,1,nargout));

% dimension of the system
nx=size(A,1);
[ny,nu]=size(D);

% precalculate the gain matrices of the online controller
[K,Pr] = predmat(A,B,C,D,p,m,Q,R);
% only the first instance is required
K1 = K(1:nu,:);
Pr = Pr(1:nu,:);

% default initial conditions
if nargin<9
    x0=zeros(nx,1);
end
if nargin<10
    u0=zeros(nu,1);
end

% the online controller
f = @ssmpc;
    function u=ssmpc(y,r)
        % update state
        x0 = A*x0 + B*u0;
        % get the reference
        nr=size(r,2);
        if nr>=p
            ref=r(:,1:p);
        else
            ref=[r r(:,end+zeros(p-nr,1))];
        end
        % model-plant mismatch
        offset = (y(:) - C*x0);
        % update optimal control
%         u0 = -K1*x0 + Pr*reshape(bsxfun(@minus,ref,offset),[],1);
        u0 = -K1*x0 + Pr*reshape(ref-offset(:,ones(p,1)),[],1);
        % controller output
        u = u0;
    end
end


function [K,Pr] = predmat(A,B,C,D,hP,hM,Q,R)
 
%%%% Initialise
[nx,nu]=size(B);
ny = size(C,1);

Gss = inv([A-eye(nx) B;C D]);
M1 = Gss(nx+1:end,nx+1:end);

Px=zeros(hP*ny,nx);  Pu=zeros(hP*ny,hP*nu);  P=C;
L=eye(ny*hP);

%%%% Use recursion to find predictions
for i=1:hP;
   Puterm = P*B;
   for j=i:hP;
         vrow=(j-1)*ny+1:j*ny;
         vcol=(j-i)*nu+1:(j-i+1)*nu;
         Pu(vrow,vcol)=Puterm;
   end
   P=P*A;
   vrow=(i-1)*ny+1:i*ny;
   Px(vrow,1:nx) = P;
end
P=Px;
M=zeros(nu*hM,ny*hP);
for i=1:hM,
    I=(i-1)*nu+1:i*nu;
    J=(i-1)*ny+1:i*ny;
    M(I,J)=M1;
end

%%% Sum last columns of H
v=(hM-1)*nu;
HH=zeros(hP*ny,nu);
for k=1:nu;
    HH(:,k) = sum(Pu(:,v+k:nu:end),2);
end
H = [Pu(:,1:(hM-1)*nu),HH];

S = H'*Q*H+R;
S = (S+S')/2;
X1 = H'*Q*P;
X2 = -H'*Q*L-R*M;

%%%% Unconstrained control law
Mi=inv(S);
K = Mi*X1;
Pr = -Mi*X2;
end