function [optiX optiLambda] = mosekSDPYalmip( G, exR, consVIndices, exV, k, number_of_it )
%function [optiX optiLambda] = mosekSDPYalmip( G, exR, consVIndices, exV, k )

%  G: 4m x 2n 
% exR: 4m x 1
% consVIndices: t x 1, t: number of constrained vertices   !!!1-based!!!
% exV: 2t x 1, position of constrained vertices
% k: the weight of positional constraAint when it is modeled as a soft constraint; set k=0 if the positional constraint is a hard constraint 
% optiX: 2n x 1
% number_of_it: maximum number of iterations

bNoninversion = 1;

m = size(G,1)/4;
n = size(G,2)/2;

mosekYMPtime = tic;

%Options = sdpsettings('verbose',1,'solver','mosek-sdp');
Options = sdpsettings('verbose',1,'solver','mosek-sdp', 'mosek.MSK_IPAR_INTPNT_MAX_ITERATIONS', number_of_it);
%Options.mosek

v = sdpvar(2*n,1); 
F = G*v;


sizeCN = size(consVIndices,1);
indices = zeros(2*sizeCN,1);
for i=1:sizeCN % could be faster
    indices(2*i-1) = 2*consVIndices(i)-1;
    indices(2*i  ) = 2*consVIndices(i);
end


if k>0 % soft positional constraint
    z = [F - exR; sqrt(k)*(v(indices) - exV)]; % z'*z
    t = sdpvar;
    
    %
    % Conic programming
    %
    objective = t;
    objectiveConstraints = cone(z,t);
    c = [objectiveConstraints];
else % hard positional constraint
    z = [F - exR];
    t = sdpvar;
    
    %
    % Conic programming
    %
    objective = t;
    objectiveConstraints = cone(z,t);
    c = [objectiveConstraints  v(indices) == exV];
end

%c = [c (v(indices) == exV(indices))];

% %
% % Quadratic programming
% %
% objective = z'*z;
% c = [];

if bNoninversion

for i=1:m
    curR = [exR(4*i-3) exR(4*i-2);exR(4*i-1) exR(4*i)];
    curF = [F(4*i-3) F(4*i-2);F(4*i-1) F(4*i)];
    c = [c, (curR'*curF+curF'*curR>=0)];
end

end

sdpYalTime  = toc(mosekYMPtime);
fprintf('sdp feed to Yalmip time = %f\n',sdpYalTime);


sdpYMPtime = tic;
res = solvesdp(c,objective,Options);
sdpYMTime  = toc(sdpYMPtime);
fprintf('sdp yalmip+mosek time = %f\n',sdpYMTime);

optiX=value(v);
optiLambda = cell(m, 1);
for i=1:m
    optiLambda{i} = dual(c(i+1));
end

end

