import mosek.fusion.*;

k=10000;
sqrtk = sqrt(k);

nF = size(G,1)/4;
nV = size(G,2)/2;
nCV = size(indices,1)/2;

SelMat = zeros(2*nCV,2*nV);
for i=1:2*nCV
    SelMat(i,indices(i)) = 1;
end
SelMat = sqrtk*sparse(SelMat);

% spIF = sparse([eye(4*nF);zeros(2*nCV,4*nF)]);
% spIV = sparse([zeros(4*nF,2*nCV);eye(2*nCV)]);
[gei,gej,gev] = find(G);
MatG = Matrix.sparse(4*nF,2*nV,gei,gej,gev);
[sei,sej,sev] = find(SelMat);
MatS = Matrix.sparse(2*nCV,2*nV,sei,sej,sev);
% MatSF = Matrix.sparse(spIF);
% MatSV = Matrix.sparse(spIV);

mosekFusTimePrepare = tic;

M = Model('NIS');
M.setLogHandler(java.io.PrintWriter(java.lang.System.out));

v = M.variable(2*nV, Domain.unbounded());
t = M.variable('t', 1, Domain.unbounded());

F = Expr.mul(MatG, v);
dF = Expr.sub(F, exR);
dV = Expr.sub(v, exV);
dSV = Expr.mul(MatS,dV);

%z = [dF; sqrtk*SelMat*dV]; 
%z = Expr.add(Expr.mul(spIF,dF),Expr.mul(spIV,dSV));
z = Expr.vstack(dF, dSV);

% t>z'*z
M.constraint(Expr.vstack(t.asExpr(), z), Domain.inQCone());

% sdp constraints
ssei = [1 2 3 4];
ssev = [1 1 1 1];
for i=1:nF
    curR = DenseMatrix([exR(4*i-3) exR(4*i-2);exR(4*i-1) exR(4*i)]);
    %slice out curF
    %curF = [F(4*i-3) F(4*i-2);F(4*i-1) F(4*i)];
    ssej = [4*i-3 4*i-2 4*i-1 4*i];
    MatSlice = Matrix.sparse(4,4*nF,ssei,ssej,ssev);  
    slicedF = Expr.mul(MatSlice, F);
    curF = Expr.reshape(slicedF, NDSet(2,2));
    %curF.toString()
    
%     ssejT = [4*i-3 4*i-1 4*i-2 4*i];
%     MatSliceT = Matrix.sparse(4,4*nF,ssei,ssejT,ssev);  
%     slicedFT = Expr.mul(MatSlice, F);
%     curFT = Expr.reshape(slicedFT, NDSet(2,2));
%     curFT.toString()
   
%    M.constraint(Expr.add(Expr.mul(curR.transpose(),curF),Expr.mul(curFT,curR)),Domain.inPSDCone(2));

    M.constraint(Expr.mul(curR.transpose(),curF),Domain.inPSDCone(2));
end

% Objective: minimize t
M.objective(ObjectiveSense.Minimize, t);
% Solve the problem

sdpYMTimePrepare  = toc(mosekFusTimePrepare);
fprintf('sdp mosek fusion time prepare = %f\n',sdpYMTimePrepare);

mosekFusTime = tic;
M.solve();
sdpYMTime  = toc(mosekFusTime);

fprintf('sdp mosek fusion time = %f\n',sdpYMTime);

v_value = v.level();
M.dispose();


% convert x to vt coordinates
checkSolution(v_value, exR, exV, indices, G, k);

optiV = x2vt(v_value);
write2obj(optiV, FList, 'MosekM.obj');

visualizeDistort( VList, FList, optiV, exR );
axis equal;
