function [Matrices, feasible] = mpt_addTset(sysStruct,Matrices,Tset,nx,nu,dyn)
%MPT_ADDTSET Adds terminal set constraint to Matrices
%
% [Matrices, feasible] = mpt_addTset(Matrices,Tset,nx,nu)
%
% ---------------------------------------------------------------------------
% DESCRIPTION
% ---------------------------------------------------------------------------
% Adds terminal set constraint to matrices of a mp-problem
%
% ---------------------------------------------------------------------------
% INPUT
% ---------------------------------------------------------------------------
% sysStruct - system structure
% Matrices  - Matrices structure as generated by mpt_constructMatrices
% Tset      - Terminal set (polytope object)
% nx        - number of states
% nu        - number of inputs
% dyn       - index of active dynamics
%
% ---------------------------------------------------------------------------
% OUTPUT                                                                                                    
% ---------------------------------------------------------------------------
% Matrices  - updated Matrices
% feasible  - boolean flag (1/0) if the problem is feasible
%

% Copyright is with the following author(s):
%
% (C) 2004 Michal Kvasnica, Automatic Control Laboratory, ETH Zurich,
%          kvasnica@control.ee.ethz.ch

% ---------------------------------------------------------------------------
% Legal note:
%          This program is free software; you can redistribute it and/or
%          modify it under the terms of the GNU General Public
%          License as published by the Free Software Foundation; either
%          version 2.1 of the License, or (at your option) any later version.
%
%          This program is distributed in the hope that it will be useful,
%          but WITHOUT ANY WARRANTY; without even the implied warranty of
%          MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
%          General Public License for more details.
% 
%          You should have received a copy of the GNU General Public
%          License along with this library; if not, write to the 
%          Free Software Foundation, Inc., 
%          59 Temple Place, Suite 330, 
%          Boston, MA  02111-1307  USA
%
% ---------------------------------------------------------------------------

if ~isfulldim(Tset),
    feasible = 1;
    return
end
[Ht,Kt] = double(Tset);
[nnc] = size(Ht,1);
nG = size(Matrices.G,2);
Matrices.G = [Matrices.G; [Ht*sysStruct.B{dyn} zeros(nnc,nG-nu)]];
Matrices.W = [Matrices.W; Kt-Ht*sysStruct.f{dyn}];
Matrices.E = [Matrices.E; -Ht*sysStruct.A{dyn}];
PGEW = polytope([Matrices.G -Matrices.E],Matrices.W,0,1);
if ~isfulldim(PGEW),
    feasible = 0;
    return
end
[HGEW,KGEW] = double(PGEW);
Matrices.G = HGEW(:,1:end-nx);
Matrices.E = -HGEW(:,end-nx+1:end);
Matrices.W = KGEW;
feasible = 1;