function [bounds, P, info] = jsr_opti_sos(M, varargin)

% JSR_OPTI_SOS Approximates the jsr using sum of squares.
%
%    [BOUNDS, P, INFO] = JSR_OPTI_SOS(M) 
%      returns lower and upper bounds on the jsr of M using SOS techniques 
%      with homogeneous polynomials of degree 2. M is a cell array of
%      matrices.
%
%    [BOUNDS, P, INFO] = JSR_OPTI_SOS(M, D, LOWER, UPPER) 
%      returns lower and upper bounds on the jsr of M using SOS techniques 
%      with homogeneous polynomials of degree 2*D (default value: D = 1).
%      The set M is a cell array containing the considered matrices.
%      The parameters LOWER and UPPER are starting bounds for the bisection
%      method and are optional.
%
%    [BOUNDS, P, INFO] = JSR_OPTI_SOS(M, OPTIONS) 
%      Does the same but uses the parameters values in the structure OPTIONS.
%      See below and help JSRSETTINGS for available parameters.
%
%      BOUNDS contains the lower and upper bounds on the JSR
%
%      P is the matrix associated to the positive polynomial
%
%      INFO is a structure containing various data about the iterations :
%         info.status         - = 0 if normal termination, 1 if maxiter
%                               reached, 2 if stopped in iteration
%                               (CTRL-C or options.maxTime reached)                               
%         info.elapsedtime 
%         info.bissec         - [lower upper] last bisection interval 
%         info.niter          - number of iterations (bisection)
%         info.opts           - the structure of options used
%
%
%  The field opts.sos (generated by jsrsettings) can be used to
%  tune the method :
%
%      sos.initBounds          - = [LOWER UPPER], provides starting bounds for
%                                bisection method, if not specified uses a
%                                few iterations of jsr_prod_bruteForce
%      sos.deg                 - half degree of homogeneous polynomials, (1)
%
%      sos.maxiter             - maximum number of iterations (for bisection),
%                                (1000)
%      sos.tol                 - asked precision for bisection. Stopping
%                                condition is 
%                                upper-lower < options.sos.tol*upper
%                                Influences the asked SeDuMi precision, 
%                                params.eps = options.sos.tol/10, (1e-8)
%
%   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
%
%
% REFERENCES
%    P.A.Parrilo, A.Jadbabaie,
%      "Approximation of the joint spectral radius using sum of squares",
%      Linear Algebra and its Applications, 428(10):2385-2402, 2008
%    R.Jungers, 
%      "The Joint Spectral Radius: Theory and Applications" 
%      Vol. 385 section 2.3.7 in Lecture Notes in Control and Information
%      Sciences, Springer-Verlag. Berlin Heidelberg, June 2009



if (nargin > 1)
    if (length(varargin) == 3)
        options = jsrsettings('sos.deg',varargin{1},'sos.initBounds',[varargin{2} varargin{3}]);
    elseif (length(varargin)==1 && isnumeric(varargin{1}) )
        options = jsrsettings('sos.deg',varargin{1});
    elseif (length(varargin)==1 && isstruct(varargin{1}) )
        options = varargin{1};
    else
        warning('Invalid optional arguments, using default parameters !');
        options = jsrsettings;
    end
else
    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_sos','wt');
        if (logFile == -1)
            warning('Could not open logfile')
        end
    else
        logFile = options.logfile;
        close = 0;
    end
else
    logFile = fopen('log_sos','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_opti_sos ******** \n \n')

% Initialization
starttime = cputime;
status = 2;
m = length(M); % number of matrices
n = length(M{1}); % dimension of the matrices
iter = 0;
opts.disp = 0;

% Parameters
d = options.sos.deg;
maxiter = options.sos.maxiter;


% Starting bounds
if(isempty(options.sos.initBounds))
maxdepth = ceil(log(42)/log(m));
optsbrute = jsrsettings('bruteforce.maxdepth',maxdepth,'verbose',0,'logfile',0);
bounds = jsr_prod_bruteForce(M, optsbrute);
msg(logFile,options.verbose>1,'Bounds from bruteforce with depth %2.0f : [%.15g, %.15g]',maxdepth,bounds(1),bounds(2)); 

upper = bounds(2);
lower = bounds(1);
else
    lower = options.sos.initBounds(1);
    upper = options.sos.initBounds(2);
end

lowerJSR = lower;
if (upper-lower <= options.sos.tol*upper),
    P = [];
    LowerSosEst = .999 * upper * min([m, nck(n+d-1, d)])^(-1/(2*d));
    bounds = [LowerSosEst, upper];
    elapsedtime = cputime - starttime;
    msg(logFile,options.verbose>0,'No bisection needed, initial bounds are tight enough');
    
    if (logFile~=-1 && close)
        fclose(logFile);
    end
    
    info.status = status;
    info.elapsedtime = elapsedtime;
    info.bissec = [lower upper];
    info.niter = iter;
    info.opts = options;
    return;
end
candidate = (upper+lower) / 2;
factor = candidate^(2*d);

% Semidefinite variables
basis = genPerms(n-1, d, 1); % homogeneous monomials (degree d, n variables)
N = size(basis, 1);
K.s = ones(1, m+1) * N;
NB_VAR = (m+1) * N^2;

% Objective function
c = sparse(NB_VAR, 1);

% Constraints
monomials = genPerms(n-1, 2*d, 1); % homogeneous monomials (degree 2d, n variables)
D = size(monomials, 1);
A = sparse(1 + D*m, NB_VAR);
b = sparse(1 + D*m, 1);
key = sparse(D, N^2);
A(end, 1:N^2) = 1;
b(end) = 1;

% Process all combinations of basis monomials
for i = 1:N,
    for j = i:N,
        product = basis(i, :) + basis(j, :);
        idx = findRow(monomials, product);
        A(idx:D:end-1, N*i+j-N) = factor;
        A(idx:D:end-1, N*j+i-N) = factor;
        A(idx:D:end-1, N*i+j-N+N^2:N^2:end) = -eye(m);
        A(idx:D:end-1, N*j+i-N+N^2:N^2:end) = -eye(m);
        key(idx, N*i+j-N) = 1;
        key(idx, N*j+i-N) = 1;
    end
end

% Process all matrices
for i = 1:m,
    current = M{i}';
    xeye = eye(n);
    
    % Pre-process all combinations
    cache = cell(n, 2*d); % cache(k,j)=((M*x)_k)^j
    for k = 1:n,
        idx = (current(:,k) ~= 0);
        basefactor = [current(idx, k), xeye(idx, :)];
        prevfactor = basefactor;
        cache{k, 1} = basefactor;
        for j = 2:2*d,
            newfactor = zeros(0, n+1);
            for m1 = 1:size(basefactor, 1),
                for m2 = 1:size(prevfactor, 1),
                    monomial = basefactor(m1, 2:end) + prevfactor(m2, 2:end);
                    coeffic = basefactor(m1, 1) * prevfactor(m2, 1);
                    [idx, insert] = findRow(newfactor(:, 2:end), monomial);
                    if (idx == 0),
                        newfactor = [newfactor(1:insert-1, :) ; coeffic, monomial ; newfactor(insert:end, :)];
                    else
                        newfactor(idx, 1) = newfactor(idx, 1) + coeffic;
                    end
                end
            end
            newfactor = newfactor(newfactor(:, 1) ~= 0, :);
            prevfactor = newfactor;
            cache{k, j} = newfactor;
        end
    end
    
    % Process all monomials
    for j = 1:D,
        processed = [1, zeros(1, n)];
        for k = find(monomials(j, :)),
            processing = zeros(0, n+1);
            procfactor = cache{k, monomials(j, k)};
            for m1 = 1:size(processed, 1),
                for m2 = 1:size(procfactor, 1),
                    monomial = processed(m1, 2:end) + procfactor(m2, 2:end);
                    coeffic = processed(m1, 1) * procfactor(m2, 1);
                    [idx, insert] = findRow(processing(:, 2:end), monomial);
                    if (idx == 0),
                        processing = [processing(1:insert-1, :) ; coeffic, monomial ; processing(insert:end, :)];
                    else
                        processing(idx, 1) = processing(idx, 1) + coeffic;
                    end
                end
            end
            processed = processing;
        end

        % Integration in the constraint
        for k = 1:size(processed, 1),
            idx = findRow(monomials, processed(k, 2:end));
            A(i*D-D+idx, 1:N^2) = A(i*D-D+idx, 1:N^2) - key(j, :) * processed(k, 1);
        end
    end
end

options.SDPsolver.solverOptions.fid = 0;
options.SDPsolver.solverOptions.maxiter = 100;
tolSolver = options.sos.tol/10;
options.SDPsolver.solverOptions.bigeps = 1e-3;
stop = 0;

msg(logFile,options.verbose>1,'\nStarting bisection method');
while (stop == 0 && iter < maxiter),
    iter = iter+1;
    valid = 0;
    
    if (iter>1000)
        if (mod(iter,100)==0)
            msg(logFile,options.verbose>0,'\n> Current upper-bound on JSR: %.15g (testing %.15g)',upper, lower, upper, candidate);
        end
    elseif (iter>150)
        if (mod(iter,50)==0)
            msg(logFile,options.verbose>0,'\n> Current upper-bound on JSR: %.15g (testing %.15g)',upper, lower, upper, candidate);
        end
    elseif (iter>20)
        if (mod(iter,10)==0)
            msg(logFile,options.verbose>0,'\n> Current upper-bound on JSR: %.15g (testing %.15g)',upper, lower, upper, candidate);
        end
    else
        msg(logFile,options.verbose>0,'\n> Current upper-bound on JSR: %.15g (testing %.15g)',upper, lower, upper, candidate);
    end

    [result,dual,feas,stopFlag] = solve_semi_definite_program(A,b,c,K,tolSolver,options);

    if (feas == 0),
        msg(logFile,options.verbose>1,'  Failure - Infeasible problem');
    elseif (stopFlag ~= 0),
        msg(logFile,options.verbose>1,'  Failure - Numerical problems');
    else
        msg(logFile,options.verbose>1,'  Success - Found feasible solution');
        valid = 1;
    end

    % Update the bounds
    if valid,
        upper = candidate;
    else
        lower = candidate;
    end

    % Stop or update the constraints
    if (upper-lower < upper*options.sos.tol),
        candidate = upper;
        
        % End of bisection loop
        stop = 1;
    else
        candidate = (upper+lower) / 2;
    end
    for i = 1:D,
        A(i:D:end-1, 1:N^2) = A(i:D:end-1, 1:N^2) + (candidate^(2*d) - factor) * repmat(key(i, :), m, 1);
    end
    factor = candidate^(2*d);
    
    % Save to file option
    if (ischar(options.saveinIt))
        if(iter==maxiter); status = 1; end;
        elapsedtime = cputime-starttime;
        
        info.status = status;
        info.elapsedtime = elapsedtime;
        info.bissec = [lower upper];
        info.niter = iter;
        info.opts = options;
        
        tempLowerSosEst = .999 * upper * min([m, nck(n+d-1, d)])^(-1/(2*d));
        bounds = [tempLowerSosEst upper];
        
        save([options.saveinIt '.mat'],'bounds','info'); 
    end
    
    if (cputime-starttime)>=options.maxTime
        msg(logFile,options.verbose>0,'\nopts.maxTime reached\n');
        break;
    end
    
end

if (iter==maxiter)
    status = 1;
elseif ((cputime-starttime)>=options.maxTime)
    status = 2;
else
    status = 0;
end

% Post-processing
result = solve_semi_definite_program(A,b,c,K,tolSolver,options);
P = mat(result(1:N^2), N);
%lowerJSR = max([.999 * upper * min([m, nck(n+d-1, d)])^(-1/(2*d)), lowerJSR]);
LowerSosEst = .999 * upper * min([m, nck(n+d-1, d)])^(-1/(2*d));
msg(logFile,options.verbose>0,'\n> Bounds on the jsr: [%.15g, %.15g]', LowerSosEst, upper);
bounds = [LowerSosEst, upper];
elapsedtime = cputime - starttime;

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

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

info.status = status;
info.elapsedtime = elapsedtime;
info.bissec = [lower upper];
info.niter = iter;
info.opts = options;

if (ischar(options.saveEnd))
    save([options.saveEnd '.mat'],'bounds','P','info')
end