function [objval d0 Ur Uf xr xf Nsim Mbk DOF]= deltamoveblocking_onestep(Nsim,nfull,nred,N,dibtype)
% move blocking for the Column A example
%
%	M is a delta blocking matrix, such that M*U == 0
% 	N is the prediction horizon
%
%	one step to solve the problem

% clear all;
% clc;
%% 1: Generate the MPC controllers
nfull = 16; nred = 16; 
nu = 2; nd = 2;
% N     = 8; 
umax  = 1; umin = -1;	% Same umin and umax for all the inputs. Not
                        % correct!
% ibtype = [2];   %input blocking type
[Mbk DOF] = make_delta_blocking(nu,N,dibtype);
[nM,dummy] = size(Mbk);	% number of move-blocking constraints
                        % we implement this on the reduced-order controller

[Gfull] = model_baltrunc(nfull);
[Gred Tl Tr] = baltrunc(Gfull,nred);
% make a discrete-time approximation
Ts	= 1;
Gfull = c2d(Gfull,Ts);
Gred = c2d(Gred,Ts);

%% make the MPC controllers
[Af Bf Cf Bd] = deal(Gfull.a,Gfull.b(:,1:nu),Gfull.c,Gfull.b(:,nu+1:end));
[Ar Br Cr Bdr] = deal(Gred.a,Gred.b(:,1:nu),Gred.c,Gred.b(:,nu+1:end));
% we ignore the D-term!

[HHf Sf] = create_CHS(Af,Bf,Cf,N);
[HHr Sr] = create_CHS(Ar,Br,Cr,N);

% define matrices for MPC
% 1: full controller
Hf = Sf'*Sf + 1*eye(nu*N);
Ff = HHf'*Sf;
Wf = [ones(nu*N,1)*umax; ones(nu*N,1)*(-umin)];
Gf = [eye(nu*N);-eye(nu*N)];
% 2: reduced controller
Hr = Sr'*Sr + 1*eye(nu*N);
Fr = HHr'*Sr;
Wr = [ones(nu*N,1)*umax; ones(nu*N,1)*(-umin)];
Gr = [eye(nu*N);-eye(nu*N)];

save controllers

% Simulation horizon
% Nsim = 15;

% Define variables 
for i = 1:Nsim
    % variables for full-order contoller
    xf{i} 	= sdpvar(nfull,1);	% initial state, full order controller
    Uf{i} 	= sdpvar(nu*N,1);	% inputs
    lambdaf{i} 	= sdpvar(length(Wf),1);
    % variables for reduced-order controller
    % variables for reduced-order controller
    xr{i} 	= sdpvar(nfull,1);	% initial state, reduced order controller
    Ur{i} 	= sdpvar(nu*N,1);	% inputs
    my{i}	= sdpvar(nM,1);		% move blocking constraints
    lambdar{i} 	= sdpvar(length(Wr),1);
end

% KKT conditions and objective value for full-order problem
i = 1;
% fulll-order controller
slackf{i} 	= Wf - Gf*Uf{i};

KKTf 	= [Hf*Uf{i} + Ff'*xf{i} + Gf'*lambdaf{i} == 0,
	    Gf*Uf{i} <= Wf,
	    lambdaf{i} >= 0];

sf{i} 	= binvar(length(slackf{i}),1);
M  	= 100;	% 'big-M'
KKTf 	= [KKTf, -M*sf{i} <= lambdaf{i} <= M*sf{i}];
KKTf 	= [KKTf, -M*(1-sf{i}) <= slackf{i} <= M*(1-sf{i})];
KKTf 	= [KKTf, -M <= lambdaf{i} <= M];


% reduced-order controller with move blocking
slackr{i} 	= Wr - Gr*Ur{i};

KKTr 	= [Hr*Ur{i} + Fr'*Tl*xr{i} + Gr'*lambdar{i} + Mbk'*my{i} == 0,
	    Gr*Ur{i} <= Wr,
	    Mbk*Ur{i} == 0,
	    lambdar{i} >= 0];

sr{i} 	= binvar(length(slackr{i}),1);
% M  	= 100;	% 'big-M'
KKTr 	= [KKTr, -M*sr{i} <= lambdar{i} <= M*sr{i}];
KKTr 	= [KKTr, -M*(1-sr{i}) <= slackr{i} <= M*(1-sr{i})];
KKTr 	= [KKTr, -M <= lambdar{i} <= M];

% calculating obj when Nsim = 1
if Nsim == 1
obj = norm(Cf*xf{i}-Cf*xr{i},'inf')
% obj = obj*10^10;                    %scaling
end


FFf 	= [];	% constraint set connecting the KKT condtions
FFr	= [];	% same for the reduced-order controller
for i = 2:Nsim
    % 1. set initial conditions for next MPC problem
    FFf  = [FFf, xf{i} == Af*xf{i-1} + Bf*Uf{i-1}(1:nu,1)];
    FFr  = [FFr, xr{i} == Af*xr{i-1} + Bf*Ur{i-1}(1:nu,1)];
    % fulll-order controller
    slackf{i} 	= Wf - Gf*Uf{i};

    KKTf 	= [KKTf,Hf*Uf{i} + Ff'*xf{i} + Gf'*lambdaf{i} == 0,
		Gf*Uf{i} <= Wf,
		lambdaf{i} >= 0];

    sf{i} 	= binvar(length(slackf{i}),1);
    KKTf 	= [KKTf, -M*sf{i} <= lambdaf{i} <= M*sf{i}];
    KKTf 	= [KKTf, -M*(1-sf{i}) <= slackf{i} <= M*(1-sf{i})];
    KKTf 	= [KKTf, -M <= lambdaf{i} <= M];


    % reduced-order controller with move blocking
    slackr{i} 	= Wr - Gr*Ur{i};

    KKTr 	= [KKTr,Hr*Ur{i} + ...
		    Fr'*Tl*xr{i} + Gr'*lambdar{i} + Mbk'*my{i} == 0,
		Gr*Ur{i} <= Wr,
		Mbk*Ur{i} == 0,
		lambdar{i} >= 0];

    sr{i} 	= binvar(length(slackr{i}),1);
    % M  	= 100;	% 'big-M'
    KKTr 	= [KKTr, -M*sr{i} <= lambdar{i} <= M*sr{i}];
    KKTr 	= [KKTr, -M*(1-sr{i}) <= slackr{i} <= M*(1-sr{i})];
    KKTr 	= [KKTr, -M <= lambdar{i} <= M];
end

% add initial disturbances as "search space"
d0 = sdpvar(nd,1);
Finit = [xf{1} == Bd*d0, xr{1} == Bd*d0, -1 <= d0 <= 1];

% calculating obj if Nsim > 1
if Nsim > 1
obj = norm(Cf*xf{i}-Cf*xr{i},inf)
% obj = obj*10^10;                    %scaling
end

% total set of constraints
% F = [FFf,FFr,KKTf,KKTr,Finit,bound];
F = [Finit,KKTr,KKTf,FFf,FFr];
% options=sdpsettings('verbose',0);
diag = solvesdp(F,-obj)%,sdpsettings('solver','glpk'))

objval = double(obj);

ur = double(Ur{1}(1:2));
uf = double(Uf{1}(1:2));


save moveblocking_results.mat

