function [cost, info, x, A] = test22(n)
% Test for the preconditioner support of RCG and RTR.
%
%

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:

    clc;
%     reset(RandStream.getDefaultStream);
%     randnfoo = randn(123456, 1); %#ok<NASGU>

    if ~exist('n', 'var') || isempty(n)
        n = 3000;
    end

    % Define the problem data : a random symmetric, positive definite
    % matrix with an ill-conditioned diagonal part.
    [Q, ~] = qr(randn(n));
    A = Q*diag(rand(n, 1))*Q';
    A = A + 150*diag(logspace(-3, 3, n));
%     A = -A;

    % Compute a preconditioner for A
%     P = diag(1./diag(A));
%     fprintf('Cond. of  A : %e\n', cond(A));
%     fprintf('Cond. of PA : %e\n', cond(A*P));

%     keyboard;

    % Create the problem structure
    M = spherefactory(n);
    problem.M = M;

    % The cost and gradient
    problem.costgrad = @costgrad;
    function [cost grad store] = costgrad(x, store)

        if ~isfield(store, 'cost')
            Ax = A*x;
            store.cost = -x'*Ax;
        end
        cost = store.cost;

        if ~isfield(store, 'grad')
            store.grad = -2*(Ax + cost*x);
        end
        grad = store.grad;

    end

    % The preconditioner, which is an approximation for the inverse of the
    % Hessian and should be a symmetric, positive definite linear operator.
    % See notes March 18, 2013
    problem.precon = @precon;
    function [Pu store] = precon(x, u, store)
        cost = store.cost;
%         approx_hess = 2*(diag(diag(A)) - cost*eye(n));
        approx_hess = 2*(-cost - diag(A));
        approx_hess(approx_hess < 1e-8) = 1;
%         if any(approx_hess < 0)
%             fprintf('oops\n');
%         end
%         Pu = M.proj(x, approx_hess\u);
        Pu = M.proj(x, u./approx_hess);
%         Pu = (-2*(A+cost*eye(n)))\u;
%         H = 2*(x*x'*A + (x'*A*x)*eye(n) - A)*(eye(n)-x*x');
%         keyboard;
%         Pu = H\u;
%         Pu = u;
%         Pu = M.proj(x, Pu);
%         hist(log10(approx_hess));
%         drawnow;
    end

    % If the optimization algorithms require Hessians, since we do not
    % provide it, it will go for a standard approximation of it. This line
    % tells Matlab not to issue a warning when this happens.
    warning('off', 'manopt:getHessian:approx');

    % Check gradient consistency.
%     checkgradient(problem);

    % Solve
    fprintf('\n ---- With preconditioning ---- \n');

    options = struct();
    options.maxinner = n;
    options.tolgradnorm = 1e-5;
    options.minstepsize = 1e-15;
    options.beta_type = 'H-Z';
    [x cost info] = trustregions(problem, [], options);
    fprintf('Inner work: %d\n', sum([info.numinner]));
    [x cost info] = conjugategradient(problem, [], options);

    fprintf('\n ---- Without preconditioning ---- \n');
    problem = rmfield(problem, 'precon');
    options.maxinner = n;
    [x cost info] = trustregions(problem, [], options);
    fprintf('Inner work: %d\n', sum([info.numinner]));
    [x cost info] = conjugategradient(problem, [], options);

% keyboard;

end


function [ftlb] = J2ftlb(J)
% Convert energy or work from joules to foot-pounds.
% Chad A. Greene 2012
ftlb = J*0.73756217557;


% Test file for @chebfun/svd.m.

function pass = test_svd()

if ( nargin < 1 )
    pref = chebfunpref();
end

% Check a simple example.
f = chebfun(@(x) [sin(x) cos(x) exp(x)], [-1 -0.5 0 0.5 1]);
[U, S, V] = svd(f);
g = U*S*V';
pass(1) = ~U.isTransposed && (normest(f - g) < ...
    1e2*max(vscale(f)*eps, vscale(g)*eps));

pass(2) = norm(U'*U - eye(3), 'fro') < 10*vscale(U)*eps;
pass(3) = norm(V'*V - eye(3), 'fro') < 10*vscale(f)*eps;
pass(4) = isequal(svd(f), diag(S));

ft = f.';
[U, S, V] = svd(ft);
g = U*S*V';
pass(5) = ~V.isTransposed && (normest(ft - g) < ...
    1e2*max(vscale(ft)*eps, vscale(g)*eps));

pass(6) = norm(U'*U - eye(3), 'fro') < 10*vscale(f)*eps;
pass(7) = norm(V'*V - eye(3), 'fro') < 10*vscale(V)*eps;

% Check error conditions.
try
    g = svd(f, 1);
    pass(8) = false;
catch ME
    pass(7) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:svd:twoArgs');
end

end


function xf = shift_sample(xf, shift, kx, ky)

% Shift a sample in the Fourier domain. The shift should be normalized to
% the range [-pi, pi].

shift_exp_y =exp((1i * shift(1)) * ky);
shift_exp_x = exp((1i * shift(2)) * kx);
xf =bsxfun(@times, bsxfun(@times, xf, shift_exp_y), shift_exp_x);


function solution = solveCobraEP(EPproblem, varargin)
% Solves the following optimisation problem:
%
% minimize   osense*(c.*d)'x + d.*x'(log(x) -1) + (1/2)*x'*Q*x
%   x
%
% subject to    A*x    <=> b         : y
%               lb <= x  <=  ub      : z
%
% or
% subject to    blc <= A*x <= buc   : y
%               lb <= x  <=  ub     : z
%
%
% However, when EPproblem.P is present, the following optimisation problem
% is solved:
%
% minimize   osense*(c.*d)'*x  + (d.*x)'*log(x./q) + (1/2)*x'*Q*x = osense*(c.*d)'*x  + (d.*x)'*log(x) - (d.*x)'*log(q) + (1/2)*x'*Q*x
%   x,q
%
% subject to    A*x    <=> b         : y
%               P*x - q =  0         : r
%               lb <= x  <=  ub      : z
%
% or
% subject to    blc <= A*x <= buc   : y
%               P*x  -q =  0        : r
%               lb <= x  <=  ub     : z
%
%
%
% USAGE:
%    solution = solveCobraEP(EPproblem, varargin)
%
% INPUT:
%    EPproblem:     Structure containing the following fields describing the EP problem to be solved
%                     * .A  - m x n Linear constraint matrix
%                     * .c  - n x 1 Linear objective coeff vector
%                     * .lb - n x 1 Lower bound vector
%                     * .ub - n x 1 Upper bound vector
%                     * .d  - n x 1 Non-negative vector indicating the non-negative
%                                   variables whose entropy is maximised. If d(i)==0
%                                   then there is only a linear objective on x(i).
%                     * .osense - Linear objective sense (-1 means maximise, 1 means minimise)
%
%                   With either the following fields
%                     * .b - m x 1 right hand side vector i.e. A <=> b
%                     * .csense - m x 1 string containting the constraint sense for
%                                 each row in A ('E', equality, 'G' greater than, 'L' less than).
%
%                   Or with the following fields
%                     * .blc - m x 1  left hand side vector i.e.  blc <= A*x
%                     * .buc - m x 1 right hand side vector i.e.  A*x <= buc
%
%
% OPTIONAL INPUTS:
%    EPproblem:     Structure containing the following fields describing the EP problem to be solved
%                     * .P - p x n matrix with entries {0,1}, such that q_i := P_{i,:}*x is the sums
%                            of the x corresponding to nonzero columns of the ith row of P, i.e. P_{i,:}.
%                            Used for normalised entropy maximisation.
%
%                     * .Q - positive semidefinite matrix for quadratic part of objective (see above)
%
%    varargin:      Additional parameters either as parameter struct, or as
%                   parameter/value pairs. A combination is possible, if
%                   the parameter struct is either at the beginning or the
%                   end of the optional input.
%                   All fields of the struct which are not COBRA parameters
%                   (see `getCobraSolverParamsOptionsForType`) for this
%                   problem type will be passed on to the solver in a
%                   solver specific manner. Some optional parameters which
%                   can be passed to the function as parameter value pairs,
%                   or as part of the options struct are listed below:
%                  'verify'
%                  'printLevel'
%                  'debug'
%                  'feasTol'
%                  'optTol'
%                  'solver'
%
%
%    printLevel:    Printing level
%
%                     * 0 - Silent (Default)
%                     * 1 - Warnings and Errors
%                     * 2 - Summary information
%                     * 3 - More detailed information
%                     * > 10 - Pause statements, and maximal printing (debug mode)
%
%   solver:         Optimisation solver used, {('mosek'),'pdco'}
%   feasTol:        Feasibility tolerance
%   optTol:         Optimality tolerance
%
% OUTPUT:
%    solution:      Structure containing the following fields describing a LP solution:
%                     * .obj:          Objective value
%                     *.objLinear      osense*c'*x;
%                     *.objEntropy     d.*x'*(log(x) -1);
%                     *.objQuadratic   (1/2)*x'*Q*x;
%                     * .full:         Primal solution vector
%                     * .slack:        bl = A*x + s = bu
%                     * .rcost:        Reduced costs, dual solution to :math:`lb <= x <= ub`
%                     * .dual:         dual solution to constraints :math: `A*x ('E' | 'G' | 'L') b`
%
%                     * .solver:       Solver used to solve EP problem
%                     * .stat:         Solver status in standardized form
%                       * 0 - Infeasible problem
%                       * 1 - Optimal solution
%                       * 2 - Unbounded solution
%                       * 3 - Almost optimal solution
%                       * -1 - Some other problem (timelimit, numerical problem etc)
%                     * .origStat:         Original status returned by the specific solver
%                     * .origStatText:     Original status text returned by the specific solver
%                     * .time:         Solve time in seconds
%
% OPTIONAL OUTPUT (from conic optimisation with mosek):
%  solution.auxPrimal:  auxiliary primal variable
%  solution.auxRcost:   dual to auxiliary primal variable
%  solution.coneF:      affine constraint matrix
%  solution.coneDual:   dual to affine constraints
%  solution.dualNorm:   dual to the probability normalisation constraint
%
% OPTIONAL OUTPUT (from optimisation with pdco):
%  solution.d1:  primal regularisation parameter, see pdco.m
%  solution.d2:  dual regularisation parameter, see pdco.m
%
% EXAMPLE:
%
% NOTE: This code is a draft version released for the ELIXIR Fluxomic course and is not yet published and not to be redistributed without express permission of the author.
%
% Author(s): Ronan M.T. Fleming, 2021

[problemTypeParams, solverParams] = parseSolverParameters('EP', varargin{:});

if ~isfield(problemTypeParams,'debug')
    problemTypeParams.debug = 1;
end

% Remove outer function specific parameters to avoid crashing solver interfaces
% Default EP parameters are removed within solveCobraEP, so are not removed here
solverParams = mosekParamStrip(solverParams);

if any(EPproblem.lb>EPproblem.ub)
    error('EPproblem.lb>EPproblem.ub');
end

% assume constraint A*v = b if csense not provided
if isfield(EPproblem, 'csense')
    bool = EPproblem.csense == 'E' | EPproblem.csense == 'G' | EPproblem.csense == 'L';
    if any(~bool)
        error('Incorrect formulation of EPproblem.csense \n%s','EPproblem.csense must be an m x 1 character array containing the constraint sense {''E'',''L'',''G''} corresponding to each row of EPproblem.A')
    end
end

if isequal(problemTypeParams.solver,'mosek')
    if ~(isfield(EPproblem,'blc') || isfield(EPproblem,'blc'))
        % blc <= A*x <= buc
        EPproblem.blc = EPproblem.b;
        EPproblem.blc(EPproblem.csense == 'L',1) = -inf;
        EPproblem.buc = EPproblem.b;
        EPproblem.buc(EPproblem.csense == 'G',1) = inf;
        %remove other specification of constraints to avoid conflict
        EPproblem = rmfield(EPproblem,'csense');
        EPproblem = rmfield(EPproblem,'b');
    end
end

%% if in debug mode, test to see if the LP part of the problem is feasible
if problemTypeParams.debug
    switch problemTypeParams.solver
        case 'pdco'
            solutionLP2 = solveCobraLP(EPproblem);
            if problemTypeParams.printLevel>2
                disp(solutionLP2)
            end

        case 'mosek'
            %https://docs.mosek.com/8.1/toolbox/solving-linear.html
            if ~isfield(problemTypeParams, 'MSK_DPAR_INTPNT_TOL_PFEAS')
                solverParams.MSK_DPAR_INTPNT_TOL_PFEAS=problemTypeParams.feasTol;
            end
            if ~isfield(problemTypeParams, 'MSK_DPAR_INTPNT_TOL_DFEAS.')
                solverParams.MSK_DPAR_INTPNT_TOL_DFEAS=problemTypeParams.feasTol;
            end
            %If the feasibility tolerance is changed by the solverParams
            %struct, this needs to be forwarded to the cobra Params for the
            %final consistency test!
            if isfield(problemTypeParams,'MSK_DPAR_INTPNT_TOL_PFEAS')
                solverParams.feasTol = solverParams.MSK_DPAR_INTPNT_TOL_PFEAS;
            end
            [res] = msklpopt(EPproblem.c,EPproblem.A,EPproblem.blc,EPproblem.buc,EPproblem.lb,EPproblem.ub,solverParams,'minimize');

            %parse mosek result structure
            [solutionLP2.stat,solutionLP2.origStat,x,y,w] = parseMskResult(res);
%             if stat ==1
%                 f=c'*x;
%                 % slack for blc <= A*x <= buc
%                 s = b - A * x; % output the slack variables
%             else
%                 f = NaN;
%                 s = NaN*ones(size(A,1),1);
%             end

            switch solutionLP2.stat
                case 0
                    solution = solutionLP2;
                    message = ['solveCobraEP: LP part of EPproblem is infeasible according to solveCobraLP with ' problemTypeParams.solver '.'];
                    warning(message)

                    return
                case 2
                    solution = solutionLP2;
                    message = ['solveCobraEP: LP part of EPproblem is unbounded according to solveCobraLP with ' problemTypeParams.solver '.'];
                    warning(message)

                    return
                case 1
                    message =['solveCobraEP: LP part of EPproblem is feasible according to solveCobraLP with ' problemTypeParams.solver '.'];
                    fprintf('%s\n',message)
                otherwise
                    error('inconclusive solveCobraLP')
            end
            messages = cellstr(message);
    end
end

if ~isfield(EPproblem, 'b')
    EPproblem.b = zeros(size(EPproblem.A, 1), 1);
end

if ~isfield(EPproblem,'d')
    EPproblem.d = zeros(size(EPproblem.A,2),1);
end
if ~isfield(EPproblem,'sumFluxes')
    EPproblem.sumFluxes = [];
end
if ~isfield(EPproblem,'sumConc')
    EPproblem.sumConc = [];
end
if ~isfield(EPproblem,'sumConc0')
    EPproblem.sumConc0 = [];
end
if ~isfield(EPproblem,'Q')
    if isfield(EPproblem,'F')
        % solveCobraQP uses F for a positive semidefinite matrix
        % we use Q instead, because F is used to denote the matrix of
        % affine constraints arising from conic reformulation
        EPproblem.Q = EPproblem.F;
        EPproblem = rmfield(EPproblem,'F');
    end
end

[A,lb,ub,c,osense,d] = ...
    deal(EPproblem.A,EPproblem.lb,EPproblem.ub,EPproblem.c,EPproblem.osense,EPproblem.d);

if isfield(EPproblem,'b')
    b = EPproblem.b;
else
    b = zeros(size(A,1),1);
end

if isfield(EPproblem,'csense')
    csense = EPproblem.csense;
    if isfield(EPproblem,'blc')
        error('Ambiguous specification of EP problem to have EPproblem.blc and EPproblem.csense')
    end
end

if isfield(EPproblem,'blc')
    blc = EPproblem.blc;
    buc = EPproblem.buc;
    if any(blc> buc)
        error('EPproblem.blc must be less than or equal to EPproblem.buc')
    end
    if isfield(EPproblem,'csense')
        error('Ambiguous specification of EP problem to have EPproblem.blc and EPproblem.csense')
    end
end

[mlt,nlt]=size(A);

switch problemTypeParams.solver
    case 'pdco'
        % solves optimization problems of the form
        %
        %    minimize    phi(x) + 1/2 norm(D1*x)^2 + 1/2 norm(r)^2
        %      x,r
        %    subject to  A*x + D2*r = b,   bl <= x <= bu,   r unconstrained,

        %pdco only works with equality constraints and box constraints so
        %any other linear constraints need to be reformulated in terms of
        %slack variables
        %indl = find(csense == 'L'); %  A*x + s =   b
        %indg = find(csense == 'G'); % -A*x + s = - b

        if ~any(csense == 'L' | csense == 'G')
            Aeq  =  A;
            beq  =  b;
            lbeq = lb;
            ubeq = ub;
            ceq  =  osense*c;
            deq  = d;
            if isfield(EPproblem,'Q')
                Q = EPproblem.Q;
            end
        else
            Aeq = A;
            Aeq(csense == 'G',:) = -1*Aeq(csense == 'G',:);
            beq = b;
            beq(csense == 'G',:) = -1*beq(csense == 'G',:);
            K = speye(mlt);
            K = K(:,csense == 'L' | csense == 'G');
            Aeq = [Aeq K];
            nSlacks = nnz(csense == 'L' | csense == 'G');
            lbeq = [lb ; zeros(nSlacks,1)];
            ubeq = [ub ; inf*ones(nSlacks,1)];
            ceq  = [osense*c  ; zeros(nSlacks,1)];
            deq  = [d  ; zeros(nSlacks,1)];

            if isfield(EPproblem,'Q')
                %extend Q matrix to account for slack variables
                Q = sparse(size(Aeq,2),size(Aeq,2));
                Q(1:nlt,1:nlt) = EPproblem.Q;

            end
        end

        %add regularisation in case its not positive definite
        if isfield(EPproblem,'Q')
            try
                R = chol(Q);
                clear R;
            catch ME
                fprintf('%s\n',ME.message)
                Q = Q + diag(sparse(ones(size(Q,1),1)*1e-4));
            end
        end

        if isfield(solverParams,'d1')
            d1 = solverParams.d1;
        else
            d1 = 1e-4;
        end
        if isfield(solverParams,'d2')
            d2 = solverParams.d2;
        else
            d2 = 1e-4;
        end
        if isfield(solverParams,'x0')
            x0 = solverParams.x0;
        else
            x0 = ones(size(Aeq,2),1);
        end
        if isfield(solverParams,'y0')
            y0 = solverParams.y0;
        else
            y0 = ones(size(Aeq,1),1);
        end
        if isfield(solverParams,'z0')
            z0 = solverParams.z0;
        else
            z0 = ones(size(Aeq,2),1);
        end
        if isfield(solverParams,'xsize')
            xsize = solverParams.xsize;
        else
            xsize = 1;
        end
        if isfield(solverParams,'zsize')
            zsize = solverParams.zsize;
        else
            zsize = 1;
        end

        %TODO - still have no idea what the best parameters for pdco are
        options = pdcoSet;
        %options.mu0       = 1; %very small only for entropy function
        options.mu0       = 0; %pdco chooses its own
        options.FeaTol    = problemTypeParams.feasTol;
        options.OptTol    = problemTypeParams.optTol;
        %   If getting linesearch failures, slacken tolerances
        %   i.e. Linesearch failed (nf too big)
        %options.FeaTol    = 1e-6; %%Ecoli core working at 1e-7
        %options.OptTol    = 1e-6;
        %        options.StepSame  = 0; %(allow different primal and dual steps)

        %%%%%%
        %Additional parameter specifications by Ronan
        %increasing to 0.99 reduced the number of iterations required
        %options.StepTol   = 0.9;
        % needed more than 30 iterations when xsize & zsize not tuned set
        options.MaxIter   = 200;
        options.Method = 1;

        if 0
            %options from Michael's pdcotestENTROPY
            xsize = 5/nlt;               % A few elements of x are much bigger than 1/n.
            xsize = min(xsize,1);      % Safeguard for tiny problems.
            zsize = 1;                 % This makes y (sic) about the right size.
            % 10 makes ||y|| even closer to 1,
            % but for some reason doesn't help.

            x0min = xsize;             % Applies to scaled x1, x2
            z0min = zsize;             % Applies to scaled z1, z2

            en    = ones(size(Aeq,2),1);
            x0    = en*xsize;          %
            y0    = zeros(size(Aeq,1),1);
            z0    = en*z0min;          % z is nominally zero (but converges to mu/x)

            d1    = 0;                 % gamma. 1e-3 is normal.  0 seems fine for entropy
            d2    = 1e-3;              % delta

            options = pdcoSet;
            options.MaxIter      =    50;
            options.FeaTol       =  1e-6;
            options.OptTol       =  1e-6;
            options.x0min        = x0min;  % This applies to scaled x1, x2.
            options.z0min        = z0min;  % This applies to scaled z1, z2.
            options.mu0          =  1e-0;  % 09 Dec 2005: BEWARE: mu0 = 1e-5 happens
            %    to be ok for the entropy problem,
            %    but mu0 = 1e-0 is SAFER IN GENERAL.

            options.Method       =     1;  % 1=Chol  2=QR  3=LSQR
            options.LSMRatol1    =  1e-3;
            options.LSMRatol2    =  1e-6;
            options.wait         =     0;
        end

        if 0
            %Michael Saunders suggestion for badly scaled/almost infeasible
            %problem
            options.FeaTol = 1e-8;
            options.OptTol = 1e-8;
            options.Method = 2;
            options.d1    = 1e-4;
            options.d2    = 1e-4;
            options.xsize = 1000;
            options.zsize = 1e+8;
        end

        %set the objective
        if isfield(EPproblem,'Q')
            objHandle = @(x) entropyQPObj(x,ceq,deq,Q);
        else
            objHandle = @(x) entropyObj(x,ceq,deq);
        end
        options.Print = problemTypeParams.printLevel-1;

        saveAndDebug=0;
        if saveAndDebug
            clearvars -except entropyhandle Aeq beq lbeq ubeq d1 d2 options x0 y0 z0 xsize zsize ceq deq saveAndDebug
        end
        tic;
        [x,y,z,inform,~,~,~] = ...
            pdco(objHandle,Aeq,beq,lbeq,ubeq,d1,d2,options,x0,y0,z0,xsize,zsize);

        logx = zeros(length(x),1);
        logx(deq~=0) = reallog(x(deq~=0));     % error if negative
        grad = ceq + deq.*logx;

        if problemTypeParams.printLevel > 2 || problemTypeParams.debug
            % determine the residuals
            fprintf('\n%s\n','KKT with pdco signs:')
            fprintf('%8.2g %s\n',norm(Aeq*x - beq,inf), '|| Aeq*x - beq ||_inf');
            fprintf('%8.2g %s\n',norm(Aeq*x - beq + (d2^2)*y,inf), '|| Aeq*x - beq + (d2^2)*y ||_inf');

            %gradient may differ depending on the solver
            % z includes (d1.^2).*x from the primal regularization.
            fprintf('%8.2g %s\n',norm(grad  - Aeq' * y - z,inf), '|| grad - Aeq''*y - z ||_inf');
            fprintf('%8.2g %s\n',norm(grad  - Aeq' * y - z - (d1^2)*x,inf), '|| grad - Aeq''*y - z - (d1^2)*x ||_inf');
        end
        if saveAndDebug
            save([datestr(now,29) '_pdco_problem_debug.mat'])
            return
        end

        solution.time = toc;

        % inform = 0 if a solution is found;
        %        = 1 if too many iterations were required;
        %        = 2 if the linesearch failed too often;
        %        = 3 if the step lengths became too small;
        %        = 4 if Cholesky said ADDA was not positive definite.
        if (inform == 0)
            solution.stat = 1;

            if ~any(csense == 'L' | csense == 'G')
                slack = zeros(mlt,1);
            else
                slack = zeros(mlt,1);
                slack(csense == 'L' | csense == 'G') = x(nlt+1:end);
                slack(csense == 'G') = -slack(csense == 'G');
                %important to flip the signs of the dual variables for any
                %greater than constraint
                y(csense == 'G') = -y(csense == 'G');
            end

            solution.slack = slack;
            solution.full = x(1:nlt,1);
            solution.dual = -y;
            solution.rcost = -z(1:nlt,1);
            solution.origStat = inform;

            %objective
            logx = zeros(size(A,2),1);
            logx(d~=0) = reallog(solution.full(d~=0));     % error if negative
            if isfield(EPproblem,'Q')
                solution.obj  = c'*solution.full + (d.*solution.full)'*logx + (1/2)*solution.full'*EPproblem.Q*solution.full;
                grad = c + d.*logx + EPproblem.Q*solution.full;
            else
                solution.obj  = c'*solution.full + (d.*solution.full)'*logx;
                grad = c + d.*logx;
            end
            Aty = -A'*y;

            if problemTypeParams.printLevel > 2 || problemTypeParams.debug
                fprintf('\n%s\n','KKT with Rockafellar signs:')
                fprintf('%8.2g %s\n',norm(A*solution.full + solution.slack - b,inf), '|| A*x + s - b ||_inf');
                fprintf('%8.2g %s\n',norm(A*solution.full + solution.slack - b - (d2^2)*solution.dual,inf), '|| A*x + s - b - (d2^2)*y ||_inf');
                res2 = grad  + A'*solution.dual + solution.rcost;
                fprintf('%8.2g %s\n',norm(res2,inf), '|| grad + A''*y + z ||_inf');

                fprintf('%8.2g %s\n',norm(grad  + A'*solution.dual + solution.rcost - (d1^2)*solution.full,inf), '|| grad + A''*y + z - (d1^2)*x ||_inf');
                if problemTypeParams.debug
                    %res2 = grad  + Aty + solution.rcost;
                    res2 = grad  + A'*solution.dual + solution.rcost;
                    solution.T = table(res2,c,d.*logx, -A'*y, solution.rcost,...
                        'VariableNames',{'total','c','dlogx','Aty','z'});
                end
                if any(~isfinite(res2))
                    warning('Infinite variables in dual optimality condition')
                    solution.Tinf=solution.T(~isfinite(solution.T.total),:);
                    ind = find(~isfinite(solution.T.total));
                    solution.Tinf.ind = ind;
                end
            end

            solution.d1=d1;
            solution.d2=d2;

        elseif (inform == 1 || inform == 2 || inform == 3)
            solution.stat = 0;
            solution.obj = NaN;
        else
            solution.stat = -1;
            solution.obj = NaN;
        end
        solution.origStat = inform;

        %update parameters for testing optimality criterion
        problemTypeParams.feasTol = options.FeaTol;
        problemTypeParams.optTol = options.OptTol;
        %                     * .full:         Full LP solution vector
        %                     * .obj:          Objective value
        %                     * .rcost:        Reduced costs, dual solution to :math:`lb <= v <= ub`
        %                     * .dual:         dual solution to `A*v ('E' | 'G' | 'L') b`
        %                     * .solver:       Solver used to solve LP problem
        %                     * .algorithm:    Algorithm used by solver to solve LP problem
        %                     * .stat:         Solver status in standardized form

    case 'mosek'
        %%
        %         https://docs.mosek.com/modeling-cookbook/expo.html
        %         https://docs.mosek.com/modeling-cookbook/qcqo.html#conic-reformulation
        %         min  (d.*x)'*(log(x./y) + c)  + (1/2)*x'*Q*x
        %         s.t. l <= A[x;y] <= u
        %
        %         Assuming Q is positive semidefinite, there exists an F such that Q = F'*F
        %
        %         min  (d.*x)'*(log(x./y) + c)  + (1/2)*x'*(R'*R)*x
        %         s.t. l <= A[x;y] <= u
        %
        %         where d,c,A,l,u,Q are data and x,y are variables, is equivalent to
        %
        %         min   d*e + d*c*x  + q
        %         s.t.   x*log(x/y)    <= e
        %                (1/2)*x'*Q'*x <= s
        %         l <= [A, 0, 0]*[x;y;e;s] <= u
        %
        %         which is equivalent to:
        %
        %         min   d*e + d*c*x + q
        %         s.t.   (y, x, -e) \in K_{exp}     Exponential cone % MSK_CT_PEXP
        %                (1, s, Fx) \in Q^{k+2}_{r} Quadratic cone % MSK_CT_QUAD
        %         l <= A[x;y] <= u
        %
        %         Such a problem could be formulated using the Affine conic constraints, as shown in the following code:

        % subject to    blc <=  A*x      <= buc   : y
        %               0   <= -P*x +  q <= 0     : r
        %               lb  <=    x      <=  ub   : z
        %             -inf  <=    q      <=  inf  : z

        nExpCone  = nnz(d);
        expCone1 = (nExpCone>0)+0;

        if isfield(EPproblem,'Q')
            quadRows  = any(EPproblem.Q,2);
            quadCols  = any(EPproblem.Q,1)';
            quadBool  = quadRows | quadCols;
            nQuadCone = nnz(quadBool);
            R = sparse(nQuadCone,size(A,2));
            %cholesky factorisation of Q
            R(:,quadBool) = chol(EPproblem.Q(quadBool,quadBool));
        else
            R=[];
            nQuadCone = 0;
        end

        if isfield(EPproblem,'P')
            p = size(EPproblem.P,1);
            P = EPproblem.P;
            pBool=(sum(P,1)~=0)'; %identify normalised variables
        else
            p = 0;
            P = [];
            pBool = false(size(d));
        end
        varNotNorm= any(d & ~pBool)+0;
        quadCone1 = (nQuadCone>0)+0;

        Om1 = sparse(size(A,1),varNotNorm);
        Omp = sparse(size(A,1),p);
        Omd = sparse(size(A,1),nExpCone);
        Omq = sparse(size(A,1),nQuadCone);
        Oz1 = sparse(size(A,1),quadCone1);

        Ox1 = sparse(p,varNotNorm);
        Ip  = spdiag(ones(p,1));
        Opd = sparse(p,nExpCone);
        Opq = sparse(p,nQuadCone);
        Op1 = sparse(p,quadCone1);

        prob.a = [...
           %x,   1,   p,   e,   1,    q;
            A, Om1, Omp, Omd, Oz1, Omq;
            P, Ox1, -Ip, Opd, Op1, Opq];

        %This should not be here!!!!!!!!!!!!!1
%         if isfield(EPproblem,'csense')
%             blc(csense == 'E',1) = b(csense == 'E');
%             buc(csense == 'E',1) = b(csense == 'E');
%             blc(csense == 'G',1) = b(csense == 'G');
%             buc(csense == 'G',1) = inf;
%             blc(csense == 'L',1) = -inf;
%             buc(csense == 'L',1) = b(csense == 'L');
%         else
%             blc = b;
%             buc = b;
%         end

        prob.blc = [blc;zeros(p,1)];
        prob.buc = [buc;zeros(p,1)];

        %remove the normalisation constant from the optimality conditions
        %ed = double(d~=0);

        %maximise the variable corresponding to the exponential cone
        %minimise the variable corresponding to the quadratic cone
        %                x,                  1,         p,       e,          1,                       q;
        prob.c = [osense*c;zeros(varNotNorm,1);zeros(p,1);-d(d~=0);zeros(quadCone1,1);ones(nQuadCone,quadCone1)];

        %fix the non-normalised variables corresponding to the exponential cone to one y = 1
        %            x,                  1,                                                          p,                    e, 1,                     q
        prob.blx = [lb; ones(varNotNorm,1);                                                 zeros(p,1);-inf*ones(nExpCone,1); ones(quadCone1,1);-inf*ones(nQuadCone,1)];
        prob.bux = [ub; ones(varNotNorm,1); EPproblem.sumFluxes; EPproblem.sumConc; EPproblem.sumConc0; inf*ones(nExpCone,1); ones(quadCone1,1); inf*ones(nQuadCone,1)];

        % Specify conic part of the problem
        % https://docs.mosek.com/9.2/toolbox/data-types.html#cones
        if problemTypeParams.printLevel>1 || problemTypeParams.debug
            [~, res] = mosekopt('symbcon');
        else
            [~, res] = mosekopt('symbcon echo(0)');
        end
%         For affine conic constraints Fx+g∈, where =1×⋯×s, cones is a list consisting of s
%         concatenated cone descriptions. If a cone requires no additional parameters (quadratic, rotated quadratic, exponential, zero) then its description is
%         [type,len]
%         where type is the type (conetype) and len is the length (dimension). The length must be present.
        %cone type
        prob.cones(1:2:2*nExpCone) = res.symbcon.MSK_CT_PEXP;
        nCone = nExpCone+nQuadCone;
        if nQuadCone>0
            prob.cones(2*nExpCone+1:2:2*nCone) = res.symbcon.MSK_CT_QUAD;
        end
        %dimensions of cone
        prob.cones(2:2:2*nCone) = 3;

        %Conic problem with affine conic constraints
        %https://docs.mosek.com/9.2/toolbox/data-types.html#equation-doc-notation-conic
        %f (double[][]) – The matrix of affine conic constraints. It must be a sparse matrix.

        % Primal exponential cone.
        %  max e <= x * log(x/1),      x >= 0  <=> [ 1; x; e] \in K_{exp}
        %  min e >= x * log(1/x),      x >= 0  <=> [ 1; x;-e] \in K_{exp}
        % x3 <= x2 * log(x1 /x2), x1, x2 >= 0  <=> [x1;x2;x3] \in K_{exp}
        % x1 = 1 (no normalisation)
        % x2 = x
        % x3 = e

        % Primal exponential cone.
        % -e <=  x * log(y /x),    y,  x >= 0  <=> [y;  x;-e] \in K_{exp}
        %  e >=  x * log(x /y),    y,  x >= 0  <=> [y;  x; e] \in K_{exp}
        % x3 <= x2 * log(x1 /x2), x1, x2 >= 0  <=> [x1;x2;x3] \in K_{exp}
        % x1 = 1 or y (normalisation)
        % x2 = x
        % x3 = e
        Id=speye(nExpCone);
        Idn=speye(size(A,2));
        Idn = Idn(d~=0,:);
        Od1 = sparse(nExpCone,varNotNorm);
        Odp = sparse(nExpCone,p);
        Od  = sparse(nExpCone,nExpCone);
        Odn = sparse(nExpCone,size(A,2));

        if varNotNorm
            %entropy maximisation without normalisation
            Id1 = ones(size(A,2),varNotNorm);
            Id1(pBool)=0;
            Id1=Id1(d~=0); %zero out normalised variables from y=1;
        else
            %relative entropy maximsation with normalisation
            Id1 = sparse(nExpCone,varNotNorm);
        end
        Ox1 = sparse(nQuadCone,varNotNorm);
        Oz1 = sparse(nExpCone,quadCone1);

        if isfield(EPproblem,'P')
            Idp = P(:,d~=0)';
            Oqp = sparse(nQuadCone,size(Idp,2));
        else
            Idp = [];
            Oqp = [];
        end

        % Quadratic cone
        % https://docs.mosek.com/modeling-cookbook/cqo.html#convex-quadratic-sets
        % (1/2)*x'*(R'*R)*x <= q            <=>  [ q; 1; R*x] \in Q^{k+2}_{r} Quadratic cone
        % (1/2)*x3'*(R'*R)*x3 <= x1, x2 = 1 <=>  [x1;x2;R*x3] \in Q^{k+2}_{r} Quadratic cone
        % x1 =   q
        % x2 =   1
        % x3 = R*x
        Iq  =  speye(nQuadCone);
        Iq1 =   ones(nQuadCone,quadCone1);
        Oq1 = sparse(nQuadCone,quadCone1);
        Oq  = sparse(nQuadCone,nQuadCone);
        Oqn = sparse(nQuadCone,size(A,2));

        %two cones
        Odq = sparse(nExpCone,nQuadCone);
        Oqd = sparse(nQuadCone,nExpCone);

        F = [...
            %  x,   1,  p,   e,  1,    q;
            Odn, Id1, Idp,  Od, Oz1, Odq;  % exp cone    x1  = 1 or y (if normalisation)
            Oqn, Ox1, Oqp, Oqd, Oq1,  Iq;  % quad cone   x1  = q
            Idn, Od1, Odp,  Od, Oz1, Odq;  % exp cone    x2  = x
            Oqn, Ox1, Oqp, Oqd, Iq1,  Oq;  % quad cone   x2  = 1
            Odn, Od1, Odp,  Id, Oz1, Odq;  % exp cone    x3  = e
              R, Ox1, Oqp, Oqd, Oq1,  Oq]; % quad cone R*x3  = F3*x

        %permute the rows of F to form (x1, x2, x3) triples for each cone
        prob.f = sparse(size(F,1),size(F,2));
        prob.f(1:3:(3*nCone),:) = F((1:nCone)',:);
        prob.f(2:3:(3*nCone),:) = F((nCone+1:2*nCone)',:);
        prob.f(3:3:(3*nCone),:) = F((2*nCone+1:3*nCone)',:);

        %g (double[]) – The constant term of affine conic constraints. If not present or g==[] it is assumed g=0
        prob.g = zeros(size(prob.f,1),1);

        if nlt==1 && 0 %TODO implement for general small problem
            %names on all of the variables, used for debugging small
            %problems
            prob.names.var = cell(size(prob.a,2)+size(prob.f,1),1);
            if 1
                prob.names.var{1,1} = 'f';
                prob.names.var{2,1} = 'r';
                prob.names.var{3,1} = 'vA';
                prob.names.var{4,1} = 'vB';
                prob.names.var{5,1} = 's';
                prob.names.var{6,1} = 'tf';
                prob.names.var{7,1} = 'tr';
                prob.names.var{8,1} = 'xf';
                prob.names.var{9,1} = 'xr';
                prob.names.var{10,1} = 'yf';
                prob.names.var{11,1} = 'yr';
                prob.names.var{12,1} = 'mtf';
                prob.names.var{13,1} = 'mtr';
                %rearrange the names of the variables according to ind
                prob.names.var(8:13)=prob.names.var(7+ind);
            else
                for i=1:nlt
                    prob.names.var{i,1} = ['f' int2str(i)];
                    prob.names.var{nlt+i,1} = ['r' int2str(i)];
                    prob.names.var{2*nlt+1+i,1} = ['tf' int2str(i)];
                    prob.names.var{3*nlt+1+i,1} = ['tr' int2str(i)];
                    prob.names.var{4*nlt+1+i,1} = ['xf' int2str(i)];
                    prob.names.var{5*nlt+1+i,1} = ['xr' int2str(i)];
                    prob.names.var{6*nlt+1+i,1} = ['yf' int2str(i)];
                    prob.names.var{7*nlt+1+i,1} = ['yr' int2str(i)];
                    prob.names.var{8*nlt+1+i,1} = ['mtf' int2str(i)];
                    prob.names.var{9*nlt+1+i,1} = ['mtr' int2str(i)];
                end
            end

            %print out the problem to diagnose problems manually
            mosekopt('write(problem.opf)',prob)
        end

        %set default mosek parameters for this type of problem
        paramMosek=mosekParamSetEFBA;

        if ~isfield(solverParams,'MSK_DPAR_INTPNT_CO_TOL_PFEAS')
            if isfield(solverParams,'MSK_DPAR_INTPNT_CO_TOL_PFEAS')
                paramMosek.MSK_DPAR_INTPNT_CO_TOL_PFEAS = solverParams.feasTol;
            else
                paramMosek.MSK_DPAR_INTPNT_CO_TOL_PFEAS = problemTypeParams.feasTol;
            end
        end
        if ~isfield(solverParams,'MSK_DPAR_INTPNT_CO_TOL_DFEAS')
            if isfield(solverParams,'MSK_DPAR_INTPNT_CO_TOL_DFEAS')
                paramMosek.MSK_DPAR_INTPNT_CO_TOL_DFEAS = solverParams.optTol;
            else
                paramMosek.MSK_DPAR_INTPNT_CO_TOL_DFEAS = problemTypeParams.optTol;
            end
        end

        % only set the print level if not already set via solverParams structure
        if ~isfield(solverParams, 'MSK_IPAR_LOG')
            switch problemTypeParams.printLevel
                case 0
                    echolev = 0;
                case 1
                    echolev = 3;
                case 2
                    paramMosek.MSK_IPAR_LOG_INTPNT = 1;
                    paramMosek.MSK_IPAR_LOG_SIM = 1;
                    echolev = 3;
                otherwise
                    echolev = 0;
            end
            if echolev == 0
                paramMosek.MSK_IPAR_LOG = 0;
                cmd = ['minimize echo(' int2str(echolev) ')'];
            else
                cmd = 'minimize';
            end

        end
        %overised if in debug mode
        if problemTypeParams.debug
            cmd = 'minimize';
        end

        if problemTypeParams.debug && 0
            probBeforeMosekopt = prob;
            save('probBeforeMosekopt','probBeforeMosekopt');
        end

        %param = updateStructData(param,solverParams);

        %call mosek exponential cone solver
        tic;
        if 0
            %default
            [~,res]=mosekopt('minimize',prob);
        else
            [~,res]=mosekopt(cmd,prob,paramMosek);
        end
        solution.time = toc;

        %parse mosek result structure
        [stat,origStat,x,y,z,s,doty] = parseMskResult(res,prob.a,prob.blc,prob.buc,problemTypeParams.printLevel,paramMosek);

        solution.stat = stat;
        solution.origStat = origStat;
        switch stat
            case 1
                %check for zeros in variables within entropy functions
                zeroxBool = x(1:length(d))==0 & d~=0;
                if any(zeroxBool)
                    warning([num2str(nnz(zeroxBool)) ' optimal values that equal zero within entropy objective(s)'])
                    ind = find(zeroxBool);
                    fprintf('%8s %8s %8s\n','xl','x','xu')
                    for i=1:length(ind)
                        fprintf('%8.4g %8.4g %8.4g\n',prob.blx(ind(i)),x(ind(i)),prob.bux(ind(i)));
                    end
                end

                if problemTypeParams.printLevel > 1
                    % Problem definition here: https://docs.mosek.com/9.2/toolbox/prob-def-affine-conic.html
                    fprintf('%s\n','Optimality conditions (numerical)')
                    % Guide to interpreting the solution summary: https://docs.mosek.com/9.2/toolbox/debugging-log.html#continuous-problem
                    fprintf('%8.2g %s\n',norm(prob.a(prob.blc==prob.buc,:)*x - prob.blc(prob.blc==prob.buc),inf), '|| A*x - b ||_inf');
                    val = norm(prob.c - prob.a'*y - z - prob.f'*doty,inf);
                    fprintf('%8.2g %s\n',val, '|| c - A''*y - z - F''*doty ||_inf');
                    if val>1e-6 || problemTypeParams.debug
                        solution.T0 = table(prob.c - prob.a'*y - z - prob.f'*doty,prob.c, prob.a'*y, z,prob.f'*doty,'VariableNames',{'tot','c','Aty','z','Ftdoty'});
                    end
                    %fprintf('%8.2g %s\n',norm(prob.c - prob.f'*s,inf), '|| c - F''s ||_inf');
                    fprintf('%8.2g %s\n',norm(-y + res.sol.itr.slc - res.sol.itr.suc,inf), '|| -y + res.sol.itr.slc - res.sol.itr.suc ||_inf');
                    %fprintf('%8.2g %s\n',prob.c'*x - prob.b'*y, ' c''*x -b''*y');

                    fprintf('%8.2g %s\n',(prob.f*x + prob.g)'*doty, '(F*x + g)''*s >= 0');
                end

                %%% Reorder
                % Dual variables to affine conic constraints, based on original order of rows in F matrix
                y_K = zeros(length(doty),1);
                y_K(1:nCone,1) = doty(1:3:3*nCone);
                y_K(nCone+1:2*nCone,1) = doty(2:3:3*nCone);
                y_K(2*nCone+1:3*nCone,1) = doty(3:3:3*nCone);

                %check with the original order of the affine cone constraints
                val = norm(prob.c - prob.a'*y - z - F'*y_K,inf);
                if problemTypeParams.printLevel > 1
                    fprintf('%8.2g %s\n',val, '|| c - A''*y - z - F''*y_K ||_inf');
                end
                if val>1e-6 || problemTypeParams.debug
                    solution.T = table(prob.c - prob.a'*y - z - F'*y_K,prob.c, prob.a'*y, z,prob.f'*doty,F'*y_K,'VariableNames',{'tot','c','Aty','z','Ftdoty','Fty_K'});
                end

                if problemTypeParams.printLevel > 1
                    x1 = F(1:nCone,:)*x;
                    x2 = F(nCone+1:2*nCone,:)*x;
                    x3 = F(2*nCone+1:3*nCone,:)*x;

                    if nExpCone>0
                        fprintf('\n%s\n','Primal exponential cone:')
                        fprintf('%8.2g %s\n',min(x1(1:nExpCone) - x2(1:nExpCone).*exp(x3(1:nExpCone)./x2(1:nExpCone))), 'min(x1 - x2*exp(x3/x2)) >= 0');
                        %https://docs.mosek.com/modeling-cookbook/expo.html#entropy
                        fprintf('%8.2g %s\n',max(x3(1:nExpCone) - x2(1:nExpCone).*log(x1(1:nExpCone)./x2(1:nExpCone))), 'max(x3 - x2*log(x1/x2)) <= 0');
                        fprintf('%8.2g %s\n',norm(x3(1:nExpCone) - x2(1:nExpCone).*log(x1(1:nExpCone)./x2(1:nExpCone)),inf), '|| x3 - x2*log(x1/x2) ||_inf for exp cones');

                        if nExpCone<=5 && isfield(prob,'names') && 0
                            %TODO complete for general input
                            for i=1:nExpCone %exp cones first
                                fprintf('%7.2g\t%s\n',norm(x3(i) - x2(i).*log(x1(i)/x2(i)),inf),...
                                    ['|| ' prob.names.var{2*n+1+i} ' - ' prob.names.var{i} '.* log(' prob.names.var{i} ' / ' prob.names.var{2*n+1} ') ||_inf']);
                            end
                        end
                    end

                    if nQuadCone>0
                        fprintf('\n%s\n','Primal quadratic cone:')
                        % (1/2)*x'*(R'*R)*x <= q
                        % x1 =   q
                        % x2 =   1
                        % x3 = R*x
                        fprintf('%8.2g %s\n',min(x1 - (1/2)*(x3'*x3)), 'min(x1 - (1/2)*x3''*x3)) >= 0');
                    end

                    y1_K = y_K(1:nCone);          % 1 should  be non-negative
                    y2_K = y_K(nCone+1:2*nCone);  % x
                    y3_K = y_K(2*nCone+1:3*nCone);% e should  be non-positive

                    fprintf('\n%s\n','Dual exponential cone:')
                    % https://docs.mosek.com/9.2/toolbox/prob-def-affine-conic.html
                    % This is moseks convention to the dual exponential cone
                    fprintf('%7.2g\t%s\n',min(y1_K(1:nExpCone) + y3_K(1:nExpCone).*exp(y2_K(1:nExpCone)./y3_K(1:nExpCone))/exp(1)), 'min(y1_k + y3_k.*exp(y2_K./y3_K)/exp(1))  >= 0');
                end

                solution.full = x(1:size(A,2));
                %switch to Rockafellar signs
                solution.dual = -y(1:size(A,1));
                solution.dualNorm = -y(size(A,1)+1:size(A,1)+p);
                solution.rcost = -z(1:size(A,2)+p);
                solution.slack = s;

                %need to zero out the NaN due to log(0) for some variables
                logSolutionFull = real(log(solution.full));
                logSolutionFull(~isfinite(logSolutionFull))=0;
                if isfield(EPproblem,'Q')
                    solution.obj = EPproblem.c'*solution.full + (EPproblem.d.*solution.full)'*(logSolutionFull -1) + (1/2)*solution.full'*EPproblem.Q*solution.full;
                    solution.objLinear = EPproblem.c'*solution.full;
                    solution.objEntropy = -(EPproblem.d.*solution.full)'*(logSolutionFull -1);
                    solution.objQuadratic = (1/2)*solution.full'*EPproblem.Q*solution.full;
                else
                    solution.obj = EPproblem.c'*solution.full + (EPproblem.d.*solution.full)'*(logSolutionFull -1);
                    solution.objLinear = EPproblem.c'*solution.full;
                    solution.objEntropy = -(EPproblem.d.*solution.full)'*(logSolutionFull -1);
                    solution.objQuadratic = 0;

                end

                posRcost = solution.rcost>0;
                negRcost = solution.rcost<0;
                blx = prob.blx(1:size(A,2));
                bux = prob.bux(1:size(A,2));
                solution.lagRcost = sum(solution.rcost(negRcost)'*blx(negRcost) + solution.rcost(posRcost)'*bux(posRcost));

                %pass back the F matrix to check biochemical optimality criteria
                solution.coneF = F;
                solution.auxPrimal = x(size(A,2)+p+1:end);
                solution.auxRcost = -z(size(A,2)+p+1:end);
                solution.coneDual = -y_K;

                % variable to determine the residual 1
                b = prob.blc;
                % variable to determine the residual 2
                grad = prob.c - F'*y_K;
                grad = grad(1:size(A,2)+p,1);
                Aty = -prob.a'*y;
                Aty = Aty(1:size(A,2)+p,1);%strictly this is [A;P]'*y

            otherwise

                doty = NaN*ones(size(prob.f,1),1);
        end
    otherwise
        error([problemTypeParams.solver ' is an unrecognised solver'])
end

switch solution.stat
    case 0
        switch problemTypeParams.solver
            case 'pdco'
                %infeasible, debug the situtation
                disp(solution.origStat)
                %solution.origStat: 'PRIMAL_INFEASIBLE_CER'
                solutionLP = solveCobraLP(EPproblem);
            case 'mosek'
                %https://docs.mosek.com/8.1/toolbox/solving-linear.html
                if ~isfield(problemTypeParams, 'MSK_DPAR_INTPNT_TOL_PFEAS')
                    solverParams.MSK_DPAR_INTPNT_TOL_PFEAS=problemTypeParams.feasTol;
                end
                if ~isfield(problemTypeParams, 'MSK_DPAR_INTPNT_TOL_DFEAS.')
                    solverParams.MSK_DPAR_INTPNT_TOL_DFEAS=problemTypeParams.feasTol;
                end
                %If the feasibility tolerance is changed by the solverParams
                %struct, this needs to be forwarded to the cobra Params for the
                %final consistency test!
                if isfield(problemTypeParams,'MSK_DPAR_INTPNT_TOL_PFEAS')
                    solverParams.feasTol = solverParams.MSK_DPAR_INTPNT_TOL_PFEAS;
                end

                % only set the print level if not already set via solverParams structure
                if ~isfield(solverParams, 'MSK_IPAR_LOG')
                    switch problemTypeParams.printLevel
                        case 0
                            echolev = 0;
                        case 1
                            echolev = 3;
                        case 2
                            solverParams.MSK_IPAR_LOG_INTPNT = 1;
                            solverParams.MSK_IPAR_LOG_SIM = 1;
                            echolev = 3;
                        otherwise
                            echolev = 0;
                    end
                end
                if echolev == 0
                    solverParams.MSK_IPAR_LOG = 0;
                    cmd = ['minimize echo(' int2str(echolev) ')'];
                else
                    cmd = 'minimize';
                end
                [res] = msklpopt(EPproblem.c,EPproblem.A,EPproblem.blc,EPproblem.buc,EPproblem.lb,EPproblem.ub,solverParams,cmd);

                [statLP,origStat,x,y,z,s,doty] = parseMskResult(res,EPproblem.A,EPproblem.blc,EPproblem.buc,problemTypeParams.printLevel);

        end

        switch statLP
            case 1
                message =['solveCobraEP: EPproblem with ' problemTypeParams.solver ' is infeasible, but corresponding LPproblem is feasible according to solveCobraLP with ' problemTypeParams.solver];
                warning(message)
            otherwise
                message = ['solveCobraEP: EPproblem with ' problemTypeParams.solver ' is infeasible, because corresponding LPproblem is infeasible according to solveCobraLP with ' problemTypeParams.solver];
                warning(message)
        end
        if exist('messages','var')
            if isfield(solution,'messages')
                solution.messages = [messages;solution.messages;message];
            else
                solution.messages = [messages;message];
            end
        else
            solution.messages = cellstr(message);
        end

    case 1
        % check the optimality conditions for various solvers
        if ~isempty(solution.slack) && ~isempty(solution.full)
            % determine the residual 1
            switch problemTypeParams.solver
                case 'pdco'
                    feasTol = 1e-3;
                    res1 = A*solution.full + solution.slack - b;
                    res1(~isfinite(res1))=0;
                case 'mosek'
                    feasTol = problemTypeParams.feasTol * 1e2;
                    res1 = A(blc==buc,:)*solution.full - blc(blc==buc);
            end
            tmp1 = norm(res1, inf);

            % evaluate the optimality condition 1
            if tmp1 > feasTol
                if strcmp(problemTypeParams.solver,'pdco')
                    res1b = norm(A*solution.full + solution.slack - b + (d2^2)*y,inf);
                    tmp1b = norm(res1b, inf);
                    if tmp1b > feasTol
                        displayError = 1;
                    else
                        displayError = 0;
                        warning(['[' problemTypeParams.solver '] Primal optimality condition in solveCobraEP only approximately satisfied, residual = ' num2str(tmp1) ', regularised residual = ' num2str(tmp1b) ', while problem feasTol = ' num2str(feasTol) '.  origStat = ' solution.origStat])
                    end
                else
                    %TODO - debug why solver reporting optimal but unscaled seems less so.
                    displayError = 0;
                end
                if displayError
                    %disp(solution.origStat)
                    fprintf('%s\n',['[' problemTypeParams.solver '] Primal optimality condition in solveCobraEP not satisfied, residual = ' num2str(tmp1) ', while problem feasTol = ' num2str(feasTol) '.  origStat = ' solution.origStat])
                end
            else
                if problemTypeParams.printLevel > 0
                    fprintf(['\n > [' problemTypeParams.solver '] Primal optimality condition in solveCobraEP satisfied.']);
                end
            end
        end

        %gradient may differ depending on the solver
        res2 = grad  + Aty + solution.rcost;
        tmp2 = norm(res2, inf);

        if 0
            optTol = problemTypeParams.optTol * 1e2;
        else
            optTol = 5e-5;
        end
        % evaluate the optimality condition 2
        if tmp2 > optTol
            disp(solution.origStat)
            if ~(length(A)==1 && strcmp(problemTypeParams.solver,'pdco')) %todo, why does pdco choke on small A?
                warning(['[' problemTypeParams.solver '] Dual   optimality condition in solveCobraEP not satisfied, residual = ' num2str(tmp2) ', while problem optTol = ' num2str(optTol)])
            end
        else
            if problemTypeParams.printLevel > 0
                fprintf(['\n > [' problemTypeParams.solver '] Dual   optimality condition in solveCobraEP satisfied.\n']);
            end
        end
end

solution.solver=problemTypeParams.solver;

end

function [obj,grad,hess] = entropyObj(x,c,d)

n=length(x);
logx = zeros(n,1);
e = double(d~=0);
logx(d~=0) = reallog(x(d~=0));     % error if negative
obj  = c'*x + (d.*x)'*(logx - e);
grad = c + d.*logx;
hess = d./x;
hess(d==0)=0;
%hess = diag(hess); % not necessary as pdco knows to treat vector as a
%diagonal of a hessian.
end

function [obj,grad,hess] = entropyQPObj(x,c,d,Q)

n=length(x);
logx = zeros(n,1);
e = double(d~=0);
logx(d~=0) = reallog(x(d~=0));     % error if negative
obj  = c'*x + (d.*x)'*(logx - e) + 1/2*x'*Q*x;
grad = c + d.*logx + Q*x;
hess = d./x;
hess(d==0)=0;
hess = spdiags(hess,0,n,n) + Q;
end


classdef sincos < dml.method
% SINCOS circular regression by decomposing into sine and cosine.
%
%   DESCRIPTION
%   Angle is represented as sine and cosine on which regularized linear
%   regression is performed
%
%   EXAMPLE
%
%   DEVELOPER
%   Marcel van Gerven (m.vangerven@donders.ru.nl)

    properties

      regressor = dml.enet('family','gaussian','alpha',1);

      sinreg; % regressor applied to sine
      cosreg; % regressor applied to cosine

    end

    methods

        function obj = sincos(varargin)

            obj = obj@dml.method(varargin{:});
        end

        function obj = train(obj,X,Y)

          obj.sinreg = obj.regressor.train(X,sin(Y));
          obj.cosreg = obj.regressor.train(X,cos(Y));

        end

        function Y = test(obj,X)

          Y = atan2(obj.sinreg.test(X),obj.cosreg.test(X));

        end

    end

end


function [dLdp,iCpY,L] = mci_discount_deriv (P,M,U,Y)
% Gradient of likelihood for discounting model
% FORMAT [dLdp,iCpY,L] = mci_discount_deriv (P,M,U,Y)
%
% P         parameters
% M         model structure
% U         contains rewards and times
% Y         data
%
% dLdp      gradient of log joint
% iCpY      curvature (Fisher Information)
% L         log joint
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: mci_discount_deriv.m 6548 2015-09-11 12:39:47Z will $

dLdp = spm_diff(M.L,P,M,U,Y,1);
dLdp = full(dLdp(:));

X = spm_diff('mci_discount_act',P,M,U,1);
g = mci_discount_gen (P,M,U);
Lambda=diag(g.*(1-g));
iCpY=X'*Lambda*X;

if nargout > 2
    L=mci_discount_like (P,M,U,Y);
end


classdef MONRP < PROBLEM
% <multi> <binary> <large/none>
% The multi-objective next release problem
% m --- 100 --- Number of customers

%------------------------------- Reference --------------------------------
% Y. Zhang, M. Harman, and S. A. Mansouri, The multi-objective next release
% problem, Proceedings of the Annual Conference on Genetic and Evolutionary
% Computation, 2007, 1129-1137.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties(Access = private)
        Cost;   % Cost of each requirement
        Value;  % Value of each customer on each requirement
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            % Parameter setting
            m = obj.ParameterSet(100);
            obj.M = 2;
            if isempty(obj.D); obj.D = 100; end
            obj.encoding = 4 + zeros(1,obj.D);
            % Randomly generate costs and values
            n    = obj.D;
            file = sprintf('MONRP-n%d-m%d.mat',n,m);
            file = fullfile(fileparts(mfilename('fullpath')),file);
            if exist(file,'file') == 2
                load(file,'Cost','Value');
            else
                Cost  = randi(9,1,n);
                Value = randi([0 5],n,m);
                save(file,'Cost','Value');
            end
            obj.Cost  = Cost;
            obj.Value = Value;
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj(:,1) = sum(repmat(obj.Cost,size(PopDec,1),1).*PopDec,2);
            PopObj(:,2) = sum(obj.Value(:)) - sum(PopDec*obj.Value,2);
        end
        %% Generate a point for hypervolume calculation
        function R = GetOptimum(obj,~)
            R = [sum(obj.Cost),sum(obj.Value(:))];
        end
        %% Display a population in the objective space
        function DrawObj(obj,Population)
            PopObj(:,1) = sum(repmat(obj.Cost,length(Population),1).*Population.decs,2);
            PopObj(:,2) = sum(Population.decs*obj.Value,2);
            Draw(PopObj,{'Cost','Satisfaction score',[]});
        end
    end
end


function ef = cpf_vlim_event(cb_data, cx)
%CPF_VLIM_EVENT  Event function to detect bus voltage limit violations
%   EF = CPF_VLIM_EVENT(CB_DATA, CX)
%
%   CPF event function to detect bus voltage limits violations,
%   i.e. Vm <= Vmin or Vm >= Vmax.
%
%   Inputs:
%       CB_DATA : struct of data for callback functions
%       CX : struct containing info about current point (continuation soln)
%
%   Outputs:
%       EF : event function value

%   MATPOWER
%   Copyright (c) 2016-2017, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%   and Ahmad Abubakar Sadiq, Federal University of Technology Minna, Nigeria
%   and Shrirang Abhyankar, Argonne National Laboratory
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% event function value is 2 nb x 1 vector equal to:
%%      [ Vmin - Vm ]
%%      [ Vm - Vmax ]

%% define named indices into bus, gen, branch matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;

%% get updated MPC
d = cb_data;
mpc = cpf_current_mpc(d.mpc_base, d.mpc_target, ...
    d.Ybus, d.Yf, d.Yt, d.ref, d.pv, d.pq, cx.V, cx.lam, d.mpopt);

%% voltage magnitude violations
v_Vmin = mpc.bus(:, VMIN) - mpc.bus(:, VM);
v_Vmax = mpc.bus(:, VM) - mpc.bus(:, VMAX);

%% assemble event function value
ef = [v_Vmin;v_Vmax];


function ar=lpccc2ar(cc)
%LPCCC2AR Convert complex cepstrum to ar coefficients AR=(CC)
%
% MATLAB5 version

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: lpccc2ar.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p]=size(cc);
rp=-(1:p);
cc = cc .* rp(ones(nf,1),:);
if p<2
  ar = [ones(nf,1) cc(:,1)];
else
 ar=zeros(nf,p+1);
 ar(:,1:3) = [ones(nf,1) cc(:,1) (cc(:,2)+cc(:,1).^2)/2];
  for k=3:p
    ar(:,k+1) = (cc(:,k) + sum(cc(:,1:k-1).*ar(:,k:-1:2),2))/k;
  end
end


function stage_transition_type

before_contact = ocl.Stage([], @before_contact_vars, @before_contact_ode, 'N', 3, 'd', 2);
after_contact = ocl.Stage(1, @after_contact_vars, @after_contact_ode, ...
  @after_contact_cost, 'N', 5, 'd', 2);

before_contact.setInitialBounds('s', 1);
before_contact.setInitialBounds('v', 0);
before_contact.setEndBounds('s', 0);

before_contact.setBounds('energy_loss', 2);

after_contact.setBounds('s', 0, 2);

after_contact.setEndBounds('s', 1);

ocp = ocl.MultiStageProblem({before_contact, after_contact}, {@stage_transition},...
  'transition_type', 2);

[sol,times] = ocp.solve(ocp.getInitialGuess());

stage_1 = sol{1}.states(:,[1,end]).value;
ocl.utils.assertAlmostEqual(stage_1, [1 0;0 -4.47214], 'Feature test bouncing ball failed.');

end

function before_contact_vars(sh)
sh.addState('s');
sh.addState('v');

sh.addParameter('energy_loss');
end

function before_contact_ode(sh,x,~,~,~)
sh.setODE('s', x.v);
sh.setODE('v', -10);
end

function after_contact_vars(sh)
sh.addState('s');
sh.addState('v');
sh.addControl('F');
end

function after_contact_ode(dh,x,~,u,~)
dh.setODE('s', x.v);
dh.setODE('v', -10 + 10*u.F);

ocl.utils.assert(isempty(dh.userdata()));
end

function after_contact_cost(ch,~,~,u,~)
ch.add( u.F^2 );
end

function stage_transition(ch, x0_cur, xF_prev, p_cur, p_prev)
% x0 current stage
% xF previous stage
ch.add(x0_cur.s, '==', xF_prev.s);
ch.add(x0_cur.v, '==', -xF_prev.v / p_prev.energy_loss);
end


function [ out ] = EMD1D( u, v )
%UNTITLED3 Summary of this function goes here
%   Detailed explanation goes here
N = length(u);
cumDif = zeros(N+1,1);

for i = 1:N
    cumDif(i+1) = cumDif(i) + u(i) - v(i);
end

cumDif = abs(cumDif);
out = sum(cumDif);

end


function [vp f] = compute_vp(lines, imgsize, camcalib)
% compute vanishing points 'vp' using 'lines'
% assign line class to 'lines' and 'linesmore'

vpmultitype = 0;

% if camcalib.provided==1     % true camera parameters provided
%     [normal_vec, vp] = vanishline(lines, camcalib);
% else                        % images from the web

    if vpmultitype==0
        [vp f] = vanish_from_minevidence(lines, imgsize);

    elseif vpmultitype==1
        [normal_vec, vp] = vanishline_weakorthoconst(lines, camcalib);

    elseif vpmultitype==2
        vpset = vanishline_multi(lines, camcalib);
        % script_dispmultivp(vpset, lines, linesmore, img);
        % figure;

    elseif vpmultitype==3
        vpset = vanishline_multi3(lines, camcalib);
        % script_dispmultivp(vpset, lines, linesmore, img);
        % figure;

    elseif vpmultitype==4
        vpset = vanishline_multi4(lines, camcalib);
        % script_dispmultivp(vpset, lines, linesmore, img);
    end

% end


%CALCHIST  Calculates a histogram of a set of arrays
%
%     H = cv.calcHist(images, ranges)
%     H = cv.calcHist(..., 'OptionName',optionValue, ...)
%
% ## Input
% * __images__ Source arrays. A numeric array, or cell array of numeric arrays
%   are accepted. They all should have the same class (`uint8`, `uint16`, or
%   `single`) and the same row/column size. Each of them can have an arbitrary
%   number of channels. Note that passing `{img1, img2, ...}` as input is
%   similar to using `cat(3, img1, img2, ...)`, i.e the function computes the
%   histogram from channels of input arrays.
% * __ranges__ Cell-array of length `N` (histogram dimensionality) of the
%   histogram bin boundaries in each dimension.
%   * When the histogram is uniform (`Uniform=true`), then for each dimension
%     `i` it is enough to specify the lower (inclusive) boundary
%     `L(1) = ranges{i}(1)` of the first histogram bin and the upper
%     (exclusive) boundary `U(n) = ranges{i}(end)` for the last histogram bin
%     `HistSize(i)`. That is, in case of a uniform histogram each of
%     `ranges{i}` is an array of 2 elements forming an interval `[L,U)` which
%     is automatically divided according to `HistSize(i)`.
%   * When the histogram is not uniform (`Uniform=false`), then each of
%     `ranges{i}` contains `HistSize(i)+1` elements, specifying the bin edges
%     of dimension `i`: `L(1), L(2), ..., L(n), U(n)` forming the half-open
%     intervals: `[L(1), U(1)), [L(2), U(2)), ..., [L(n-1), U(n-1)), [L(n), U(n))`
%     where `U(1)==L(2), U(2)==L(3), ..., U(n-1)==L(n)`, and `n` is the
%     histogram size of the current dimension (`n = HistSize(i)`). The array
%     elements, that are not between `L(1)` and `U(n)`, are not counted in the
%     histogram.
%
% ## Output
% * __H__ Output histogram, which is a dense or sparse N-dimensional array of
%   type `single` (`N` is the histogram dimensionality that must be positive
%   and not greater than 32 in the current OpenCV version). The size of the
%   output N-D array is `HistSize(1)-by-HistSize(2)-by-...-by-HistSize(N)`.
%
% ## Options
% * __Channels__ List of channels used to compute the histogram (as 0-based
%   indices). The number of channels must match the histogram dimensionality
%   `N`. The first array channels are numerated from `0` to
%   `size(images{1},3)-1`, the second array channels are counted from
%   `size(images{1},3)` to `size(images{1},3) + size(images{2},3)-1`, and so
%   on. By default, all channels from all images are used to compute the
%   histogram, i.e default is `0:sum(cellfun(@(im)size(im,3), images))-1` when
%   input `images` is a cell array, and `0:(size(images,3)-1)` when input
%   `images` is a numeric array.
% * __Mask__ Optional mask. If the matrix is not empty, it must be an 8-bit or
%   logical array of the same row/column size as `images{i}`. The non-zero
%   mask elements mark the array elements (pixels) counted in the histogram.
%   Not set by default.
% * __HistSize__ Array of histogram sizes in each dimension. Use together
%   with the `Uniform` flag. Default is `cellfun(@numel,ranges)-1`.
%   * When the histogram is uniform, the range specified in `ranges{i}` is
%     divided into `HistSize(i)` uniform bins. The interval is divided into
%     bins using equally-spaced boundaries defined as:
%     `ranges{i} = linspace(ranges{i}(1), ranges{i}(end), HistSize(i)+1)`.
%   * When the histogram is not uniform, `ranges{i}` is used as is for the
%     bin boundaries without considering `HistSize`.
% * __Uniform__ Logical flag indicating whether the histogram is uniform or
%   not (see above). default false.
% * __Hist__ Input histogram, used in accumulation mode. Either a dense or
%   sparse array, see `H`. If it is set, the output histogram is initialized
%   with it instead of being cleared in the beginning when it is allocated.
%   This feature enables you to compute a single histogram from several sets
%   of arrays, or to update the histogram in time. Not set by default.
% * __Sparse__ Logical flag indicating whether the output should be sparse.
%   default false (i.e output histogram is a dense array). Keep in mind that
%   MATLAB only supports 2D sparse matrices, so use you must use dense arrays
%   if the histogram has more than two dimensions.
%
% The function cv.calcHist calculates the histogram of one or more arrays. The
% elements of a tuple used to increment a histogram bin are taken from the
% corresponding input arrays at the same location.
%
% ## Example
% The sample below shows how to compute a 2D Hue-Saturation histogram for a
% color image:
%
%     hsv = cv.cvtColor(img, 'RGB2HSV');
%     edges = {linspace(0,180,30+1), linspace(0,256,32+1)};
%     H = cv.calcHist(hsv(:,:,1:2), edges);
%
% Here is another example showing the different options:
%
%     % read some image, and convert to HSV colorspace
%     imgRGB = imread(fullfile(mexopencv.root(),'test','img001.jpg'));
%     imgHSV = cv.cvtColor(imgRGB, 'RGB2HSV');
%
%     % quantize the hue to 30 levels, and the saturation to 32 levels
%     histSize = [30, 32];
%     hranges = linspace(0, 180, histSize(1)+1);  % hue varies from 0 to 179
%     sranges = linspace(0, 256, histSize(2)+1);  % sat varies from 0 to 255
%     ranges = {hranges, sranges};
%
%     % one way
%     H = cv.calcHist(imgHSV(:,:,[1 2]), ranges);
%
%     % another way
%     H = cv.calcHist(imgHSV, ranges, 'Channels',[1 2]-1, 'HistSize',histSize);
%
%     % or similarly
%     H = cv.calcHist({imgHSV(:,:,1), imgHSV(:,:,2)}, {[0,180], [0,256]}, ...
%         'HistSize',histSize, 'Uniform',true);
%
%     % show H-S histogram
%     imagesc(H, 'YData',[0 180], 'XData',[0 256])
%     axis image; colormap gray; colorbar
%     ylabel('Hue'); xlabel('Saturation'); title('Histogram')
%
% See also: cv.calcBackProject, cv.compareHist, cv.EMD, hist, histc,
%  histogram, histcounts, histcounts2, discretize, imhist, hist3
%


function realtest_AD1()
% Test AD for a real optimization problem on a product manifold (struct)

    % Verify that Manopt was indeed added to the Matlab path.
    if isempty(which('spherefactory'))
        error(['You should first add Manopt to the Matlab path.\n' ...
		       'Please run importmanopt.']);
    end

    % Verify that the deep learning tool box was installed
    assert(exist('dlarray', 'file') == 2, ['Deep learning tool box is '...
    'needed for automatic differentiation.\n Please install the'...
    'latest version of the deep learning tool box and \nupgrade to Matlab'...
    ' R2021b if possible.'])

    % Generate the problem data.
    n = 100;
    A = randn(n);
    A = .5*(A+A');

    % Create the product manifold
    S = spherefactory(n);
    manifold.x = S;
    manifold.y = S;
    problem.M = productmanifold(manifold);

    % Define the problem cost function
    problem.cost  = @(X) -X.x'*(A*X.y);

    % Define the gradient and the hessian via automatic differentiation
    problem = manoptAD(problem);

    % Numerically check gradient and Hessian consistency.
    figure;
    checkgradient(problem);
    figure;
    checkhessian(problem);

    % Solve.
    [x, xcost, info] = trustregions(problem);          %#ok<ASGLU>

    % Test
    ground_truth = svd(A);
    distance = abs(ground_truth(1) - (-problem.cost(x)));
    fprintf('The distance between the ground truth and the solution is %e \n',distance);

end


% l1_softth - soft threshold function for L1 regularization
%
% Copyright(c) 2009 Ryota Tomioka
% This software is distributed under the MIT license. See license.txt

function [vv,ss]=l1_softth(vv,lambda,info)

n = size(vv,1);

Ip=find(vv>lambda);
In=find(vv<-lambda);

vv=sparse([Ip;In],1,[vv(Ip)-lambda;vv(In)+lambda],n,1);

ss=abs(vv);


% RES = corrDn(IM, FILT, EDGES, STEP, START, STOP)
%
% Compute correlation of matrices IM with FILT, followed by
% downsampling.  These arguments should be 1D or 2D matrices, and IM
% must be larger (in both dimensions) than FILT.  The origin of filt
% is assumed to be floor(size(filt)/2)+1.
%
% EDGES is a string determining boundary handling:
%    'circular' - Circular convolution
%    'reflect1' - Reflect about the edge pixels
%    'reflect2' - Reflect, doubling the edge pixels
%    'repeat'   - Repeat the edge pixels
%    'zero'     - Assume values of zero outside image boundary
%    'extend'   - Reflect and invert (continuous values and derivs)
%    'dont-compute' - Zero output when filter overhangs input boundaries
%
% Downsampling factors are determined by STEP (optional, default=[1 1]),
% which should be a 2-vector [y,x].
%
% The window over which the convolution occurs is specfied by START
% (optional, default=[1,1], and STOP (optional, default=size(IM)).
%
% NOTE: this operation corresponds to multiplication of a signal
% vector by a matrix whose rows contain copies of the FILT shifted by
% multiples of STEP.  See upConv.m for the operation corresponding to
% the transpose of this matrix.

% Eero Simoncelli, 6/96, revised 2/97.

function res = corrDn(im, filt, edges, step, start, stop)

%% NOTE: THIS CODE IS NOT ACTUALLY USED! (MEX FILE IS CALLED INSTEAD)

% fprintf(1,'WARNING: You should compile the MEX version of "corrDn.c",\n         found in the MEX subdirectory of matlabPyrTools, and put it in your matlab path.  It is MUCH faster, and provides more boundary-handling options.\n');

%------------------------------------------------------------
%% OPTIONAL ARGS:

if (exist('edges') == 1)
  if (strcmp(edges,'reflect1') ~= 1)
    warning('Using REFLECT1 edge-handling (use MEX code for other options).');
  end
end

if (exist('step') ~= 1)
	step = [1,1];
end

if (exist('start') ~= 1)
	start = [1,1];
end

if (exist('stop') ~= 1)
	stop = size(im);
end

%------------------------------------------------------------

% Reverse order of taps in filt, to do correlation instead of convolution
filt = filt(size(filt,1):-1:1,size(filt,2):-1:1);

tmp = rconv2(im,filt);
res = tmp(start(1):step(1):stop(1),start(2):step(2):stop(2));


% planarcams ... alignment under assumption of planar camera arrangements
%
% [align,cam] = planarmove(in,cam,config,idx)
% in, cam, config ... see the main GOCAL script
% idx ... indexes of cameras which are suppose to be in one plane
%
% align ... structures aligned wit the specified world frame
%
% $Id: planarcams.m,v 1.1 2003/07/03 15:36:55 svoboda Exp $

function [align,cam] = planarcams(in,cam,config,idx)

% fit a plane to the reconstructed points and estimate normal

plane.n = planefit(in.Ce(:,idx)');

new.n = [0,0,1]'; % align the xy plane horizontally

rotaxis = cross(new.n,plane.n);
rotangle = acos( (plane.n'*new.n)/norm(plane.n)*norm(new.n) );

R = nfi2r(rotaxis,rotangle);
s = 3;
t = [0,0,1]' - s*R*mean(in.Xe(1:3,:)')';

align.simT.s = s;
align.simT.R = R;
align.simT.t = t;

[align.P, align.X]	= align3d(in.Pe,in.Xe,align.simT);
% save aligned data
if 1 % SAVE_STEPHI | SAVE_PGUHA
	[align.Cst,align.Rot] = savecalpar(align.P,config);
end
drawscene(align.X,align.Cst',align.Rot,61,'cloud','Graphical Output Validation: Aligned data, TopView',config.cal.cams2use);

set(gca,'CameraTarget',[0,0,0]);
set(gca,'CameraPosition',[0,0,1]);

figure(61),
% print -depsc graphevalaligned.eps
eval(['print -depsc ', config.paths.data, 'topview.eps'])

drawscene(align.X,align.Cst',align.Rot,62,'cloud','Graphical Output Validation: Aligned data, SideView',config.cal.cams2use);

set(gca,'CameraTarget',[0,0,0.9]);
set(gca,'CameraPosition',[2,0,0.9]);

figure(62),
% print -depsc graphevalaligned.eps
eval(['print -depsc ', config.paths.data, 'sideview.eps'])

return


 function [xs, info] = pwls_sps_os(x, yi, wi, Ab, R, niter, pixmax, ...
		denom, aai, relax0, chat)
%function [xs, info] = pwls_sps_os(x, yi, wi, Ab, R, niter, pixmax, ...
%|		denom, aai, relax0, chat)
%|
%| penalized weighted least squares estimation/reconstruction
%| using separable paraboloidal surrogates algorithm with
%| (optionally relaxed) ordered subsets.  (relaxation ensures convergence.)
%|
%| cost(x) = (y-Gx)' W (y-Gx) / 2 + R(x)
%|
%| in
%|	x	[np 1]		initial estimate
%|	yi	[nb na]		measurements (noisy sinogram)
%|	wi	[nb na]		weighting sinogram (or [] for uniform)
%|	Ab	[nd np]		Gblock object, aij >= 0 required!
%|					or sparse matrix (implies nsubset=1)
%|	R			penalty object (see Reg1.m)
%|	niter			# of iterations (including 0)
%|
%| optional
%|	pixmax	[1] or [2]	max pixel value, or [min max] (default [0 inf])
%|	denom	[np 1]		precomputed denominator
%|	aai	[nb na]		precomputed row sums of |Ab|
%|	relax0	[1] or [2]	relax0 or (relax0, relax_rate)
%|	chat
%|
%| out
%|	xs	[np niter]	iterates
%|	info	[niter 1]	time
%|
%| Copyright 2002-2-12, Jeff Fessler, University of Michigan

if nargin < 4, ir_usage, end

cpu etic
info = zeros(niter,1);

Ab = block_op(Ab, 'ensure'); % make it a block object (if not already)
nblock = block_op(Ab, 'n');
starts = subset_start(nblock);

if ~isvar('niter')	|| isempty(niter),	niter = 1;	end
if ~isvar('pixmax')	|| isempty(pixmax),	pixmax = inf;	end
if ~isvar('chat')	|| isempty(chat),	chat = false;	end
if isempty(wi)
	wi = ones(size(yi));
end
if ~isvar('aai') || isempty(aai)
	aai = reshape(sum(Ab'), size(yi)); % a_i = sum_j |a_ij|
					% requires real a_ij and a_ij >= 0
end

if ~isvar('relax0') || isempty(relax0)
	relax0 = 1;
end
if length(relax0) == 1
	relax_rate = 0;
elseif length(relax0) == 2
	relax_rate = relax0(2);
	relax0 = relax0(1);
else
	error relax
end

if length(pixmax) == 2
	pixmin = pixmax(1);
	pixmax = pixmax(2);
elseif length(pixmax) == 1
	pixmin = 0;
else
	error pixmax
end

%
% likelihood denom, if not provided
%
if ~isvar('denom') || isempty(denom)
	denom = Ab' * col(aai .* wi);	% requires real a_ij and a_ij >= 0
end, clear aai

if ~isvar('R') || isempty(R)
	pgrad = 0;		% unregularized default
	Rdenom = 0;
end

[nb na] = size(yi);

%
% loop over iterations
%

xs = zeros(numel(x), niter, class(x));
x = max(x,pixmin);
x = min(x,pixmax);
xs(:,1) = x;

for iter = 2:niter
	ticker(mfilename, iter, niter)

	relax = relax0 / (1 + relax_rate * (iter-2));

	%
	% loop over subsets
	%
	for iset=1:nblock
		iblock = starts(iset);
		ia = iblock:nblock:na;

		li = Ab{iblock} * x;
		li = reshape(li, nb, length(ia));
		resid = wi(:,ia) .* (yi(:,ia) - li);
		grad = Ab{iblock}' * resid(:); % G' * W * (y - G*x)

		if ~isempty(R)
			pgrad = R.cgrad(R, x);
			Rdenom = R.denom(R, x);
		end

		num = nblock * grad - pgrad;
		den = denom + Rdenom;

		x = x + relax * num ./ den;	% relaxed update
		x = max(x,pixmin);		% lower bound
		x = min(x,pixmax);		% upper bound
	end

	if chat, printm('range(x) = %g %g', min(x), max(x)), end
	xs(:,iter) = x;
	info(iter,1) = cpu('etoc');
end


function TAU = int_tau(Z)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Program:    Tau Integral Computation for Hydrogen Composition Program
%               in Atmospheric Model
%   Author:     Brent Lewis(RocketLion@gmail.com)
%               University of Colorado-Boulder
%   History:    Original-1/10/2007
%   Input:      Z:      Altitude value
%   Output:     TAU:    Integral Value
%   Note:       This program computes the value of Tau directly with the
%               integral done by hand and only the second integration limit
%               needing to be inputed
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%   Constants
L_K_9 = 12;
T_10 = 360;
T_inf = 1000;
Z_10 = 120;
g_0 = 9.80665;
r_E = 6.356766e3;
R = 8.31432e3;
lambda = L_K_9/(T_inf-T_10);
M_H = 1.00797;

%   Value of Integration limit computed previously
tau_11 = 8.329503912749350e-004;

tau_Z = M_H*g_0*r_E^2/R*...
    log((exp(lambda*(Z-Z_10)*(r_E+Z_10)/(r_E+Z))-1)*T_inf+T_10)/...
    (lambda*T_inf*(r_E+Z_10)^2);

TAU = tau_Z-tau_11;


% written by Ali A. Eftekhari
clc
L = 50;  % domain length
Nx = 20; % number of cells
m = createMesh3D(Nx, Nx, Nx, L, L, L);
BC = createBC(m); % all Neumann boundary condition structure
BC.left.a(:) = 0; BC.left.b(:)=1; BC.left.c(:)=1; % Dirichlet for the left boundary
BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=0; % right boundary
D_val = 1; % value of the diffusion coefficient
D = createCellVariable(m, D_val); % assign the diffusion coefficient to the cells
D_face = harmonicMean(D); % calculate harmonic average of the diffusion coef on the cell faces
Mdiff = diffusionTerm(D_face); % matrix of coefficients for the diffusion term
[Mbc, RHSbc] = boundaryCondition(BC); % matix of coefficients and RHS vector for the BC
M = Mdiff + Mbc; % matrix of cefficients for the PDE
c = solvePDE(m,M, RHSbc); % send M and RHS to the solver
visualizeCells(c); % visualize the results


clear
%Process definition
ar = rc2arset([1 -.4 .2]);
ma = rc2arset([1 .8]);
%End of Process definition
n_obs = 10;

rch = [1 -.6 -.3 -.2];
Lar = length(rch)-1;
arh_set = rc2arset(rch,0:Lar);

%function pe_set = prederrAR(rc,cov)
%PREDERRAR Prediction error of AR models of increasing order
vareps = 1/pgain(ar,ma);
cor = arma2cor(ar,ma,Lar);

r = [cor(end:-1:1) cor(2:end)];
pe(1) = 1;
eo = r; %cor;
fo = r; %cor;
pe_old(1) = 1;
for p = 1:Lar
    carh = [arh_set{p+1} zeros(1,Lar-p)];
    crc = rch(p+1);
    ahahp = convol(fliplr(carh),carh)
    ahah  = convol(carh,carh);
    e = xcorr(ahah,r);  e = e(Lar+1:3*Lar+1);
    f = xcorr(ahahp,r); f = f(2*Lar+1:3*Lar+1);
    %Recursieve berekening van e en f.
    er = NaN*ones(1,Lar+1);
    for q = p:Lar
        er(q+1) = eo(q+1) + 2*crc*fo(q-p+1) + crc^2*eo(2*p-q+1);
    end
    fr = NaN*ones(1,Lar+1);
%     for q = 0:Lar-p
%         fr(q+1) = (1+crc^2)*fo(q+1) + crc*eo(p+q+1); %Klopt alleen voor witte ruis!
%     end
%     e,er
%     f,fr;
    disp(' ')
    pe(p+1) = f(Lar+1) %werkt

    %Preparations for next step
    eo = e;
    fo = f;

    %Old PE
    pe_old(p+1) = (moderr(carh,1,ar,ma,1)+1)*vareps;
end


function X = Inv_CoarseCurveCoeff(C);
% Inv_CoarseCurveCoeff: Reconstruct the low-frequency subband from
%                           the coarsest scale curvelet coefficients
%  Usage:
%    X = Inv_CoarseCurveCoeff(C);
%  Inputs:
%    C   m by m matrix, m = 2*2^L
%  Outputs:
%    X   m by m matrix
% See Also
%   CoaseCurveCoeff, Inv_Curvelet02Xform
%
% By Emmanuel Candes, 2003-2004

X = fft2_mid0(C)/sqrt(prod(size(C)));


function plot3d (type)
global A cds opt holdon leg label figmain smmethod subpl

% Check if there IS data to plot
if isempty(A) errordlg ('No data present. Sure you read a file yet?','Error'); return; end

% Add or substitute plot
figure (figmain);
subplot(subpl(1),subpl(2),subpl(3),'Parent',figmain);
if holdon hold on; else 	hold off; end

% Columns
X= A(:,opt.xc(1));
Y= A(:,opt.yc(1));
M= A(:,opt.zc);

switch type
	case 'histo'
		hist(Y,10);
	case 'histo3'
		hist3([X,Y],[10,10]);
	case 'corrxy'
		[rho,pval]= corr(X,Y);
		plot(X,Y,'o');
		L= ['rho= ',num2str(rho),' p< ',num2str(pval)];
		legend(L);
	case 'corrall'
		fprintf('Col Row  Rho  Pval \n')
		output= mcorr(A,'sig');
		fprintf('  %d   %d %4.2f  %4.2f \n',output)
	case 'qqplot'
		qqplot(X,Y);
	case 'normplot'
		normplot(X);

end


function [Y, R, E] = IsomapII(D, n_fcn, n_size, options);

% ISOMAPII   Computes Isomap embedding using an advanced version of
%             the algorithm in Tenenbaum, de Silva, and Langford (2000),
%             which can take advantage of sparsity in the graph and
%             redundancy in the distances.
%
% [Y, R, E] = isomapII(D, n_fcn, n_size, options);
%
% Input:
%    D = input-space distances between pairs of N points, which can
%     take 1 of 3 forms:
%      (1) a full N x N matrix (as in isomap.m)
%      (2) a sparse N x N matrix (missing entries are treated as INF)
%      (3) the name of a function (e.g. 'd_fun') that takes
%            one argument, i, and returns a row vector containng the
%            distances from all N points to point i.
%
%    n_fcn = neighborhood function ('epsilon' or 'k')
%    n_size = neighborhood size (value for epsilon or k)
%
%    options = optional structure of options:
%      options.dims = (row) vector of embedding dimensionalities to use
%                        (1:10 = default)
%      options.comp = which connected component to embed, if more than one.
%                        (1 = largest (default), 2 = second largest, ...)
%      options.display = plot residual variance and 2-D embedding?
%                        (1 = yes (default), 0 = no)
%      options.overlay = overlay graph on 2-D embedding?
%                        (1 = yes (default), 0 = no)
%      options.verbose = display progress reports?
%                        (1 = yes (default), 0 = no)
%      options.dijkstra = use dijkstra's algorithm for shortest paths with
%                         full N x N distance matrix.
%                         (1 = yes (default), 0 = use Floyd; Floyd should
%                          be used only if you are unable to MEX dijkstra.cpp)
%      options.Kmax = maximum number of neighbors (used for sparse versions
%                        of epsilon; by default, estimated by random sample)
%      options.landmarks = (row) vector of landmark points to use in MDS.
%                 (MDS finds the configuration that best approximates
%                  the distances from all points to the landmark points.
%                  The default landmark points are 1:N (i.e. all the points),
%                  which is equivalent to classical MDS.  Good
%                  results may often be obtained using a number of
%                  landmarks that is much smaller than N, but much
%                  larger than the data's intrinsic dimensionality.
%                  Note that this extension is experimental!  For
%                  discussion, see Steyvers, de Silva, and Tenenbaum
%                  (in preparation).)
%
% Output:
%    Y = Y.coords is a cell array, with coordinates for d-dimensional embeddings
%         in Y.coords{d}.  Y.index contains the indices of the points embedded.
%    R = residual variances for embeddings in Y
%    E = edge matrix for neighborhood graph
%

%    BEGIN COPYRIGHT NOTICE
%
%    Isomap II code -- (c) 1998-2000 Josh Tenenbaum
%
%    This code is provided as is, with no guarantees except that
%    bugs are almost surely present.  Published reports of research
%    using this code (or a modified version) should cite the
%    article that describes the algorithm:
%
%      J. B. Tenenbaum, V. de Silva, J. C. Langford (2000).  A global
%      geometric framework for nonlinear dimensionality reduction.
%      Science 290 (5500): 2319-2323, 22 December 2000.
%
%    Comments and bug reports are welcome.  Email to jbt@psych.stanford.edu.
%    I would also appreciate hearing about how you used this code,
%    improvements that you have made to it, or translations into other
%    languages.
%
%    You are free to modify, extend or distribute this code, as long
%    as this copyright notice is included whole and unchanged.
%
%    END COPYRIGHT NOTICE

%%%%% Step 0: Initialization and Parameters %%%%%

if nargin < 3
     error('Too few input arguments');
elseif nargin < 4
     options = struct('dims',1:10,'overlay',1,'comp',1,'display',1,'dijkstra',1,'verbose',1);
end

if ischar(D)
     mode = 3;
     d_func = D;
     N = length(feval(d_func,1));
elseif issparse(D)
     mode = 2;
     N = size(D,1);
     if ~(N==size(D,2))
          error('D must be a square matrix');
     end;
else
     mode = 1;
     N = size(D,1);
     if ~(N==size(D,2))
         error('D must be a square matrix');
     end;
end

if n_fcn=='k'
     K = n_size;
     if ~(K==round(K))
         error('Number of neighbors for k method must be an integer');
     end
     if ((mode==2) & ~(min(sum(D'>0))>=K))
         error('Sparse D matrix must contain at least K nonzero entries in each row');
     end
elseif n_fcn=='epsilon'
     epsilon = n_size;
     if isfield(options,'Kmax')
         K = options.Kmax;
     elseif (mode==3)    %% estimate maximum equivalent K %%
         tmp = zeros(10,N);
         for i=1:10
             tmp(i,:) = feval(d_func,ceil(N*rand));
         end
         K = 2*max(sum(tmp'<epsilon));    % just to be safe
     end
else
     error('Neighborhood function must be either epsilon or k');
end

if (mode == 3)
     INF = inf;
else
     INF =  1000*max(max(D))*N;  %% effectively infinite distance
end

if ~isfield(options,'dims')
     options.dims = 1:10;
end
if ~isfield(options,'overlay')
     options.overlay = 1;
end
if ~isfield(options,'comp')
     options.comp = 1;
end
if ~isfield(options,'display')
     options.display = 1;
end
if ~isfield(options,'verbose')
     options.verbose = 1;
end
if ~isfield(options,'landmarks')
     options.landmarks = 1:N;
end
if ~isfield(options,'dijkstra')
     options.dijkstra = 1;
end
dims = options.dims;
comp = options.comp;
overlay = options.overlay;
displ = options.display;
verbose = options.verbose;
landmarks = options.landmarks;
use_dijk = options.dijkstra;

Y.coords = cell(length(dims),1);
R = zeros(1,length(dims));

%%%%% Step 1: Construct neighborhood graph %%%%%
disp('Constructing neighborhood graph...');

if ((mode == 1) & (use_dijk == 0))
     if n_fcn == 'k'
         [tmp, ind] = sort(D);
         tic;
         for i=1:N
             D(i,ind((2+K):end,i)) = INF;
             if ((verbose == 1) & (rem(i,50) == 0))
                 disp([' Iteration: ' num2str(i) '     Estimated time to completion: 'num2str((N-i)*toc/60/50) ' minutes']); tic;
             end
         end
     elseif n_fcn == 'epsilon'
         warning off    %% Next line causes an unnecessary warning, so turn it off
         D =  D./(D<=epsilon);
         D = min(D,INF);
         warning on
     end
     D = min(D,D');    %% Make sure distance matrix is symmetric
elseif ((mode == 1) & (use_dijk == 1))
     if n_fcn == 'k'
         [tmp, ind] = sort(D);
         tic;
         for i=1:N
             D(i,ind((2+K):end,i)) = 0;
             if ((verbose == 1) & (rem(i,50) == 0))
                 disp([' Iteration: ' num2str(i) '     Estimated time to completion: 'num2str((N-i)*toc/60/50) ' minutes']); tic;
             end
         end
     elseif n_fcn == 'epsilon'
         D =  D.*(D<=epsilon);
     end
     D = sparse(D);
     D = max(D,D');    %% Make sure distance matrix is symmetric
elseif (mode == 2)
     if n_fcn == 'k'
         Di = zeros(N*K,1);      Dj = zeros(N*K,1);       Ds = zeros(N*K,1);
         counter = 0;
         [a,b,c] = find(D);
         tic;
         for i=1:N
             l = find(a==i);
             [g,f] = sort(c(l));
             Di(counter+(1:K)) = i;
             Dj(counter+(1:K)) = b(l(f(1:K)));
             Ds(counter+(1:K)) = g(1:K);
             counter = counter+K;
             if ((verbose == 1) & (rem(i,50) == 0))
                  disp([' Iteration: ' num2str(i) '     Estimated time to completion: 'num2str((N-i)*toc/60/50) ' minutes']); tic;
             end
         end
         D = sparse(Di(1:counter), Dj(1:counter), Ds(1:counter));
         clear Di Dj Ds counter;
     elseif n_fcn == 'epsilon'
         D =  D.*(D<=epsilon);
     end
     D = max(D,D');    %% Make sure distance matrix is symmetric
elseif (mode == 3)
     Di = zeros(N*(K+1),1);      Dj = zeros(N*(K+1),1);       Ds = zeros(N*(K+1),1);
     counter = 0;
     tic;
     for i=1:N
         d = feval(d_func,i);
         if n_fcn == 'k'
             [c,b] = sort(d);
             Di(counter+(1:(K+1))) = i;
             Dj(counter+(1:(K+1))) = b(1:(K+1));
             Ds(counter+(1:(K+1))) = c(1:(K+1));
             counter = counter+(K+1);
         elseif n_fcn == 'epsilon'
             [a,b,c] = find(d.*(d<=epsilon));
             l = length(a);
             Di(counter+(1:l)) = i;
             Dj(counter+(1:l)) = b;
             Ds(counter+(1:l)) = c;
             counter = counter+l;
         end
         if ((verbose == 1) & (rem(i,50) == 0))
              disp([' Iteration: ' num2str(i) '     Estimated time to completion: 'num2str((N-i)*toc/60/50) ' minutes']); tic;
         end
     end
     D = sparse(Di(1:counter), Dj(1:counter), Ds(1:counter));
     clear Di Dj Ds counter;
     D = max(D,D');    %% Make sure distance matrix is symmetric
end

if (overlay == 1)
     if ((mode == 1) & (use_dijk == 0))
         E = int8(1-(D==INF));  %%  Edge information for subsequent graph overlay
     else
         [a,b,c] = find(D);
         E = sparse(a,b,ones(size(a)));
     end
end

%%%%% Step 2: Compute shortest paths %%%%%
disp('Computing shortest paths...');

if ((mode==1) & (use_dijk == 0))
     tic;
     for k=1:N
         D = min(D,repmat(D(:,k),[1 N])+repmat(D(k,:),[N 1]));
         if ((verbose == 1) & (rem(k,20) == 0))
              disp([' Iteration: ' num2str(k) '     Estimated time to completion: 'num2str((N-i)*toc/i/60) ' minutes']);
         end
     end
else
     D = dijkstra(D, landmarks);
end

%%%%% Step 3: Construct low-dimensional embeddings (Classical MDS) %%%%%
disp('Constructing low-dimensional embeddings (Classical MDS)...');

%%%%% Remove outliers from graph %%%%%
disp('  Checking for outliers...');

if ((mode == 1) & (use_dijk == 0))
     [tmp, firsts] = min(D==INF);     %% first point each point connects to
else
     [tmp, firsts] = min(D==inf);     %% first point each point connects to
end
[comps, I, J] = unique(firsts);    %% first point in each connected component
n_comps = length(comps);           %% number of connected components
size_comps = sum((repmat(firsts,n_comps,1)==((1:n_comps)'*ones(1,N)))');
                                   %% size of each connected component
[tmp, comp_order] = sort(size_comps);  %% sort connected components by size
comps = comps(comp_order(end:-1:1));
size_comps = size_comps(comp_order(end:-1:1));
if (comp>n_comps)
     comp=1;                              %% default: use largest component
end
Y.index = find(firsts==comps(comp)); %% list of points in relevant component
Y.index = setdiff(Y.index,find(isinf(min(D)))); %% prune points that don't connect
                                                %% to any landmarks
N = length(Y.index);
[tmp, landmarks, land_ind] = intersect(landmarks,Y.index);
                                       %% list of landmarks in component
nl = length(landmarks);
D = full(D(landmarks,Y.index))';
disp(['    Number of connected components in graph: ' num2str(n_comps)]);
disp(['    Embedding component ' num2str(comp) ' with ' num2str(length(Y.index)) ' points.']);

dims = unique(min(dims,nl-1));    %% don't embed in more dimensions than landmarks-1
if (nl==N)
     opt.disp = 0;
     [vec, val] = eigs(-.5*(D.^2 - sum(D.^2)'*ones(1,N)/N - ones(N,1)*sum(D.^2)/N + sum(sum(D.^2))/(N^2)), max(dims), 'LR', opt);
else
     subB = -.5*(D.^2 - sum(D'.^2)'*ones(1,nl)/nl - ones(N,1)*sum(D.^2)/N+sum(sum(D.^2))/(N*nl));
     opt.disp = 0;
     [alpha,beta] = eigs(subB'*subB, max(dims), 'LR', opt);
     val = beta.^(1/2);
     vec = subB*alpha*inv(val);
     clear subB alpha beta;
end
h = real(diag(val));
[foo,sorth] = sort(h);  sorth = sorth(end:-1:1);
val = real(diag(val(sorth,sorth)));
vec = vec(:,sorth);

D = reshape(D,N*nl,1);
for di = 1:length(dims)
     Y.coords{di} = real(vec(:,1:dims(di)).*(ones(N,1)*sqrt(val(1:dims(di)))'))';
     r2 = 1-corrcoef(reshape(real(L2_distance(Y.coords{di}, Y.coords{di}(:,land_ind))),N*nl,1),D).^2;
     R(di) = r2(2,1);
     if (verbose == 1)
         disp(['  Isomap on ' num2str(N) ' points with dimensionality ' num2str(dims(di)) '  --> residual variance = ' num2str(R(di))]);
     end
end

clear D;

%%%%%%%%%%%%%%%%%% Graphics %%%%%%%%%%%%%%%%%%

if (displ==1)
     %%%%% Plot fall-off of residual variance with dimensionality %%%%%
     figure;
     hold on
     plot(dims, R, 'bo');
     plot(dims, R, 'b-');
     hold off
     ylabel('Residual variance');
     xlabel('Isomap dimensionality');

     %%%%% Plot two-dimensional configuration %%%%%
     twod = find(dims==2);
     if ~isempty(twod)
         figure;
         hold on;
         plot(Y.coords{twod}(1,:), Y.coords{twod}(2,:), 'ro');
         if (overlay == 1)
             gplot(E(Y.index, Y.index), [Y.coords{twod}(1,:); Y.coords{twod}(2,:)]');
             title('Two-dimensional Isomap embedding (with neighborhood graph).');
         else
             title('Two-dimensional Isomap.');
         end
         hold off;
     end
end

return;


function A = makehatch_plus(hatch,n,m)
%MAKEHATCH_PLUS Predefined hatch patterns
%
% Modification of MAKEHATCH to allow for selection of matrix size. Useful whe using
%   APPLYHATCH_PLUS with higher resolution output.
%
% input (optional) N    size of hatch matrix (default = 6)
% input (optional) M    width of lines and dots in hatching (default = 1)
%
%  MAKEHATCH_PLUS(HATCH,N,M) returns a matrix with the hatch pattern for HATCH
%   according to the following table:
%      HATCH        pattern
%     -------      ---------
%        /          right-slanted lines
%        \          left-slanted lines
%        |          vertical lines
%        -          horizontal lines
%        +          crossing vertical and horizontal lines
%        x          criss-crossing lines
%        .          square dots
%        c          circular dots
%        w          Just a blank white pattern
%        k          Just a totally black pattern
%
%  See also: APPLYHATCH_PLUS

%  By Ben Hinkle, bhinkle@mathworks.com
%  This code is in the public domain.

% Modified Brian FG Katz    8-aout-03
% Modified David M Kaplan    19-fevrier-08

if ~exist('n','var'), n = 6; end
if ~exist('m','var'), m = 1; end
n=round(n);

switch (hatch)
  case '\'
    [B,C] = meshgrid( 0:n-1 );
    B = B-C;
    clear C
    A = abs(B) <= m/2;
    A = A | abs(B-n) <= m/2;
    A = A | abs(B+n) <= m/2;
  case '/'
    A = fliplr(makehatch_plus('\',n,m));
  case '|'
    A=zeros(n);
    A(:,1:m) = 1;
  case '-'
    A = makehatch_plus('|',n,m);
    A = A';
  case '+'
    A = makehatch_plus('|',n,m);
    A = A | A';
  case 'x'
    A = makehatch_plus('\',n,m);
    A = A | fliplr(A);
  case '.'
    A=zeros(n);
    A(1:2*m,1:2*m)=1;
  case 'c'
    [B,C] = meshgrid( 0:n-1 );
    A = sqrt(B.^2+C.^2) <= m;
    A = A | fliplr(A) | flipud(A) | flipud(fliplr(A));
  case 'w'
    A = zeros(n);
  case 'k'
    A = ones(n);
  otherwise
    error(['Undefined hatch pattern "' hatch '".']);
end