Figure 7.8

% Example explicit MPC for a 2D system From "Optimal control of constrained
% piecewise affine discrete-time systems" (Mayne and Rakovic, 2002).




% Choose system.
A = [1,0.1; 0,1];
B = [0; 0.0787];
Q = [1, 0; 0, 0];
R = 0.1;
ulb = -1;
uub = 1;
xlb = -inf(2, 1);
xub = inf(2, 1);
N = 20;
[Nx, Nu] = size(B);

% Find LQR terminal set.
[K, P] = dlqr(A, B, Q, R);
K = -K;
[Xn, Xnverts] = findXn(A, B, K, N, xlb, xub, ulb, uub, 'lqr');
A_lqr = Xn{1}.A;
b_lqr = Xn{1}.b;
feas = Xn{end}; % Gives region X_N.

% Get mpQP, solve, and plot.
mpqp = ss2mpqp('A', A, 'B', B, 'Q', Q, 'R', R, 'P', P, 'N', N, ...
               'ulb', ulb, 'uub', uub, 'Af', A_lqr, 'bf', b_lqr);
regions = solvempqp(mpqp, 'plot', false());


xbox = [-6, 6; -3, 3];
Npts = 10000;
Nmax = 20000;
xpts = NaN(Nx, Npts);
lookuptimes = NaN(Npts, 1);
qptimes = NaN(Npts, 1);
jx = 0;
rand('state', 917);
for i = 1:Nmax
    xmin = xbox(:,1);
    dx = xbox(:,2) - xbox(:,1);
    xtry = xmin + dx.*rand(Nx, 1);
    if all(feas.A*xtry - feas.b <= 0)
        jx = jx + 1;
        if mod(jx, 500) == 1
            fprintf('Point %d of %d\n', jx, Npts);
        end
        xpts(:,jx) = xtry;

        % Do PWA lookup.
        tic();
        [zlookup, id] = pwalookup(xtry, regions, feas);
        lookuptimes(jx) = toc();

        % Solve QP.
        tic();
        H = mpqp.Q;
        q = mpqp.C*xtry;
        A = mpqp.A;
        b = mpqp.b + mpqp.S*xtry;
        if isOctave()
            z0 = zeros(Nu*N, 1);
            [zqp, ~, status] = qp(z0, H, q, [], [], [], [], [], A, b);
            okay = (status.info == 0);
        else
            H = 0.5*(H + H'); % Matlab is picky.
            options =  optimoptions('quadprog', 'display', 'off', ...
                                    'OptimalityTolerance', 1e-15, ...
                                    'StepTolerance', 1e-15, ...
                                    'MaxIterations', 500);
            prob = struct('H', H, 'f', q, 'Aineq', A, 'bineq', b, ...
                          'solver', 'quadprog', 'options', options);
            [zqp, ~, status] = quadprog(prob);
            okay = (status == 1);
        end
        if ~okay
            warning('QP failed at step %d!', jx);
        end
        qptimes(jx) = toc();

        % Make sure the solutions agree.
        if max(abs(zqp - zlookup)) > 1e-5
            warning('Mismatch for point %d!', jx);
        end

        % Stop if we've taken enough points.
        if jx == Npts
            break
        end
    end
end
fprintf('Sampled %d points.\n', jx);
xpts = xpts(:,1:jx);
lookuptimes = 1000*lookuptimes(1:jx); % Convert to ms.
qptimes = 1000*qptimes(1:jx); % Conver to ms.


sig = 1; % Kernel width in ms.
tplot = linspace(0, 100, 1001); % Plot range in ms.

lookupdensity = kerneldensity(lookuptimes, sig, tplot);
qpdensity = kerneldensity(qptimes, sig, tplot);

figure();
plot(tplot, lookupdensity, '-r', tplot, qpdensity, '-g');
legend('Explicit', 'Implicit', 'Location', 'NorthEast');
title(sprintf('%d Samples', size(xpts, 2)));
xlabel('Solution Time (ms)');
ylabel('Samples');

scatterplot(xpts, lookuptimes, tplot, 'Explicit');
scatterplot(xpts, qptimes, tplot, 'Implicit');

function regions = solvempqp(prob, varargin)
% regions = solvempqp(prob, 'key', value, ...)
%
% Solves the mpQP given by
%
%   min  0.5*z'*Q*z + p'*C'*z
%    z
%   s.t. A*z <= b + S*z
%
% for all p satisfying
%
%   E*p <= e
%
% with prob a struct containing fields Q, C, A, b, and S. Note that Q must be
% strictly positive definite. E and e can also be provided (they default to
% empty matrices).
%
% Available options are as follows:
%
% - 'display' : Whether to display status of iterations (default true)
% - 'printevery' : Number of iterations for each print (default 100)
% - 'logfile' : Filename to write detailed log (default '', which means no log)
% - 'vertices' : Whether to find vertices for each region (default true for
%                Np = 2, false for Np > 2)
% - 'plot' : Whether to make a plot of all the regions (default false; ignored
%            if 'vertices' is not true); only possible if length(p) == 2
% - 'maxiter' : Maximum integer number of iterations (default inf)
% - 'maxtime' : Maximum time to run (in seconds; default inf).
% - 'ascell' : Whether to return cell array (default true)
%
% Returned value is cell array of structs containing the halfspace and
% vertex representations of the polytopes. If 'ascell' is false, instead returns
% a struct whose fields are hexidecimal ID numbers and values contain the other
% information.
%
% Essential mpQP algorithm is from "A multiparametric quadratic programming
% algorithm with polyhedral computations based on nonnegative least squares"
% (Bemporad, 2015), although the polyhedral computations are performed using
% qhull and linprog rather than nonnegative least squares.
global SUPPRESS_OUTPUT
if nargin() < 1
    error('Argument prob is required!');
end
args = struct(varargin{:});
defaults = {'display', true(); 'logfile', ''; 'vertices', NaN(); ...
            'plot', false(); 'maxiter', inf(); 'ascell', true(); ...
            'maxtime', inf(); 'printevery', 100};
options = struct();
for i = 1:size(defaults, 1)
    f = defaults{i, 1};
    options.(f) = structGet(args, f, defaults{i, 2});
end
if ~isempty(options.logfile)
    logfile = fopen(options.logfile, 'w');
    log_(logfile); % Enables logging.
end
log_('Log started %s\n', datestr(now()));

% Disable output if a global SUPPRESS_OUTPUT is set (overrides user preference).
if ~isempty(SUPPRESS_OUTPUT) && SUPPRESS_OUTPUT
    options.display = false();
end

% Convert to nomenclature from Bemporad (2015).
pmpqp = struct(); % Primal mpQP.
pmpqp.G = prob.A;
pmpqp.W = prob.b;
pmpqp.S = prob.S;
pmpqp.Q = prob.Q;
pmpqp.Q = 0.5*(pmpqp.Q + pmpqp.Q'); % Matlab is picky about symmetry.
pmpqp.F = prob.C;
pmpqp.c = zeros(size(pmpqp.F, 1), 1);
pmpqp.Qhalf = chol(pmpqp.Q);
pmpqp.E = structGet(prob, 'E', zeros(0, size(prob.S, 2)));
if isempty(pmpqp.E)
    pmpqp.e = zeros(0, 1);
else
    if ~isfield(prob, 'e')
        error('prob.e must be provided if prob.E is given!');
    end
    pmpqp.e = prob.e;
end
if isnan(options.vertices)
    options.vertices = (size(prob.S, 2) == 2);
end

% Convert to dual problem (with fewer parameters).
dmpqp = struct(); % Dual mpQP.
dmpqp.H = pmpqp.G*(pmpqp.Q\pmpqp.G');
dmpqp.D = pmpqp.G*(pmpqp.Q\pmpqp.F) + pmpqp.S;
dmpqp.d = pmpqp.G*(pmpqp.Q\pmpqp.c) + pmpqp.W;

% Choose qhull options.
qhullopts = {'Qt', 'QbB', 'Pp'};
if size(prob.S, 2) >= 5
    qhullopts = [qhullopts, {'Qx'}];
end

% To start, we know no constraints are active at the origin.
Ncon = size(dmpqp.D, 1);
opensets = struct('N1', struct('active', false(Ncon, 1))); % All cons inactive.
opensets.N1.id = getcrid(opensets.N1.active);
nopen = 1;
nfound = 0;

regions = struct();
probedregions = struct(opensets.N1.id, []);

niter = 0;
starttime = cputime();
options.maxtime = options.maxtime + starttime;
while nopen > 0 && niter < options.maxiter && cputime() < options.maxtime
    niter = niter + 1;
    if options.display && mod(niter, options.printevery) == 0
        fprintf('Iteration %d (%d found, %d left to search).\n', ...
                niter, nfound, nopen);
    end
    log_('Iteration %d\n', niter);

    activestr = sprintf('N%d', nopen);
    nopen = nopen - 1;
    currentset = opensets.(activestr);
    opensets.(activestr) = [];
    crid = currentset.id;
    log_('    Getting %s\n', crid);
    [cr, okay] = getcriticalregion(pmpqp, dmpqp, currentset);
    if okay
        log_('    Found representation\n');
        [nonredundant, cr.A, cr.b] = removeredundantcon(cr.A, cr.b, cr.x, ...
                                                        [], qhullopts);
        cr.cind = cr.cind(nonredundant);
        cr.ctype = cr.ctype(nonredundant);
        if options.vertices
            cr.V = halfspace2vertex(cr.A, cr.b, cr.x);
        end
        regions.(crid) = cr;
        nfound = nfound + 1;

        % Try to find adjacent critical regions.
        log_('    Searching for adjacent regions\n');
        for i = 1:length(nonredundant)
            n = cr.cind(i);
            newactive = currentset.active;

            tryqpsearch = true();
            switch cr.ctype(i)
            case 'w'
                newactive(n) = true();
            case 'y'
                newactive(n) = false();
                tryqpsearch = false(); % QP doesn't help in this case.
            otherwise
                continue
            end
            keepgoing = true();
            while keepgoing
                % Check whether the new active combination has full rank.
                % If so, add it. Otherwise, try once to fix it.
                newcrid = getcrid(newactive);
                if ~isfield(probedregions, newcrid)
                    probedregions.(newcrid) = [];
                    [newfullrank, newHiihalf] = checkcrfullrank(dmpqp, newactive);
                    if newfullrank
                        newcr = struct('active', newactive, 'id', newcrid, ...
                                       'Hiihalf', newHiihalf);
                        [opensets, nopen] = addcr(opensets, nopen, newcr);
                        keepgoing = false();
                    else
                        log_('        %s is not full-rank\n', newcrid);
                        if tryqpsearch
                            tryqpsearch = false();
                            log_('        Trying QP neighbor search\n');
                            cr.facet = i;
                            [newactive, keepgoing] = findadjacentcr(cr, pmpqp);
                            if ~keepgoing
                                log_('        No neighbor found\n');
                            end
                        else
                            keepgoing = false();
                        end
                    end
                else
                    keepgoing = false();
                end
            end
        end
    else
        log_('    %s is infeasible\n', crid);
    end
    log_('    %d regions open\n', nopen);
end
log_('Finished with %d open regions\n', nopen);
log_(); % Closes the log.
if options.display
    fprintf(['Found %d regions (%d left to search) after %d iterations (in ' ...
             '%g s).\n'], nfound, nopen, niter, cputime() - starttime);
end

% Make a plot.
if options.plot && size(pmpqp.S, 2) == 2
    fprintf('Making plot (may take some time).\n');
    figure();
    hold('on');
    fields = fieldnames(regions);
    colors = viridiscolors(length(fields));
    [~, cperm] = sort(sin(1:length(fields)));
    colors = colors(cperm,:); % (Pseudo-)randomize colors.
    for i = 1:length(fields)
        f = fields{i};
        fill(regions.(f).V(:,1), regions.(f).V(:,2), colors(i,:));
    end
    xlabel('p_1');
    ylabel('p_2', 'rotation', 0);
end

% Reshape into cell array.
if options.ascell
    regionsstruct = regions;
    fields = fieldnames(regionsstruct);
    regions = cell(length(fields), 1);
    for i = 1:length(fields)
        f = fields{i};
        regions{i} = regionsstruct.(f);
        regions{i}.id = f;
    end
end

end%function

function [fullrank, Hiihalf] = checkcrfullrank(dmpqp, activecon)
    % [fullrank, Hiihalf] = checkcrfullrank(dmpqp, activecon)
    %
    % Checks whether the critical region defined by the set of active
    % constraints is full-rank. If so, fullrank is true(), and the Cholesky
    % decomposition of the reduced Hessian is also returned. If not, fullrank is
    % false, and the second output is meaningless.
    narginchk(2, 2);
    i = activecon;
    Hii = dmpqp.H(i,i);
    if isempty(Hii)
        Hiihalf = Hii;
        fullrank = true();
    else
        [Hiihalf, flag] = chol(Hii);
        fullrank = (flag == 0) && all(diag(Hiihalf) > 1e-6);
    end
end%function

function [cr, okay] = getcriticalregion(pmpqp, dmpqp, crset)
    % [cr, okay] = getcriticalregion(pmpqp, dmpqp, crset)
    %
    % Returns corresponding critical region based on given set of active
    % constraints. cr is a struct with fields A, b, x (with x giving an interior
    % point of the critical region), cind (giving indices of w or y for each
    % row of the constraint matrix), and ctype (giving 'w' or 'y').
    %
    % If activecon does not defin an optimal set of constraints, then cr is
    % empty, and okay is false.
    narginchk(3, 3);

    % Check for cholesky decomposition in crset.
    activecon = crset.active;
    if isfield(crset, 'Hiihalf')
        Hiihalf = crset.Hiihalf;
        okay = true();
    else
        [okay, Hiihalf] = checkcrfullrank(dmpqp, activecon);
    end

    % Get inequalities that define critical region (often redundant).
    cr = struct();
    if okay
        i = activecon;
        j = ~activecon;
        Y = -(Hiihalf\(Hiihalf'\[dmpqp.D(i,:), dmpqp.d(i)]));
        W = dmpqp.H(j,i)*Y + [dmpqp.D(j,:), dmpqp.d(j)];
        P = [W; Y]; % Feasible region is P*[x; 1] >= 0.
        A = -P(:,1:end-1);
        b = P(:,end);

        % Screen out near-zero rows of A. In this case, we just set them to
        % actual zero so the LP solver can handle it.
        Anorms = max(abs(A), [], 2);
        badrows = (Anorms < 1e-10);
        if any(b(badrows) < 0)
            okay = false();
        end
        A(badrows,:) = 0;
        b(badrows) = 1;

        % Add the inequalities that define the region of interest for x.
        A = [A; pmpqp.E];
        b = [b; pmpqp.e];
        [A, b] = scalerows(A, b);
        Ne = length(pmpqp.e);

        % Attempt to find an interior point.
        if okay
            [x, okay] = findinteriorpoint(A, b);
            if okay
                % Critical region.
                cr.A = A;
                cr.b = b;
                cr.cind = [find(j); find(i); -1*(1:Ne)'];
                cr.ctype = [repmat('w', size(W, 1), 1); ...
                            repmat('y', size(Y, 1), 1); ...
                            repmat('e', Ne, 1)];
                cr.x = x;

                % Optimal solution.
                law = -pmpqp.Qhalf\(pmpqp.Qhalf'\(pmpqp.G(i,:)'*Y ...
                                                  + [pmpqp.F, pmpqp.c]));
                cr.Z = law(:,1:end-1);
                cr.z0 = law(:,end);
            end
        end
    end
end%function

function [z, feas, slack, active] = solveprimalqp(x, pmpqp)
    % Solves the primal qp at the given value of x using quadprog (Matlab)
    % or qp (Octave).
    narginchk(2, 2);
    Q = pmpqp.Q;
    f = pmpqp.F*x;
    A = pmpqp.G;
    b = pmpqp.S*x + pmpqp.W;
    Aeq = [];
    beq = [];
    if isOctave()
        zguess = zeros(size(Q, 1), 1);
        [z, ~, flag] = qp(zguess, Q, f, Aeq, beq, [], [], [], A, b);
        feas = (flag.info == 0);
    else
        prob = struct('H', Q, 'f', f, 'Aineq', A, 'bineq', b, ...
                      'Aeq', Aeq, 'beq', beq, 'options', ...
                      optimoptions('quadprog', 'display', 'off'), ...
                      'solver', 'quadprog');
        [z, ~, flag] = quadprog(prob);
        feas = (flag == 1);
        if isempty(z)
            z = NaN(length(f), 1);
        end
    end
    slack = pmpqp.G*z - pmpqp.S*x - pmpqp.W;
    active = (abs(slack) < 1e-5);
end%function

function [opensets, nopen] = addcr(opensets, nopen, newcr)
    % Adds the new set to the list of open sets.
    nopen = nopen + 1;
    openstr = sprintf('N%d', nopen);
    opensets.(openstr) = newcr;
    log_('        Adding %s\n', newcr.id);
end%function

function [newactive, feas] = findadjacentcr(cr, pmpqp)
    % Finds an adjacent CR by stepping in the direction of the shared facet and
    % solving the QP.
    narginchk(2, 2);

    % Get point on the facet.
    ineq = true(size(cr.A, 1), 1);
    ineq(cr.facet) = false();
    [x0, okay] = findinteriorpoint(cr.A(ineq,:), cr.b(ineq), ...
                                   cr.A(cr.facet,:), cr.b(cr.facet));
    if okay
        normal = cr.A(cr.facet,:)';
        x0p = x0 + 1e-4*normal/norm(normal);
        [~, feas, ~, newactive] = solveprimalqp(x0p, pmpqp);
    else
        feas = false();
    end
    if ~okay || ~feas
        newactive = [];
    end
end%function

function [A, b, scale] = scalerows(A, b, zerotol)
    % [A, b, scale] = scalerows(A, b, [zerotol])
    %
    % Scales the rows of A and b so that each row of [A, b] has unit norm.
    % If zerotol is given, any entries of A with absolute value less than
    % zerotol (after scaling) are set to zero. The default for zerotol is 1e-12.
    %
    % Any trivial rows (with A(i,:) = 0 and b(i) = 0) are set to b = 1.
    narginchk(2, 3);
    if nargin() < 3
        zerotol = 1e-12;
    end

    % Perform scaling.
    Z = [A, b];
    scale = sqrt(sum(Z.^2, 2));
    trivrows = (scale == 0);
    scale(trivrows) = 1; % Don't do anything to these guys.

    A = bsxfun(@rdivide, A, scale);
    b = b./scale;
    b(trivrows) = 1;

    % Screen out any elements that are very small.
    A(abs(A) < 1e-12) = 0;
end%function

function hex = bin2hex(bits)
    % Converts logical vector bits to hexidecimal string.
    hex = '0123456789ABCDEF';
    bits = bits(:);
    Npad = 4 - mod(length(bits), 4);
    if Npad == 4
        Npad = 0;
    end
    bits = [zeros(Npad, 1); double(bits)];
    Nhex = length(bits)/4;
    digits = [8, 4, 2, 1]*reshape(bits, 4, Nhex);
    hex = hex(digits + 1);
end%function

function crid = getcrid(active)
    % Returns the string ID of the given set of active constraints.
    crid = ['CR_', bin2hex(active)];
end%function

function log_(file_or_str, varargin)
    % Writes to log file using sprintf syntax. Pass integer file ID to start
    % logging. Call with no arguments to close.
    persistent logfile
    if nargin() == 1 && ~ischar(file_or_str) && isscalar(file_or_str)
        % Open log file.
        logfile = file_or_str;
        if logfile == -1
            error('Invalid log file!');
        end
    elseif nargin() == 0
        if ~isempty(logfile)
            fclose(logfile);
            logfile = [];
        end
    elseif ~isempty(logfile)
        fprintf(logfile, file_or_str, varargin{:});
        if isOctave()
            fflush(logfile);
        end
    end
end%function

function [mpqp, T] = ss2mpqp(varargin)
% mpqp = ss2mpqp(sys)
%
% Converts the linear state-space system described by sys.
%
% sys must contain the following fields:
%
% - A, B: matrices that describe model x^+ = A*x + B*u
% - Q, R: matrices for stage cost x'*Q*x + u'*Q*u
% - S: cross-term matrix for 2*x'*S'u (defaults to 0).
% - P: matrix for terminal penalty x'*P*x (defaults to Q).
% - N: integer choosing the horizon
%
% sys can optionally contain the following fields:
%
% - xlb, xub, ulb, uub: vectors of bounds for x or u
% - C, D, ylb, yub: output matrices and bounds
% - Af, bf: matrices that define Xf as polyhedron Af*xf <= bf
% - keepstates: True or False whether to eliminate the state matrices. If True,
%               variables are interleaved as [u(0); x(1); u(1); x(2); ...].
% - dualstates: True or False whether to dualize the state equality constraints.
%               If True, variables are interleaved as [u(0); lam(1); x(1); ...]
%               Note that this implicitly sets keepstates=True.
%
% Values can also be passed as (key, value) pairs instead of as a struct.
%
% Note that all of these bounds must be chosen so that the feasible set is
% full-dimensional. In particular, lb < ub, and (Af, bf) cannot contain
% implicit equality constraints.
%
% Returned value is a struct with fields to describe the mpQP
%
%     min  0.5*z'*Q*x + p'*C*z
%      z
%     s.t. A*z <= b + S*p
%
% with p = x(0) and z = [u(0); u(1); ...].
if nargin() == 1
    sys = varargin{1};
else
    sys = struct(varargin{:});
end
if ~isstruct(sys)
    error('Input sys must be a struct!');
elseif any(~isfield(sys, {'A', 'B', 'Q', 'R', 'N'}))
    error('Input sys is missing required fields!');
end

% Get main matrices and sizes.
A = sys.A;
B = sys.B;
Q = sys.Q;
R = sys.R;
[Nx, Nu] = size(B);
Nt = sys.N;

% Get optional values.
S = structGet(sys, 'S', zeros(Nx, Nu));
P = structGet(sys, 'P', Q);
xlb = structGet(sys, 'xlb', -inf(Nx, 1));
xub = structGet(sys, 'xub', inf(Nx, 1));
ulb = structGet(sys, 'ulb', -inf(Nu, 1));
uub = structGet(sys, 'uub', inf(Nu, 1));
C = structGet(sys, 'C', zeros(0, Nx));
Ny = size(C, 1);
D = structGet(sys, 'D', zeros(Ny, Nu));
ylb = structGet(sys, 'ylb', -inf(Ny, 1));
yub = structGet(sys, 'yub', inf(Ny, 1));
Af = structGet(sys, 'Af', zeros(0, Nx));
if isempty(Af)
    bf = zeros(0, 1);
else
    if ~isfield(sys, 'bf')
        error('Must provide bf if Af is given!');
    else
        bf = sys.bf;
    end
end
dualstates = structGet(sys, 'dualstates', false());
keepstates = dualstates || structGet(sys, 'keepstates', false());

% Compute state transition matrices that give
%
%   w = T*z
%
% with z = [x(0); u(0); u(1); ...; u(N - 1)], and
% w = [x(0); u(0); x(1); u(1); ...; x(N - 1); u(N - 1); x(N)].
Nxu = Nx + Nu;
Nw = Nt*Nxu + Nx;

% Rows corresponding to x(k).
ix = bsxfun(@plus, (1:Nx)', Nxu:Nxu:(Nxu*Nt));

% Columns for x(0) and u(k).
jx0 = (1:Nx)';
ju = bsxfun(@plus, (1:Nu)', Nx:Nxu:(Nxu*Nt));

% Get state transition matrix.
T = eye(Nw);
if keepstates
    Nz = Nw;
    keep = true(Nw, 1);
else
    Cont = zeros(Nx, Nw);
    Cont(:,jx0) = eye(Nx);
    for k = 1:Nt
        % Update controllability matrix.
        Cont = A*Cont;
        Cont(:,ju(:,k)) = B;

        % Add entries for x(k).
        T(ix(:,k),:) = Cont;
    end
    Nz = Nt*Nu + Nx;
    keep = [jx0(:); ju(:)];
end
T = T(:,keep);

% Get model constraints.
if keepstates
    Aeq = kron(eye(Nt + 1), [A, B]) + kron(diag(ones(Nt, 1), 1), ...
                                           [-eye(Nx), zeros(size(B))]);
    Aeq = Aeq(1:(end - Nx),1:(end - Nu));
    beq = zeros(Nt*Nx, 1);
else
    Aeq = zeros(0, Nw);
    beq = zeros(0, 1);
end

% Get output constraints.
A_y = [kron(eye(Nt), [C, D]), zeros(Ny*Nt, Nx)];
A_y = [A_y; -A_y];
b_y = [repmat(yub, Nt, 1); repmat(-ylb, Nt, 1)];

% Get polyhedron for bound constraints.
lb = [-inf(Nx, 1); repmat([ulb; xlb], Nt, 1)];
ub = [inf(Nx, 1); repmat([uub; xub], Nt, 1)];
[A_lim, b_lim] = hyperrectangle(lb, ub);

% Get polyhedron for terminal constraints.
A_Xf = [zeros(size(Af, 1), Nxu*Nt), Af];
b_Xf = bf;

% Formulate objective function.
H = blkdiag(kron(eye(Nt), [Q, S; S', R]), P);

% Reduce all of the matrices using the state transition matrix.
Alt = [A_lim; A_y; A_Xf]*T;
blt = [b_lim; b_y; b_Xf];
Aeq = Aeq*T;
H = T'*H*T;

% Dualize if requested.
if dualstates
    [H, Alt] = dualize(H, Alt, Aeq, [Nx, Nx + Nu]);
    Nz = Nz + Nx*Nt;
    Aeq = zeros(0, Nz);
end

% Remove trivial rows.
keep = (blt ~= inf());
Alt = Alt(keep,:);
blt = blt(keep);

% Formulate QP matrices.
mpqp = struct();

x0 = [true(Nx, 1); false(Nz - Nx, 1)];

mpqp.A = Alt(:,~x0);
mpqp.b = blt;
mpqp.S = -Alt(:,x0);

mpqp.Aeq = Aeq(:,~x0);
mpqp.beq = beq;
mpqp.Seq = -Aeq(:,x0);

mpqp.Q = H(~x0,~x0);
mpqp.C = H(~x0,x0);

end%function

function [H_, Alt_] = dualize(H, Alt, Aeq, blocksize)
    % Dualizes the equality constraints for the given QP.
    %
    % blocksize should give the size of each variable block in Aeq. Dual
    % multipliers are then interleaved between the blocks.
    Nvar = size(H, 1);
    Nblocks = size(Aeq, 1)/blocksize(1);
    if round(Nblocks) ~= Nblocks
        error('blocksize(1) must be a multiple of size(Aeq, 1)!');
    end

    % Get logical masks for variables and dual multipliers.
    var = [repmat([true(blocksize(2), 1); ...
                   false(blocksize(1), 1)], Nblocks, 1); ...
           true(Nvar - blocksize(2)*Nblocks, 1)];
    mul = ~var;

    Ntot = length(var);

    H_ = zeros(Ntot, Ntot);
    H_(var,var) = H;
    H_(mul,var) = Aeq;
    H_ = 0.5*(H_' + H_); % Make symmetric.

    Alt_ = zeros(size(Alt, 1), Ntot);
    Alt_(:,var) = Alt;
end%function

function [Xn, V, Z] = findXn(A, B, K, N, xlb, xub, ulb, uub, terminal, doplot)
% [Xn, V, Z] = findXn(A, B, K, N, xlb, xub, ulb, uub, terminal, [doplot])
%
% Calculates the various sets for the given system and parameters.
%
% Most inputs are self-explanatory. terminal should be either the string
% 'termeq' to use a terminal equality constraint, or 'lqr' to use the
% maximum LQR-invariant set.
%
% Output Xn is a cell array defining each of the \bbX_n sets. Xn{1} is the
% terminal set, Xn{2} is \bbX_1, etc. V is a cell array with each entry giving
% the extreme vertices of the corresponding Xn (as a 2 by N matrix). Z is a
% structure defining the feasible space.
%
% Also makes a plot of all the sets unless doplot is False.
narginchk(9, 10);
if nargin() < 10
    doplot = true();
end

% Define constraints for Z.
Nx = size(A, 1);
[Az, bz] = hyperrectangle([xlb; ulb], [xub; uub]);
Z = struct('G', Az(:,1:Nx), 'H', Az(:,(Nx + 1):end), 'psi', bz);

% Decide terminal constraint.
switch terminal
case 'termeq'
    % Equality constraint at the origin.
    Xf = [0; 0];
case 'lqr'
    % Build feasible region considering x \in X and Kx \in U.
    [A_U, b_U] = hyperrectangle(ulb, uub);
    A_lqr = A_U*K;
    b_lqr = b_U;
    [A_X, b_X] = hyperrectangle(xlb, xub);
    Acon = [A_lqr; A_X];
    bcon = [b_lqr; b_X];

    % Use LQR-invariant set.
    Xf = struct();
    ApBK = A + B*K; % LQR evolution matrix.
    [Xf.A, Xf.b] = calcOinf(ApBK, Acon, bcon);
    [~, Xf.A, Xf.b] = removeredundantcon(Xf.A, Xf.b);
otherwise
    error('Unknown value for terminal: %s', terminal);
end

% Now do feasible sets computation.
if doplot
    figure();
    hold('on');
    colors = jet(N + 1);
end
Xn = cell(N + 1, 1);
Xn{1} = Xf;
names = cell(N + 1, 1);
names{1} = 'Xf';
V = cell(N + 1, 1);
for n = 1:(N + 1)
    % Plot current set.
    if n > 1
        names{n} = sprintf('X%d', n - 1);
    end
    if doplot
        plotargs = {'-o', 'color', colors(n,:)};
    else
        plotargs = {false()};
    end
    V{n} = plotpoly(Xn{n}, plotargs{:});
    if n == (N + 1)
        break
    end

    % Compute next set. Also need to prune constraints.
    nextXn = computeX1(Z, A, B, Xn{n});
    [~, nextXn.A, nextXn.b] = removeredundantcon(nextXn.A, nextXn.b);
    Xn{n + 1} = nextXn;
end
if doplot
    legend(names{:});
end

end%function

function [A, b] = hyperrectangle(lb, ub)
% [A, b] = hyperrectangle(lb, ub)
%
% Returns halfspace representation of hyperrectangle with bounds lb and ub.
% Any infinite or NaN bounds are ignored.

narginchk(2, 2);
if ~isvector(lb) || ~isvector(ub) || length(lb) ~= length(ub)
    error('Inputs must be vectors of the same size!');
end

A = kron(eye(length(lb)), [1; -1]);
b = reshape([ub(:)'; -lb(:)'], 2*length(lb), 1);

goodrows = ~isinf(b) & ~isnan(b);
A = A(goodrows,:);
b = b(goodrows);

end%function

function [AOinf, bOinf, tstar] = calcOinf(F, A, b, tmax)
% [AOinf, bOinf, tstar] = calcOinf(F, A, b, [tmax])
%
% Calculates the maximum admissible set for x^+ = F*x subject to A*x <= b. Note
% that if F is unstable, this procedure will not work.
%
% tmax is the value of t to stop at, as an upper bound is not known a-priori.
% The default value is 100. If this bound is reached without termination, then
% tstar is set to inf.

% Arguments and sizes.
narginchk(3, 4);
if nargin() < 4
    tmax = 100;
end
Nx = size(F, 1);
if size(F, 2) ~= Nx
    error('F must be square!');
end
Nc = size(A, 1);
if size(A, 2) ~= Nx
    error('A must have the same number of columns as F!');
elseif ~isvector(b) || length(b) ~= Nc
    error('b must be a vector with an entry for each row of A!');
end
b = b(:);

% Define linear programming function.

if isOctave()
    solvelp = @solvelp_octave;
else
    solvelp = @solvelp_matlab;
end

% Start the algorithm.
Ft = eye(Nx);
AOinf = zeros(0, Nx);
bOinf = zeros(0, 1);
tstar = inf();

for t = 0:tmax
    % Add constraints for new time point.
    AOinf = [AOinf; A*Ft];
    bOinf = [bOinf; b];

    % Recalculate objective.
    Ft = F*Ft;
    fobj = A*Ft;

    % Maximize each component, stopping early if any bounds are violated.
    okay = true();
    for i = 1:Nc
        [obj, feas] = solvelp(fobj(i,:), AOinf, bOinf);
        if ~feas || obj > b(i)
            okay = false(); % N isn't high enough Need to keep going.
            continue
        end
    end

    % If everything was feasible, then we're done.
    if okay
        tstar = t;
        break
    end
end

end%function

function [obj, feas] = solvelp_octave(f, A, b)
    % Octave version to solve LP.
    Nx = size(A, 2);
    lb = -inf(Nx, 1);
    ub = inf(Nx, 1);
    ctype = repmat('U', size(A, 1), 1);
    vtype = repmat('C', Nx, 1);
    sense = -1; % Maximization.
    [~, obj, err, extra] = glpk(f, A, b, lb, ub, ctype, vtype, sense, ...
                                struct('msglev', 0));
    status = extra.status;
    feas = (err == 0) && (status == 5);
end%function


function [obj, feas] = solvelp_matlab(f, A, b)
    % Matlab version to solve LP.
    prob = struct('f', -f, 'Aineq', A, 'bineq', b, 'options', ...
                  optimoptions('linprog', 'Algorithm', 'dual-simplex', ...
                               'display', 'off'), 'solver', 'linprog');
    [~, obj, exitflag] = linprog(prob);
    obj = -obj; % Fix sign of objective value.
    feas = (exitflag == 1);
end%function

function [nr, Anr, bnr, h, x0] = removeredundantcon(A, b, x0, tol, qhullopts)
% [nr, Anr, bnr, h, x0] = removeredundantcon(A, b, [x0], [tol], qhullopts)
%
% Finds the non-redundant constraints for the polyhedron Ax <= b. nr is a
% column vector with the non-redundant rows. If requested, Anr and bnr are the
% non-redundant parts of A and b.
%
% If x0 is supplied, it must be in the strict interior of A. Otherwise an error
% is thrown. Specifying a valid x0 will speed up the function.
%
% tol is used to decide how much on the interior we need to be. If not supplied,
% the default value is 1e-8*max(1, norm(b)/N). Note that this may be too large
% if b is poorly scaled.
%
% qhullopts is a string of options to pass to qhull. Defaults are described in
% the documentation for convhulln (see `help convhulln`).
%
% h is the output of convhulln, which actually describes the facets, and x0
% is a point on the interior of the polyhedron. Note that if the input
% polyhedron is unbounded, h may have zeros in some entries corresponding to the
% row of all zeros that needs to be added for the method to work.
%
% Note that this requires finding convex hull in N + 1 dimensions, where N
% is the number of columns of A. Thus, this will get very slow if A has a lot
% of columns.
narginchk(2, 5);

% Force b to column vector and check sizes.
b = b(:);
if isrow(b)
    b = b';
end
if size(A, 1) ~= length(b)
    error('A and b must have the same number of rows!');
end
if nargin() < 3
    x0 = [];
end
if nargin() < 4 || isempty(tol)
    tol = 1e-8*max(1, norm(b)/length(b));
elseif tol <= 0
    error('tol must be strictly positive!')
end
if nargin() < 5
    if size(A, 2) <= 4
        qhullopts = {'Qt'};
    else
        qhullopts = {'Qt', 'Qx'};
    end
end


% Save copies before things get messed up.
Anr = A;
bnr = b;

% First, get rid of any rows of A that are all zero.
Anorms = max(abs(A), [], 2);
badrows = (Anorms == 0);
if any(b(badrows) < 0)
    error('A has infeasible trivial rows.')
end
A(badrows, :) = [];
b(badrows) = [];
goodrows = [0; find(~badrows)]; % Add zero for all zero row that gets added.

% Need to find a point in the interior of the polyhedron.
if isempty(x0)
    if all(b > 0)
        % If b is strictly positive, we know the origin works.
        x0 = zeros(size(A, 2), 1);
    else
        error('Must supply an interior point!');
    end
else
    x0 = x0(:);
    if isrow(x0)
        x0 = x0';
    end
    if length(x0) ~= size(A,2)
        error('x0 must have as many entries as A has columns.')
    end
    if any(A*x0 >= b - tol)
        error('x0 is not in the strict interior of Ax <= b!')
    end
end

% Now, project the rows of P and find the convex hull.
btilde = b - A*x0;
if any(btilde <= 0)
    warning('Shifted b is not strictly positive. convhull will likely fail.')
end
Atilde = [zeros(1, size(A, 2)); bsxfun(@rdivide, A, btilde)];
h = convhulln(Atilde, qhullopts);
u = unique(h(:));

nr = goodrows(u);
if nr(1) == 0
    nr(1) = [];
end
h = goodrows(h);

% Finally, grab the appropriate rows for Anr and bnr.
Anr = Anr(nr, :);
bnr = bnr(nr);

end%function

function X1 = computeX1(Z, A, B, Xf)
% X1 = computeX1(Z, [A], [B], [Xf])
%
% Computes the feasible set X_1 for the system x^+ = Ax + Bu subject to
% constraints Gx + Hu <= psi and x^+ \in Xf.
%
% Z must be a struct with fields G, H, and psi.
%
% A and B are only necessary if the terminal constraint is given.
%
% Xf can be either a struct with fields A and b to define a polytope, or a
% single vector to define a point constraint. If not provided, it is assumed
% that Xf is the entire space.
%
% X1 is returned as a struct with fields A and b defining a set of inequality
% constraints.

% Check arguments.
narginchk(1, 4);
if ~isstruct(Z) || ~all(isfield(Z, {'G', 'H', 'psi'}))
    error('Invalid input for Z!');
end
Nx = size(Z.G, 2);
Nu = size(Z.H, 2);
if nargin() >= 3
    sys = struct('A', A, 'B', B); % Save these.
end

% Preallocate constraint matrices.
A = [Z.G, Z.H];
b = Z.psi;
Aeq = zeros(0, Nx + Nu);
beq = zeros(0, 1);

% Do some more stuff if there is a terminal constraint.
if nargin() >= 4 && ~isempty(Xf)
    if ~isequal([size(sys.A), size(sys.B)], [Nx, Nx, Nx, Nu])
        error('Incorrect size for A or B!');
    end
    if isstruct(Xf)
        % Polyhedron.
        if ~all(isfield(Xf, {'A', 'b'}))
            error('Struct Xf must have fields A and b!');
        end
        A = [A; Xf.A*[sys.A, sys.B]];
        b = [b; Xf.b];
    elseif isvector(Xf) && length(Xf) == Nx
        % Terminal equality constraint.
        Aeq = [sys.A, sys.B];
        beq = Xf;
    else
        error('Invalid input for Xf!');
    end
end

% Now do the projection step.
for i = 1:Nu
    [A, b, Aeq, beq] = fouriermotzkin(A, b, Aeq, beq);
end
X1 = struct('A', A, 'b', b);

end%function

function p = plotpoly(p, varargin)
% p = plotpoly(p, ...)
% p = plotpoly({A, b}, ...)
% p = plotpoly(struct('A', A, 'b', b), ...)
%
% Plots the polyhedron with vertices given in the 2 by N matrix p or given by
% the extreme points of A*x <= b. Any additional arguments are passed to plot.
%
% Returns the extreme points matrix p. If you only want this matrix, pass
% false as the only additional argument, and the plot will not be made.
if nargin() < 1
    error('Argument p is required!');
end

% First, find the vertices if {A, b} given.
if iscell(p)
    p = halfspace2vertex(p{1}, p{2})';
elseif isstruct(p)
    p = halfspace2vertex(p.A, p.b)';
end

% Next, sort the vertices.
ptilde = bsxfun(@rdivide, bsxfun(@plus, p, -mean(p, 2)), std(p, 0, 2));
x = ptilde(1,:);
y = ptilde(2,:);
[th, r] = cart2pol(x, y);
thneg = (th < 0);
th(thneg) = th(thneg) + 2*pi(); % Makes theta in [0, 2*pi] instaed of [-pi, pi].
[~, s] = sortrows([th', r']); % Sort on theta then r.
p = p(:,s);

% Duplicate first data point to give closed cycle.
p = p(:,[1:end,1]);

% Now plot.
if length(varargin) == 0 || ~isequal(varargin{1}, false())
    plot(p(1,:), p(2,:), varargin{:});
end
end%function

function [x0, okay, feas, margin] = findinteriorpoint(A, b, Aeq, beq, tol, maxinside)
% [x0, okay, feas, margin] = findinteriorpoint(A, b, [Aeq], [beq], [tol])
%
% Find a strictly feasible point x0 so that A*x0 <= b - tol. If no such point
% can be found, okay is set to False.
%
% If there is at least a feasible point (but not necessarily on the interior),
% then feas is true, and x0 gives that point. If both okay and feas are false,
% then x0 is meaningless.
%
% margin gives the value e such that A*x0 <= b - e.
%
% The origin is always checked first. If it does not work, an LP is solved
% to find a valid point.
%
% tol is used to decide how much on the interior we need to be. If not
% supplied, the default value is 1e-8*max(1, norm(b)/N). Note that this may
% be too large if b is poorly scaled.
if nargin() < 2
    error('A and b are mandatory.')
elseif nargin() < 5
    tol = 1e-8*max(1, norm(b)/length(b));
end
if nargin() < 6
    maxinside = 1;
else
    maxinside = max(tol, maxinside);
end
[m, n] = size(A);
if nargin() < 4 || isempty(Aeq)
    Aeq = zeros(0, n);
    beq = [];
end
meq = size(Aeq, 1);
okay = false();

% Check whether the origin is on the inside.
if all(abs(beq) < tol) && all(b > tol)
    x0 = zeros(n, 1);
    okay = true();
    feas = true();
    margin = min(b);
end

% Try to use fminsearch if there are no equality constraints. Doesn't work
% well if the number of dimensions is very high, so we cap it at 10.
if ~okay && meq == 0 && m <= 10
    options = optimset('display', 'off');
    [x0, maxr] = fminsearch(@(x) max(max(A*x - b), -1e5*tol), A\b, options);
    okay = (maxr < -tol);
    feas = okay;
    margin = -maxr;
end

% Solve LP otherwise.
if ~okay
    c = [zeros(n, 1); -1];
    AA = [A, ones(m, 1)];
    AAeq = [Aeq, zeros(meq,1)];
    lb = [-inf(n, 1); 0];
    ub = [inf(n, 1); maxinside];
    if isOctave()
        ctype = [repmat('U', m, 1); repmat('S', meq, 1)];
        [xtilde, ~, err, extra] = glpk(c, [AA; AAeq], [b; beq], lb, ub,  ...
            ctype, repmat('C', n + 1, 1), 1, struct('msglev', 0));
        okay = (err == 0 && extra.status == 5);
    else
        options = optimoptions('linprog', 'display', 'off', 'algorithm', 'dual-simplex');
        [xtilde, ~, exitflag] = linprog(c, AA, b, AAeq, beq, lb, ub, [], options);
        okay = (exitflag == 1);
    end
    if isempty(xtilde)
        margin = -inf();
    else
        margin = xtilde(end); % Amount constraints could be tightened.
    end
    okay = okay && (margin >= tol);
    if isempty(xtilde)
        x0 = zeros(n, 1);
        okay = false();
    else
        x0 = xtilde(1:end-1);
    end

    % Check feasibility of x0.
    feas = all(A*x0 - b < tol);
    if feas && ~isempty(Aeq)
        feas = all(abs(Aeq*x0 - beq) < tol);
    end
    okay = okay && feas;
end
end%function

function [V, nr] = halfspace2vertex(A, b, x0)
% [V, nr] = halfspace2vertex(A, b, [x0])
%
% Finds extreme points of polyhedron A*x <= b. Note that the polyhedron must
% have a point strictly on its interior.
%
% If provided, x0 must be a point on the interior of the polyhedron. If it is
% not given, one is found by solving a linear program.
%
% V is returned as an N by 2 matrix with each row giving an extreme point.
%
% Second output nr is a list of the non-redundant constraints of the polytope.

% Check inputs.
narginchk(2, 3);
Nc = size(A, 1);
Nx = size(A, 2);
if ~isvector(b) || length(b) ~= Nc
    error('b is the incorrect size!');
end
b = b(:); % Make sure b is a column vector.

% Sort out interior point.
if nargin() < 3
    if all(b > 0)
        % The origin is on the interior. Can rescale rows so that b = 1.
        x0 = zeros(Nx, 1);
        A = bsxfun(@rdivide, A, b);
        b = ones(size(b));
    else
        x0 = findinteriorpoint(A, b);
    end
elseif ~isvector(x0) || length(x0) ~= Nx
    error('Invalid size for x0!');
end
x0 = x0(:); % Make sure x0 is a column vector.

% Get non-redundant constraints from A and b.
[nr, ~, ~, k] = removeredundantcon(A, b, x0);

% Now loop through facets to find vertices.
V = zeros(size(k, 1), Nx);
keep = true(size(k, 1), 1);
for ix = 1:size(k, 1)
    F = A(k(ix,:),:);
    g = b(k(ix,:));
    [keep(ix), V(ix,:)] = fullranksolve(F, g);
end

V = V(keep,:);
[~, u] = unique(round(V*1e6), 'rows');
V = V(u,:);

% If in 2D, sort the vertices.
if Nx == 2
    V = polarsort(V);
end

end%function

function [fullrank, x] = fullranksolve(A, b);
    % Checks whether the system is full rank and if so, solves it. If it is not
    % full rank, a vector of NaNs are returned.
    Nx = size(A, 1);
    [U, S, V] = svd(A);
    s = diag(S);
    tol = Nx*eps(s(1)); % Rank tolerance.
    fullrank = all(s >= tol);
    if fullrank
        x = V*diag(1./s)*U'*b;
    else
        x = NaN(Nx, 1);
    end
end%function

function [p, s] = polarsort(p)
    % [p, s] = polarsort(p)
    %
    % Sorts the [n by 2] matrix p so that the points are counter-clockwise starting
    % at the theta = 0 axis. For ties in theta, sorts on radius.
    x = p(:,1);
    y = p(:,2);
    x = (x - mean(x))/std(x); % Normalize so that the origin is at the center.
    y = (y - mean(y))/std(y);
    [th, r] = cart2pol(x, y);
    [~, s] = sortrows([th, r]); % Sort on theta then r.
    p = p(s,:);
end%function

function [A, b, Aeq, beq] = fouriermotzkin(A, b, Aeq, beq, ielim)
% [A, b, Aeq, beq] = fouriermotzkin(A, b, [Aeq], [beq], [ielim])
%
% Perform one step of Fourier-Motzkin elimination for the set defined by
%
%   {x \in R^n : A*x <= b, Aeq*x = beq}
%
% Note that the inequality constrants can be degenerate, although degeneracy
% will create significantly more constraints compared to explicit equality
% constraints.
%
% Optional argument ielim decides which column to eliminate. Default is the last
% column.

% Check arguments and get sizes.
narginchk(2, 5);
Nlt = size(A, 1);
if ~isvector(b) || length(b) ~= Nlt
    error('Invalid size for b!');
end
Nx = size(A, 2);
if nargin() < 3 || isempty(Aeq)
    Aeq = zeros(0, Nx);
    beq = zeros(0, 1);
    Neq = 0;
elseif nargin() < 4
    error('beq is required if Aeq is given!');
else
    if size(Aeq, 2) ~= Nx
        error('Aeq has the wrong number of columns!');
    end
    Neq = size(Aeq, 1);
    if ~isvector(beq) || length(beq) ~= Neq
        error('Invalid size for beq!');
    end
end
if nargin() < 5
    ielim = Nx;
elseif ~isscalar(ielim) || ielim <= 0 || ielim > Nx || round(ielim) ~= ielim
    error('ielim must be a scalar positive integer less than Nx!');
end

% Now decide what to do. First, look for an equality constraint with the
% variable of interest.
[pivval, pivrow] = max(abs(Aeq(:,ielim)));
if isempty(pivrow) || pivval == 0
    % No suitable equality constraint found. Need to change inequality
    % constraints.
    [A, b] = fmelim(A, b, ielim);
    Aeq(:,ielim) = [];
else
    % An equality constraint is available. Handle that.
    a = Aeq(pivrow,:);
    c = a(ielim);

    % Make the pivots.
    Aeq = Aeq - Aeq(:,ielim)*a/c;
    beq = beq - Aeq(:,ielim)*beq(pivrow)/c;

    A = A - A(:,ielim)*a/c;
    b = b - A(:,ielim)*beq(pivrow)/c;

    % Get rid of the appropriate rows and columns.
    A(:,ielim) = [];
    Aeq(:,ielim) = [];
    Aeq(pivrow,:) = [];
    beq(pivrow) = [];
end

% Zap any rows that are all zeros.
[A, b] = removezerorows(A, b);
[Aeq, beq] = removezerorows(Aeq, beq);

end%function


% *****************************************************************************
% Helper Functions
% *****************************************************************************

function [Ae, be] = fmelim(A, b, ielim)
    % Performs one step of Fourier-Motzkin elimination for inequality
    % constraints.
    c = A(:,ielim);
    I0 = find(c == 0);
    Ip = find(c > 0);
    Im = find(c < 0);

    Nx = size(A, 2);
    Ne = length(I0) + length(Ip)*length(Im);

    E = [A(:,1:ielim - 1), A(:,ielim + 1:end), b];
    Ee = [E(I0,:); kron(c(Ip), E(Im,:)) - kron(E(Ip,:), c(Im))];

    Ae = Ee(:,1:end-1);
    be = Ee(:,end);
end%function

function [A, b] = removezerorows(A, b)
    % Removes rows of A that are all zeros.
    keeprows = any(A, 2);
    A = A(keeprows,:);
    b = b(keeprows);
end%function

function [z, id] = pwalookup(x, regions, feas)
    % [z, id] = pwalookup(x, regions, [feas])
    %
    % Returns optimal point z. If feas is provided, it should be a struct with
    % fields A and b to specify the feasible region. If not provided, all
    % regions will be searched if x is infeasible.
    z = [];
    id = '';
    if nargin() < 3 || all(feas.A*x - feas.b <= 0)
        for i = 1:length(regions)
            region = regions{i};
            if all(region.A*x <= region.b)
                z = region.Z*x + region.z0;
                id = region.id;
                found = true();
                break
            end
        end
    end

function p = kerneldensity(xsamples, sigma, x)
    % Returns kernel density estimate at points x using samples in xsamples and
    % a Gaussian kernel with the given standard deviation.
    xsamples = xsamples(:);
    x = x(:)';
    den = sqrt(2*pi())*sigma*length(xsamples);
    pdf = @(x, mu) exp(-(x - mu).^2./(2*sigma.^2))./den;
    p = sum(bsxfun(pdf, x, xsamples));

function scatterplot(x, y, t, titlestr)
    figure();
    scatter(x(1,:), x(2,:), 5, y, 'filled');
    caxis([t(1), t(end)]);
    colorbar();
    title(titlestr);

Figure 7.8:

Solution times for explicit and implicit MPC for N = 20.

Code for Figure 7.8

main.m

solvempqp.m

ss2mpqp.m

findXn.m

hyperrectangle.m

calcOinf.m

removeredundantcon.m

computeX1.m

plotpoly.m

findinteriorpoint.m

halfspace2vertex.m

fouriermotzkin.m

pwalookup.m

kerneldensity.m

scatterplot.m