%%target plant
A=[2,-1;1,0];
B=[1;0];
C=[0.5,0.5];
D=0;
ts=0.1;
nx=size(A,1);
nu=size(B,2);
ny=size(C,1);
%tf([1 -1],[1 4 5],'InputDelay', 0.3)
%ans.OutputDelay=0.3
%ss(c2d(ans,0.1,'foh'))
%%
sys=ss(A,B,C,D);
sysd=c2d(sys,ts);
Ad=sysd.A;
Bd=sysd.B;
Aad1=[Ad,Bd;zeros(nu,nx),ones(nu,nu)];
Bad1=[zeros(nx,nu);zeros(nu,nu)];
Aad0=[Ad,zeros(nx,nu);zeros(nu,nx),zeros(nu,nu)];
Bad0=[Bd;ones(nu,nu)];
%%
% MPC data
Q   = eye(nx+nu);
R   = 0.01;
N   = 3;
xref = [0;0;0];
% State and input constraints
xmin = [-10;-10;-10];
xmax = [  8;  8; 8];
umin = -0.5;
umax =   1 ;
%%
% States x(k), ..., x(k+N)
x = sdpvar(repmat(nx+nu,1,N),repmat(1,1,N));
% Inputs u(k), ..., u(k+N) (last one not used)
u = sdpvar(repmat(nu,1,N),repmat(1,1,N));
% Scheduling parameter
th = binvar(repmat(2,1,N),repmat(1,1,N));
% Epigraph variable
sdpvar w;
yopts = sdpsettings('robust.polya',1);
%%
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];
    Ath = [Aad1 Aad0]*kron(th{k},eye(nx+nu));
    Bth = [Bad1 Bad0]*kron(th{k},eye(nu));
    xp  = Ath*x{k} + Bth*u{k};
    % Input constraints
    F = [F, umin <= u{k} <=  umax];

    % 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+nu),[1 -1]') zeros(2*(nx+nu),nu)];
        b = zeros(2*(nx+nu),1);
        % |u| written as max(c[x;u]+d)
        c = [zeros(2*nu,(nx+nu)) 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*u{k}]+bb + norm(Q*(x{k}-xref),inf) <= w];
        obj = w;
    elseif k > 1
     % Intermediate step
        S = unique( ([reshape([sol{k+1}{1}.Bi{:}]', nx+nu, [])'  reshape([sol{k+1}{1}.Ci{:}]', [], nu)]),'rows');
        a = [S(:,1:nx+nu) zeros(size(S,1),nu)];
        b = S(:,nx+1:end);
    else
     % Final step
    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