// 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.1-en.txt
//
function [%Xlist,%OPT]=lmisolver(%Xinit,%evalfunc,%options)
    %OPT=[];%Xlist=list();
    [LHS,RHS]=argn(0);

    if RHS < 2 then
        error(msprintf(_("%s: Wrong number of input arguments: %d to %d expected.\n"),"lmisolver",2,3))
    end

    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 max(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 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=min(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