%Written by: Ramprasad Yelchuru, PhD student, NTNU, Trondheim.
%Date : 13th Nov 2009 
function [H,loss,HGy,nmes,ct]= cplex_miqp_full_final(Gy,Gdy,Juu,Jud,Wd,Wn,nm,bigM)
% Use the function cplexmiqp_miqp_full to solve a mixed-integer quadratic
% programming problem associated to full H case in Self Optimizing Control

%-----------------------------------------------------------------------
% Vectorization of matrices (see VectorizationProcedure.pdf) for details
C = inv(Juu)*Jud;
B = [(Gy*C - Gdy)*Wd Wn];
Juuhalf = sqrtm(Juu);
[ny,nu]= size(Gy);
nd = size(Gdy,2);
Jn = []; Fn = []; GnyT = [];
for i = 1:nu
    GnyT = daug(GnyT,Gy');
    Jn = [Jn;Juuhalf(:,i)];
    Fn = daug(Fn,B);
end
Jn = Jn;
Fxb = Fn*Fn';
F    = 2*daug(Fxb,zeros(ny,ny));
c    = zeros(size(F,1),1);
%-----------------------------------------------------------------------
M = [];
for i = 1:ny
    M1 = bigM*ones(nu,1);
    M = daug(M,M1);
end
P = []; Hmin = [];
for k = 1:nu
    Hmin = [Hmin;ny*(k-1)+1:ny*k];
end
zy = reshape(Hmin,1,nu*ny);

for zzy = 1:nu*ny
    i = zy(zzy);
    P = [P;zeros(1,i-1) ones(1,1) zeros(1,nu*ny-i)];
end
A1 = [-P -M;P -M];
GnewyT = [GnyT zeros(nu*nu,ny)];
LconB = [zeros(1,ny*nu) ones(1,ny)];

x_L  = [-Inf*ones(nu*ny,1);zeros(ny,1)];
x_U  = [Inf*ones(nu*ny,1);ones(ny,1)];


for xy = 1:ny*nu
    str(1,xy)='C';
end
for xy = 1:ny
    str(1,ny*nu+xy)='B';
end

%Inequality constraints to bound the elements in decision variables between
%-M to M
Aineq = [-P -M;P -M]; %LHS of inequality constraints

Aeq = [GnewyT;LconB]; %LHS of equality constraints

bineq  = [zeros(2*nu*ny,1)]; %RHS of inequality constraints

beq = [Jn;nm]; %RHS of equality constraints

%String to indicate the decision variables as
% continuous and binary variables
ctype = char(str);

%To set IBM ILOG CPLEX solver options
options = cplexoptimset;
options.MaxTime = 3600;
% options.TolFun = 1e-8;
options.Diagnostics = 'off';
options.Display = '';

%Using cplexmiqp function in IBM ILOG CPLEX to solve MIQP
[x, fval, exitflag, output] = cplexmiqp(F, c, Aineq, bineq, Aeq, beq,...
    [ ],[ ],[ ],x_L,x_U,ctype,[],options);
xn = x;
ct = output.time; %Computational time taken by cplex to solve MIQP
ny = size(Gy,1);
nd = size(Gdy,2);
H = reshape(xn(1:ny*nu),ny,nu)'; %Obtaining H matrix from decision vector xn
C = inv(Juu)*Jud;
F = [(Gy*C - Gdy)*Wd Wn];
loss1 = norm(sqrtm(Juu)*inv(H*Gy)*(H*F),'fro')^2/(6*(nm+nd));
loss = norm(sqrtm(Juu)*inv(H*Gy)*(H*F),'fro')^2/2; % Loss calculation using optimal H
HGy = H*Gy;
no = (1:ny)';
nsy = xn(nu*ny+1:end).*no;
nsy(find(abs(nsy) < 1e-3)) = 0;
nmes = nonzeros(nsy); %To pass the optimal measurements

