mpc_matrices(S, P, x, D, k, ops)

function MPC = mpc_matrices(S, P, x, D, k, ops)
%MPC_MATRICES prepares the MPC matrices necessary for the formulation of an
%MPC problem as a constrained conved QP problem.
%
% Syntax:   MPC = MPC_MATRICES(S,P,x,D,k);
%
%MPC_MATRICES admits the following input arguments, all of which are
%mandatory:
%
% S   - A structure that describes the discrete-time dynamical system x+ =
%       A*x+B*u+Gd*d, where x is the nx-dimensional state of the system, u
%       is the nu-dimensional input vector and d is the nd-dimensional
%       vector of disturbances. Additionally, we consider that the input
%       variable u and the distrurbance d are coupled through the equality
%       constraint E*u+Ed*d=0, where E and Ed and matrices of appropriate
%       dimensions. For more information about the structure of S, type
%       'help DWN_INFO'.
%
% P   - A structure with information about the MPC problem. Among other, P
%       has the following entries:
%        P.Hu : is the control horizon and
%        P.Hp : is the prediction horizon
%       we consider that from time k and on the following actuation values are
%       applied to the system U_k = {u_k, u_{k+1}, ..., u_{k+Hu}, u_{k+Hu+1},
%       ..., u_{k+Hp-1}}, where u_{k+Hu+i}=u_{k+Hu} for all i. That is, after
%       the time instant k+Hu we assume that the same constant input value
%       is applied to the system up to the time instant k+Hp-1. For simplicity,
%       we consider only the part of U_k up to u_{k+Hu}, knowning that all
%       trailing input values are equal to u_{k+Hu}.
%
% x   - is the current state (measured).
%
% D   - is a vector of demands, vertically packed, i.e., D = {d_k,
%       d_{k+1}, ..., d_{k+Hp-1}}, where d_k is the current measured disturbance
%       while d_{k+i} for i=1,...,Hp-1 are forecast disturbances using some
%       model. The length of the vector D should be S.nd*P.Hp.
%
% k   - is the time instant when the measurement x is taken. This is
%       important to be specified as the overall MPC problem is time-varying.
%
% ops - Options.
%
% mpc_matrices will return a structure with the following fields:
% Sx, Su, Sd  -  The matrices of the batch MPC formulation. Let X_k={x_{k+1},
%                x_{k+2}, ..., x_{k+Hp}} and U_k={u_{k}, u_{k+1}, ..., u_{k+Hu}}
%                and D_k={d_{k}, d_{k+1}, ..., d_{k+Hp-1}}, then X_k =
%                Sx*x_k + Su*U_k + S_d*D_k.
% H, f        -  The matrices of the cost function J(y)=y'Hy + f'y where
%                y=[U_k; Xi_k].
% F, phi      -  The set of constraints of the MPC problem can be written
%                compactly in the form F*y <= phi, where y is as above.
% G, gamma    -  The set of equality constraints of the form
%                E*u_{k+i}+Ed*d_{k+i}=0 can be compactly written in the
%                form G*y=gamma, where y is as above.
% yL, yH      -  Box-type constraints for the vector y, i.e., y >= yL and
%                y<=yH.
%
% Handling of soft constraints:
%  A softened version of the constraint x>=xs is used, where xs is a field
%  specified in P. If xs is not provided, but P.beta is available, then
%  P.xs is calculated as P.xs = P.beta * S.xmax.
%
% See also
% DWN_INFO

defaultops = struct('use_soft_constraints',true,'use_beta',false);
if (nargin==6)
    options=ops;
elseif (nargin==5)
    options=defaultops;
end
if (~isfield(S,'nx'))
    S.nx=size(S.A,1);
end
if (~isfield(S,'nu'))
    S.nu=size(S.B,2);
end
if (~isfield(S,'nd'))
    S.nd=size(S.Gd,2);
end
if (isempty(S)||isempty(P)||isempty(D)||isempty(x)||isempty(k))
    error('Empty input arguments are not allowed.');
end
[checkS, errorMessage] = check_system(S);
if (~checkS)
    error('System structure error: %s',errorMessage);
end
[checkP, errorMessage] = check_problem(P);
if (~checkP)
    error('Problem structure error: %s',errorMessage);
end


% Calculate Delta
Delta=zeros(S.nu*P.Hu, S.nu*(P.Hu+1));
Delta(sub2ind([S.nu*P.Hu, S.nu*(P.Hu+1)],1:S.nu*P.Hu,1:S.nu*P.Hu))=-1;
for i=1:P.Hu*S.nu,
    Delta(i,i+S.nu)=1;
end
MPC.Delta=Delta;


% Caclulate Sx
MPC.Sx=zeros(S.nx*P.Hp,S.nx);
MPC.Sx(1:S.nx,:)=S.A;
for i=2:P.Hp
    MPC.Sx(1+(i-1)*S.nx:i*S.nx,:)=MPC.Sx(1+(i-2)*S.nx:(i-1)*S.nx,:)*S.A;
end

% Calculate Su
MPC.Su=zeros(S.nx*P.Hu,S.nu*(P.Hu+1));
MPC.Su(1:S.nx,1:S.nu)=S.B;
for i=1:P.Hp-1,
    MPC.Su(i*S.nx+1:(i+1)*S.nx, 1:S.nu) = S.A * MPC.Su((i-1)*S.nx+1:i*S.nx, 1:S.nu);
end
for di=1:P.Hu,
    block = MPC.Su((di-1)*S.nx+1:di*S.nx, 1:S.nu);
    for i=1:P.Hu-di+2
        MPC.Su((i+di-2)*S.nx+1:(i+di-1)*S.nx,(i-1)*S.nu+1:i*S.nu)=block;
    end
end
if (P.Hu<P.Hp-1)
    for i=1:P.Hp-P.Hu-1,
        for s=1:P.Hu,
            MPC.Su((P.Hu+i)*S.nx+1:(P.Hu+i+1)*S.nx,s*S.nu+1:(1+s)*S.nu)=...
                MPC.Su((P.Hu+i-1)*S.nx+1:(P.Hu+i)*S.nx,(s-1)*S.nu+1:s*S.nu);
        end
    end
    Mtemp=S.B;
    for i=1:P.Hp-P.Hu-1
        MPC.Su((P.Hu+i)*S.nx+1:(P.Hu+i+1)*S.nx, end-S.nu+1:end)=...
            MPC.Su((P.Hu+i)*S.nx+1:(P.Hu+i+1)*S.nx, end-S.nu+1:end)+Mtemp;
        Mtemp = Mtemp + MPC.Su(i*S.nx+1: (i+1)*S.nx, 1:S.nu);
    end
end


% Calculate Sd
MPC.Sd=zeros(S.nx*P.Hp, S.nd*P.Hp);
MPC.Sd(1:S.nx,1:S.nd)=S.Gd;
for i=1:P.Hp-1,
    MPC.Sd(i*S.nx+1:(i+1)*S.nx, 1:S.nd) = S.A * MPC.Sd((i-1)*S.nx+1:i*S.nx, 1:S.nd);
end
for di=1:P.Hp,
    block = MPC.Sd((di-1)*S.nx+1:di*S.nx, 1:S.nd);
    for i=1:P.Hp-di+1
        MPC.Sd((i+di-2)*S.nx+1:(i+di-1)*S.nx,(i-1)*S.nd+1:i*S.nd)=block;
    end
end

% Calculate Wat, Wxt and Wut
MPC.Wat = kron(ones(P.Hu+1,1),P.alpha1) + vec(P.alpha2(k:k+P.Hu,:)');
MPC.Wxt = kron(eye(P.Hp),P.Wx);
MPC.Wut = Delta'*kron(eye(P.Hu), P.Wu)*Delta;

% Cost Function Matrices
MPC.H = 2*blkdiag(MPC.Wut, MPC.Wxt);
MPC.f = [MPC.Wat; zeros(S.nx*P.Hp,1)];

% Calculate the matrices of the inequality constraints
MPC.Xmin=kron(ones(P.Hp,1), S.xmin);
MPC.Xmax=kron(ones(P.Hp,1), S.xmax);
MPC.Umin=kron(ones(P.Hu+1,1), S.umin);
MPC.Umax=kron(ones(P.Hu+1,1), S.umax);
if (isfield(P,'xs') && ~options.use_beta)
    MPC.Xs=kron(ones(P.Hp,1), P.xs);
elseif (isfield(P,'beta'))
    MPC.Xs=P.beta*MPC.Xmax;
end

% Upper and Lower Bounds
MPC.ymin = [MPC.Umin; zeros(S.nx*P.Hp,1)];
MPC.ymax = [MPC.Umax; Inf(S.nx*P.Hp,1)];

% Equality Constraints
ne = size(S.E,1);
MPC.G = zeros(ne*(P.Hu+1),S.nu*(P.Hu+1)+S.nx*P.Hp);
MPC.G(1:ne*(P.Hu+1), 1:S.nu*(P.Hu+1))=kron(eye(P.Hu+1), S.E);
MPC.gamma = kron(-eye(P.Hu+1), S.Ed)*D(1:S.nd*(P.Hu+1));

% construct the matrix F:
MPC.F = zeros(3*S.nx*P.Hp, S.nx*P.Hp+S.nu*(P.Hu+1));
MPC.F(1:S.nx*P.Hp,1:S.nu*(P.Hu+1))=MPC.Su;
MPC.F(S.nx*P.Hp+1:2*S.nx*P.Hp,1:S.nu*(P.Hu+1))=-MPC.Su;
if (options.use_soft_constraints), % soft constraints here
    MPC.F(2*S.nx*P.Hp+1:3*S.nx*P.Hp,1:S.nu*(P.Hu+1))=-MPC.Su;
    MPC.F(2*S.nx*P.Hp+1:3*S.nx*P.Hp,S.nu*(P.Hu+1)+1:end)=-eye(S.nx*P.Hp);
end

% construct the vector phi:
MPC.phi = [ +MPC.Xmax - MPC.Sx*x - MPC.Sd*D
    -MPC.Xmin + MPC.Sx*x + MPC.Sd*D];
if (options.use_soft_constraints),
    MPC.phi=[ MPC.phi;
        -MPC.Xs + MPC.Sx*x + MPC.Sd*D];
else
    MPC.phi=[ MPC.phi;
        zeros(size(MPC.Xs))];
end


% Auxiliary Functions

    function [checkS, errorMessage] = check_system(S)
        %CHECK_SYSTEM checks whether S is a valid system structure.
        checkS=true;
        errorMessage = [];
        fields = { 'A','B','Gd', 'xmin', 'xmax', 'umin', 'umax', 'E', 'Ed'};
        for j=1:length(fields)
            field_i = fields{j};
            if (~isfield(S,field_i))
                checkS=false;
                errorMessage = ['Element ' field_i ...
                    ' is missing from the system struct.'];
                return;
            end
        end
        if (size(S.A,1)~=size(S.B,1))
            checkS=false;
            errorMessage = 'Sizes of A and B are inconsistent.';
            return;
        end
        if (size(S.A,1)~=size(S.Gd,1))
            checkS=false;
            errorMessage = 'Sizes of A and Gd are inconsistent.';
            return;
        end
        if (size(S.E,1)~=size(S.Ed,1))
            checkS=false;
            errorMessage = 'Sizes of E and Ed are inconsistent.';
            return;
        end
        if (sum(size(S.xmin)==[S.nx 1])~=2)
            checkS=false;
            errorMessage = 'xmin has wrong size - should be a nx-by-1 vector.';
            return;
        end
        if (sum(size(S.xmax)==[size(S.A,1) 1])~=2)
            checkS=false;
            errorMessage = 'xmin has wrong size - should be a nx-by-1 vector.';
            return;
        end
        if (sum(size(S.umin)==[size(S.B,2) 1])~=2)
            checkS=false;
            errorMessage = 'xmin has wrong size - should be a nx-by-1 vector.';
            return;
        end
        if (sum(size(S.umax)==[size(S.B,2) 1])~=2)
            checkS=false;
            errorMessage = 'xmin has wrong size - should be a nx-by-1 vector.';
            return;
        end
    end

    function [checkP, errorMessage] = check_problem(P)
        %CHECK_PROBLEM checks whether P is a valid problem structure.
        checkP=true;
        errorMessage = [];
        fields = { 'Hu','Hp', 'beta', 'alpha1', 'alpha2', 'Wx', 'Wu'};
        for j=1:length(fields)
            field_i = fields{j};
            if (~isfield(P,field_i))
                checkP=false;
                errorMessage = ['Element ' field_i ...
                    ' is missing from the system struct.'];
                return;
            end
        end
        if (P.Hu>=P.Hp)
            checkP=false;
            errorMessage='Control Horizon must be less than the prediction horizon.';
            return;
        end
        if (P.Hu==0)
            checkP=false;
            errorMessage='The control horizon should be at least equal to 1.';
        end
    end


    function x = vec(M)
        %VEC packs the elements of a matrix into a vector.
        N = numel(M);
        x = reshape(M,N,1);
    end

end