function triangulation_example
% triangulation example 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,'approxTri'];
figure_name1 = ['.',filesep,'figures',filesep,'optimalCost'];
figure_name2 = ['.',filesep,'figures',filesep,'tri_approxU'];
figure_name3 = ['.',filesep,'figures',filesep,'tri_errU'];

%% core code
% run the approximation or load the computed data?
       
fprintf('---> Setting up problem\n');

% weight factor
epsilon = 2;

% load data
dat = defineEx;

% Compute vertices 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));

% extract and adjust the vertices
V = p.V;
V = [V;zeros(1,size(V,2))];
V = (1-1e-5)*V;

% extract the parametrized cost function
dat.opt = optimizer(dat.constraints, dat.objective, [], dat.variables.x(:,1), dat.objective);

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

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

usrd = input('If you want to run the approximation, type number greater than zero. Otherwise the stored data will be loaded.\n');
if usrd>0
   
    % Compute approximation
    fprintf('---> Computing approximation\n');
    
    h = [];
    for i = 1:size(V,1)
        vert(i).v = V(i,:)';
        vert(i).J = NaN;
        
        vert(i).inTerminal = termSet.contains(vert(i).v);
    end
    vert0 = length(vert);
    tri = [];
    
    start = cputime;
    delta = -inf;
    while any(delta < -1e-3)
        
        % Compute optimal value at each vertex
        for i = 1:length(vert)
            if ~isnan(vert(i).J), continue; end
            vert(i).J = theFunc(vert(i).v);
        end
        
        % Triangulate
        V = [vert.v]';
        TRI = delaunay(V(:,1),V(:,2));
        prevTri = tri; tri = [];
        for i = 1:size(TRI,1)
            % Check if we've already got this triangle
            found = false;
            for j = 1:length(prevTri)
                if prevTri(j).ind == TRI(i,:),
                    found=true;
                    if i == 1
                        tri = prevTri(j);
                    else
                        tri(i) = prevTri(j);
                    end
                    break;
                end
            end
            if found, continue; end
            
            ind = TRI(i,:);
            tri(i).ind = ind;
            tri(i).v = [vert(ind).v]';
            tri(i).J = [vert(ind).J]';
            tri(i).R = Polyhedron(tri(i).v);
            
            T = tri(end).J'*inv([tri(i).v';ones(1,size(tri(i).v,1))]);
            f = AffFunction(T(1:end-1),T(end));
            tri(i).R.addFunction(f,'Interp');
            tri(i).T = T;
            tri(i).R.minVRep;
            
            tri(i).delta = NaN; % Untested
            tri(i).xMax = [];
            tri(i).inTerm = NaN;
        end
        
        % Drop the triangles that are not full-dim
        rem = [];
        for i = 1:length(tri)
            if size(tri(i).R.V,1) < 3
                rem(end+1) = i;
            end
        end
        %   if length(rem) > 0, fprintf('Dropping %i vertical regions\n', length(rem)); end
        tri(rem) = [];
        
        % Test each region
        x = dat.variables.x(:,1);
        Q = dat.ctrl.model.x.penalty.H;
        lam = sdpvar(length(vert),1);
        ulam = sdpvar(length(vert),1);
        cost = dat.objective + epsilon*x'*Q*x - [vert.J]*lam;
        con  = [dat.constraints;
            0 <= lam <= ulam;
            sum(lam) == 1;
            x == [vert.v]*lam ];
        
        tt = optimizer(con, cost, [], ulam, [cost;x]);
        
        % in case there is solver problem, trying to solve using different solver
        sttgs=sdpsettings;
        sttgs.pureexport = true;
        [interfacedata,recoverdata,solver,diagnostic,F,Fremoved,ForiginalQuadratics] = compileinterfacedata(con,[],[],cost,sttgs,0,1);
        interfacedata.parametric_variables = [];
        for jj = 1:length(ulam)
            interfacedata.parametric_variables = [interfacedata.parametric_variables(:);find(ismember(recoverdata.used_variables,getvariables(ulam(jj))))];
        end
        interfacedata.requested_variables = [];
        uvar = [cost;x];
        for jj = 1:length(uvar)
            interfacedata.requested_variables = [interfacedata.requested_variables(:);find(ismember(recoverdata.used_variables,getvariables(uvar(jj))))];
        end
        mat = yalmip2mpt(interfacedata);
        setmpt=mptopt;
        mat.lb(mat.lb==Inf) = setmpt.infbound;
        mat.lb(mat.lb==-Inf) = -setmpt.infbound;
        mat.ub(mat.ub==Inf) = setmpt.infbound;
        mat.ub(mat.ub==-Inf) = -setmpt.infbound;
        % get parametric version
        problem = Opt(mat);
        
        
        tic
        skips = 0;
        for i = 1:length(tri)
            %     fprintf('%i of %i\n', i, length(tri));
            if isnan(tri(i).delta)
                
                % Check if all the vertices are in the terminal region
                if all([vert(tri(i).ind).inTerminal])
                    tri(i).delta = 0;
                    tri(i).xMax = [];
                    tri(i).inTerm = true;
                else
                    
                    t = zeros(length(vert),1);
                    t(tri(i).ind) = 1;
                    
                    [q,isProblem] = tt{t};
                    if isProblem,
                        S.H = problem.H;
                        S.f = problem.f + problem.pF*t;
                        S.A = problem.A;
                        S.b = problem.b + problem.pB*t;
                        S.lb = problem.lb;
                        S.ub = problem.ub;
                        % solve the problem in mpt
                        res = mpt_solve(S);
                        if res.exitflag~=setmpt.OK
                            error('The optimization solver returned infeasible result.');
                        end
                        q = res.xopt;
                        
                    end
                    
                    tri(i).delta = q(1);
                    tri(i).xMax  = q(2:end);
                end
            else
                skips = skips + 1;
                %       fprintf('Skipping bound test\n');
            end
        end
        fprintf('Computed error bound: %8.2f (%3i of %3i computed in %.1f seconds)\n', min([tri.delta]), length(tri)-skips, length(tri), toc);
        
        % plot intermediate results
        if mod(length(vert)-vert0,10) == 0
            fprintf('Creating plot...\n');
            close all
            
            % Plot cost function            
            eval(['fig',num2str(length(vert)),'a = figure;']);
            grid on
            pRtmp = PolyUnion([tri.R]);
            pRtmp.fplot('color',[0.9 0.9 0.9],'linewidth',line_width);
            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 0 30]);
            set(gca,'XTick',[-5 0 5]);
            set(gca,'YTick',[-5 0 5]);
            set(gca,'ZTick',0:10:30);

            
            % save
            eval(['gga = fig',num2str(length(vert)),'a;']);
            saveas(gga, [figure_name,'_',num2str(length(vert)),'_matlab'], 'fig');
            
            % print
            laprint(gga, [figure_name,'_',num2str(length(vert)),'_tex'],'scalefonts','off');

            
            % Plot triangulation
            eval(['fig',num2str(length(vert)),'b = figure;']);
            grid on
            pRtmp.plot('linewidth',line_width,'colormap','lightgray');
            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([-5 5 -5 5]);

            % save
            eval(['ggb = fig',num2str(length(vert)),'b;']);
            saveas(ggb, [figure_name,'_',num2str(length(vert)),'_triangulation_matlab'], 'fig');
            
            % print
            laprint(ggb, [figure_name,'_',num2str(length(vert)),'_triangulation_tex'],'scalefonts','off');

            
            % Plot error
            ptmp = p.copy;
            ptmp.addFunction(Function(@(x)pwaFuncEval(pRtmp,vert,tri,Q*epsilon,dat,x(:),'Interp')), 'err_upper');

            eval(['fig',num2str(length(vert)),'c = figure;']);
            grid on
            ptmp.fplot('err_upper','grid',40,'linewidth',line_width,'color',[0.9 0.9 0.9],'showgrid',true);
            set(gca,'LineWidth',axeswidth)
            set(gca,'FontSize', tick_font_size);
            
            % Plot horizontal plane
            hold on
            ptmp.plot('color',0.7*[1 1 1],'linewidth',line_width);
            
            % Add worst-case points
            if min([tri.delta]) <= -0.1
                JJ = find([tri.delta] <= min([tri.delta])+1e-3);
                xx = [tri(JJ).xMax];
                for i = 1:size(xx,2)
                    h=plot3(xx(1,i),xx(2,i),-tri(JJ(i)).delta,'or');
                    set(h,'markerFaceColor','k','markerSize',10,'markerEdgeColor','k');
                end
            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(length(vert)),'c;']);
            saveas(ggc, [figure_name,'_',num2str(length(vert)),'_err_matlab'], 'fig');
            
            % print
            laprint(ggc, [figure_name,'_',num2str(length(vert)),'_err_tex'],'scalefonts','off');

            disp('Hit any key to continue');
            pause
        end
        
        
        if min([tri.delta]) > -0.1, break; end
        
        [d,I] = sort([tri.delta]);
        toAdd = 3;
        for i = 1:length(I)
            if toAdd < 0, break; end
            
            j = I(i);
            x = tri(j).xMax;
            err = [vert.v]'-repmat(x',length(vert),1); err = sum(err.*err,2);
            if any(err < 1e-3), continue; end % Already got this point
            
            toAdd = toAdd - 1;
            vert(end+1).v = x;
            vert(end).J = NaN;
            vert(end).inTerminal = termSet.contains(x);
            break
        end
    end
    
else
    % load all data
    load triangulation_data
end

% Plot cost function
eval(['fig',num2str(length(vert)),'a = figure;']);
grid on
pRtmp = PolyUnion([tri.R]);
pRtmp.fplot('color',[0.9 0.9 0.9],'linewidth',line_width);
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 0 30]);
set(gca,'XTick',[-5 0 5]);
set(gca,'YTick',[-5 0 5]);
set(gca,'ZTick',0:10:30);


% save
eval(['gga = fig',num2str(length(vert)),'a;']);
saveas(gga, [figure_name,'_',num2str(length(vert)),'_matlab'], 'fig');

% print
laprint(gga, [figure_name,'_',num2str(length(vert)),'_tex'],'scalefonts','off');


% Plot triangulation
eval(['fig',num2str(length(vert)),'b = figure;']);
grid on
pRtmp.plot('linewidth',line_width,'colormap','lightgray');
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([-5 5 -5 5]);

% save
eval(['ggb = fig',num2str(length(vert)),'b;']);
saveas(ggb, [figure_name,'_',num2str(length(vert)),'_triangulation_matlab'], 'fig');

% print
laprint(ggb, [figure_name,'_',num2str(length(vert)),'_triangulation_tex'],'scalefonts','off');


% Plot error
ptmp = p.copy;
ptmp.addFunction(Function(@(x)pwaFuncEval(pRtmp,vert,tri,Q*epsilon,dat,x(:),'Interp')), 'err_upper');

eval(['fig',num2str(length(vert)),'c = figure;']);
grid on
ptmp.fplot('err_upper','grid',40,'linewidth',line_width,'color',[0.9 0.9 0.9],'showgrid',true);
set(gca,'LineWidth',axeswidth)
set(gca,'FontSize', tick_font_size);

% Plot horizontal plane
hold on
ptmp.plot('color',0.7*[1 1 1],'linewidth',line_width);

% Add worst-case points
if min([tri.delta]) <= -0.1
    JJ = find([tri.delta] <= min([tri.delta])+1e-3);
    xx = [tri(JJ).xMax];
    for i = 1:size(xx,2)
        h=plot3(xx(1,i),xx(2,i),-tri(JJ(i)).delta,'or');
        set(h,'markerFaceColor','k','markerSize',10,'markerEdgeColor','k');
    end
end

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);


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(['ggc = fig',num2str(length(vert)),'c;']);
saveas(ggc, [figure_name,'_',num2str(length(vert)),'_err_matlab'], 'fig');

% print
laprint(ggc, [figure_name,'_',num2str(length(vert)),'_err_tex'],'scalefonts','off');

disp('Hit any key to continue');
pause


% add cost functions 
Q = dat.ctrl.model.x.penalty.H;
p.addFunction(Function(@(x)optFunc(x, dat)),'cost');
p.addFunction(Function(@(x)optFunc(x, dat)+epsilon*x(:)'*Q*x(:)),'cost_gam');

% add error bounds
for i = 1:length(tri)
  tri(i).R.addFunction(Function(@(x)tri(i).T*[x(:);1] - optFunc(x, dat)+epsilon*x(:)'*Q*x(:)),'err_upper');
  tri(i).R.addFunction(Function(@(x)tri(i).T*[x(:);1] - optFunc(x, dat)),'err_lower');
end

% Compute the input interpolation
for i = 1:length(tri)
  for j = 1:size(tri(i).v,1)
    x = tri(i).v(j,:);
    tri(i).u(j,1) = dat.ctrl.evaluate(x');
  end
end

% add the approximated control law
for i = 1:length(tri)
  t=tri(i).u'*inv([tri(i).v';ones(1,3)]);
  tri(i).R.addFunction(AffFunction(t(1:2),t(end)),'approxU');
end

% create polyunion object for plotting
pR = PolyUnion([tri.R]);

% add the approximated control law
p.addFunction(@(x)pR.feval(0.99*x(:),'approxU'),'approxU');

% add the input error
p.addFunction(@(x) pR.feval(0.99*x(:), 'approxU') - dat.ctrl.evaluate(x), 'errU');

% plot the cost function
disp('hit any key to plot the cost function');
pause

fig1 = figure;
hold on
grid on
p.fplot('cost','wire',1,'grid',60,'color',[0.9 0.9 0.9],'showgrid',true,'linewidth',line_width); 
p.fplot('cost_gam','wire',1,'grid',60,'color',[0.7 0.7 0.7],'showgrid',true,'linewidth',line_width);

set(gca,'LineWidth',axeswidth)
set(gca,'FontSize', tick_font_size);
axis([-6 6 -6 6 0 40]);
set(gca,'XTick',[-5 0 5]);
set(gca,'YTick',[-5 0 5]);
set(gca,'ZTick',0:10:40);


hx1 = xlabel('$x_1$');
set(hx1, 'FontSize', label_font_size);
hy1 = ylabel('$x_2$');
set(hy1, 'FontSize', label_font_size);
%hz1 = zlabel('$J^\star(x)$');
%set(hz1, 'FontSize', label_font_size);



% plot the approximated control law
disp('hit any key to plot the approximated control law');
pause

fig2 = figure;
hold on
grid on
p.fplot('approxU','grid',30,'color',[0.9 0.9 0.9], 'showgrid',true, 'linewidth', line_width);
view(-121,20)
set(gca,'LineWidth',axeswidth)
set(gca,'FontSize', tick_font_size);
pos = get(gca,'Position');
set(gca,'Position',[0.17 0.17 pos(3:4)]);
axis([-6 6 -6 6 -1 1]);
set(gca,'XTick',[-5 0 5]);
set(gca,'YTick',[-5 0 5]);
set(gca,'ZTick',-1:0.5:1);


hx2 = xlabel('$x_1$');
set(hx2, 'FontSize', label_font_size);
hy2 = ylabel('$x_2$');
set(hy2, 'FontSize', label_font_size);
hz2 = zlabel('$\tilde{u}(x)$');
set(hz2, 'FontSize', label_font_size);
posz2 = get(hz2,'Position');
set(hz2,'Position',[posz2(1)+5 posz2(2:3)]);



% plot the input error
disp('hit any key to plot the input error');
pause

fig3 = figure;
hold on
grid on
p.fplot('errU','grid',40,'color',[0.9 0.9 0.9],'showgrid',true,'linewidth',line_width);
view(-121,20)
v=axis;
axis([v(1:4) -1 1])
set(gca,'LineWidth',axeswidth)
set(gca,'FontSize', tick_font_size);
pos = get(gca,'Position');
set(gca,'Position',[0.17 0.17 pos(3:4)]);
axis([-6 6 -6 6 -1 1]);
set(gca,'XTick',[-5 0 5]);
set(gca,'YTick',[-5 0 5]);
set(gca,'ZTick',-1:0.5:1);


hx3 = xlabel('$x_1$');
set(hx3, 'FontSize', label_font_size);
hy3 = ylabel('$x_2$');
set(hy3, 'FontSize', label_font_size);
hz3 = zlabel('$\tilde{u}(x)-u^{*}(x)$');
set(hz3, 'FontSize', label_font_size);
posz3 = get(hz3,'Position');
set(hz3,'Position',[posz3(1)+1.8 posz3(2:3)]);


%% save book figures
disp('Hit any key to save the figures.');
pause
% save
saveas(fig1, [figure_name1,'_matlab'], 'fig');

% print 
laprint(fig1, [figure_name1,'_tex'],'scalefonts','off');

% save
saveas(fig2, [figure_name2,'_matlab'], 'fig');

% print 
laprint(fig2, [figure_name2,'_tex'],'scalefonts','off','figcopy','off');

% save
saveas(fig3, [figure_name3,'_matlab'], 'fig');

% print 
laprint(fig3, [figure_name3,'_tex'],'scalefonts','off','figcopy','off');

end


function v = pwaFuncEval(p, vert, tri, Q, dat, x, name)
%
% subfunction for intermediate plot

[isin, inwhich, closest] = p.contains(x, true);
if ~isin
  xx = p.Set(closest).project(x);
  x = xx.x;
  ii = closest;
else
  if iscell(inwhich)
    inwhich = inwhich{1};
  end
  ii = inwhich;
end

% Check if we're in a region within the terminal set
if all([vert(tri(ii).ind).inTerminal])
  v = 0;
else
  v = p.Set(ii).feval(x,name) - optFunc(x,dat);
end
v = v - x(:)'*Q*x(:);

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 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

