// Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
// Copyright (C) INRIA
// 
// This file must be used under the terms of the CeCILL.
// This source file is licensed as described in the file COPYING, which
// you should have received as part of this distribution.  The terms
// are also available at    
// http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt
//
function [%Xlist,%OPT]=lmisolver(%Xinit,%evalfunc,%options) 
  %OPT=[];%Xlist=list();
  [LHS,RHS]=argn(0);

  if RHS==2 then 
    %Mb = 1e3;%ato = 1e-10;%nu = 10;%mite = 100;%rto = 1e-10;
  else
    %Mb=%options(1);%ato=%options(2);%nu=%options(3);%mite=%options(4);%rto=%options(5);
  end

  %to=1e-5
  %tol=1e-10

  [%Xinit,%ind_X]=aplat(%Xinit);

  %dim_X=[]
  for %ia=1:size(%Xinit)
    %dim_X=[%dim_X;size(%Xinit(%ia))]
  end

  %x0=list2vec(%Xinit);
  %nvars=size(%x0,'*')

  //Testing feasibility of initial guess
  [%E,%I,%O]=%evalfunc(vec2list(%x0,%dim_X,%ind_X));
  if size(%O,'*')==0 then //only feasible point is searched
    if lmicheck(aplat(%E),aplat(%I)) then
      %Xlist=vec2list(%x0,%dim_X,%ind_X);
      lmisolvertrace(msprintf(_('%s: initial guess is feasible.'),'lmisolver'));
      // only feasibility claimed and given initial value is feasible, so
      // there in nothoting to do!
      return; 
    end
  end
    
 
  //Construction of canonical representation:
  //A first transformation is applied to form explicit linear equations: 
  // LMIs Hj(X1,X2,...,XN) > 0  gives I0 + I1*x1+...+In*xn >0
  // LMEs Gi(X1,X2,...,XN)=0    gives E0 + E1*x1+...+En*xn =0
  // Obj   O(X1,X2,...,XN)      gives O0 + O1*x1+...+On*xn
  // where Fi, Ci are matrices and fi scalars this is done using the
  // cannonical basis for the vector space of {X1,X2,...,XN}. The xi i=1..n are
  // the unknown components for this cannonical basis
 
  // A second transformation I0v=I0(:); Iiv=Fi(:) ;
  //                         E0v=V0(:); Eiv=Ei(:) ;
  // allows to rewrite the LMIs as I0v+I*X, the LMEs as E0v+E*X; 
  // and the Objective as O0+O*X
  // where 
  // X is a column vector of all undknowns
  // I=[I1v, ..., Inv]
  // E=[E1v, ..., Env]
  // O=[O1, ...,  On]
  // allows to rewrite
  
  //compute affine parts of LME LMI and OBJ
  [%E0,%I0,%O0]=%evalfunc(vec2list(zeros(%nvars,1),%dim_X,%ind_X));
  
  %E0v=list2vec(aplat(%E0));
  %I0=aplat(%I0);
  %O0v=list2vec(aplat(%O0));
  
  %blck_szs=[];
  for %lmii=%I0
    [%mk,%mk]=size(%lmii);%blck_szs=[%blck_szs,%mk]
  end
  %blck_szs=%blck_szs(find(%blck_szs~=0));
  [%I0v,%dim_I]=list2vec(%I0);


  %E=[];%I=[];%O=[]; 

  lmisolvertrace(msprintf(_('%s: Construction of canonical representation.'),'lmisolver'));
  %spI0=sparse(%I0v); //the sparse representation of F0
  %spE0=sparse(%E0v); //the sparse representation of C0
  %lX=size(%Xinit)
  
  %XZER=%Xinit
  for %ka=1:%lX
    %XZER(%ka)=sparse(0*%Xinit(%ka));
  end
  //construct a generators for LMI, LME and Obj ranges using a cannonical
  //basis for {X1, ..., Xn}
  for  %ja=1:%lX //loop on matrices Xi
    %row=%dim_X(%ja,1)
    %coll=%dim_X(%ja,2)
    for %ca=1:%coll //loop on columns of Xi
      for %ra=1:%row  //loop on rows of Xi
	//set the cannonical basis vector
	%XZER(%ja)(%ra,%ca)=1;
	
	//compute LME LMI and OBJ component for this base vector
	[%Ei,%Ii,%Oi]=%evalfunc(recons(%XZER,%ind_X));
	//transform into sparse column vectors
	%Eiv=splist2vec(%Ei)-%spE0;
	%Iiv=splist2vec(%Ii)-%spI0;
	//assemble the matrices
	%E=[%E,%Eiv];
	%I=[%I,%Iiv];
	%O=[%O,%Oi-%O0];
	
	//reset XZER to zero
	%XZER(%ja)(%ra,%ca)=0;
      end
    end
  end
  clear %spI0 %spE0
  // all the LMIs may be generated by %I*X   + %I0v 
  //     the LMEs may be generated by %E*X  + %E0v
  //     the OBJs may be generated by %O*X + %O0
  // for any column vector X
  


  if size(%E,'*')==0 then
    %kerE=speye(%nvars,%nvars);
  else
    lmisolvertrace(msprintf(_('%s: Basis Construction.'),'lmisolver'));
    //reduce the LMEs: all X solution of %E*X  + %E0v can be written
    //  X=X0+ker(%E)*W
    //where 
    //  X0 is a X such that  %E*X  + %E0v=0
    //and
    //  W is arbitrary (the new unknown)
    
    [%x0,%kerE]=linsolve(%E,%E0v,%x0);
    clear %E
    //now %kerE contains the kernel
  end
 // all the LMIs may then be generated by %I*(X0+ker(%E)*W) + %I0v
 //     the LMEs                       by %E*(X0+ker(%E)*W) + %E0v
 //     the OBJs                       by %O*(X0+ker(%E)*W) + %O0
  %I0v=%I0v+%I*%x0;
  %I=%I*%kerE;
  %O0=%O0+%O*%x0;
  %O=%O*%kerE;
  clear %E
  //with this updated notations
  // all the LMIs may then be generated by %I*W   + %I0v 
  //     the OBJs                       by %O*W + %O0
  // The initial unknown may be  obtained by X=%x0+%kerE*W

  if %blck_szs == [] then
    // is objective constant on LME constraint set, Xinit is feasible 
    if maxi(abs(%O+0)) < %to then
      lmisolvertrace(msprintf(_('%s: Objective constant.'),'lmisolver'));
      %Xlist=vec2list(%x0,%dim_X,%ind_X);
      %Xopt=%O0;
      return
    else
      error(msprintf(_('%s: solution unbounded.'),'lmisolver'));
    end
  end

  [%fm,%m]=size(%I);
  //Testing well-posedness
  if %fm<%m then 
    error(msprintf(_('%s: Ill-posed problem. Number of unknowns (%s) >"+...
		     " number of of constraints (%s)'),'lmisolver',%m,%fm));
  end

  
  //Testing rank deficiency
  if size(%I,'*')<>0 then
 
    [%ptr,%rk]=lufact([%I spzeros(%fm,%fm-%m)]',[%tol,0.001]);
    if %rk<%m then
      [%P,%L,%U,%Q]=luget(%ptr);%L=[];%U=[];%Q=[];
      %P=%P';%P=%P(1:%rk,1:%m)';
      warning(msprintf(_('%s: rank deficient problem'),'lmisolver'));
      ludel(%ptr);
      //Testing to see if linobj is in the range of F_is
      if size(%O,'*') <> 0 then
	[%ptr,%rk2]=lufact([[%I;%O] spzeros(%fm+1,%fm+1-%m)]',[%tol,0.001]);
	ludel(%ptr);
	if %rk<%rk2 then
	  error(msprintf(_('%s: solution unbounded.'),'lmisolver'));
	end
      end
      %O=%O*%P
      %I=%I*%P;
      %kerE=%kerE*%P;
      %m=%rk;
      %P=[];
 
    end
  end

  //Testing to see if solution or the LMI value is unique
  if size(%I,'*')==0 then //the LMI reduces to %I0 >0
    //checking positiveness of  %I0
    if ~lmicheck(list(),vec2list(%I0v,%dim_I))
      error(msprintf(_('%s: not feasible or badly defined problem.'),'lmisolver'));
    else
      %Xlist=vec2list(%x0,%dim_X,%ind_X);
      return; 
    end
  end

  //Testing feasibility of initial guess
  //are LMIs positive?
  [ok,%sm]=lmicheck(list(),vec2list(%I0v,%dim_I))
  
  if ok&size(%O,'*')==0 then
    //LMIs are positive, problem is feasible, return
    %Xlist=vec2list(%x0,%dim_X,%ind_X);
    return; 
  end

  %M=%Mb*norm([%I0v,%I],1)


  if ~(%sm>%to) then
    //given initial point is not feasible. Look for a feasible initial point.
    lmisolvertrace(msprintf(_('%s:     FEASIBILITY PHASE.'),'lmisolver'));

    // mineigI is the smallest eigenvalue of I0
    %mineigI=min(real(flat_block_matrix_eigs(%I0v,%blck_szs)))

    // Id is the identity
    %Id = build_flat_identity(%blck_szs)
    if (%M < %Id'*%I0v+1e-5), 
      error(msprintf(_('%s: Mbound too small.'),'lmisolver')); 
    end;

    // initial x0 
    %x00 = [zeros(%m,1); max(-1.1*%mineigI, 1e-5)];

    //Compute  Z0  the projection of Id on the space Tr Ii*Z = 0

    %Z0=%Id-%I*(%I\%Id);
    if %f then
      //check: trace(Ii*Z0) = 0 <=> %Id'*%Z0= 0 
      %I'*%Z0
    end
    //compute  mineigZ is the smallest eigenvalue of Z0
    %mineigZ=min(real(flat_block_matrix_eigs(%Z0,%blck_szs)));
    %ka=sum(%blck_szs.^2);
    %Z0(%ka+1) = max( -1.1 *%mineigZ, 1e-5 );  // z  
    %Z0(1:%ka) = %Z0(1:%ka) + %Z0(%ka+1)*%Id; 
    %Z0 = %Z0 / (%Id'*%Z0(1:%ka));    // make Tr Z0 = 1

    if %f then //for checking semidef
      Z=sysdiag(matrix(%Z0(1:16),4,-1),%Z0(17))
      F0=full(sysdiag(matrix(%I0v,4,-1), %M-%Id'*%I0v));
      for i=1:10,
	Fi=full(sysdiag(matrix(%I(:,i),4,-1),-%Id'*%I(:,i)));
	mprintf('i=%d %e\n',i,abs(trace(Fi*Z)-%c(i)));
      end
      F11=sysdiag(matrix(%Id,4,-1),0);
      mprintf('i=%d %e\n',11,abs(trace(F11*Z)-%c(11)))

    end
    
    //Pack Z0 and I
    %Z0=pack(%Z0,[%blck_szs,1]);

    %temp=full(pack([%I0v,        %I,       %Id;
		     %M-%Id'*%I0v, -%Id'*%I, 0   ],[%blck_szs,1]));
    %c=[zeros(%m,1); 1];

    [%xi,%Z0,%ul,%info]=semidef(%x00,%Z0,%temp,[%blck_szs,1],%c,[%nu,%ato,-1,0,%mite]);
    %temp=[];
    %xi=%xi(1:%m);

    select %info(1)
    case 1
      error(msprintf(_('%s: Max. iters. exceeded.'),'lmisolver'))
    case 2 then
      lmisolvertrace(msprintf(_('%s: Absolute accuracy reached.'),'lmisolver'))
    case 3 then
      lmisolvertrace(msprintf(_('%s: Relative accuracy reached.'),'lmisolver'))
    case 4 then
      lmisolvertrace(msprintf(_('%s: Target value reached.'),'lmisolver'))
    case 5 then
      error(msprintf(_('%s: Target value not achievable.'),'lmisolver'))
    else
      warning(msprintf(_('%s: No feasible solution found.'),'lmisolver'))
    end


    if %info(2) == %mite then 
      error(msprintf(_('%s: max number of iterations exceeded.'),'lmisolver'));
    end
    if (%ul(1) > %ato) then 
      error(msprintf(_('%s: No feasible solution exists.'),'lmisolver'));
    end
    //       if (%ul(1) > 0) then %I0v=%I0v+%ato*%Id;end

    lmisolvertrace(msprintf(_('%s: feasible solution found.'),'lmisolver'));

  else

    lmisolvertrace(msprintf(_('%s: Initial guess feasible.'),'lmisolver'));
    %xi=zeros(%m,1);
  end


  if size(%O,'*')<>0 then

    lmisolvertrace(msprintf(_('%s:       OPTIMIZATION PHASE.') ,'lmisolver'));

    %M = max(%M, %Mb*sum(abs([%I0v,%I]*[1; %xi])));  

    // Id is the identity
    %Id = build_flat_identity(%blck_szs)
    // M must be greater than trace(F(x0))   for bigM.sci
    [%ptr,%rkA]=lufact(%I'*%I,[%tol,0.001]);
    %Z0=lusolve(%ptr,full(%I'*%Id-%O'));
    %Z0=%Id-%I*%Z0;
    ludel(%ptr)

    //check: trace(Ii*Z0) = c <=> %I(:,k)'*%Z0= %O(k) (k = 1:m)
    // mineigZ is the smallest eigenvalue of Z0
    %mineigZ=min(real(flat_block_matrix_eigs(%Z0,%blck_szs)))
    %ka=sum(%blck_szs.^2);
    %Z0(%ka+1) = max(1e-5, -1.1*%mineigZ);  
    %Z0(1:%ka) = %Z0(1:%ka) + %Z0(%ka+1)*%Id; 

    if (%M < %Id'*[%I0v,%I]*[1;%xi] + 1e-5), 
      error(msprintf(_('%s: M must be strictly greater than trace of F(x0).'),'lmisolver')); 
    end;

   
    // add scalar block Tr F(x) <= M

    %blck_szs = [%blck_szs,1];
    
    temp=full(pack([%I0v,          %I; 
		    %M-%Id'*%I0v, -%Id'*%I],%blck_szs));
    
    [%xopt,%z,%ul,%info]=semidef(%xi,pack(%Z0,%blck_szs),temp,%blck_szs,full(%O),[%nu,%ato,%rto,0.0,%mite]);
    clear temp
    if %info(2) == %mite then 
      warning(msprintf(_('%s: max number of iterations exceeded, solution may not be optimal'),'lmisolver'));
    end;
    if sum(abs([%I0v,%I]*[1; %xopt])) > 0.9*%M then 
      lmisolvertrace(msprintf(_('%s: may be unbounded below'),'lmisolver'));
    end;
    if %xopt<>[]&~(%info(2) == %mite) then 
      lmisolvertrace(msprintf(_('%s: optimal solution found'),'lmisolver'));
    else %xopt=%xi;
    end
  else
    %xopt=%xi;
  end

  %Xlist=vec2list((%x0+%kerE*%xopt),%dim_X,%ind_X);
  %OPT=%O0+%O*%xopt;
endfunction


function [bigVector]=splist2vec(li)
//li=list(X1,...Xk) is a list of matrices
//bigVector: sparse vector [X1(:);...;Xk(:)] (stacking of matrices in li)
  bigVector=[];
  li=aplat(li)
  for mati=li
    sm=size(mati);
    bigVector=[bigVector;sparse(matrix(mati,prod(sm),1))];
  end

endfunction

function [A,b]=spaff2Ab(lme,dimX,D,ind)
//Y,X,D are lists of matrices. 
//Y=lme(X,D)= affine fct of Xi's; 
//[A,b]=matrix representation of lme in canonical basis.
  [LHS,RHS]=argn(0)
  select RHS
  case 3 then
    nvars=0;
    for k=dimX'
      nvars=nvars+prod(k);
    end
    x0=zeros(nvars,1);
    b=list2vec(lme(vec2list(x0,dimX),D));
    A=[];
    for k=1:nvars
      xi=x0;xi(k)=1;
      A=[A,sparse(list2vec(lme(vec2list(xi,dimX),D))-b)];
    end

  case 4 then
    nvars=0;
    for k=dimX'
      nvars=nvars+prod(k);
    end
    x0=zeros(nvars,1);
    b=list2vec(lme(vec2list(x0,dimX,ind),D));
    A=[];
    for k=1:nvars
      xi=x0;xi(k)=1;
      A=[A,sparse(list2vec(lme(vec2list(xi,dimX,ind),D))-b)];
    end
  end
endfunction

function lmisolvertrace(txt)
  mprintf("%s\n",txt)
endfunction


function [ok,%sm,%nor]=lmicheck(E,I)

  //checking positiveness of the LMI
  %sm=100;
  for %w=I
    if %w~=[] then 
      s=mini(real(spec(%w)))
      %sm=min(%sm,s)
    end
  end  
  ok=%sm>=-%tol
    
  //Checking norm of the LME
  %nor=0
  for %w=E
    if %w~=[] then 
      n=norm(%w,1)
      %nor=max(%nor,n)
    end
  end     
    
  ok=%sm>=-%tol & %nor<%tol
endfunction
function e=flat_block_matrix_eigs(V,blck_szs)
//  Computes the eigenvalues of each block of a flatten block matrix
    ka=0; e=[];
    for n=matrix(blck_szs,1,-1)
      e=[e;spec(matrix(V(ka+[1:n^2]),n,n))]
      ka=ka+n^2;   
    end;
endfunction

function Id = build_flat_identity(blck_szs)
  //build a flat representation of a block identity matrix
  ka=0; 
  for n=matrix(blck_szs,1,-1)
    Id(ka+[1:n^2]) = matrix(eye(n,n),-1,1);   // identity
    ka=ka+n^2; 
  end;
endfunction