function [bounds, prodOpt, prodV, info] = jsr_norm_conitope(M,options)
%
% JSR_NORM_CONITOPE Approximates the jsr of a set of matrices using 
%                   conitopes.
%
%  [BOUNDS, PRODOPT, PRODV, INFO] = JSR_NORM_CONITOPE(M)
%      returns lower and upper bounds on the jsr of M using conitopes. 
%      Uses default values for the parameters, see below. M must be a cell 
%      array of square matrices.
%
%  [BOUNDS, PRODOPT, PRODV, INFO] = JSR_NORM_CONITOPE(M,OPTIONS)
%      Uses the values of parameters specified in the structure OPTIONS.
%      See JSRSETTINGS and below for available parameters.
%
%      BOUNDS contains the best lower and upper bounds on the JSR
%
%      PRODOPT contains the indices of an optimal product at last iteration
%              ( to use with buildProduct ) 
%      PRODV   contains the index of the products that have led to the
%              the essential set
%
%      INFO is a structure containing various data about the iterations :
%         info.status         - = 0 if normal termination, 1 if maxStep
%                               reached, 2 if stopped in iteration
%                               (CTRL-C or options.maxTime reached)
%         info.elapsedtime 
%         info.niter          - number of iterations
%         info.timeIter       - cputtime - start time in s. at the end of 
%                               iteration. Note : diff(info.timeIter) gives                              
%                               the time taken by each iteration.
%         info.popHist        - number of points in essential set [1x(niter) double]
%         info.allLb          - evolution of the lower bound  [1x(niter+1) double]
%         info.allUb          - evolution of the upper bound  [1x(niter) double]
%         info.opts           - the structure of options used
%
%  The field options.conitope (generated by jsrsettings) can be used 
%  to tune the method :
%
%      conitope.initProd      - indices of an initial optimal product, if 
%                               not given starts from the matrix with largest
%                               spectral radius, ([])      
%      conitope.maxiter       - max number of iterations (updates of the unit
%                               ball), (60)
%      conitope.tol           - tolerance corresponding to ending
%                               condition; stops when no image of the points has   
%                               a polylifted norm more than
%                               options.conitope.tol out of the current unit
%                               ball.
%                               The polylifted norm is computed using 
%                               SOLVE_SEMI_DEFINITE_PROGRAM with a tolerance 
%                               of 0.1*options.conitope.tol, (1e-8)
%      conitope.per           - update period of the lower bound on the JSR, (1)
%
%      conitope.reinitBall    - if 1, when a new optimal product has been
%                               found, reinitialize the set of points to
%                               the images of the leading eigenvector of
%                               the new optimal product. 0 disables it
%                               , (1)
%  
%      conitope.plotBounds    - if 1 plots the evolution of the bounds, (0)
%      
%      conitope.plotpopHist   - if 1 plots  evolution of the size of the
%                               essential set, (0)
%
%      conitope.plotTime      - if 1 plots evolution of the bounds w.r.t. time of
%                               computation, (0)
%
%   NOTE :  This function uses the interface solve_semi_definite_program 
%           in order to call a LMI solver (default : SeDuMi). 
%           See help solve_semi_definite_program for more information.
%
%  See also JSRSETTINGS, SOLVE_SEMI_DEFINITE_PROGRAM
%
% Requires SeDuMi [By default] ( http://sedumi.ie.lehigh.edu/ )
%
%
% REFERENCES
%    A.Cicone, N.Guglielmi, R.Jungers. Lifted polytope methods for the computation of 
%    joint spectral characteristics, preprint, 2011.
%
%


if nargin < 2
    options = jsrsettings;
elseif (~isstruct(options))
    warning('OPTIONS not a strucure, set to default. See help jsrsettings')
    options = jsrsettings;
end

% logfile opening
close =1;
if (ischar(options.logfile) )    
    logFile = fopen(options.logfile,'wt');
    if (logFile == -1)
        warning(sprintf('Could not open file %s',options.logfile));
    end
elseif isnumeric(options.logfile)
    if (options.logfile==0)
        logFile = -1;
    elseif (options.logfile==1)
        logFile = fopen('log_conitope','wt');
        if (logFile == -1)
            warning('Could not open logfile')
        end
    else
        logFile = options.logfile;
        close = 0;
    end
else
    logFile = fopen('log_conitope','wt');
    if (logFile == -1)
        warning('Could not open logfile')
    end
end

if (logFile~=-1)
    fprintf(logFile,[datestr(now) '\n\n']);
end

msg(logFile,options.verbose>1,'\n \n******** Starting jsr_norm_conitope ******** \n \n')
starttime = cputime;

% Parameters
maxiter = options.conitope.maxiter; 
per = options.conitope.per; 
tol = options.conitope.tol;
tollb = eps;
m = length(M);
n = size(M{1}, 1);


% Initialization
cont = 1;  % Ending condition on main loop
maxTime = 0; % Boolean indicating if we stop for opts.maxTime
status = 2; 
nit = 0;      % Number of points
nvhist = zeros(1,maxiter);
V = cell(1,50); % Set of points
prodV = sparse(maxiter,500);   % Products leading to the points
prodOpt = zeros(maxiter,1);  % Indices of the optimal product
lbrho = zeros(1,maxiter+1);
maxnorm = zeros(1,maxiter); % upper bounds on the jsr: max norm of an image of a vertex (rescaled in code)
timeit = zeros(1,maxiter);

% Iteration 0
if isempty(options.conitope.initProd)
    [lbrho(1), ind] = max(rho(M));
    prodOpt(1) = ind;
    prodOptM = M{ind};
    lengthprodOpt = 1;
else
    initProd = deperiod(options.conitope.initProd);
    lengthprodOpt = sum(initProd~=0);
    prodOpt(1:lengthprodOpt) = initProd(initProd~=0);
    prodOptM = buildProduct(M,prodOpt);
    lbrho(1) = rho(prodOptM)^(1/lengthprodOpt);
end

M = cellDivide(M,lbrho(1));

% Initialization of the ball
if ( options.conitope.reinitBall )
    
    [nv, V, prodV, Vvec] = reinitBall(M,prodOpt,prodOptM,options,logFile);
    
    if (nv<n)
        msg(logFile,1,'\n ***********************\n Matrices appear to be jointly block-triangularizable ! \n ***********************\n');
        msg(logFile,1,'\nIn this case the Joint Spectral Radius of the set is equal to max JSR of each set of blocks.\n');
        msg(logFile,1,'The function try to find triangularization with jointTriangul, will terminate and return the sets found as output.\n')
        msg(logFile,1,'\nPlease use quickElim and launch this and/or other method(s) on the relevant sets. \n');
        msg(logFile,1,'\nSee also jointTriangul and quickElim \n')
        msg(logFile,1,'\nREFERENCES')
        msg(logFile,1,'   R.Jungers,')
        msg(logFile,1,'   "The Joint Spectral Radius: Theory and Applications"')
        msg(logFile,1,'   Vol. 385 section 1.2.2.5 in Lecture Notes in Control and Information')
        msg(logFile,1,'   Sciences, Springer-Verlag. Berlin Heidelberg, June 2009')

        [triag, blocks, B] = jointTriangul(M,Vvec);
      
        bounds.blocks = blocks;
        bounds.B = B;
        
        info = 'Matrices appear to be jointly block triangularizable, see printed message.';
        
        return;
    end
    
else
    V{1} = eye(n);
    nv = 1;
end

% Main loop
while(cont==1 && nit < maxiter)
    
    nit = nit + 1;
    
    W = V(1:nv);         % Working points and their product indexes for this iteration
    prodW = prodV(:,1:nv);
    nw = nv;
    
    % Update of the lower bound and rescale of the matrices (not for first iteration)
    if (mod(nit,per)==0 && nit > 1)
        lb = 1;
        
        for cp = 1:nw
            lengthprod = sum(prodW(:,cp)~=0);
            llb = rho(buildProduct(M,prodW(:,cp)))^(1/lengthprod);
            
            if (llb > lb + tollb)
                lb = llb;
                prodOpt = prodW(:,cp);
            end
            
        end

        lbrho(nit+1) = lbrho(nit)*lb;
                        
        % Update of the matrices and points
        if (lb>1)
            ind = find(prodOpt,1,'last');

            msg(logFile,options.verbose>1,'\n-- Iteration %d prodOpt actualised to : %s',nit,num2str(prodOpt(1:ind)'))
            
            M = cellDivide(M,lb);
            
            for i = 1:nv
                lengthprod = sum(prodV(:,i)~=0);
                V{i} = V{i}./(lb^lengthprod);
            end
        end
        
        % Update unit ball to image of the leading eigenvector
        if ( lb > 1 && options.conitope.reinitBall )
            
            time1 = cputime;
            [nv, V, prodV, Vvec] = reinitBall(M,prodOpt,prodOptM,options,logFile);
            
            if (nv<n)
                msg(logFile,1,'\n ***********************\n Matrices appear to be jointly block-triangularizable ! \n ***********************\n');
                msg(logFile,1,'\nThe joint spectral radius of the set is equal to max jsr of each set of blocks.\n');
                msg(logFile,1,'The function will terminate and return the sets in as output. Please use quickElim and launch  \n');
                msg(logFile,1,'this and/or other method(s) on the relevant sets.\n')
                msg(logFile,1,'See also jointTriangul and quickElim \n')
                msg(logFile,1,'\nREFERENCES')
                msg(logFile,1,'   R.Jungers,')
                msg(logFile,1,'   "The Joint Spectral Radius: Theory and Applications"')
                msg(logFile,1,'   Vol. 385 section 1.2.2.5 in Lecture Notes in Control and Information')
                msg(logFile,1,'   Sciences, Springer-Verlag. Berlin Heidelberg, June 2009')

                [triag, blocks, B] = jointTriangul(M,Vvec);

                bounds.blocks = blocks;
                    
                return;
            end
            
            normunit = polyliftedNorm(V(1:nv),eye(n),tol/10,logFile,options);
            
            msg(logFile,options.verbose>1,'\n ****** Polylifted norm of identity matrix %4.2e  \n',normunit);

            
            % Reset parameters
            W = V;
            prodW = prodV;
            nw = nv;
            
            msg(logFile,options.verbose>0,'\n >>> Ball reinitialized in %4.2f s <<< \n',cputime-time1);
                        
        end
    else
        lbrho(nit+1) = lbrho(nit);
    end
    
    
    % Update of the unit ball
    MW = cell(1,m*nw); % Cell with the m*nw new points
    prodMW = sparse(size(prodV,1),m*nw);
    nadd = 0;
    

    msg(logFile,options.verbose>1,'Starting computation of %d new points',m*nw);
    
    
    for imat = 1:m
        for cp = 1:nw
            
            % Computes the image of the point
            ai = M{imat}*W{cp}*M{imat}';
            prodai = prodW(:,cp);           
            ind = find(prodai,1,'last'); % index of last non-zero entry
            if(isempty(ind))
                ind = 0;
            end
            prodai(ind+1,1) = imat;
            
            % In case JSR = 0 
            if norm(full(ai))<1e-12
                normai=0;
            else
                % Computes its norm w.r.t. the current essential system
                [normai, probl] = polyliftedNorm(V(1:nv),ai,tol/10,logFile,options);
            end
            
            if (normai>maxnorm(nit))
                maxnorm(nit) = normai+tol/10; % max norm is the bound that this iteration gives on the jsr
            end

            % Keep the new point if it is more than tol out of the unit ball
            if (normai>1+tol)
                nadd = nadd + 1;
                MW{nadd} = ai;
                prodMW(:,nadd) = prodai;
            end

        end
    end
    

    msg(logFile,options.verbose>1,'maxnorm vectors -1 before scaling : %6.3e',maxnorm(nit)-1)
 
    
    maxnorm(nit) = maxnorm(nit)*lbrho(nit+1); % Rescales the upper bound    
    
    if (nadd>0)
        V(nv+1:(nv+nadd)) = MW(1:nadd);
        prodV(:,nv+1:(nv+nadd)) = prodMW(:,1:nadd);
        

        msg(logFile,options.verbose>1,'%d new point(s) added, %d point(s) in set',nadd,nv+nadd);
        
    else

        msg(logFile,options.verbose>0,'\n Final system of vertices found (w.r.t. tol)')
        
        nvhist(nit) = nv;
        timeit(nit) = cputime - starttime;
        % End of main loop
        break;
    end


        
   % Construction of an essential system   
   nbremoved = 0;
   nvold = nv;
   nv = nv+nadd;
   nvinit = nv;
   ncheck = 0;
   indcheck = 1;
   

   msg(logFile,options.verbose>1,'Starting construction of an essential system')
   
   
   while(ncheck < nvinit && nv > 1) 
       ncheck = ncheck+1;
       
       v = V{indcheck};
       ind = 1:nv;
       Vtemp = V(ind(ind~=indcheck));
       
       [normv, probl] = polyliftedNorm(Vtemp,v,tol/10,logFile,options);
       
       if (normv <=1+tol && probl==0)
           V = Vtemp;
           prodV = prodV(:,ind(ind~=indcheck));
           nv = nv-1;
           nbremoved = nbremoved+1;
           
           if (indcheck <= nvold)
               nvold = nvold - 1;
           end
       else
           indcheck = indcheck+1;
       end
   end

   msg(logFile,options.verbose>1,'%d point(s) removed, %d point(s) in essential set',nbremoved,nv)
   
   
   nvhist(nit) = nv;
   
   msg(logFile,options.verbose>0,'\n > Iteration %d - current bounds: [%.15g, %.15g] \n', nit, lbrho(nit+1), maxnorm(nit));
   
   normunit = polyliftedNorm(V(1:nv),eye(n),tol/10,logFile,options);
   
   if (options.verbose>1)
   msg(logFile,options.verbose>1,'\n ****** Polylifted norm of identity matrix %4.2e  \n',normunit);
   end
   
   timeit(nit) = cputime - starttime;
   
   % Save to file option
   if (ischar(options.saveinIt))
       if (nit==maxiter);status=1;end
       elapsedtime = cputime-starttime;    
       
       ind = find(prodOpt,1,'last'); if(isempty(ind));ind = 0;end
       
       bounds = [lbrho(nit+1), min(maxnorm(1:nit))];
       ind = max(rem(find(prodV),size(prodV,1)));
       prodVinIt = full(prodV(1:ind,1:nv));
       ind = find(prodOpt,1,'last'); if(isempty(ind));ind = 0;end
       prodOptinIt = full(prodOpt(1:ind));
       info.status = status;
       info.elapsedtime = elapsedtime;
       info.niter = nit;
       info.timeIter = timeit(1:nit);
       info.popHist = nvhist(1:nit);
       info.allLb = lbrho(1:nit+1);
       info.allUb = maxnorm(1:nit);
       info.opts = options;
       
       save([options.saveinIt '.mat'],'bounds','prodOptinIt','prodVinIt','info')
       clear bounds prodOptinIt prodVinIt info
   end

    % Check projected time for next iteration
    if(nit>1)
        limProj = (cputime + timeit(nit)-timeit(nit-1) - starttime)>options.maxTime;
    else
        limProj = 0;
    end
   
   if (timeit(nit)>=options.maxTime || limProj)
       maxTime = 1;
       msg(logFile,options.verbose>0,'\nopts.maxTime reached\n');
       break;
   end
end

if(nit==maxiter)
    status = 1;
elseif maxTime
    status = 2;
else
    status = 0;
end

msg(logFile,options.verbose>0,'\n> Bounds on the jsr: [%.15g, %.15g]', lbrho(nit+1), min(maxnorm(1:nit)));


elapsedtime = cputime-starttime;

msg(logFile,options.verbose>1,'\n End of algorithm after %5.2f s',elapsedtime)


% Post-processing
if (logFile~=-1 && close)
   fclose(logFile);
end

lowerBound = lbrho(1:nit+1);
upperBound = maxnorm(1:nit);
bounds = [lowerBound(end), min(upperBound)];
ind = max(rem(find(prodV),size(prodV,1)));
prodV = full(prodV(1:ind,1:nv));
ind = find(prodOpt,1,'last'); % index of last non-zero entry
if(isempty(ind))
    ind = 0;
end
prodOpt = full(prodOpt(1:ind))';

info.status = status;
info.elapsedtime = elapsedtime;
info.niter = nit;
info.timeIter = timeit(1:nit);
info.popHist = nvhist(1:nit);
info.allLb = lowerBound;
info.allUb = upperBound;
info.opts = options;


if (ischar(options.saveEnd))
    save([options.saveEnd '.mat'],'bounds','prodV','prodOpt','info')
end
   
if (options.conitope.plotBounds)
   figure 
   iter = 1:nit; 
   plot(iter,upperBound,'-*r',[0 iter],lowerBound,'-g+')
   title('conitope : Evolution of the bounds on the JSR')
   legend('Upper bound','Lower bound')
   xlabel(sprintf('Iterations, ( lower bound actualised every %d step(s) )',options.conitope.per))
end


if (options.conitope.plotpopHist)
   figure
   plot(1:nit,nvhist(1:nit),'-o');
   title('conitope : Evolution of size of essential system');
   xlabel('Iteration')
   xlim([1 max(2,nit)])
end

if (options.conitope.plotTime)
    figure
    if (timeit(nit) > 600)
        time = info.timeIter/60;
        plot([0 time],info.allLb,'-+g',time,info.allUb,'-*r','MarkerSize',10);
        xlabel('time from start in min')
    else
        plot([0 info.timeIter],info.allLb,'-+g',info.timeIter,info.allUb,'-*r','MarkerSize',10)
        xlabel('time from start in s')
    end
    title('conitope : Evolution of the bounds w.r.t. time')
    legend('Lower bound','Upper bound')  
end
end

function [nv, V, prodV, Vvec] = reinitBall(M,prodOpt,prodOptM,options,logFile)

    n = length(M{1});
    maxiter = options.conitope.maxiter;
    lengthprodOpt = sum(prodOpt~=0);
    maxInd = max(maxiter,lengthprodOpt);
    eigOpts.disp=0;
    
    if ( issparse(prodOptM) )
        [leadval, leadvec] = eigs(prodOptM,1,'LM',eigOpts);
    else
        [eigvec D] = eig(prodOptM);
        [leadval ind] = max(abs(diag(D)));
        leadvec = eigvec(:,ind);
    end

    % Build full rank set of vectors :
    Vvec = zeros(n,n);
    V = cell(1,n);
    prodV = zeros(maxInd,n);
    Vvec(:,1) = leadvec;
    V{1} = leadvec*leadvec';

    for i=1:lengthprodOpt-1
        Vvec(:,i+1) = M{prodOpt(i)}*Vvec(:,i);
        V{i+1} = Vvec(:,i+1)*Vvec(:,i+1)';
        prodV(:,i+1) = [prodOpt(1:i) ; zeros(maxInd-i,1)];
    end
    nv = rank(Vvec(:,1:lengthprodOpt),1e-12);
            
    if (nv<lengthprodOpt && nv<n)

        msg(logFile,options.verbose>1,'\n \n nv ~= lengthprodOpt && nv<n  \n\n')

        % Take out linearly dependent vectors
        while (nv < length(Vvec(1,:)))
            for i=1:length(Vvec(1,:))
                Vvecnew = Vvec;
                Vvecnew(:,i) = [];
                ranknew = rank(Vvecnew,1e-12);

                if (ranknew==nv)
                    Vvec = Vvecnew;
                    prodV(:,i) = zeros(maxInd,1);
                    V(i) = [];
                    break
                end
            end
        end

    end
    
    nvnew = 1;
    indNew = 1;
    
    while(nv<n && (nvnew>0))
        [nvnew, Vvec, V, prodV] = appMat(M,Vvec,V,prodV,nv,indNew,options,logFile);        
        nv = nv+nvnew;           
    end
            
  
end

function [nvnew, Vvecnew, Vnew, prodVnew] = appMat(M,Vvec,V,prodV,nv,indNew,options,logFile)

m = length(M);

Vvecnew = Vvec(:,1:nv);
Vnew = V;
prodVnew = prodV;
nvnew = 0;

for imat = 1:m
    for ivec = indNew:nv
        vectemp = M{imat}*Vvec(:,ivec);
        normvec = norm(vectemp);
        
        if(normvec > 1e-10)
            Vvectemp = [Vvecnew(:,1:(nv+nvnew)) vectemp];
            rankTemp = rank(Vvectemp,1e-12);
            
            if (rankTemp == nv+nvnew+1)
                Vvecnew = Vvectemp;
                Vnew{nv+nvnew+1} = vectemp*vectemp';
                ind = find(prodVnew(:,ivec),1,'last');
                if isempty(ind)
                    ind = 0;
                end
                
                prodVnew(:,nv+nvnew+1) = prodVnew(:,ivec);
                prodVnew(ind+1,nv+nvnew+1) = imat;
                msg(logFile,options.verbose>1,'   nv = %d,   Added %s to set \n',nv+nvnew+1,num2str(full(prodVnew(1:ind+1,nv+nvnew+1))'))
                
                nvnew = nvnew+1;
            end
        end
    end
end

end