function dd_approx_example
% double description approximation of a double integrator
%
% C.N. Jones, EPFL, 2013
% Revised by M. Herceg, ETH Zurich 2014
%
% requires MPT3, YALMIP


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,'approxDD'];

%% core code
fprintf('---> Setting up problem\n');

% weight factor
epsilon = 2;

% load data
dat = defineEx;

% Compute verticies of the feasible set
var = [dat.variables.x(:,2:end)]; var = [var(:);dat.variables.u'];
o = Opt(dat.constraints,dat.objective,dat.variables.x(:,1),var);
oi = o.eliminateEquations;
P = Polyhedron([-oi.pB oi.A], oi.b);

% feasible set
p = P.projection(1:size(oi.pB,2));

dat.opt = optimizer(dat.constraints, dat.objective, [], dat.variables.x(:,1), dat.objective);
p.addFunction(Function(@(x)optFunc(x, dat)),'cost');
Q = dat.ctrl.model.x.penalty.H;
p.addFunction(Function(@(x)optFunc(x, dat)+epsilon*x(:)'*Q*x(:)),'cost_gam');

% Compute maximum invariant set (terminal set)
termSet = dat.ctrl.model.LQRSet;

% The function that we're trying to approximate
theFunc = @(x) optFunc(x, dat);

dat.variables.J = sdpvar; % Epigraph variable
x = dat.variables.x(:,1);
epi.con = set(dat.variables.J >= dat.objective + epsilon*x'*Q*x) + dat.constraints;
epi.var = [dat.variables.x(:,1);dat.variables.J];
epi.v = sdpvar(3,1);
epi.obj = (epi.v - epi.var)'*(epi.v - epi.var);
epi.sep = optimizer(epi.con, epi.obj, [],epi.v,[epi.obj;epi.var]);


% Define epigraph
epi.P = Polyhedron('A',[p.A zeros(size(p.A,1),1);0 0 -1],'b',[p.b;0]);
% minimal representatioin
epi.P.minHRep();

while 1
  % Compute vertices
  V=epi.P.minVRep.V;
  
  % Compute optimal value at each vertex
  J = [];
  for i= 1:size(V,1)
    J(i) = theFunc(V(i,1:end-1)');
  end
  
  % Eliminate vertices that are in the terminal set from consideration
  Iterm = termSet.contains(V(:,1:2)');
  J(Iterm)   = [];
  V(Iterm,:) = [];
  
  % Cut off vertices that are the most below the optimal epigraph
  I = find(J'-V(:,end) >= max([J'-V(:,end)]) - 1e-3);
  
  if max(J'-V(:,end)) < -1e-3, break; end
  
  II = epi.P.H(:,3)<-1e-3 & abs(epi.P.H(:,3)+1)>1e-3;
  if sum(II) > 0   
    % Plot the solution so far
    close all

    eval(['fig',num2str(size(epi.P.H,1)),'a = figure;']);    
    epi.h = plotEpi(epi.P, [0.9 0.9 0.9], 1);
    set(epi.h,'linewidth',line_width);
    grid on
    drawnow
    view(3)    
    axis([-6 6 -6 6 0 40])
    
    set(gca,'LineWidth',axeswidth)
    set(gca,'FontSize', tick_font_size);
    set(gca,'XTick',[-5 0 5]);
    set(gca,'YTick',[-5 0 5]);
    set(gca,'ZTick',0:10:40);

    
    hx = xlabel('$x_1$');
    set(hx, 'FontSize', label_font_size);
    hy = ylabel('$x_2$');
    set(hy, 'FontSize', label_font_size);
    set(gca,'LineWidth',axeswidth)
    set(gca,'FontSize', tick_font_size);
    
    % save
    eval(['gga = fig',num2str(size(epi.P.H,1)),'a;']);
    saveas(gga, [figure_name,'_',num2str(size(epi.P.H,1)),'_matlab'], 'fig');
    
    % print
    laprint(gga, [figure_name,'_',num2str(size(epi.P.H,1)),'_tex'],'scalefonts','off');

    
    % Plot the partition
    eval(['fig',num2str(size(epi.P.H,1)),'b = figure;']);
    grid on
    R = epiPartition(epi.P);
    R.plot('colormap','lightgray','linewidth',line_width);
    axis([-5 5 -5 5]);
    set(gca,'LineWidth',axeswidth)
    set(gca,'FontSize', tick_font_size);
    
    hx = xlabel('$x_1$');
    set(hx, 'FontSize', label_font_size);
    hy = ylabel('$x_2$');
    set(hy, 'FontSize', label_font_size);
    set(gca,'LineWidth',axeswidth)
    set(gca,'FontSize', tick_font_size);

    % save
    eval(['ggb = fig',num2str(size(epi.P.H,1)),'b;']);
    saveas(ggb, [figure_name,'_',num2str(size(epi.P.H,1)),'_partition_matlab'], 'fig');
    
    % print
    laprint(ggb, [figure_name,'_',num2str(size(epi.P.H,1)),'_partition_tex'],'scalefonts','off');

    
    % Plot the error
    eval(['fig',num2str(size(epi.P.H,1)),'c = figure;']);
    grid on
    T = [epi.P.H(II,1:2) epi.P.H(II,end)] ./ repmat(-epi.P.H(II,3),1,3);
    p.addFunction(Function(@(x) -max(T*[x(:);-1]) + optFunc(x, dat)), 'err');
    p.fplot('err','grid',60,'color',[0.9 0.9 0.9],'linewidth',line_width,'showgrid',true);
    
    
    % Plot horizontal plane
    hold on
    p.plot('color',0.7*[1 1 1],'linewidth',line_width);
    
    % Add worst-case points
    t = J(I)'-V(I,3);
    if max(t) > 0
      plot3(V(I,1),V(I,2),t,'or','markerFaceColor','k','markerSize',10,'markerEdgeColor','k');
    end
    
    set(gca,'LineWidth',axeswidth)
    set(gca,'FontSize', tick_font_size);
    
    hx = xlabel('$x_1$');
    set(hx, 'FontSize', label_font_size);
    hy = ylabel('$x_2$');
    set(hy, 'FontSize', label_font_size);
    set(gca,'LineWidth',axeswidth)
    set(gca,'FontSize', tick_font_size);
    
    axis([-6 6 -6 6 -10 15]);   
    set(gca,'XTick',[-5 0 5]);
    set(gca,'YTick',[-5 0 5]);
    set(gca,'ZTick',-10:5:15);

    
    % save
    eval(['ggc = fig',num2str(size(epi.P.H,1)),'c;']);
    saveas(ggc, [figure_name,'_',num2str(size(epi.P.H,1)),'_err_matlab'], 'fig');
    
    % print
    laprint(ggc, [figure_name,'_',num2str(size(epi.P.H,1)),'_err_tex'],'scalefonts','off');

    disp('Hit any key to continue');
    pause
    
  end
  
  
  % Compute the seperating hyperplanes
  sepV = epi.sep{V(I,:)'}';
  sepJ = sepV(:,1);
  sepV = sepV(:,2:end);
  
  % Compute normals and offsets
  n = V(I,:)-sepV;
  b = sum(n.*sepV,2);
  
  % Add normals to the epigraph
  epi.P = intersect(epi.P, Polyhedron('A',n,'b',b));
  
end

end

function dat = defineEx(soft)
%
% Define optimizer for double integrator
%
% C.N. Jones, EPFL, 2013
% Revised by M. Herceg, ETH Zurich 2014
%
% requires MPT3, YALMIP


if nargin < 1
  soft = false;
end

% Define an LTI dynamics x^+ = A x + B u
% (note that we can ommit the output equation)
A = [1 1; 0 1];
B = [1; 0.5];
lti = LTISystem('A', A, 'B', B);

% Add constraints on predicted states
lti.x.min = [-5; -5];
lti.x.max = [5; 5];

if soft
  lti.x.with('softMax');
  lti.x.softMax.maximalViolation = 100*[1;1];
  lti.x.softMax.penalty = QuadFunction(eye(2));
  lti.x.with('softMin');
  lti.x.softMin.maximalViolation = 100*[1;1];
  lti.x.softMin.penalty = QuadFunction(eye(2));
end

% Add constraints on predicted control inputs
lti.u.min = -1;
lti.u.max = 1;

% Use quadratic state penalty with identity weighting matrix
lti.x.penalty = QuadFunction(0.1*eye(2));
lti.u.penalty = QuadFunction(1);


% Define an MPC controller using "lti" as the prediction model
ctrl = MPCController(lti, 10);

% export to YALMIP 
dat = ctrl.toYALMIP;
dat.ctrl = ctrl;
dat.lti  = lti;

end

function R = epiPartition(P)
% Return a PolyUnion for the partition defined by the facets of the
% epigraph
%
% C.N. Jones, EPFL, 2013


P.incidenceMap; % Compute min reps
H = P.H; V = P.V;

H(H(:,3) > -1e-3,:) = [];   % Remove vertical facets
inc = abs(H*[V -ones(size(V,1),1)]')' < 1e-4;

% Convert from incidence map to Matlab face matrix
[I,J] = find(inc);
Faces = zeros(size(H,1),max(sum(inc,1)));
for i=1:size(H,1)
  v = I(J==i);
  R(i) = Polyhedron(V(v,1:end-1));
end

R = PolyUnion(R);

end

function J = optFunc(x, dat)
%
% Compute the function that we're trying to evaluate
%
% J(x) = Jstar(x) - x'*P*x
% 
% C.N. Jones, EPFL, 2013
% Revised by M. Herceg, ETH Zurich 2014
%
% requires MPT3, YALMIP

[J,isProblem] = dat.opt{x(:)};
% if isProblem, error('Could not solve optimization'); end
% if isProblem, fprintf('Could not solve optimization\n'); end
if iscell(J(1))
  J = J{1};
end

end

function h = plotEpi(P, col, alpha)
% plot epigraph 
%
% C.N. Jones, EPFL, 2013
%
% Assume that the polyhedron is an epigraph, and plot

P.incidenceMap;
H = P.H; V = P.V;
H(H(:,3) > -1e-3,:) = [];   % Remove vertical facets
inc = abs(H*[V -ones(size(V,1),1)]')' < 1e-4;

% Convert from incidence map to Matlab face matrix
[I,J] = find(inc);
Faces = zeros(size(H,1),max(sum(inc,1)));
for i=1:size(H,1)
  v = I(J==i);
  
  % Order vertices for plot
  ord = orderForPlot(V(v,:),H(i,1:end-1));
  v = v(ord);
  
  Faces(i,:) = [v' NaN*ones(1,size(Faces,2)-length(v))];
end

% Plot!
set(gcf,'renderer','opengl');
h = patch('Vertices', V, 'Faces', Faces, ...
  'FaceAlpha',           alpha,...
  'facevertexcdata', V(:,end),...
  'FaceColor',           col);

%colormap(vivid(col));
end