% YALMIP options
yalmip('clear')
yopts = sdpsettings('robust.polya',1);
% Model data
A1 = [0.85 0;0.25 0.65];
A2 = [0.85 0;-0.3 0.65];
B1 = [1;-1];
B2 = [1;1];
B{1} = B1;
B{2} = B2;
% System sizes
nx   = 2; % Number of states
nu   = 1; % Number of inputs
ndyn = 2; % Number of vertex systems
% State and input constraints
xmin = [-10;-10];
xmax = [  8;  8];
umin = -0.5;
umax =   1 ;
% MPC data
Q   = eye(nx);
R   = 0.01;
N   = 3;
xref = [0;0];
% States x(k), ..., x(k+N)
x = sdpvar(repmat(nx,1,N),repmat(1,1,N));
% Inputs u(k), ..., u(k+N) (last one not used)
u = sdpvar(repmat(nu*ndyn,1,N),repmat(1,1,N));
% Scheduling parameter
th = binvar(repmat(ndyn,1,N),repmat(1,1,N));
% Epigraph variable
sdpvar w;
for k = N:-1:1   % shifted: N-1:-1:0
% Parameter simplex
F = [uncertain(th{k}), sum(th{k}) == 1, 0 <= th{k} <= 1];
% Epigraph variable, MPT requires a bounded set so let us add artificial upper bound
F = [F, 0 <= w <= 10000];
% Uncertain predictions and control
uth = kron(th{k},eye(nu))'*u{k};    % u(th)
Ath = [A1 A2]*kron(th{k},eye(nx));  % A(th) = A1*th1 + A2*th2 + ...
Bth = [B1 B2]*kron(th{k},eye(nu));  % B(th)
xp  = Ath*x{k} + Bth*uth;
% Input constraints
F = [F, repmat(umin,ndyn,1) <= u{k} <=  repmat(umax,ndyn,1)];
% State constraints
F = [F, xmin  <= x{k} <= xmax];
% Insert step-specific code here

 if k == N
% Initial step
% |x| written as max(a[x;u]+b)
     a = [kron(eye(nx),[1 -1]') zeros(2*nx,nu)];
     b = zeros(2*nx,1);
% |u| written as max(c[x;u]+d)
     c = [zeros(2*nu,nx) kron(eye(nu),[1 -1]')];
     d = zeros(2*nu,1);
% |x|+|u| written as max(aa[x;u]+bb)
     aa = repmat(a,2*nu,1) + kron(c,ones(size(a,1),1));
     bb = repmat(b,2*nu,1) + kron(d,ones(size(a,1),1));
% Final state constraints
     F = [F, xmin  <= xp   <= xmax];
% Cost function
     F   = [F, aa*[Q*(xp-xref);R*uth]+bb + norm(Q*(x{k}-xref),inf) <= w];
     obj = w;

elseif k > 1
% Intermediate step
% Get the hyperplanes for cost-to-go
S = unique(([reshape([sol{k+1}{1}.Bi{:}]' ,nx,[])'  reshape([sol{k+1}{1}.Ci{:}]' ,[],nu)]),'rows');
a = [S(:,1:nx) zeros(size(S,1),nu)];
b = S(:,nx+1:end);
% Jp+|u| = max(a[x;u]+b)+|u|=max(a[x;u]+b)+max(c[x;u]+d)=max(aa[x;u]+bb)
aa = repmat(a,2*nu,1) + kron(c,ones(size(a,1),1));
bb = repmat(b,2*nu,1) + kron(d,ones(size(a,1),1));
% Constrain predicted state
[H,K] = double(sol{k+1}{1}.Pfinal);
F     = [F, H*xp <= K];
% Cost function
F   = [F, aa*[xp;R*uth]+bb <= w];
obj = norm(Q*(x{k}-xref),inf) + w;

else
% Final step
% Get the hyperplanes for cost-to-go
S = unique(([reshape([sol{k+1}{1}.Bi{:}]',nx,[])' reshape([sol{k+1}{1}.Ci{:}]',[],nu)]),'rows');
a = [S(:,1:nx) zeros(size(S,1),nu)];
b = S(:,nx+1:end);
% Jp+|u| = max(a[x;u]+b)+|u|=max(a[x;u]+b)+max(c[x;u]+d)=max(aa[x;u]+bb)
aa = repmat(a,2*nu,1) + kron(c,ones(size(a,1),1));
bb = repmat(b,2*nu,1) + kron(d,ones(size(a,1),1));
% State update equation
xp = x{k} + Bth*uth; % x{1} = z
% Constrain predicted state
[H,K] = double(sol{k+1}{1}.Pfinal);
F     = [F, H*xp <= K];
% Cost function
F   = [F, aa*[xp;R*uth]+bb <= w];
obj = w;
% add eps-penalty on vertex predictions
for v = 1:ndyn
xpv{v} = x{k} + B{v}*u{k}(v);
obj = obj + 0.001*norm(Q*xpv{v},inf);
end
end

% Determine robust counterpart
[F,obj] = robustify(F,obj,yopts,th{k});
% Solve multi-parametric problem
[sol{k},diagnost{k},Uz{k},J{k},Optimizer{k}] = solvemp(F,obj,yopts,x{k},u{k});
end
