function [objval ur uf xr xf Nsim d0] = yalmip_useCHS_trajectory(nfull,nred,Nsim)
% nfull = 16;
% nred = 3;
% function that does 'everything'
%
% In this file we try to use the "create_CHS" to set up the MPC
% controller

%% 1: Generate the MPC controllers
% nfull = 16; nred = 4; 
nu = 2; nd = 2;
umax  = 1; umin = -1;
N     = 8; 
[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:2),Gfull.c,Gfull.b(:,3:4));
[Ar Br Cr Bdr] = deal(Gred.a,Gred.b(:,1:2),Gred.c,Gred.b(:,3:4));
% 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)];

% Simulation horizon
% Nsim = 5;


% Define variables 
for i = 1:Nsim
    % variables for full-order contoller
    xf{i} 	= sdpvar(nfull,1);	% initial state, full order controller
    Uf{i} 	= sdpvar(2*N,1);	% inputs
    lambdaf{i} 	= sdpvar(length(Wf),1);
    % variables for reduced-order controller
    xr{i} 	= sdpvar(nfull,1);	% initial state, full order controller
    Ur{i} 	= sdpvar(2*N,1);	% inputs
    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

slackr{i} 	= Wr - Gr*Ur{i};

KKTr 	= [Hr*Ur{i} + Fr'*Tl*xr{i} + Gr'*lambdar{i} == 0,
	    Gr*Ur{i} <= Wr,
	    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];

% add some bounds
bound   = [-10 <= xf{i} <= 10];
bound   = [bound, -10 <= xr{i} <= 10];

% objective function

obj = norm(Cf*xf{i}-Cf*xr{i},'inf') %calc. obj. in Nsim = 1


% trajectory
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:2,1)];
    FFr  = [FFr, xr{i} == Af*xr{i-1} + Bf*Ur{i-1}(1:2,1)];
    % 2. solve this MPC problem (KKT conditions)
    % 2a. full 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];
    bound   	= [bound, -10 <= xf{i} <= 10];
    % 2b. reduced controller    
    slackr{i} 	= Wr - Gr*Ur{i};

    KKTr 	= [KKTr,Hr*Ur{i} + Fr'*Tl*xr{i} + Gr'*lambdar{i} == 0,
		Gr*Ur{i} <= Wr,
		lambdar{i} >= 0];
    sr{i} 	= binvar(length(slackr{i}),1);
    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];
    bound   	= [bound, -10 <= xr{i} <= 10];
    
    % update objective in half of simulation
%     if i == round(Nsim/2)
%     obj = obj + norm(Cf*xf{i}-Cf*xr{i},'inf') %sim 1
%     end
    %update objecitve function
    obj = obj + norm(Cf*xf{i}-Cf*xr{i},'inf')

end

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

% total set of constraints
F = [FFf,FFr,KKTf,KKTr,Finit,bound];

diag = solvesdp(F,-obj)

objval = double(obj);

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

save trajectory_results.mat
