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


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 [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;


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 [Q2,Bg2,delta_X2,Pk2]=ESKF(Q1,Bg1,delta_X1,ImuData,t,Vm,Pk1)

% ESKF( Error State Kalman Filter)
% derivation <Quaternion Kinematics for the Error-state Kalman Filter>
% author Zhang xin

if isempty(Pk1)
    Para_delta_theta0=0.00001;
    Para_delta_Bg0=0.00001;
    Pk1=diag([[1,1,1]*Para_delta_theta0,[1,1,1]*Para_delta_Bg0]);
end

norm_a=norm(ImuData(1,2:4));
%norm_g=norm(ImuData(1,5:7)-Bg1);
wx=(ImuData(1,5)-Bg1(1))*t;
wy=(ImuData(1,6)-Bg1(2))*t;
wz=(ImuData(1,7)-Bg1(3))*t;

Qp=[ 1    , -wx/2 , -wy/2 , -wz/2  ;...
     wx/2 ,   1   ,  wz/2 , -wy/2  ;...
     wy/2 , -wz/2 ,   1   ,  wx/2  ;...
     wz/2 ,  wy/2 , -wx/2 ,   1   ]*Q1';
Qp=Qp/norm(Qp);

delta_theta=norm([wx,wy,wz]);

if delta_theta==0
    R_u_delta_theta=eye(3);
else
    u= [wx,wy,wz]'/delta_theta;
    R_u_delta_theta=eye(3)-Skew_symmetric(u)*sin(delta_theta)+Skew_symmetric(u)*Skew_symmetric(u)*(1-cos(delta_theta));
end

F_delta_X=[R_u_delta_theta, -eye(3)*t;...
           zeros(3,3),       eye(3) ];
Para_delta_theta=0.000001;
Para_delta_Bg=0.000001;
Q_delta_X=diag([[1,1,1]*Para_delta_theta,[1,1,1]*Para_delta_Bg]);

delta_Xp = F_delta_X*delta_X1';

P_k=F_delta_X*Pk1*F_delta_X'+Q_delta_X;

if abs(norm_a-9.8)<2 %&& norm_g< 2

   % R1=[2*Qp(1)^2+2*Qp(2)^2-1;...
   %     2*Qp(2)*Qp(3)-2*Qp(1)*Qp(4);...
   %     2*Qp(1)*Qp(3)+2*Qp(2)*Qp(4)];

    R2=[2*Qp(2)*Qp(3)+2*Qp(1)*Qp(4);...
        2*Qp(1)^2+2*Qp(3)^2-1;...
        2*Qp(3)*Qp(4)-2*Qp(1)*Qp(2)];

    R3=[2*Qp(2)*Qp(4)-2*Qp(1)*Qp(3);...
        2*Qp(3)*Qp(4)+2*Qp(1)*Qp(2);...
        2*Qp(1)^2+2*Qp(4)^2-1 ];

   % J1=2*[ Qp(1), Qp(2),-Qp(3),-Qp(4);...
   %       -Qp(4), Qp(3), Qp(2), Qp(1);...
   %        Qp(3), Qp(4), Qp(1), Qp(2)];

    J2=2*[ Qp(4), Qp(3), Qp(2) , Qp(1);...
           Qp(1),-Qp(2), Qp(3) ,-Qp(4);...
          -Qp(2),-Qp(1), Qp(4) , Qp(3)] ;

    J3=2*[-Qp(3), Qp(4),-Qp(1) , Qp(2);...
           Qp(2), Qp(1), Qp(4) , Qp(3);...
           Qp(1),-Qp(2),-Qp(3) , Qp(4)] ;

    h_acc= R3;
    h_mag = Vm(2)*R2+Vm(3)*R3;

    H_acc = [J3,zeros(3,3)];
    H_mag = [Vm(2)*J2+Vm(3)*J3,zeros(3,3)];

    Q_delta_theta= 1/2*[ -Qp(2), -Qp(3), -Qp(4) ;...
                          Qp(1), -Qp(4),  Qp(3) ;...
                          Qp(4),  Qp(1), -Qp(2) ;...
                         -Qp(3),  Qp(2),  Qp(1) ];

    X_delta_x=[Q_delta_theta, zeros(4,3);...
                zeros(3,3),      eye(3)];

    Hk= [H_acc;H_mag]* X_delta_x;

    para_Rk_acc=0.1;
    para_Rk_mag=0.2;
    Rk=diag([[1,1,1]*para_Rk_acc,[1,1,1]*para_Rk_mag]);

    Kk=P_k*Hk'*inv(Hk*P_k*Hk'+Rk);

    delta_X_hat=Kk*([ImuData(1,2:4)'/norm_a;ImuData(1,8:10)'/norm(ImuData(1,8:10))]-[h_acc;h_mag]);

    Q2=quaternProd(Qp, [1;delta_X_hat(1:3)/2])';
    Q2=Q2/norm(Q2);
    Bg2=Bg1+delta_X_hat(4:6)';

    Pk2_=(eye(6)-Kk*Hk)*P_k;

    delta_X2=delta_Xp'+delta_X_hat';

    G=[eye(3)-Skew_symmetric(-delta_X2(1:3)/2),zeros(3,3);...
        zeros(3,3)   ,                 eye(3)];

    Pk2=G*Pk2_*G';

else
    Q2=Qp';
    Bg2=Bg1;
    delta_X2=delta_Xp';
    Pk2=P_k;

end
    delta_X2(1:3)=[0,0,0];

    if Q2(1)<0
        Q2=-Q2;
    end

end

function S=Skew_symmetric(u)
%Skew_Operator

S=[  0  , -u(3) ,  u(2) ;...
    u(3),   0   , -u(1) ;...
   -u(2),  u(1),    0 ];

end

function ab = quaternProd(a, b)
    ab(1,1) = a(1)*b(1)-a(2)*b(2)-a(3)*b(3)-a(4)*b(4);
    ab(2,1) = a(1)*b(2)+a(2)*b(1)+a(3)*b(4)-a(4)*b(3);
    ab(3,1) = a(1)*b(3)-a(2)*b(4)+a(3)*b(1)+a(4)*b(2);
    ab(4,1) = a(1)*b(4)+a(2)*b(3)-a(3)*b(2)+a(4)*b(1);
    if ab(1)<0
        ab=-ab;
    end
end


% Replication of the trivariate VAR in Stock and Watson (2001, JEP).
% Figure 1 and Table 1.B.
%==========================================================================
% The VAR Toolbox 3.0 is required to run this code. To get the
% latest version of the toolboxes visit:
% https://github.com/ambropo/VAR-Toolbox
%==========================================================================
% Ambrogio Cesa Bianchi, November 2020
% ambrogio.cesabianchi@gmail.com

%% PRELIMINARIES
%==========================================================================
clear all; clear session; close all; clc
warning off all

% Load data
[xlsdata, xlstext] = xlsread('SW2001_Data.xlsx','Sheet1');
X = xlsdata;
dates = xlstext(3:end,1);
vnames_long = xlstext(1,2:end);
vnames = xlstext(2,2:end);
nvar = length(vnames);
data   = Num2NaN(xlsdata);
% Store variables in the structure DATA
for ii=1:length(vnames)
    DATA.(vnames{ii}) = data(:,ii);
end
% Convert the first date to numeric
year = str2double(xlstext{3,1}(1:4));
quarter = str2double(xlstext{3,1}(6));
% Observations
nobs = size(data,1);

%% VAR ESTIMATION
%==========================================================================
% Set deterministics for the VAR
det = 1;
% Set number of nlags
nlags = 4;
% Estimate VAR
[VAR, VARopt] = VARmodel(X,nlags,det);
% Print estimation on screen
VARopt.vnames = vnames;
[TABLE, beta] = VARprint(VAR,VARopt,2);

%% COMPUTE IR AND VD
%==========================================================================
% Set options some options for IRF calculation
VARopt.nsteps = 24;
VARopt.ident = 'short';
VARopt.vnames = vnames_long;
VARopt.FigSize = [26,12];
% Compute IRF
[IRF, VAR] = VARir(VAR,VARopt);
% Compute error bands
[IRinf,IRsup,IRmed,IRbar] = VARirband(VAR,VARopt);
% Plot
VARirplot(IRbar,VARopt,IRinf,IRsup);

% Compute VD
[VD, VAR] = VARvd(VAR,VARopt);
% Compute VD error bands
[VDinf,VDsup,VDmed,VDbar] = VARvdband(VAR,VARopt);
% Plot VD
VARvdplot(VDbar,VARopt);

%% Print Table 1.B on screen
%==========================================================================
% Retrieve Forecast Error Variance Decomposition
FEVD_Table(1, :) = VD(1,:,1);
FEVD_Table(2, :) = VD(4,:,1);
FEVD_Table(3, :) = VD(8,:,1);
FEVD_Table(4, :) = VD(12,:,1);

FEVD_Table(5, :) = VD(1,:,2);
FEVD_Table(6, :) = VD(4,:,2);
FEVD_Table(7, :) = VD(8,:,2);
FEVD_Table(8, :) = VD(12,:,2);

FEVD_Table(9, :) = VD(1,:,3);
FEVD_Table(10,:) = VD(4,:,3);
FEVD_Table(11,:) = VD(8,:,3);
FEVD_Table(12,:) = VD(12,:,3);

% Print on screen
disp(' ')
disp('Variance Decomposition of Inflation (t=1,4,8,12)')
disp('---------------------------------------------------')
mprint(FEVD_Table(1:4,:));
disp('Variance Decomposition of Unemployment (t=1,4,8,12)')
disp('---------------------------------------------------')
mprint(FEVD_Table(5:8,:));
disp('Variance Decomposition of Fed Funds (t=1,4,8,12)')
disp('---------------------------------------------------')
mprint(FEVD_Table(9:12,:));


function  [h, compUp] =  lfmComputeH4(gamma1_p, gamma1_m, sigma2, t1, preFactor, preExp,...
    mode, term )
% LFMCOMPUTEH4 Helper function for computing part of the LFM kernel.
% FORMAT
% DESC computes a portion of the LFM kernel.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode: indicates in which way the vectors t1 and t2 must be transposed
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : David Luengo, 2007
%
% COPYRIGHT : Mauricio Alvarez, 2008
%
% MODIFICATIONS : Neil D. Lawrence, 2007
%
%
%
% SEEALSO : lfmKernParamInit, lfmXlfmKernCompute

% KERN

% Evaluation of h

if nargin<8
    term =[];
end

if ~mode
    if ~term
        if nargout>1
            compUp = lfmComputeUpsilonVector(gamma1_p,sigma2, t1);
            h = compUp*( preExp/preFactor(1) - conj(preExp)/preFactor(2)).';
        else
            h = lfmComputeUpsilonVector(gamma1_p,sigma2, t1)*( preExp/preFactor(1) - conj(preExp)/preFactor(2)).';
        end
    else
        if nargout>1
            compUp = lfmComputeUpsilonVector(gamma1_p,sigma2, t1);
            h = compUp*(preExp/preFactor(1)).' - conj(compUp)*(preExp/preFactor(2)).';
        else
            upsilon = lfmComputeUpsilonVector(gamma1_p,sigma2, t1);
            h = upsilon*(preExp/preFactor(1)).' - conj(upsilon)*(preExp/preFactor(2)).';
        end
    end
else
    if nargout > 1
        compUp{1} = lfmComputeUpsilonVector(gamma1_p,sigma2, t1);
        compUp{2} = lfmComputeUpsilonVector(gamma1_m,sigma2, t1);
        h =  compUp{1}*( preExp(:,1)/preFactor(1) - preExp(:,2)/preFactor(2)).' ...
            + compUp{2}*( preExp(:,2)/preFactor(3) - preExp(:,1)/preFactor(4)).';
    else
        h =  lfmComputeUpsilonVector(gamma1_p,sigma2, t1)*( preExp(:,1)/preFactor(1) - preExp(:,2)/preFactor(2)).' ...
            + lfmComputeUpsilonVector(gamma1_m,sigma2, t1)*( preExp(:,2)/preFactor(3) - preExp(:,1)/preFactor(4)).';
    end
end


function beta_vector = scattering_coefficient_random(number_of_images,...
    parameters)
%SCATTERING_COEFFICIENT_RANDOM  Generate scattering coefficient values for a set
%of images uniformly at random.
%   Inputs:
%       -|number_of_images|: number of images for which scattering coefficient
%       is simulated.
%       -|parameters|: structure containing miscellaneous parameters, such as
%       range of values for scattering coefficient and type of random number
%       generator. Guarantees uniformity with other functions that implement
%       generation of scattering coefficient values.
%
%   Outputs:
%       -|beta_vector|: 1-by-|number_of_images| vector containing the random
%       values of scattering coefficient for every image in the set.

% Determine the range of random values for scattering coefficient.
maximum_value = parameters.maximum_value;
minimum_value = parameters.minimum_value;

% Get type of random number generator, e.g. 'default'.
random_generator = parameters.random_generator;

% Get binary flag that indicates whether the random number generator should be
% configured or not.
configure_random_generator = parameters.configure_random_generator;

% Optionally configure random number generation for repeatability.
if configure_random_generator
    rng(random_generator);
end

% Generate random scattering coefficient for each image following a uniform
% distribution inside the specified range.
beta_vector = minimum_value + (maximum_value - minimum_value) *...
    rand(1, number_of_images);

end


function matrix = genMatEllPPIFE3D(pde,mesh,fem,femI,femIF,PPtype,sig)
%% Generate global matrices and load vector of PPIFEM for 3D ellipic eq
%     -div(A grad u)  = f,    x\in \Omega
%      where A is a piecewise constant on Omega^+ and Omega^-.
% INPUTS:
% pde --- given data function from equation, e.g.
%         pde.A --- diffusion coefficient
%         pde.f --- right hand side function
%         pde.gD --- Dirichlet boundary value function
%         pde.one --- constant function 1.
% mesh --- mesh structure.
% fem --- global degree of freedom of FEM
% femI --- quadrature info on interface cells
% femIF --- quadrature info on interface faces, required in PPIFE.
% PPtype --- partial penalty type possible value
%            'N' : Nonsymmetric PPIFE (e = 1)
%            'S' : Symmetric PPIFE (e = -1)
%            'I' : Incomplete PPIFE (e = 0)
% OUTPUTS:
% matrix.S --- stiffness matrix (w/o boundary condition)
% matrix.E --- consistence matrix (w/o boundary condition)
% matrix.P --- penalty matrix (w/o boundary condition)
% matrix.A --- final FEM matrix (after boundary condition)
% matrix.rhsF --- load vector (w/o boundary condition)
% matrix.rhsE --- consistence vector (=0 if no interface face on boundary)
% matrix.rhsP --- penalty vector (=0 if no interface face on boundary)
% matrix.f --- final RHS matrix (after boundary condition)

% Last Modified: 08/07/2020 by Xu Zhang

%% 1. Stiffness Matrix
S = globMatrixIFE3DStiff(pde.A,mesh,femI,fem,fem);
% This matrix generator is a little faster than doing it separately.
% Sx = globMatrixIFE3D(pde.A,mesh,femI,fem,[1,0,0],fem,[1,0,0]);
% Sy = globMatrixIFE3D(pde.A,mesh,femI,fem,[0,1,0],fem,[0,1,0]);
% Sz = globMatrixIFE3D(pde.A,mesh,femI,fem,[0,0,1],fem,[0,0,1]);
% S = Sx+Sy+Sz;

%% 2. Face Matrices
d1 = 0; j1 = 1; d2 = 1; j2 = 0;
E = globMatrixIFE3DFace(pde.one, pde.A, fem, fem, femIF, d1,j1, d2,j2);

d1 = 0; j1 = 1; d2 = 0; j2 = 1;
P = globMatrixIFE3DFace(pde.one, pde.one,fem,fem, femIF, d1,j1, d2,j2);

%% 3. Generate the Right Hand Side Vector
rhsF = globRHSIFE3D(pde.f, mesh, fem, femI, [0,0,0]);
rhsE = globRHSIFE3DFace(pde.A, pde.exactu, fem, femIF, 1);
rhsP = globRHSIFE3DFace(pde.one, pde.exactu, fem, femIF, 0);

%% 4. Dirichlet Boundary Conditions
if strcmp(PPtype,'N')
    e = 1;
elseif strcmp(PPtype,'S')
    e = -1;
elseif strcmp(PPtype,'I')
    e = 0;
end
h = mesh.p(2,1) - mesh.p(1,1);
Atotal = S - E + e*E' + (sig/h)*P;
rhs = rhsF + e*rhsE + (sig/h)*rhsP;
tu = feval(pde.gD,fem.p(:,1),fem.p(:,2),fem.p(:,3));
ub = tu;
ub(fem.mapper) = 0;
rhsB = Atotal*ub;
A = Atotal(fem.mapper,fem.mapper);
f = rhs(fem.mapper) - rhsB(fem.mapper);

%% 5. Outputs
matrix = struct('A',A,'f',f,'S',S,'E',E,'P',P,'rhsF',rhsF,'rhsE',rhsE,...
    'rhsP',rhsP);


function [post nlZ dnlZ] = infExactWarp(hyp, mean, cov, lik, x, y)

% Exact inference for a GP with Gaussian likelihood. Compute a parametrization
% of the posterior, the negative log marginal likelihood and its derivatives
% w.r.t. the hyperparameters. See also "help infMethods".
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2015-07-13.
%                                      File automatically generated using noweb.
%
% See also INFMETHODS.M.

if iscell(lik), likstr = lik{1}; else likstr = lik; end
if ~ischar(likstr), likstr = func2str(likstr); end
if ~strcmp(likstr,'likGaussWarpExact')               % NOTE: no explicit call to likGaussWarpExact
  error('Exact inference only possible with warped exact Gaussian likelihood');
end
warp = lik{2}; if ischar(warp); warp = str2func(warp); end

ng = feval(warp{:});               % number of hyperparameters for the warping function
nhyp = feval(lik{:});              % number of hyperparameters
% if nargin<4, varargout = {nhyp}; return, end       % report number of parameters
nhyp = eval(nhyp);
if nhyp>length(hyp.lik), error('not enough hyperparameters'), end

[gy,lgpy] = feval(warp{:},y,hyp.lik(1:ng));                     % evaluate warping function

% figure(100); scatter(y,gy); drawnow;

[n, D] = size(x);
K = feval(cov{:}, hyp.cov, x);                      % evaluate covariance matrix
m = feval(mean{:}, hyp.mean, x);                    % evaluate mean vector
[gm,lgpm] = feval(warp{:},m,hyp.lik(1:ng));         % evaluate warped mean

sn2 = exp(2*hyp.lik(end));                          % noise variance of likGauss
if sn2<1e-6                        % very tiny sn2 can lead to numerical trouble
  L = chol(K+sn2*eye(n)); sl =   1;   % Cholesky factor of covariance with noise
  pL = -solve_chol(L,eye(n));                            % L = -inv(K+inv(sW^2))
else
  L = chol(K/sn2+eye(n)); sl = sn2;                       % Cholesky factor of B
  pL = L;                                           % L = chol(eye(n)+sW*sW'.*K)
end
alpha = solve_chol(L,gy-gm)/sl;

post.alpha = alpha;                            % return the posterior parameters
post.sW = ones(n,1)/sqrt(sn2);                  % sqrt of noise precision vector
post.L = pL;

if nargout>1                               % do we want the marginal likelihood?
  nlZ = (gy-gm)'*alpha/2 + sum(log(diag(L))) + n*log(2*pi*sl)/2 - sum(lgpy);   % -log marg lik
  if nargout>2                                         % do we want derivatives?
    dnlZ = hyp;                                 % allocate space for derivatives
    Q = solve_chol(L,eye(n))/sl - alpha*alpha';     % precompute for convenience
    for i = 1:numel(hyp.cov)
      dnlZ.cov(i) = sum(sum(Q.*feval(cov{:}, hyp.cov, x, [], i)))/2;
    end
    for i = 1:ng
        [dgy,dlgpy] = feval(warp{:},y,hyp.lik(1:ng),i);
        dgm = feval(warp{:},m,hyp.lik(1:ng),i);
        dnlZ.lik(i) = -sum(dlgpy) + (dgy - dgm)'*alpha;
    end
    dnlZ.lik(ng+1) = sn2*trace(Q);
    for i = 1:numel(hyp.mean)
      dnlZ.mean(i) = -(exp(lgpm)'.*feval(mean{:}, hyp.mean, x, i)')*alpha;
    end
  end

end


function [chirpF, Freqs] = bst_chirplet(sRate, nTime, chirpCenterFreqs)
% BST_CHIRPLET: Compute the Phase-Amplitude Coupling in one of several time series (directPAC)
%
% INPUTS:
%    - sRate  : Signal sampling rate (in Hz)
%    - nTime  : Number of time points of the signal to filter
%    - chirpCenterFreqs: Center frequencies of the chirplets to calculate
%
% DOCUMENTATION:
%    - The current code is inspired from Ryan Canolty's code provided originally with the article:
%         Canolty RT, Edwards E, Dalal SS, Soltani M, Nagarajan SS, Kirsch HE, Berger MS, Barbaro NM, Knight RT,
%         "High gamma power is phase-locked to theta oscillations in human neocortex",
%         Science, 2006 Sep 15;313(5793):1626-8.

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Ryan Canolty, 2006
%          Sylvain Baillet, 2011-2013
%          Francois Tadel, 2013

% ===== PARSE INPUTS =====
% To avoid out-of-memeory issues: Check this on your machine, machine-specific threshold
% ***** TODO: EVALUALATE THOSE LINES *****
if (nTime > 2^23)
    nFreq = nTime;
else
    % Fixed parameter for computational ease
    nFreq = 2^ceil(log2(nTime));
end
% Raw time_support
Freqs = (sRate/nFreq) * (0:nFreq-1);
inds = Freqs > (sRate/2);
Freqs(inds) = Freqs(inds) - sRate;
% Reduce storage space
% % ***** TODO: EVALUALATE THIS LINES *****
% Freqs = single(Freqs);

% ===== CALCULATE CHIRPLETS =====
% Initialize returned matrix
chirpF = zeros(1, nFreq, length(chirpCenterFreqs));
% Make set of chirplets
fbw = 0.15;
for iif = 1:length(chirpCenterFreqs)
    % Assign or compute duration parameter
    v0 = chirpCenterFreqs(iif);  % center_frequency
    c0 = 0;  % chirp_rate
    s0 = log((2*log(2)) / (fbw^2*pi*v0^2));
    % Frequency support
    std_multiple = 6;
    vstd = sqrt((exp(-s0) + c0^2*exp(s0)) / (4*pi));
    v = Freqs; % in Hz
    iFreq = find(...
        (v0 - std_multiple * vstd <= v) & ...
        (v <= v0 + std_multiple * vstd));
    % Shorten to include only chirplet support
    v = v(iFreq);
    % Chirplet in frequency domain:
    Gk = 2^(1/4)*sqrt(-1i*c0+exp(-s0))^-1 * exp(-s0/4 + (exp(s0)*pi*(v-v0).^2)/(-1+1i*c0*exp(s0)));
    n1 = sqrt(length(Freqs)) / norm(Gk);
    % Because of discrete sampling and different time/freq sample numbers
    Gk = n1 * Gk;  % filter
    % Report in returned structures
    chirpF(1, iFreq, iif) = Gk;
end


function [mu, sigma, sigma_points] = prediction_step(mu, sigma, u)
% Updates the belief concerning the robot pose according to the motion model.
% mu: state vector containing robot pose and poses of landmarks obeserved so far
% Current robot pose = mu(1:3)
% Note that the landmark poses in mu are stacked in the order by which they were observed
% sigma: the covariance matrix of the system.
% u: odometry reading (r1, t, r2)
% Use u.r1, u.t, and u.r2 to access the rotation and translation values

% For computing lambda.
global scale;

% Compute sigma points
sigma_points = compute_sigma_points(mu, sigma);

% Dimensionality
n = length(mu);
% lambda
lambda = scale - n;

% TODO: Transform all sigma points according to the odometry command
% Remember to vectorize your operations and normalize angles
% Tip: the function normalize_angle also works on a vector (row) of angles
for i=1:2*n+1
    sigma_points(1:3,i) = sigma_points(1:3,i) + [u.t*cos(sigma_points(3,i) + u.r1); u.t*sin(sigma_points(3,i) + u.r1); u.r1 + u.r2];
    sigma_points(3,i) = normalize_angle(sigma_points(3,i));
endfor
% Computing the weights for recovering the mean
wm = [lambda/scale, repmat(1/(2*scale),1,2*n)];
wc = wm;

% -------- My initialization ----------- %
xbar = 0;
ybar = 0;
mu = zeros(n,1);
sigma = zeros(n);
% TODO: recover mu.
% Be careful when computing the robot's orientation (sum up the sines and
% cosines and recover the 'average' angle via atan2)
for i=1:2*n+1
    mu = mu + wm(i)*sigma_points(:,i);
    xbar = xbar + wm(i)*cos(sigma_points(3,i));
    ybar = ybar + wm(i)*sin(sigma_points(3,i));
endfor
mu(3) = atan2(ybar,xbar);
mu(3) = normalize_angle(mu(3));

% TODO: Recover sigma. Again, normalize the angular difference
for i=1:2*n+1
    tmp = sigma_points(:,i) - mu;
    tmp(3) = normalize_angle(tmp(3));
    sigma = sigma + wc(i)*tmp*tmp';
endfor

% Motion noise
motionNoise = 0.1;
R3 = [motionNoise, 0, 0;
     0, motionNoise, 0;
     0, 0, motionNoise/10];
R = zeros(size(sigma,1));
R(1:3,1:3) = R3;

% TODO: Add motion noise to sigma
sigma = sigma + R;

end


function varargout = weights_int(mode, n, t0, Hdyn, Zdyn, Tdyn, Rdyn, Qdyn, Hmat, Zmat, Tmat, Rmat, Qmat, P, tol)
% mode:
%   0 - all primitive outputs.
%   1 - calculate Kalman filter weights.
%   2 - calculate Kalman smoother weights.

%% Initialization %%
m       = size(P, 1);
D       = (P == Inf);
init    = any(any(D)); % use exact initialization if init = true.
if init, d = n+1; P(D) = 0; P_inf = double(D);
else d = 0; end
stationary  = ~Hdyn && ~Zdyn && ~Tdyn && ~Rdyn && ~Qdyn;
converged   = false;
RQdyn       = Rdyn || Qdyn;
if ~Hdyn, H = Hmat; end
if ~Zdyn, Z = Zmat; end
if ~Tdyn, T = Tmat; end
if ~Rdyn, R = Rmat; end
if ~Qdyn, Q = Qmat; end
if ~RQdyn, RQRt = R*Q*R'; end

%% Preallocate Output Results %%
switch mode
    case 0 % all primitive outputs
        Result_P    = cell(1, n);
        Result_P{1} = P;
        Result_invF = cell(1, n);
        Result_K    = cell(1, n);
        Result_L    = cell(1, n);
        Result_N    = cell(1, n);
        Result_W    = cell(1, n);
    case 1 % calculate Kalman filter weights
        Result_K        = cell(1, t0-1);
        Result_L        = cell(1, t0-1);
        Result_omega    = cell(1, t0-1);
    case 2 % calculate Kalman smoother weights
        Result_invF         = cell(1, n);
        Result_K            = cell(1, n);
        Result_L            = cell(1, n);
        Result_LLasc        = cell(1, n);
        if t0 > 1, Result_LLasc{t0-1} = eye(m);
        else Result_P = P; end % t0 == 1
        Result_omega        = cell(1, t0-1);
        Result_omegaalpha   = cell(1, n);
end
Fns     = true(1, n); % Is F_inf nonsingular for each iteration

%% Kalman filter loop %%
for t = 1 : n
    if Hdyn, H = Hmat{t}; end
    if Zdyn, Z = Zmat{t}; end
    if Tdyn, T = Tmat{t}; end
    if Rdyn, R = Rmat{t}; end
    if Qdyn, Q = Qmat{t}; end
    if RQdyn, RQRt = R*Q*R'; end
    if ~converged
        if init
            %% Exact initial Kalman filter %%
            M       = P*Z';
            M_inf   = P_inf*Z';
            A_inf   = T*P_inf;
            if abs(M_inf) < tol % F_inf is zero
                Fns(t)  = false;
                invF    = inv(Z*M + H);
                K       = T*M*invF;
                L       = T - K*Z;
                P       = T*P*L' + RQRt;
                P_inf   = A_inf*T';
            else % F_inf is assumed to be nonsingular
                invF    = inv(Z*M_inf); % This is actually invF1
                F2      = -invF*(Z*M + H)*invF;
                K       = T*M_inf*invF;
                K1      = T*(M*invF + M_inf*F2);
                L       = T - K*Z;
                L1      = -K1*Z;
                P       = A_inf*L1' + T*P*L' + RQRt;
                P_inf   = A_inf*L';
            end
            if abs(P_inf) < tol, d=t; init=false; end
        else
            %% Normal Kalman filter %%
            M       = P*Z';
            invF    = inv(Z*M + H);
            K       = T*M*invF;
            L       = T - K*Z;
            prevP   = P;
            P       = T*P*L' + RQRt;
            if stationary, if abs(P-prevP) < tol, converged = true; end, end
        end
    end
    %% Store results for this time point %%
    switch mode
        case 0 % all primitive outputs
            Result_P{t+1}   = P;
            Result_invF{t}  = invF;
            Result_K{t}     = K;
            Result_L{t}     = L;
        case 1 % calculate Kalman filter weights
            Result_K{t}     = K;
            Result_L{t}     = L;
            if t >= t0 - 1, break; end
        case 2 % calculate Kalman smoother weights
            if t == t0 - 1, Result_P    = P; end
            if t == t0, Result_LLasc{t} = L'; end
            Result_invF{t}  = invF;
            Result_K{t}     = K;
            Result_L{t}     = L;
            if t > t0
                Result_LLasc{t} = Result_LLasc{t-1}*L';
            end
    end
end

%% Backwards recursion %%
switch mode
    case 0 % all primitive outputs
        N       = zeros(m);
        for t = n : -1 : 1
            if Hdyn, H = Hmat{t}; end
            if Zdyn, Z = Zmat{t}; end
            invF        = Result_invF{t};
            K           = Result_K{t};
            L           = Result_L{t};
            Result_W{t} = H*(invF*Z - K'*N*L);
            Result_N{t} = N;
            if t <= d && Fns(t), N = L'*N*L; else N = Z'*invF*Z + L'*N*L; end
        end
    case 1 % calculate Kalman filter weights
        LLdsc   = eye(m);
        for t = t0-1 : -1 : 1
            Result_omega{t} = LLdsc*Result_K{t};
            LLdsc           = LLdsc*Result_L{t};
        end
    case 2 % calculate Kalman smoother weights
        N       = zeros(m);
        LLdsc   = eye(m);
        for t = n : -1 : 1
            if t >= t0
                if t <= d && Fns(t)
                    K       = Result_K{t};
                    L       = Result_L{t};
                    W2      = -K'*N*L;
                    N       = L'*N*L;
                else
                    if Zdyn, Z = Zmat{t}; end
                    invF    = Result_invF{t};
                    K       = Result_K{t};
                    L       = Result_L{t};
                    W2      = invF*Z - K'*N*L;
                    N       = Z'*invF*Z + L'*N*L;
                end
                if t == t0
                    Result_omegaalpha{t} = Result_P*W2';
                    IPN = eye(m) - Result_P*N;
                else Result_omegaalpha{t} = Result_P*Result_LLasc{t-1}*W2'; end
            else
                Result_omega{t}         = LLdsc*Result_K{t};
                LLdsc                   = LLdsc*Result_L{t};
                Result_omegaalpha{t}    = IPN*Result_omega{t};
            end
        end
end

%% Output Results %%
switch mode
    case 0 % all primitive outputs
        varargout = {Result_P Result_invF Result_K Result_L Result_W Result_N};
    case 1 % calculate Kalman filter weights
        varargout = {Result_omega};
    case 2 % calculate Kalman smoother weights
        varargout = {Result_omega Result_omegaalpha};
end