%This routine is from ALEC JACOBSON's SGP paper:
% http://igl.ethz.ch/projects/monotonic/

%It has been MODIFIED to solve semidefinite problems with MOSEK 7.

function Z = conic_semidefinite_multi(varargin)
  % CONIC optimization wrapper for mosek, uses same prototype for conic and
  % quadprog 
  % minimize     1/2||Fx||^2 + c'x
  % subject to:  Aeq x = beq
  %              Aleq x >=bleq
  %              x >=lx
  %              x <=ux
  %              x(b) = bc
  % 
  % Z = conic(F,c,Aeq,beq,Aleq,bleq,lx,ux,b,bc,'ParameterName',ParameterValue)
  %
  % Inputs:
  %    F  #F by #x quadratic, least-squares matrix
  %    c  #x by 1 linear term
  %    Aeq  neq by #x  linear equality matrix
  %    beq  neq by 1  linear equality rhs
  %    Aleq  nleq by #x  linear inequality matrix
  %    bleq  neq by 1  linear inequality rhs
  %    lx  #x by 1 lower bounds
  %    ux  #x by 1 upper bounds
  %    b  #b list of boundary (fixed) indices in x
  %    bc  #b list of boundary (fixed) values for b
  %  Optional:
  %    'Quiet'  followed by 'echo(0)' or '' to be quite or loud
  %    'OptType' followed by 'conic' or 'quad'
  %    'Param' followed by mosek param struct
  % 
%tic;
  FF = varargin{1};
  c = varargin{2};
  Aeq = varargin{3};
  beq = varargin{4};
  Aleq = varargin{5};
  bleq = varargin{6};
  lx = varargin{7};
  ux = varargin{8};
  b = varargin{9};
  bc = varargin{10};
  GPrimeStack = varargin{11}; %Matrix G with some zeros added
  numTris = varargin{12}; %number of triangles in our mesh.
  rStarStack = varargin{13};
  max_iteration = varargin{14}; % maximum number of iterations

%   param = default_mosek_param();
  quiet = 'echo(0)';
  opt_type = 'conic';

  %[r, res] = mosekopt('symbcon');
  
%   ii = 11;
%   while(ii <= nargin)
%     switch varargin{ii}
%     case 'Quiet'
%       ii = ii + 1;
%       assert(ii<=nargin);
%       quiet = varargin{ii};
%     case 'Param'
%       ii = ii + 1;
%       assert(ii<=nargin);
%       param = varargin{ii};
%     case 'OptType'
%       ii = ii + 1;
%       assert(ii<=nargin);
%       opt_type = varargin{ii};
%     otherwise
%       error(['Unsupported parameter: ' varargin{ii}]);
%     end
%     ii = ii + 1;
%   end
  
   % tic;
  [r, res] = mosekopt('symbcon');
   % toc;
    
  % number of primary variables
  n = size(FF,2);
  nt = size(FF,1);
  % identity the size of rows in FF
  I = speye(nt,nt);
  % number of equality constraints
  neq = size(Aeq,1);
  % number of less than or equals constraints
  nleq = size(Aleq,1);

  % avoid warnings when Aeq and Aleq are empty
  if isempty(Aeq)
    Aeq = sparse(0,n);
  end
  if isempty(Aleq)
    Aleq = sparse(0,n);
  end

%   switch opt_type
%   case 'conic'
    % set up mosek problem
    prob = [];
    prob.bardim = 2*ones(1,numTris);
    %we are not specifying barc,  because it's all zero. This is because
    %we are not including any semidefinite variables in our objective.
    prob.c = [c;zeros(nt,1);1;0];
    
    prob.blc = [zeros(nt,1);beq;-Inf(nleq,1)];
    prob.buc = [zeros(nt,1);beq;bleq];
    prob.blx = [lx;-Inf(nt,1);0;1];
    prob.bux = [ux;Inf(nt,1);Inf;1];
    
    prob.a = [ ...
      FF    -I             zeros(nt,2); ...
      Aeq  sparse(neq,nt)  zeros(neq,2)      ; ...
      Aleq sparse(nleq,nt) zeros(nleq,2)     ; ...
      ];
  
    old_a_rows = size(prob.a,1);
    %time to add some more things onto Alec's formulation to support our
    %semidefinite constraints
%     
    for i=1:numTris
        GstarIdx = 4*(i-1) + 1;
        GPrime = GPrimeStack(GstarIdx:GstarIdx+3,:);
        rVec = rStarStack(GstarIdx:GstarIdx+3,:);
        
        r11 = rVec(1);
        r12 = rVec(3);
        r21 = rVec(2);
        r22 = rVec(4);
        
        s1trans = 2*(r11*GPrime(1,:) + r21*GPrime(2,:));
        s2trans = r12*GPrime(1,:) + r22*GPrime(2,:) + r11*GPrime(3,:) + r21*GPrime(4,:); 
        s3trans = 2*(r12*GPrime(3,:) + r22*GPrime(4,:));
        prob.a = [prob.a; -s1trans; -2*s2trans; -s3trans];
    end
    
    prob.a = sparse(prob.a);
    
    prob.blc = [prob.blc; zeros(3*numTris,1)];
    prob.buc = [prob.buc; zeros(3*numTris,1)];

    prob.bara.subi = (old_a_rows + 1):(old_a_rows + 3*numTris);
    subJrect = [(1:numTris); (1:numTris); (1:numTris)];
    prob.bara.subj = reshape(subJrect,1,3*numTris);
    prob.bara.subk = repmat([1, 2, 2],1,numTris);
    prob.bara.subl = repmat([1, 1, 2],1,numTris);
    prob.bara.val  = repmat([1.0, 1.0, 1.0],1,numTris);

%     prob.bara.subi = [old_a_rows + 1, old_a_rows + 2, old_a_rows + 3];
%     prob.bara.subj = [1, 1, 1];
%     prob.bara.subk = [1, 2, 2];
%     prob.bara.subl = [1, 1, 2];
%     prob.bara.val  = [1.0, 1.0, 1.0];
  
    %cones should remain the SAME as in Alec's formulation. We aren't
    %modifying the CONIC constraint in any way.
    prob.cones = cell(1,1);
    prob.cones{1}.type = [res.symbcon.MSK_CT_RQUAD];
    v_index = n+nt+1;
    w_index = n+nt+2;
    t_indices = n+(1:nt);
    prob.cones{1}.sub = [v_index w_index t_indices];

    % Enforce fixed values
    prob.bux(b) = bc;
    prob.blx(b) = bc;

    fprintf('Semidefinite optimization using mosek...\n');
%   end

  %ticid = tic;

  %[r,res]=mosekopt(['minimize ' quiet],prob,param);
  %toc;
  %disp('SETUP 2 TIME ^');
  %tic;
  
  param = [];
  param.MSK_IPAR_INTPNT_MAX_ITERATIONS = max_iteration;
  
  mOptStTime = tic;
  [r,res]=mosekopt(['minimize '],prob, param);
  mOptTime = toc(mOptStTime);
  fprintf('MosekSolveTime is %f\n',mOptTime);

  %toc;
  %disp('MOSEK time!! ^');

  try
    report_mosek_error(r,res);
  catch exception
      disp 'Near-optimal, not optimal!'
  end
  % extract solution from result
  x = res.sol.itr.xx;
  % set weights to solution in weight matrix
  Z = x(1:n);
end
