function mpqp_example
% mpqp example
%
% M. Herceg, Automatic Control Laboratory, 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_name = ['.',filesep,'figures',filesep,'Example7-6'];
% laprint options
set(0,'defaulttextinterpreter','none')
width = 10;

%% core code
% matrices of MPQP formulation G*x <= w + S*th
G=[1 0;-1 0;0 1;0 -1;1 -1; -1 1];
w=[1;1;1;1;0;0];
S=[0 0;0 0;0 0;0 0;1 1;-2 1];
Ath = [eye(2); -eye(2)]; bth = 1*ones(4,1); 

% formulate MPQP, min 0.5*x'*H*x + (pF*th+f)'*x s.t. A*x <= b + pB*th, Ath*th <= bth
problem = Opt('H',eye(2),'pF',[1 1;1 -1],'A',G,'b',w,'pB',S,'Ath',Ath,'bth',bth);
solution=problem.solve; 

% plot partition
figure 
hold on
plot(solution.xopt,'linewidth',0.8,'wirestyle','--','color',[0.9 0.9 0.9])
axis([-1.2 1.2 -1.2 1.2]);
grid on

% list active sets
for j=1:length(solution.xopt.Set)
    % get a point inside each polytope
    chebC=solution.xopt.Set(j).chebyCenter;
    
    % solve optimization problem for a fixed point to get the active set
    problem_fixed = Opt('H',eye(2),'f',[1 1;1 -1]*chebC.x,'A',G,'b',w+S*chebC.x);
    solution_fixed=problem_fixed.solve;
    
    % display active set
    disp(['Active set in Region ',num2str(j),' is:'])
    ActiveSet=find(G*solution_fixed.xopt-(w+S*chebC.x)>=-1e-6);
    ActiveSet'
    % nonzero Lagrange multpliers for inequality constraints
    lam = solution.xopt.feval(chebC.x, 'dual-ineqlin')';
    find(lam >= 1e-6)

    % cost function is stored as x = F*th + g
    disp(['Cost function is in Region ',num2str(j),' is:'])
    [solution.xopt.Set(j).getFunction('obj').F solution.xopt.Set(j).getFunction('obj').g]
    ht3 = text(chebC.x(1)-0.04,chebC.x(2),num2str(j));
    set(ht3, 'FontSize', label_font_size);
end


%% save book figures
% font size of tick labels
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([-1,-0.5,0,0.5,1]);
yl = transpose([-1,-0.5,0,0.5,1]);
set(gca,'XTick',xl);
set(gca,'YTick',yl);
set(gca,'XTickLabel',{'-1','','','','1'});
set(gca,'YTickLabel',{'-1','','','','1'});

% print
disp('Hit any key to print the figure and continue.');
pause
saveas(gcf, [figure_name,'_matlab'], 'fig');

laprint(gcf, [figure_name,'_tex'],'scalefonts','off','width',width);

end