function FeasibilityExampleSlide
% feasibility study on double integrator example
% generates colored figure
% 
% Thomas Besselmann, 18.08.08
% Francesco Borrelli July 05 2009
% M. Herceg, ETH Zurich, 2014
%
% requires MPT3

close all

%% printing parameters
label_font_size = 14;
tick_font_size = 10;
line_width = 0.8;
axeswidth=0.2;
% create new folder for storing figures
mkdir('figures');
figure_name1 = ['.',filesep,'figures',filesep,'Example13-1a_slide'];


%% core code
% system dynamics
A=[1 1; 0 1];
B = [0;1];
model = LTISystem('A',A,'B',B);
model.x.min = [-5;-5];
model.x.max = [ 5; 5];
model.u.min=-0.5;
model.u.max=0.5;

% penalties
model.x.penalty = QuadFunction(eye(2));
model.u.penalty = QuadFunction(10);
model.x.with('terminalPenalty');
model.x.terminalPenalty = model.x.penalty;

% online controller
horizon = 3;
ctrl = MPCController(model, horizon);

% plot the openloop trajectory for two initial points
x0 = [-4.5, -4.5;
         2, 3];

fig1=figure;
hold on
for kk=1:2
    x = x0(:,kk);
    X = x;
    for j=1:16
        [u, feasible, openloop] = ctrl.evaluate(x);
        if feasible
            plot(openloop.X(1,:), openloop.X(2,:), 'k*-.','LineWidth',line_width)
            x = openloop.X(:,2);
            X = [X, x];
        end
    end
    if kk==1,
        plot(X(1,:), X(2,:), 'gs-','LineWidth',line_width,'MarkerSize',10); 
    else
        plot(X(1,:), X(2,:), 'ro-','LineWidth',line_width,'MarkerSize',10);
    end
end
grid on
axis([-5 5 -0.5 3.5])

set(gca,'LineWidth',axeswidth)
set(gca,'FontSize', tick_font_size);

title('')
hx=xlabel('$x_{1}$');
set(hx, 'FontSize', label_font_size);
hy=ylabel('$x_{2}$');
set(hy, 'FontSize', label_font_size);

xl = transpose(-5:1:5);
yl = transpose(-5:1:5);
set(gca,'XTick',xl);
set(gca,'YTick',yl);
set(gca,'XTickLabel',num2str(xl));
set(gca,'YTickLabel',num2str(yl));



%% save figure
saveas(fig1, figure_name1, 'epsc');


end

