function [c,objective,v] = mosekSOCPYalmipPreparation(dim,  G, exR, consVIndices, exV, k )
%function [optiX optiLambda] = mosekSDPYalmip( G, exR, consVIndices, exV, k )

%  G: dim*dim*m x dim*n 
% exR: dim*dim*m x 1
% consVIndices: t x 1, t: number of constrained vertices   !!!1-based!!!
% exV: dim*t 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: dim*n x 1
% number_of_it: maximum number of iterations

bNoninversion = 1;

dim_square = dim*dim;

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

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


sizeCN = size(consVIndices,1);
indices = zeros(dim*sizeCN,1);
if (dim == 2)
    for i=1:sizeCN % could be faster
        indices(2*i-1) = 2*consVIndices(i)-1;
        indices(2*i  ) = 2*consVIndices(i);
    end
elseif (dim == 3)
    %not supported
    return;
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


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

if bNoninversion

if dim == 2
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)];
    RtFSym = curR'*curF+curF'*curR;
    a = (RtFSym(1,1)+RtFSym(2,2))/2;
    b = (RtFSym(1,1)-RtFSym(2,2))/2;
    d = RtFSym(1,2);
    c = [c, cone([b,d],a)];
end
elseif dim == 3
    return;
end

end

end

