classdef SymBuilder < handle
%SYMBUILDER  Create a SYMBUILDER object for creating symbolic optimization problems
%
%   B = SymBuilder() creates a blank optimization problem
%
%   See also SymBuilder.AddObj SymBuilder.AddCon SymBuilder.Build
%
%   Copyright (C) 2012 Jonathan Currie (www.inverseproblem.co.nz)

    properties(SetAccess=private)
        vars        %Symbolic array of symbolic variables
        sobj        %Symbolic matrix of equations
        jac         %Symbolic jacobian of equations
        hess        %Structure of hessians of each equation
        hesslag     %Symbolic hessian of the lagrangian
        Opt         %OPTI object
        lastSolveOpts %Last Options passed when Solve() was called
    end

    properties(SetAccess=private)%,GetAccess=private)
        eqs         %Equations symbolic array
        cl          %Constraint Lower Bounds
        cu          %Constraint Upper Bounds
        constnt     %Cell array of constants
        exprsn      %Cell array of expressions
        bnds        %Cell array of bounds
        lb          %Double array of lower bounds
        ub          %Double array of upper bounds
        vartypes    %Cell array of variable types
        xtype       %Char array of variable types
        indobj      %Index of objectives in equations
        objlin      %Index of linear, quadratic and nonlinear objectives
        conlin      %Index of linear, quadratic and nonlinear constraints
        objbias     %Problem bias terms (for linear and quadratic problems)
        noObjs      %Counter of number objectives
        noCons      %Counter of number of constraints
        bldstat     %Build Status
        bldtime     %Build Time
        resgrp      %Cell array of result groups
        resexp      %Cell array of result expressions
        verbose     %Verbosity mode
    end

    methods
        function B = SymBuilder(verbose)
            if(nargin < 1), verbose = true; end
            if (~exist('syms.m','file')), error('The Symbolic Math Toolbox must be installed to use SymBuilder!'); end
            %Initialization
            B.noObjs = 0;
            B.noCons = 0;
            B.verbose = verbose;
            Unbuild(B);
        end

        function B = AddObj(B,str)
        %Add a General Objective (Linear or Nonlinear)

            %Check for string
            if(ischar(str))
                digits(16);
                wstate = warning('off','symbolic:sym:sym:DeprecateExpressions');
                s = sym(str);
                warning(wstate);
            elseif(isa(str,'sym'))
                s = str;
            else
                error('Unknown objective form');
            end

            %Concatenate with existing equations
            B.eqs = [B.eqs;s];
            B.cl = [B.cl;NaN];
            B.cu = [B.cu;NaN];
            if(isempty(B.indobj))
                B.indobj = length(B.cl);
            else
                B.indobj = [B.indobj;length(B.cl)];
            end
            %Add no objs
            B.noObjs = B.noObjs + 1;
            %Indicate Rebuild Required
            Unbuild(B);
        end

        function B = AddCon(B,str_in,cl,cu)
        %Add a General Constraint (Linear or Nonlinear)

            %Check for string
            if(ischar(str_in))
                %Parse constraint string
                [str,l,u] = SymBuilder.parseConstraint(str_in);
                digits(16);
                wstate = warning('off','symbolic:sym:sym:DeprecateExpressions');
                s = sym(str);
                warning(wstate);
            elseif(isa(str_in,'sym'))
                if(nargin < 4 || ~isnumeric(cl) || ~isnumeric(cu) || length(cl)~=length(cu) || length(cl)~=length(str_in))
                    error('When supplying a symbolic expression to AddCon, you must supply as AddCon(sym_con,cl,cu) with cl, cu as numeric vectors of the same length.');
                end
                if(size(str_in,2) > 1)
                    s = str_in.';
                else
                    s = str_in;
                end
                l = cl; u = cu;
            else
                 error('Unknown constraint form');
            end
            %Concatenate with existing equations if not empty
            if(~isempty(s))
                B.eqs = [B.eqs;s];
                B.cl = [B.cl;l];
                B.cu = [B.cu;u];
                %Add no cons
                B.noCons = B.noCons + length(l);
                %Indicate Rebuild Required
                Unbuild(B);
            else
                optiwarn('symb:emptycon','The following constraint will be ignored by SymBuilder as both sides appear to be numeric.\n  %s\n',str_in);
            end
        end

        function B = AddLinCon(B,x,A,rl,ru)
        %Add a Linear Constraint

            if(nargin < 5 || ~isnumeric(rl) || ~isnumeric(ru) || length(rl)~=length(ru) || length(rl)~=size(A,1))
                error('When supplying linear constraints, you must supply as AddLinCon(x,A,rl,ru) with rl, ru as numeric vectors of the same length.');
            elseif(~isa(x,'sym') || length(x) ~= size(A,2))
                error('x must be a symbolic variable vector = size(A,2)');
            end
            %Evaluate Matrix & Store
            B.eqs = [B.eqs;A*x];
            B.cl = [B.cl;rl];
            B.cu = [B.cu;ru];
            %Add no cons
            B.noCons = B.noCons + length(rl);
            %Indicate Rebuild Required
            Unbuild(B);
        end

        function B = AddConstraint(B,str)
        %Same as AddCon()
            B = AddCon(B,str);
        end

        function B = AddConstant(B,str,val,varargin)
        %Identify variable as a constant

            %Concatenate with existing constants
            B.constnt = [B.constnt; {str val}];
            %If user has passed multiple constants, process all
            if(nargin > 3 && ~isempty(varargin))
                if(mod(length(varargin),2))
                    error('You must supply a name and value for each constant');
                else
                    for i = 1:2:length(varargin)
                        B.constnt = [B.constnt; varargin(i:i+1)];
                    end
                end
            end
            %Indicate Rebuild Required
            Unbuild(B);
        end

        function B = AddExpression(B,str,grp,ex)
        %Identify variable as an expression

            %Parse expression string
            [str,exp] = SymBuilder.parseExpression(str);
            %Concatenate with existing expressions
            B.exprsn = [B.exprsn; {str exp}];
            %If group passed, add to result group
            if(nargin > 3), B = AddResultExp(B,grp,exp,ex);
            elseif(nargin > 2), B = AddResultExp(B,grp,exp); end
            %Indicate Rebuild Required
            Unbuild(B);
        end

        function B = AddBound(B,str)
        %Add variable bound

            %Parse expression string
            [var,llb,lub] = SymBuilder.parseBound(str);
            %Concatenate with existing bounds
            B.bnds = [B.bnds; {var llb lub}];
            %Indicate Rebuild Required
            Unbuild(B);
        end

        function B = AddBounds(B,x,lb,ub)
        %Add numerical bounds

            bds = cell(length(x),3);
            for i = 1:length(x)
                bds{i,1} = char(x(i));
                bds{i,2} = sprintf('%1.15g',lb(i));
                bds{i,3} = sprintf('%1.15g',ub(i));
            end
            %Concatenate with existing bounds
            B.bnds = [B.bnds; bds];
            %Indicate Rebuild Required
            Unbuild(B);
        end

        function B = AddInteger(B,str)
        %Add integer constraint

            %Parse expression string
            [var,xx] = SymBuilder.parseInteger(str);
            %Concatenate with existing constants
            B.vartypes = [B.vartypes; {var xx}];
            %Indicate Rebuild Required
            Unbuild(B);
        end

        function B = AddIntegers(B,x,str)
        %Add integer constraints with character string

            xx = cell(length(x),2);
            for i = 1:length(x)
                xx{i,1} = char(x(i));
                xx{i,2} = str(i);
            end
            %Concatenate with existing constants
            B.vartypes = [B.vartypes; xx];
            %Indicate Rebuild Required
            Unbuild(B);
        end

        function B = AddResultGroup(B,name,str)
        %Add Result Group
            %Concatenate with existing groups
            B.resgrp = [B.resgrp; {name str}];
        end

        function B = AddResultExp(B,name,str,bin)
        %Add Result Expression
            %Parse expression string
            [group,name] = SymBuilder.parseResExp(name);
            %Concatenate with existing groups
            if(nargin > 3)
                B.resexp = [B.resexp; {group name str bin}];
            else
                B.resexp = [B.resexp; {group name str []}];
            end
        end

        function B = Draft(B)
        %Draft the object by solving for the system Jacobian
            t = tic;
            %Build Symbolic representation of the equations
            if(B.verbose), fprintf('\nGenerating Symbolic Representation of Equations...'); end
            buildSymRep(B);
            if(B.verbose), fprintf('Done\n'); end
            %Generate Equations Jacobian
            if(B.verbose), fprintf('Generating Symbolic Jacobian...'); end
            buildJac(B);
            if(B.verbose), fprintf('Done\n'); end
            %Determine Linear & Nonlinear Equations
            if(B.verbose), fprintf('Generating Equation Linearity...'); end
            detLinearity(B);
            if(B.verbose), fprintf('Done\n'); end
            %Save build time
            B.bldtime = toc(t);
            %Assign build Status
            B.bldstat = 'draft';
        end

        function B = Build(B)
        %Build the object by solving for the system Hessian
            t = tic;
            %Build Symbolic representation of the equations
            if(B.verbose), fprintf('\nGenerating Symbolic Representation of Equations...'); end
            buildSymRep(B);
            if(B.verbose), fprintf('Done\n'); end
            %Generate Equations Jacobian
            if(B.verbose), fprintf('Generating Symbolic Jacobian...'); end
            buildJac(B);
            if(B.verbose), fprintf('Done\n'); end
            %Generate Equations Hessians (also determines linearity)
            if(B.verbose), fprintf('Generating Symbolic Hessian...'); end
            buildHess(B);
            if(B.verbose), fprintf('Done\n'); end
            %Save build time
            B.bldtime = toc(t);
            %Assign build Status
            B.bldstat = 'built';
        end

        function B = Unbuild(B)
        %Unbuild object (free memory)

            B.jac = [];
            B.hess = [];
            B.hesslag = [];
            B.bldstat = 'unbuilt';
            B.bldtime = 0;
            B.Opt = [];

            clear symb_cb
        end

        function prob = GetLinProb(B)
        %Get Linear Problem Matrices [min f'*x s.t. rl <= A*x <= ru]

            %Check is built
            IsBuilt(B,1);
            prob = optiprob;
            %Get Linear Objective Index
            ind = B.objlin == 1;
            if(sum(ind) == 0)
                error('This object does not contain a linear objective');
            end
            prob.f = double(B.jac(ind,:))';
            %Get Linear Constraint Indices
            ind = B.conlin == 1;
            %Check we have some constraints
            if(B.noCons && sum(ind) > 0)
                if(isempty(B.objbias)), B.objbias = zeros(size(ind)); end %HACK
                %Get Linear Jacobian
                prob.A = sparse(double(B.jac(ind,:)));
                prob.rl = B.cl(ind) - B.objbias(ind);
                prob.ru = B.cu(ind) - B.objbias(ind);
            end
            %Bounds & Integer Vars
            prob.lb = B.lb;
            prob.ub = B.ub;
            prob.int = B.xtype;
            prob.objbias = B.objbias(B.indobj);
            %Problem Name
            if(any(B.xtype=='I'))
                prob.Name = 'SymBuilder MILP';
            elseif(any(B.xtype=='B'))
                prob.Name = 'SymBuilder BILP';
            else
                prob.Name = 'SymBuilder LP';
            end
        end

        function prob = GetQuadProb(B)
        %Get Quadratic Problem Matrices [min 0.5*x'*H'x + f'*x
        %s.t. rl <= A*x <= ru, qrl <= x'*Q*x + l'*x <= qru]

            %Check is built
            IsBuilt(B,1);
            prob = optiprob;
            %Get quadratic objective indices
            ind = B.objlin == 2;
            if(isempty(ind))
                error('This object does not contain an objective');
            elseif(sum(ind) > 1)
                error('This interface only supports a single quadratic objective');
            end
            %Get Hessian (DON'T NEED 0.5!)
            if(~any(ind)) %linear objective
                ind = B.objlin == 1;
                n = size(B.jac,2);
                prob.H = spalloc(n,n,0);
                prob.f = double(B.jac(ind,:));
            else
                prob.H = sparse(double(B.hess(ind).H));
                %Get Gradient, remove 2nd derivative parts
                f = B.jac(ind,:); v = symvar(f);
                prob.f = double(subs(f,v,{zeros(size(v))}).');
            end

            %Get Linear Constraint Indices
            ind = B.conlin == 1;
            %Check we have some constraints
            if(B.noCons && sum(ind) > 0)
                %Get Linear Jacobian
                prob.A = sparse(double(B.jac(ind,:)));
                prob.rl = B.cl(ind) - B.objbias(ind);
                prob.ru = B.cu(ind) - B.objbias(ind);
            end
            %Get Quadratic Constraint Indices
            ind = B.conlin == 2;
            %Check we have some constraints
            if(B.noCons && sum(ind) > 0)
                prob.qrl = B.cl(ind) - B.objbias(ind);
                prob.qru = B.cu(ind) - B.objbias(ind);
                ind = find(ind);
                %Get each quadratic constraint
                len = length(ind);
                prob.Q = cell(len,1);
                prob.l = cell(len,1);
                for i = 1:len
                    %Get Hessian (0.5* requried!)
                    prob.Q{i} = 0.5*sparse(double(B.hess(ind(i)).H));
                    %Get Gradient, remove 2nd derivative parts
                    f = B.jac(ind(i),:); v = symvar(f);
                    prob.l{i} = double(subs(f,v,{zeros(size(v))}).');
                end
            end
            %Bounds & Integer Vars
            prob.lb = B.lb;
            prob.ub = B.ub;
            prob.int = B.xtype;
            prob.objbias = B.objbias(B.indobj);
            %Problem Name
            if(any(B.xtype=='I'))
                prob.Name = 'SymBuilder MIQP';
            else
                prob.Name = 'SymBuilder QP';
            end
        end

        function prob = GetNLProb(B,opts)
        %Get Nonlinear Problem Callbacks, generating files as we go

            if(nargin < 2), opts = symbset; end
            opts.verbose = B.verbose;
            %Check is built
            IsBuilt(B,1);
            prob = optiprob;
            %Determine most efficient callback form IF auto selected
            if(strcmpi(opts.cbmode,'auto'))
                %Only relevant if we have any nonlinear terms & > 8 variables
                if((any(B.objlin > 2) || any(B.conlin > 2)) && length(B.vars) > 8)
                    %See if we have 2nd deriv info
                    if(~isempty(B.hess))
                        %Generate Hess Lag (will need it later anyway)
                        buildHessLag(B);
                        %Build Hessian Structure
                        lHstr = zeros(size(B.hesslag));
                        lHstr(logical(B.hesslag ~= 0)) = 1;
                        %If greater than 10% dense, use CppAD
                        if(nnz(lHstr)/numel(lHstr) > 0.1)
                            if(SymBuilder.CheckCppADCompile(false))
                                opts.cbmode = 'cppad';
                            else
                                opts.cbmode = 'mcode';
                            end
                        else
                            opts.cbmode = 'mcode';
                        end
                    %Only 1st deriv info
                    else
                        ind = B.conlin > 0;
                        ajac = B.jac(ind,:);
                        %Build Jacobian Structure
                        jacstr = zeros(size(ajac));
                        jacstr(logical(ajac ~= 0)) = 1;
                        %If greater than 10% dense, use CppAD
                        if(nnz(jacstr)/numel(jacstr) > 0.1)
                            if(SymBuilder.CheckCppADCompile(false))
                                opts.cbmode = 'cppad';
                            else
                                opts.cbmode = 'mcode';
                            end
                        else
                            opts.cbmode = 'mcode';
                        end
                    end
                else
                    opts.cbmode = 'mcode';
                end
            end
            %Determine whether we are generating C or M files
            if(strcmpi(opts.cbmode,'mcode'))
                fgen = 'M';
            elseif(strcmpi(opts.cbmode,'cppad'))
                fgen = 'A';
            else
                fgen = 'C';
            end
            %Create New Callback Function based on req'd callbacks
            opts.havCon = any(B.conlin > 0);
            if(fgen == 'M')
                B.buildMFun('new',[],[],opts);
            else
                B.buildCFun('new',[],[],opts);
            end
            %Get Objective (Nonlinear or not, returned as f(x))
            ind = B.objlin >= 1;
            if(sum(ind) == 0)
                error('This object does not contain an objective');
            end
            %Build Objective Callback
            if(fgen == 'M')
                prob.fun = B.buildMFun('obj',B.sobj(ind),B.vars,opts);
            else
                prob.fun = B.buildCFun('obj',B.sobj(ind),B.vars,opts);
            end
            %If requested, build gradient callback
            if(strcmpi(opts.use1stDerivs,'yes'))
                if(fgen == 'A')
                    prob.f = @(x) symb_ccb('grad',x); %no need to generate
                else
                    ajac = B.jac(ind,:);
                    if(fgen == 'M')
                        prob.f = B.buildMFun('grad',ajac,B.vars,opts);
                    else
                        prob.f = B.buildCFun('grad',ajac,B.vars,opts);
                    end
                end
            elseif(fgen=='C')
                B.buildCFun('grad'); %default empty
            end

            %If we have constraints, create callbacks
            if(opts.havCon)
                %Get ALL Constraint Indices
                ind = B.conlin > 0;
                %Build Con Callback + row constraints
                if(fgen == 'M')
                    prob.nlcon = B.buildMFun('con',B.sobj(ind),B.vars,opts);
                else
                    prob.nlcon = B.buildCFun('con',B.sobj(ind),B.vars,opts);
                end
                prob.cl = B.cl(ind);
                prob.cu = B.cu(ind);
                %If we want derivatives, create jacobian + structure
                if(strcmpi(opts.use1stDerivs,'yes'))
                    if(fgen == 'A')
                        prob.nljac = @(x) symb_ccb('jac',x); %no need to generate
                        prob.nljacstr = @() symb_ccb('jacstr');
                    else
                        ajac = B.jac(ind,:);
                        if(fgen == 'M')
                            prob.nljac = B.buildMFun('jac',ajac,B.vars,opts);
                        else
                            prob.nljac = B.buildCFun('jac',ajac,B.vars,opts);
                        end
                        %Build Jacobian Structure Callback
                        jacstr = zeros(size(ajac));
                        jacstr(logical(ajac ~= 0)) = 1;
                        prob.nljacstr = @() sparse(jacstr);
                    end
                elseif(fgen=='C')
                    B.buildCFun('jac'); %default empty
                end
            elseif(fgen=='C' || fgen=='A')
                B.buildCFun('con',[],[],opts); %default empty
                B.buildCFun('jac',[],[],opts); %default empty
            end
            %If we want 2nd derivatives, add them too
            if(strcmpi(opts.use2ndDerivs,'yes'))
                if(fgen == 'A')
                    prob.H = @(x,sigma,lambda) symb_ccb('hess',x,sigma,lambda); %no need to generate
                    prob.Hstr = @() symb_ccb('hstr');
                else
                    %Check if we have done a full build
                    if(isempty(B.hess))
                        if(B.verbose), optiwarn('No 2nd Derivatives Added - You may only obtain the Hessian of the Lagrangian if you called Build().\nDraft() only computes 1st Derivatives%s\n','.'); end
                        if(fgen=='C')
                            B.buildCFun('hess'); %default empty
                        end
                    else
                        %Build HessLag
                        buildHessLag(B);
                        %Build NL Hessian Callback
                        if(fgen=='M')
                            prob.H = B.buildMFun('hess',B.hesslag,B.vars,opts);
                        else
                            prob.H = B.buildCFun('hess',B.hesslag,B.vars,opts,B.noCons);
                        end
                        %Build Hessian Structure
                        lHstr = zeros(size(B.hesslag));
                        lHstr(logical(B.hesslag ~= 0)) = 1;
                        prob.Hstr = @() sparse(lHstr);
                    end
                end
            elseif(fgen=='C')
                B.buildCFun('hess'); %default empty
            end

            %Bounds & Integer Vars
            prob.lb = B.lb;
            prob.ub = B.ub;
            prob.int = B.xtype;
            %Problem Name
            if(any(B.xtype=='I'))
                prob.Name = 'SymBuilder MINLP';
            else
                prob.Name = 'SymBuilder NLP';
            end

            %Generate pretend x0 incase none given
            prob.x0 = (prob.ub - prob.lb)./2;
            prob.x0(isnan(prob.x0)) = 0;
            prob.x0(isinf(prob.x0)) = 0;

            %Compile if C Code
            if(fgen ~= 'M')
                if(B.verbose), fprintf('Compiling....'); end
                %If compiling with VS2015 but pre R2015a, need to manually add in UCRT location
                cc = mex.getCompilerConfigurations(); post = [];
                for i = 1:length(cc)
                    if(~isempty(strfind(cc(i).Name,'Microsoft Visual C++')) && str2double(cc(i).Version) >= 14 && verLessThan('matlab','8.5'))
                        post = opti_FindUCRT();
                        break;
                    end
                end
                if(fgen == 'A')
                    src = 'symb_ccb.cpp';
                    %find cppad source (expected in utilities folder)
                    str = which('asl');
                    if(isempty(str)), error('Cannot find asl to locate CppAD source!'); end
                    idx = strfind(str,filesep); str = str(1:idx(end));
                    str = [str 'Source'];
                    str = ['mex -largeArrayDims symb_ccb.cpp -I"' str '" ' post];
                    if(strcmpi(computer,'PCWIN'))
                        str = [str ' -DCPPAD_SIZE_T_SAME_UNSIGNED_INT=1'];
                    end
                	eval(str);
                else
                    src = 'symb_ccb.c';
                    str = ['mex -largeArrayDims symb_ccb.c ' post];
                    eval(str);
                end
                if(B.verbose), fprintf('Done\n'); end
                if(strcmpi(opts.srckeep,'no'))
                    delete(src);
                end
            end
        end

        %Get Build Status
        function tf = IsBuilt(B,doErr)
            if(nargin < 2), doErr = false; end
            if(strcmp(B.bldstat,'unbuilt'))
                tf = false;
                if(doErr)
                    error('Please build the object using Draft() or Build()');
                end
            else
                tf = true;
            end
        end

        function [x,fval,ef,info] = solve(B,x0,opts)
        %Lowercase version
            if(nargin < 3), opts = []; end
            if(nargin < 2), x0 = []; end
            [x,fval,ef,info] = Solve(B,x0,opts);
        end

        function [x,fval,ef,info] = Solve(B,x0,opts)
        %Solve Object using OPTI

            %Check is built
            if(~IsBuilt(B))
                error('Please build the object first using Draft() or Build()');
            end
            if(nargin < 3 || isempty(opts)), opts = symbset; end
            if(nargin < 2), x0 = []; end

            %Cusomtize settings based on solver
            switch(lower(opts.solver))
                %White box solvers
                case {'scip','baron'}
                     opts = symbset(opts,'use1stDerivs','no','use2ndDerivs','no','preallocate','no');
                %Derivative free solvers
                case {'nomad','pswarm','gmatlab'}
                    opts = symbset(opts,'use1stDerivs','no','use2ndDerivs','no');
                %No Hessian Support
                case {'filtersd','lbfgsb','nlopt'}
                    opts = symbset(opts,'use2ndDerivs','no');
            end
            if (isempty(B.Opt) || isempty(B.lastSolveOpts) || ~isequal(B.lastSolveOpts, opts))
                if(B.verbose), fprintf('\nGenerating OPTI Object....\n'); end
                B.Opt = GetOPTI(B,opts);
                B.lastSolveOpts = opts;
                if(B.verbose), fprintf('Done\n\n'); end
            else
                if(B.verbose), fprintf('\nRe-using OPTI Object\n'); end
            end
            %Solve
            [x,fval,ef,info] = solve(B.Opt,x0);
        end

        function B = plus(B,C)
        %Add objective, constraint, bound or integer declaration to SymBuilder Object
            if(~isa(B,'SymBuilder'))
                error('The LHS object must be a SymBuilder Object');
            end
            if(~ischar(C))
                error('The RHS object must be a string');
            end
            %Check for objective
            idx = strfind(C,'min');
            if(~isempty(idx))
                if(length(idx) > 1), error('min most only appear once in the objective'); end
                B.AddObj(C(idx(1)+4:end));
                return;
            end
            idx = strfind(C,'max');
            if(~isempty(idx))
                if(length(idx) > 1), error('max most only appear once in the objective'); end
                B.AddObj(['-(' C(idx(1)+4:end) ')']);
                return;
            end
            %Check for bound or integer (no mathematical ops)
            idx = regexp(C,{'*','+','-','/','^'});
            if(~isempty([idx{:}]))
                B.AddCon(C);
            else
                idx = regexp(C,{'<=','>='});
                if(~isempty([idx{:}]))
                    B.AddBound(C);
                else
                    B.AddInteger(C);
                end
            end

        end
    end

    methods(Access=private)

        function buildSymRep(B)
        %Build symbolic representation of the equations

            %Check if something new has been added
            if(strcmp(B.bldstat,'unbuilt'))
                %Build Symbolic Object
                symobj = B.eqs;
                %Substitute expressions
                if(~isempty(B.exprsn))
                    %Now subs into full equation system
                    wstate = warning('off','symbolic:sym:sym:DeprecateExpressions');
                    symobj = subs(symobj,B.exprsn(:,1),B.exprsn(:,2));
                    warning(wstate);
                    %Have to repeat until all nested expressions are sub'd
                    n = 10;  %max depth
                    no = size(B.exprsn,1);
                    se = sym(B.exprsn(:,1));
                    while n > 0
                        v = symvar(symobj); alldone = 1;
                        for i = 1:no
                            if(any(se(i) == v))
                                wstate = warning('off','symbolic:sym:sym:DeprecateExpressions');
                                symobj = subs(symobj,B.exprsn(i,1),B.exprsn(i,2));
                                warning(wstate);
                                alldone = 0;
                            else
                                if(i == no && alldone) %ensure we have checked them all
                                    n = -10;
                                    break;
                                end
                            end
                        end
                        n = n - 1;
                    end
                    if(n == 0)
                        error('Maximum expression recursion depth reached!');
                    else
%                         fprintf('%d exp recursions required\n',n+10);
                    end
                end
                %Convert constants to decimal form
%                 digits(16); %set vpa digits
%                 BV = cell(size(B.constnt,1),1);
%                 for i = 1:size(B.constnt,1)
%                     BV{i} = sym(B.constnt{i,2},'d');
%                 end
                %Substitute constants
                if(~isempty(B.constnt))
                    wstate = warning('off','symbolic:sym:sym:DeprecateExpressions');
                    symobj = subs(symobj,B.constnt(:,1),B.constnt(:,2));
                    warning(wstate);
                end
                %Save symbolic vector
                B.sobj = symobj;
                %Save Variables
                B.vars = symvar(B.sobj);

                %Build default bounds and xtype
                n = length(B.vars);
                B.lb = -1*Inf(n,1);
                B.ub = Inf(n,1);
                B.xtype = repmat('C',1,n);
                %Get names of variables and their indices
                names = SymBuilder.detVarNames(B.vars);
                if(isempty(names) && size(B.bnds,1) > 0)
                    error('Cannot process bounds without declared variables!');
                end

                %Process bound declarations
                for i = 1:size(B.bnds,1)
                    %Check if bounds are numerical
                    llb = str2double(B.bnds{i,2});
                    if(isnan(llb)) %not a number, check constants
                        try
                            ind = strcmp(B.constnt(:,1),B.bnds{i,2});
                            if(any(ind))
                                llb = B.constnt{ind,2};
                            else
                                error('Unknown lower bound: %s',B.bnds{i,2});
                            end
                        catch
                            error('Error processing lower bound for var %d',i);
                        end
                    end
                    lub = str2double(B.bnds{i,3});
                    if(isnan(lub)) %not a number, check constants
                        try
                            ind = strcmp(B.constnt(:,1),B.bnds{i,3});
                            if(any(ind))
                                lub = B.constnt{ind,2};
                            else
                                error('Unknown upper bound: %s',B.bnds{i,3});
                            end
                        catch
                            error('Error processing upper bound for var %d',i);
                        end
                    end
                    %Check if variable exists
                    ind = logical(B.vars == sym(B.bnds{i,1}));
                    if(any(ind))
                        if(~isinf(llb)), B.lb(ind) = llb; end
                        if(~isinf(lub)), B.ub(ind) = lub; end
                    else %could be applied to multiple variables
                        ind = strcmp(names(:,1),B.bnds{i,1});
                        if(isempty(ind) || all(ind  == 0))
                            error('Unknown bound: %s',B.bnds{i,1});
                        elseif(sum(ind) > 1)
                            error('Bound name is ambiguous and matches more than one variable name');
                        else
                            %Extract indices
                            start = names{ind,2}(1);
                            send = names{ind,2}(end);
                            if(~isinf(llb)), B.lb(start:send) = llb; end
                            if(~isinf(lub)), B.ub(start:send) = lub; end
                        end
                    end
                end

                %Process integer declarations
                for i = 1:size(B.vartypes,1)
                    %Check if variable exists
                    ind = logical(B.vars == sym(B.vartypes{i,1}));
                    if(any(ind))
                         B.xtype(ind) = B.vartypes{i,2};
                    else %could be applied to multiple variables
                        ind = strcmp(names(:,1),B.vartypes{i,1});
                        if(isempty(ind))
                            error('Unknown variable: %s',B.vartypes{i,1});
                        elseif(sum(ind) > 1)
                            error('Variable name is ambiguous and matches more than one variable name');
                        else
                            %Extract indices
                            start = names{ind,2}(1);
                            send = names{ind,2}(end);
                            B.xtype(start:send) = B.vartypes{i,2};
                        end
                    end
                end
            end
        end

        function buildJac(B)
        %Evaluate Jacobian of entire equation system

            if(isempty(B.sobj))
                error('You must build a symbolic representation of the system first!');
            end
            %Call SymToolbox jacobian function
            B.jac = jacobian(B.sobj);
        end

        function buildHess(B)
        %Evaluate Hessian(s) of entire equation system

            if(isempty(B.jac))
                error('You must evaluate the Jacobian of the system first!');
            end
            noeq = B.noObjs + B.noCons;
            B.hess = struct('H',cell(noeq,1),'lin',cell(noeq,1));
            %Build Index Vectors
            B.conlin = zeros(noeq,1);
            B.objlin = zeros(noeq,1);
            B.objbias = zeros(noeq,1);
            %Go through each equation
            for i = 1:noeq
                J = B.jac(i,:);
                %Determine if linear
                if(isempty(symvar(J)))
                    B.hess(i).lin = 1;
                    B.hess(i).H = 0;
                else
                    %Hessian is the Jacobian of the Jacobian (in this case)
                    H = jacobian(J,B.vars);
                    s = symvar(H);
                    %Determine if quadratic
                    if(isempty(s))
                        B.hess(i).lin = 2;
                        B.hess(i).H = H;
                    else
                        %Otherwise must be nonlinear, save entire symobj (for now)
                        B.hess(i).lin = 3;
                        B.hess(i).H = H;
                    end
                end
                %If linear or quadratic, save bias term
                if(B.hess(i).lin <= 2)
                    eq = B.sobj(i);
                    v = symvar(eq);
                    %Find bias term
                    B.objbias(i) = double(subs(eq,v,{zeros(size(v))}));
                end

                %Save linearity
                if(any(B.indobj == i))
                    B.objlin(i) = B.hess(i).lin;
                else
                    B.conlin(i) = B.hess(i).lin;
                end
            end
        end

        function buildHessLag(B)
        %Build Lagrangian Hessian assuming IPOPT (tril) format

            if(isempty(B.hess))
                error('You must evaluate the Hessian of the system first!');
            elseif(~isempty(B.hesslag))
                return; %already done
            end
            %Start with objective
            nvars = length(B.vars);
            ind = B.objlin >= 2;
            if(any(ind)) %check we have a nonlinear (or quadratic) objective
                if(sum(ind) > 1)
                    error('This interface only support single objective problems');
                end
                s = sym('sigma');
                objH = B.hess(ind).H;
                H = s*objH;
            else
                H = zeros(nvars);
            end
            %Now each nonlinear (or quadratic) constraint
            ind = B.conlin > 1;
            %Two sets of indexes, one for lambda, one for B.hess
            index_BH = find(ind);
            ind(B.indobj) = [];
            index_L = find(ind);
            wstate = warning('off','symbolic:sym:sym:DeprecateExpressions');
            for i = 1:length(index_BH)
                l = sym(sprintf('lambda(%d)',index_L(i)));
                H = H + l*B.hess(index_BH(i)).H;
            end
            warning(wstate);
            %Save resulting Hessian
            B.hesslag = tril(H);
        end

        function detLinearity(B)
        %Determine linearity of the supplied equations (without Hessian)

            if(isempty(B.jac))
                error('You must evaluate the Jacobian of the system first!');
            end
            %Build Index Vectors
            noeq = B.noCons + B.noObjs;
            B.conlin = zeros(noeq,1);
            B.objlin = zeros(noeq,1);
            B.objbias = zeros(noeq,1);
            %Determine Linearity (based on existence of symvars)
            for i = 1:noeq %must search all equations
                %Gather first derivative vars
                s = symvar(B.jac(i,:));
                %Objective
                if(any(B.indobj == i))
                    if(isempty(s))
                        B.objlin(i) = 1; %lin
                    else
                        B.objlin(i) = 3; %nonlin
                    end
                else
                    if(isempty(s))
                        B.conlin(i) = 1;
                    else
                        B.conlin(i) = 3;
                    end
                end
            end
            % Find objective bias terms
            for i = 1:noeq
                % If linear, save bias term
                if(B.objlin(i) == 1 || B.conlin(i) == 1)
                    eq = B.sobj(i);
                    v = symvar(eq);
                    %Find bias term
                    B.objbias(i) = double(subs(eq,v,{zeros(size(v))}));
                end
            end
        end
    end

    methods(Static)
        %Parse Constraint String
        [str,l,u] = parseConstraint(str);
        %Extract RHS of expression string, remove from eq, and return LHS
        [str,exp] = parseExpression(str);
        %Extract Group and Name from Result Expression Name
        [group,name] = parseResExp(name);
        %Parse bound string
        [var,lb,ub] = parseBound(str);
        %Parse integer string
        [var,xtype] = parseInteger(str);
        %Determine Variable names
        varn = detVarNames(svar);
        %Build MATLAB function file
        cb = buildMFun(mode,sobj,svar,opts);
        %Build C Code function file
        cb = buildCFun(mode,sobj,svar,opts,nocon);
        %Convert symbolic expression into matlab function
        fun = sym2fun(sobj,svar,var,skipSubs);

        %Check we have a compiler suitable for use with Cppad
        function ok = CheckCppADCompile(doerr)
            ok = true;
            c = mex.getCompilerConfigurations('c++');
            if(isempty(c) || (isempty(strfind(c.Name,'Microsoft Visual C++')) && isempty(strfind(c.Name,'Intel C++'))))
                str = sprintf(['CppAD compilation is only available with Microsoft Visual Studio or Intel as the C++ compiler.\n\nPlease either select it'...
                           ' as the MEX compiler (via mex -setup), or download from Microsoft:\n\n %s (2012, recommended)\n\nor\n\n %s (2013)'],...
                           'http://www.microsoft.com/en-nz/download/details.aspx?id=34673','http://www.visualstudio.com/en-us/products/visual-studio-express-vs.aspx');
                if(doerr)
                    error(str) %#ok<SPERR>
                else %warning, auto has selected it
                    ok = false;
                    optiwarn('symb:cppad',sprintf('SymBuilder has chosen CppAD has the most effective callback strategy, but %s',str));
                end
            end
        end
    end

end


function [F10, F01] = retrieveQ1001(level, o, H, L)
%------------------------------------------------------------------------------
%
% This function extracts gridfunction F10 and F01 from H.
% F10 is of colour '10' and F01 is of colour '01' such that together they
% constitute a quincunx gridfunction.
%
%                H   H   H   H
%              V   V   V   V   V       V ~ F10
%                H   H   H   H         H ~ F01
%              V   V   V   V   V
%                H   H   H   H
%              V   V   V   V   V
%                H   H   H   H
%              V   V   V   V   V
%                H   H   H   H
%       (domain of quincunx gridfunction)
%
% F10 & F01 are supposed to be uniquely determined by the integer L (level)
% and character o describing its type.
% Function retrieveQ1001 is the counterpart of storeQ1001.
% This function is a two-dimensional lifting scheme utility.
%
% F10 = 2D gridfunction of coefficients of colour '10' extracted from H.
%
% F01 = 2D gridfunction of coefficients of colour '01' extracted from H.
%
% level = integer designated as the level of both F10 and F01.
%
% o = character, should be either 'a' or 'd',
%     describing the type of both F10 and F01:
%     'a' relates to approximation (coefficients) and
%     'd' relates to detail (coefficients)
%
% L = 2D integer array of bookkeeping, see function storeR.
%
% H = 1D array that functions as storage (heap). The coefficients of both F10
%     and F01 are extracted from H as a result of calling retrieveQ1001.
%
% See also: retrieveQ1001, retrieveQ, retrieveQ0110.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: October 17, 2002.
% (c) 1999-2002 Stichting CWI, Amsterdam.
%------------------------------------------------------------------------------
F10 = retrieveQ(level, '10', o, H, L);
F01 = retrieveQ(level, '01', o, H, L);
if isempty(F10) || isempty(F01)
  disp([' retrieveQ1001 - ERROR at type ' o ' with level ' num2str(level)]);
  error(' retrieveQ1001 - empty result ')
else
  [n10, m10]=size(F10);
  [n01, m01]=size(F01);
  if m01 > m10
    disp([' retrieveQ1001 - ERROR at type ' o ' with level ' num2str(level)]);
    error(' retrieveQ1001 - dimensions do not match for m01 > m10 ')
  end
  if n10 > n01
    disp([' retrieveQ1001 - ERROR at type ' o ' with level ' num2str(level)]);
    error(' retrieveQ1001 - dimensions do not match for n10 > n01 ')
  end
  if m10 > m01+1
    disp([' retrieveQ1001 - ERROR at type ' o ' with level ' num2str(level)]);
    error(' retrieveQ1001 - dimensions do not match for m10 > m01+1 ')
  end
  if n01 > n10+1
    disp([' retrieveQ1001 - ERROR at type ' o ' with level ' num2str(level)]);
    error(' retrieveQ1001 - dimensions do not match for n01 > n10+1 ')
  end
%
% m01 <= m10 <= m01+1 is satisfied
% n10 <= n01 <= n10+1 is satisfied
%
end
%------------------------------------------------------------------------------


function [l, L_rf, L_sf, L_obs, L_n, N] = retroProjLmk(Rob,Sen,Obs,Opt)

% RETROPROJLMK  Retro project landmark.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

switch Sen.type

    % camera pinHole
    case {'pinHole'}
        % type of lmk to init
        switch Opt.init.initType
            case {'idpPnt'}
                % INIT LMK OF TYPE: Inverse depth point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjIdpPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'hmgPnt'}
                % INIT LMK OF TYPE: Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjHmgPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'ahmPnt'}
                % INIT LMK OF TYPE: Anchored Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjAhmPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'fhmPnt'}
                % INIT LMK OF TYPE: Framed Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjFhmPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'plkLin'}
                % INIT LMK OF TYPE: Plucker line
                [hm, HM_obs] = seg2hmgLin(Obs.meas.y);
                [l, L_rf, L_sf, L_k, L_hm, L_n] = ...
                    retroProjPlkLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    hm, ...
                    Opt.init.plkLin.nonObsMean) ;
                L_obs = L_hm*HM_obs;

                N = diag(Opt.init.plkLin.nonObsStd.^2) ;

            case 'aplLin'
                % INIT LMK OF TYPE: Anchored Plucker line
                [hm, HM_obs] = seg2hmgLin(Obs.meas.y);
                [l, L_rf, L_sf, L_k, L_hm, L_n] = ...
                    retroProjAplLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    hm, ...
                    Opt.init.plkLin.nonObsMean) ;
                L_obs = L_hm*HM_obs;

                N = diag(Opt.init.plkLin.nonObsStd.^2) ;

            case 'idpLin'
                % INIT LMK TYPE: Inverse-depth line.
                nMean = Opt.init.idpPnt.nonObsMean*[1;1]; % non-measured prior
                nStd  = Opt.init.idpPnt.nonObsStd*[1;1];

                [l, L_rf, L_sf, L_k, L_obs, L_n] = ...
                    retroProjIdpLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Obs.meas.y, ...
                    nMean) ;

                N = diag(nStd.^2);

            case 'hmgLin'
                % INIT LMK TYPE: Homogenous points line.
                nMean = Opt.init.idpPnt.nonObsMean*[1;1]; % non-measured prior
                nStd  = Opt.init.idpPnt.nonObsStd*[1;1];

                [l, L_rf, L_sf, L_k, L_obs, L_n] = ...
                    retroProjHmgLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Obs.meas.y, ...
                    nMean) ;

                N = diag(nStd.^2);

            case 'ahmLin'
                % INIT LMK TYPE: Anchored Homogenous points line.
                nMean = Opt.init.idpPnt.nonObsMean*[1;1]; % non-measured prior
                nStd  = Opt.init.idpPnt.nonObsStd*[1;1];

                [l, L_rf, L_sf, L_k, L_obs, L_n] = ...
                    retroProjAhmLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Obs.meas.y, ...
                    nMean) ;

                N = diag(nStd.^2);

            otherwise
                error('??? Unknown landmark type ''%s'' for initialization.',Opt.init.initType)
        end

        % Pin hole camera with depth information
    case 'pinHoleDepth'

        switch Opt.init.initType
            case 'eucPnt'
                % INIT LMK OF TYPE: Euclidean point
                [l, L_rf, L_sf, L_k, L_c, L_obs] = ...
                    retroProjEucPntFromPhdOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y) ;

                L_n = zeros(3,0);
                N   = [] ;

            otherwise
                error('??? Unknown landmark type ''%s'' for initialization.',Opt.init.initType)
        end

    % Omnidirectional camera
    case {'omniCam'}
        % type of lmk to init
        switch Opt.init.initType
            case {'ahmPnt'}
                % INIT LMK OF TYPE: Anchored Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjAhmPntFromOmniCamOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;
            otherwise
                error('??? Unknown landmark type ''%s'' for initialization.',Opt.init.initType)
        end

    otherwise % -- Sen.type
        % Print an error and exit
        error('??? Unknown sensor type ''%s''.',Sen.type);
end % -- Sen.type

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB 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 should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [ a, R, T, T3D, params, error, shapeOrtho ] = fit_PDM_ortho_proj_to_2D( M, E, V, shape2D, f, cx, cy)
%FITPDMTO2DSHAPE Summary of this function goes here
%   Detailed explanation goes here

    params = zeros(size(E));

    hidden = false;

    % if some of the points are unavailable modify M, V, and shape2D (can
    % later infer the actual shape from this)
    if(sum(shape2D(:)==0) > 0)

        hidden = true;
        % which indices to remove
        inds_to_rem = shape2D(:,1) == 0 | shape2D(:,2) == 0;

        shape2D = shape2D(~inds_to_rem,:);

        inds_to_rem = repmat(inds_to_rem, 3, 1);

        M_old = M;
        V_old = V;

        M = M(~inds_to_rem);
        V = V(~inds_to_rem,:);

    end

    num_points = numel(M) / 3;

    m = reshape(M, num_points, 3)';
    width_model = max(m(1,:)) - min(m(1,:));
    height_model = max(m(2,:)) - min(m(2,:));

    bounding_box = [min(shape2D(:,1)), min(shape2D(:,2)),...
                    max(shape2D(:,1)), max(shape2D(:,2))];

    a = (((bounding_box(3) - bounding_box(1)) / width_model) + ((bounding_box(4) - bounding_box(2))/ height_model)) / 2;

    tx = (bounding_box(3) + bounding_box(1))/2;
    ty = (bounding_box(4) + bounding_box(2))/2;

    % correct it so that the bounding box is just around the minimum
    % and maximum point in the initialised face
    tx = tx - a*(min(m(1,:)) + max(m(1,:)))/2;
    ty = ty - a*(min(m(2,:)) + max(m(2,:)))/2;

    R = eye(3);
    T = [tx; ty];

    currShape = getShapeOrtho(M, V, params, R, T, a);

    currError = getRMSerror(currShape, shape2D);

    reg_rigid = zeros(6,1);
    regFactor = 20;
    regularisations = [reg_rigid; regFactor ./ E]; % the above version, however, does not perform as well
    regularisations = diag(regularisations)*diag(regularisations);

    red_in_a_row = 0;

    for i=1:1000

        shape3D = M + V * params;
        shape3D = reshape(shape3D, numel(shape3D) / 3, 3);

        % Now find the current residual error
        currShape = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
        currShape = currShape';

        error_res = shape2D - currShape;

        eul = Rot2Euler(R);

        p_global = [a; eul'; T];

        % get the Jacobians
        J = CalcJacobian(M, V, params, p_global);

        % RLMS style update
        p_delta = (J'*J + regularisations) \ (J'*error_res(:) - regularisations*[p_global;params]);

        [params, p_global] = CalcReferenceUpdate(p_delta, params, p_global);

        a = p_global(1);
        R = Euler2Rot(p_global(2:4));
        T = p_global(5:6);

        shape3D = M + V * params;
        shape3D = reshape(shape3D, numel(shape3D) / 3, 3);
        currShape = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
        currShape = currShape';

        error = getRMSerror(currShape, shape2D);

        if(0.999 * currError < error)
            red_in_a_row = red_in_a_row + 1;
            if(red_in_a_row == 5)
                break;
            end
        end

        currError = error;

    end

    if(hidden)
        shapeOrtho = getShapeOrtho(M_old, V_old, params, R, T, a);
    else
        shapeOrtho = currShape;
    end
    if(nargin == 7)

        Zavg = f / a;
        Xavg = (T(1) - cx) / a;
        Yavg = (T(2) - cy) / a;

        T3D = [Xavg;Yavg;Zavg];
    else
        T3D = [0;0;0];
    end

end

function [shape2D] = getShapeOrtho(M, V, p, R, T, a)

    % M - mean shape vector
    % V - eigenvectors
    % p - parameters of non-rigid shape
    % R - rotation matrix
    % T - translation vector (tx, ty)
    shape3D = getShape3D(M, V, p);
    shape2D = a * R(1:2,:)*shape3D' + repmat(T, 1, numel(M)/3);
    shape2D = shape2D';
end

function [shape2D] = getShapeOrthoFull(M, V, p, R, T, a)

    % M - mean shape vector
    % V - eigenvectors
    % p - parameters of non-rigid shape
    % R - rotation matrix
    % T - translation vector (tx, ty)
    T = [T; 0];
    shape3D = getShape3D(M, V, p);
    shape2D = a * R*shape3D' + repmat(T, 1, numel(M)/3);
    shape2D = shape2D';
end

function [shape3D] = getShape3D(M, V, params)

    shape3D = M + V * params;
    shape3D = reshape(shape3D, numel(shape3D) / 3, 3);

end

function [error] = getRMSerror(shape2Dv1, shape2Dv2)

    error = sqrt(mean(reshape(shape2Dv1 - shape2Dv2, numel(shape2Dv1), 1).^2));

end

% This calculates the combined rigid with non-rigid Jacobian
function J = CalcJacobian(M, V, p, p_global)

    n = size(M, 1)/3;

    non_rigid_modes = size(V,2);

    J = zeros(n*2, 6 + non_rigid_modes);

    % now the layour is
    % ---------- Rigid part -------------------|----Non rigid part--------|
    % dx_1/ds, dx_1/dr1, ... dx_1/dtx, dx_1/dty dx_1/dp_1 ... dx_1/dp_m
    % dx_2/ds, dx_2/dr1, ... dx_2/dtx, dx_2/dty dx_2/dp_1 ... dx_2/dp_m
    % ...
    % dx_n/ds, dx_n/dr1, ... dx_n/dtx, dx_n/dty dx_n/dp_1 ... dx_n/dp_m
    % dy_1/ds, dy_1/dr1, ... dy_1/dtx, dy_1/dty dy_1/dp_1 ... dy_1/dp_m
    % ...
    % dy_n/ds, dy_n/dr1, ... dy_n/dtx, dy_n/dty dy_n/dp_1 ... dy_n/dp_m

    % getting the rigid part
    J(:,1:6) = CalcRigidJacobian(M, V, p, p_global);

    % constructing the non-rigid part
    R = Euler2Rot(p_global(2:4));
    s = p_global(1);

    % 'rotate' and 'scale' the principal components

    % First reshape to 3D
    V_X = V(1:n,:);
    V_Y = V(n+1:2*n,:);
    V_Z = V(2*n+1:end,:);

    J_x_non_rigid = s*(R(1,1)*V_X + R(1,2)*V_Y + R(1,3)*V_Z);
    J_y_non_rigid = s*(R(2,1)*V_X + R(2,2)*V_Y + R(2,3)*V_Z);

    J(1:n, 7:end) = J_x_non_rigid;
    J(n+1:end, 7:end) = J_y_non_rigid;

end

function J = CalcRigidJacobian(M, V, p, p_global)

 	n = size(M, 1)/3;

	% Get the current 3D shape (not affected by global transform, as this
	% is how the Jacobian was derived (for derivation please see
	% ../derivations/orthoJacobian
	shape3D = GetShape3D(M, V, p);

	% Get the rotation matrix corresponding to current global orientation
	R = Euler2Rot(p_global(2:4));
	s = p_global(1);

    % Rigid Jacobian is laid out as follows
    % dx_1/ds, dx_1/dr1, dx_1/dr2, dx_1/dr3, dx_1/dtx, dx_1/dty
    % dx_2/ds, dx_2/dr1, dx_2/dr2, dx_2/dr3, dx_2/dtx, dx_2/dty
    % ...
    % dx_n/ds, dx_n/dr1, dx_n/dr2, dx_n/dr3, dx_n/dtx, dx_n/dty
    % dy_1/ds, dy_1/dr1, dy_1/dr2, dy_1/dr3, dy_1/dtx, dy_1/dty
    % ...
    % dy_n/ds, dy_n/dr1, dy_n/dr2, dy_n/dr3, dy_n/dtx, dy_n/dty

    J = zeros(n*2, 6);

    % dx/ds = X * r11  + Y * r12 + Z * r13
    % dx/dr1 =  s*(r13 * Y - r12 * Z)
    % dx/dr2 = -s*(r13 * X - r11 * Z)
    % dx/dr3 =  s*(r12 * X - r11 * Y)
    % dx/dtx = 1
    % dx/dty = 0

    % dy/ds = X * r21  + Y * r22 + Z * r23
    % dy/dr1 =  s * (r23 * Y - r22 * Z)
    % dy/dr2 = -s * (r23 * X - r21 * Z)
    % dy/dr3 =  s * (r22 * X - r21 * Y)
    % dy/dtx = 0
    % dy/dty = 1

    % set the Jacobian for x's

    % with respect to scaling factor
    J(1:n,1) = shape3D * R(1,:)';

    % with respect to angular rotation around x, y, and z axes

    % Change of x with respect to change in axis angle rotation
	dxdR = [      0,  R(1,3), -R(1,2);
            -R(1,3),       0,  R(1,1);
             R(1,2), -R(1,1),      0];

    J(1:n,2:4) = s*(dxdR * shape3D')';

    % with respect to translation
    J(1:n,5) = 1;
    J(1:n,6) = 0;

    % set the Jacobian for y's

    % with respect to scaling factor
    J(n+1:end,1) = shape3D * R(2,:)';

     % with respect to angular rotation around x, y, and z axes

    % Change of y with respect to change in axis angle rotation
    dydR = [      0,  R(2,3), -R(2,2);
            -R(2,3),       0,  R(2,1);
             R(2,2), -R(2,1),      0];

     J(n+1:end,2:4) = s*(dydR * shape3D')';

     % with respect to translation
     J(n+1:end,5) = 0;
     J(n+1:end,6) = 1;

end

% This updates the parameters based on the updates from the RLMS
function [non_rigid, rigid] = CalcReferenceUpdate(params_delta, current_non_rigid, current_global)

    rigid = zeros(6, 1);
    % Same goes for scaling and translation parameters
    rigid(1) = current_global(1) + params_delta(1);
    rigid(5) = current_global(5) + params_delta(5);
    rigid(6) = current_global(6) + params_delta(6);

    % for rotation however, we want to make sure that the rotation matrix
    % approximation we have
	% R' = [1, -wz, wy
	%       wz, 1, -wx
	%       -wy, wx, 1]
    % is a legal rotation matrix, and then we combine it with current
    % rotation (through matrix multiplication) to acquire the new rotation

	R = Euler2Rot(current_global(2:4));

    wx = params_delta(2);
    wy = params_delta(3);
    wz = params_delta(4);

    R_delta = [1, -wz, wy;
               wz, 1, -wx;
               -wy, wx, 1];

	% Make sure R_delta is orthonormal
	R_delta = OrthonormaliseRotation(R_delta);

    % Combine rotations
	R_final = R * R_delta;

	% Extract euler angle
	euler = Rot2Euler(R_final);

	rigid(2:4) = euler;

    if(length(params_delta) > 6)
        % non-rigid parameters can just be added together
        non_rigid = params_delta(7:end) +  current_non_rigid;
    else
        non_rigid = current_non_rigid;
    end

end

function R_ortho = OrthonormaliseRotation(R)

    % U * V' is basically what we want, as it's guaranteed to be
    % orthonormal
    [U, ~, V] = svd(R);

    % We also want to make sure no reflection happened

    % get the orthogonal matrix from the initial rotation matrix
    X = U*V';

    % This makes sure that the handedness is preserved and no reflection happened
    % by making sure the determinant is 1 and not -1
    W = eye(3);
    W(3,3) = det(X);
    R_ortho = U*W*V';
end


function plotTree(tree, varNames)
%PLOTTREE   Plot a syntax tree (as stored in a TREEVAR object).
%   Calling sequence:
%      TREEVAR.PLOTTREE(TREE, VARNAMES)
%   where the input is:
%      TREE:     A MATLAB struct, describing the syntax tree of a mathematical
%                expression.
%      VARNAMES: An optional cellstring, whose elements are the name of the
%                variables that appear in a problem.
%
%   Usually, this method is called from within the TREEVAR plot() method.
%
%   Example:
%      % First, define a TREEVAR and carry out some operations:
%      u = treeVar();
%      v = cos(u);
%      w = sin(diff(u));
%      t = v + w;
%      % The following are equivalent:
%      plot(t)
%      treeVar.plotTree(t.tree)
%
% See also TREEVAR.PLOT.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Number of variables involved in the problem
numVars = length(tree.ID);

% Default variable names if none are passed:
if ( nargin < 2 )
    varNames = cell(1, numVars + 1);
    varNames{1} = 't';
    if ( numVars == 1 )
        % Simpler output for scalar case
        varNames{2} = 'u';
    else
        for varCounter = 1:numVars
            varNames{varCounter + 1} = sprintf('u%i', varCounter);
        end
    end
end

% Starting values for plotting
startx = .5;
deltax = .25;

% Layout the tree. The returned tree will store information about where
% it's supposed to be plotted.
tree = layoutNodes(tree, startx, deltax, tree.height + 1, tree.height + 1);

% Create a figure, and plot the first node
plot(tree.x, tree.y, '.','markersize', 25);
hold on

% Maximum differential orders that appear in the tree
maxDiffOrder = tree.diffOrder;

% Start the recursive calls to the actual plotting method:
plotTreePlot(tree, maxDiffOrder, varNames);

% Some final massaging of the figure:
xlim([0, 1])
ylim([0, 1])
hold off
set(gca, 'xtick', [])
set(gca, 'ytick', [])
title('Function evaluation tree')
end

function tree = layoutNodes(tree, treex, deltax, currHeight, maxHeight)
%LAYOUTNODES   Return coordinates for where to plot nodes of syntax trees.

if ( ~isstruct(tree) )
    % In case of scalars or CHEBFUNS appearing in the expression, the
    % corresponding nodes will not be syntax trees. Need to treat this case
    % separately.

    % Store the value of the current "tree":
    treeVal = tree;

    % Create a dummy tree, needed for storing the information required when the
    % tree is plotted:
    tree = [];
    tree.numArgs = 0;
    if ( isnumeric(treeVal) )
        tree.method = num2str(treeVal, '%2.2f');
    else
        tree.method = 'chebfun';
    end

    % Store coordinates where the node should be plotted:
    tree.y = currHeight/(maxHeight+1);
    tree.x = treex;

    % Scalars and CHEBFUNS have zero diffOrders.
    tree.diffOrder = 0;
    return
end

% Store coordinates where the current node should be plotted:
tree.y = currHeight/(maxHeight+1);
tree.x = treex;

% Lay out nodes recursively
switch tree.numArgs
    case 0
        % Do nothing
    case 1
        % Current tree has one child:
        tree.center = layoutNodes(tree.center, treex, deltax, ...
            currHeight - 1, maxHeight);
    case 2
        % Current tree has two childs:
        tree.left  = layoutNodes(tree.left, treex - deltax, ...
            deltax/2, currHeight - 1, maxHeight);
        tree.right  = layoutNodes(tree.right, treex + deltax, ...
            deltax/2, currHeight - 1, maxHeight);
end

end

function plotTreePlot(tree, maxDiffOrder, varNames)
%PLOTREEPLOT   The actual plotting of the syntax tree.

% Nice modern Matlab colours for lines and nodes:
lineColor = [0 0.447 0.741];
depVarColor = [0.85 0.325 0.098];
indVarColor = [0.929 0.694 0.125];

% Plot, depending on the number of arguments that the method has.
switch tree.numArgs
    case 0
        % Plot the constructor leaves in different colours.
        if ( strcmp(tree.method, 'constr') )
            if ( ~any(tree.ID) )
                % Independent variable
                col = indVarColor;
            else
                col = depVarColor;
            end
            plot(tree.x, tree.y, '.','markersize', 25, 'color', col);
        end

    case 1
        % Plot syntax tree for univariate methods:
        plot(tree.center.x, tree.center.y, '.','markersize', 25, ...
            'color', lineColor);
        plot([tree.x tree.center.x], [tree.y tree.center.y], 'color',lineColor)
        plotTreePlot(tree.center, maxDiffOrder, varNames)

    case 2
        % Plot syntax tree for bivariate methods. Start with left sub-tree.
        plot(tree.left.x, tree.left.y, '.','markersize', 25, ...
            'color', lineColor);
        plot([tree.x tree.left.x], [tree.y tree.left.y], 'color',lineColor)
        plotTreePlot(tree.left, maxDiffOrder, varNames)

        % Plot right sub-tree.
        plot(tree.right.x, tree.right.y, '.','markersize', 25, ...
            'color', lineColor);
        plot([tree.x tree.right.x], [tree.y tree.right.y], 'color',lineColor)
        plotTreePlot(tree.right, maxDiffOrder, varNames)
end

% Add the method name to the plot:
if ( strcmp(tree.method, 'constr') )
    % If we're at a constructor leaf, change the text slightly:
    varID = find(tree.ID == 1);
    if ( isempty(varID) )
        % Independent variable
        varString = varNames{1};
    else
        varString = varNames{varID + 1};
    end

    text(tree.x + 0.02, tree.y - 0.01, varString, 'Interpreter', 'none', ...
        'fontsize',14)
else
    text(tree.x + 0.02, tree.y - 0.01, tree.method, 'Interpreter', 'none', ...
        'fontsize',14)
end

end


function [ f,resL2,qualMeasOut] = OS_AwPCSD(proj,geo,angles,maxiter,varargin)
%OS_AwPCSD solves the reconstruction problem using adaptive-weighted
%projection-controlled steepest descent method
%
%   OS_AwPCSD(PROJ,GEO,ALPHA,NITER) solves the reconstruction problem using
%   the projection data PROJ taken over ALPHA angles, corresponding to the
%   geometry described in GEO, using NITER iterations.
%
%   OS_AwPCSD(PROJ,GEO,ALPHA,NITER,OPT,VAL,...) uses options and values for solving. The
%   possible options in OPT are:
%
%
%   'lambda':      Sets the value of the hyperparameter for the SART iterations.
%                  Default is 1
%
%   'lambdared':   Reduction of lambda.Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
%       'init':    Describes diferent initialization techniques.
%                   •  'none'     : Initializes the image to zeros(default)

%                   •  'FDK'      : intializes image to FDK reconstrucition
%
%   'TViter':      Defines the amount of TV iterations performed per SART
%                  iteration. Default is 20
%
%   'maxL2err'     Maximum L2 error to accept an image as valid. This
%                  parameter is crucial for the algorithm, determines at
%                  what point an image should not be updated further.
%                  Default is 20% of the FDK L2 norm.
%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.
%   'delta'        Defines Parameter to control the amount of smoothing
%                  for pixels at the edges. A large 'delta' is not able to
%                  differentiate image gradients at different pixels. A
%                  small 'delta' give low weights to almost every pixels,
%                  making the algorithm inefficient in removing noise or
%                  straking artifacts. Default is -0.00055
%   'QualMeas'     Asks the algorithm for a set of quality measurement
%                  parameters. Input should contain a cell array of desired
%                  quality measurement names. Example: {'CC','RMSE','MSSIM'}
%                  These will be computed in each iteration.
%   'BlockSize':   Sets the projection block size used simultaneously. If
%                  BlockSize = 1 OS-SART becomes SART and if  BlockSize = length(alpha)
%                  then OS-SART becomes SIRT. Default is 20.
% 'OrderStrategy'  Chooses the subset ordering strategy. Options are
%                  'ordered' :uses them in the input order, but divided
%                  'random'  : orders them randomply
%                  'angularDistance': chooses the next subset with the
%                                     biggest angular distance with the ones used.
% 'redundancy_weighting': true or false. Default is true. Applies data
%                         redundancy weighting to projections in the update step
%                         (relevant for offset detector geometry)
%  'groundTruth'  an image as grounf truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri and Manasavee Lohvithee
%--------------------------------------------------------------------------
%% parse inputs
[beta,beta_red,f,ng,verbose,epsilon,delta,blocksize,OrderStrategy,QualMeasOpts,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,varargin);

measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<3 && measurequality
    warning("Image metrics requested but none catched as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);
resL2=zeros(1,niter);
if nargout>1
    computeL2=true;
else
    computeL2=false;
end

[alphablocks,orig_index]=order_subsets(angles,blocksize,OrderStrategy);
%
% angles=cell2mat(alphablocks);
% index_angles=cell2mat(orig_index);

if measurequality
    qualMeasOut=zeros(length(QualMeasOpts),maxiter);
end

% does detector rotation exists?
if ~isfield(geo,'rotDetector')
    geo.rotDetector=[0;0;0];
end

%% Create weigthing matrices for the SART step
% the reason we do this, instead of calling the SART fucntion is not to
% recompute the weigths every AwASD-POCS iteration, thus effectively doubling
% the computational time
% Projection weigth, W

% Projection weight, W
W=computeW(geo,angles,gpuids);

% Back-Projection weigth, V
V=computeV(geo,angles,alphablocks,orig_index,'gpuids',gpuids);

if redundancy_weights
    % Data redundancy weighting, W_r implemented using Wang weighting
    % reference: https://iopscience.iop.org/article/10.1088/1361-6560/ac16bc

    num_frames = size(proj,3);
    W_r = redundancy_weighting(geo);
    W_r = repmat(W_r,[1,1,num_frames]);
    % disp('Size of redundancy weighting matrix');
    % disp(size(W_r));
    W = W.*W_r; % include redundancy weighting in W
end

iter=0;
offOrigin=geo.offOrigin;
offDetector=geo.offDetector;
rotDetector=geo.rotDetector;
stop_criteria=0;
DSD=geo.DSD;
DSO=geo.DSO;
while ~stop_criteria %POCS
    % If quality is going to be measured, then we need to save previous image
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = f; % only store if necesary
    end
    if (iter==0 && verbose==1);tic;end
    iter=iter+1;

    %Estimation error in the projection domain
    est_proj=Ax(f,geo,angles,'interpolated','gpuids',gpuids);
    delta_p=im3Dnorm(est_proj-proj,'L2');

    %Enforcing ART along all projections if squared delta_p > epsilon
    if (delta_p^2)>epsilon
        for jj=1:length(alphablocks)
            if size(offOrigin,2)==size(angles,2)
                geo.offOrigin=offOrigin(:,jj);
            end
            if size(offDetector,2)==size(angles,2)
                geo.offDetector=offDetector(:,jj);
            end
            if size(rotDetector,2)==size(angles,2)
                geo.rotDetector=rotDetector(:,jj);
            end
            if size(DSD,2)==size(angles,2)
                geo.DSD=DSD(jj);
            end
            if size(DSO,2)==size(angles,2)
                geo.DSO=DSO(jj);
            end

            f=f+beta* bsxfun(@times,1./sum(V(:,:,jj),3),Atb(W(:,:,jj).*(proj(:,:,orig_index{jj})-Ax(f,geo,alphablocks{:,jj},'gpuids',gpuids)),geo,alphablocks{:,jj},'gpuids',gpuids));

        end
    end

    %Non-negativity projection on all pixels
    if nonneg
        f=max(f,0);
    end
    geo.offDetector=offDetector;
    geo.offOrigin=offOrigin;

    if measurequality
        qualMeasOut(:,iter)=Measure_Quality(res_prev,f,QualMeasOpts);
    end

    % Compute L2 error of actual image. Ax-b
    dd=im3Dnorm(Ax(f,geo,angles,'gpuids',gpuids)-proj,'L2');

    % Compute change in the image after last SART iteration
    dp_vec=(f-f0);

    if iter==1
        step=1;
    else
        step=delta_p/delta_p_first;
    end
    f0=f;
    %  TV MINIMIZATION
    % =========================================================================
    %  Call GPU to minimize AwTV
    f=minimizeAwTV(f0,step,ng,delta,'gpuids',gpuids);    %   This is the MATLAB CODE, the functions are sill in the library, but CUDA is used nowadays
    %                                             for ii=1:ng
    %                                                 %delta=-0.00038 for thorax phantom
    %                                                 df=weighted_gradientTVnorm(f,delta);
    %                                                 df=df./im3Dnorm(df,'L2');
    %                                                 f=f-(step.*df);
    %                                             end
    % Compute change by TV min
    dg_vec=(f-f0);

    if iter==1
        delta_p_first=im3Dnorm((Ax(f0,geo,angles,'interpolated','gpuids',gpuids))-proj,'L2');
    end

    % Reduce SART step
    beta=beta*beta_red;

    % Check convergence criteria
    % ==========================================================================

    %Define c_alpha as in equation 21 in the journal
    c=dot(dg_vec(:),dp_vec(:))/(norm(dg_vec(:),2)*norm(dp_vec(:),2));
    %This c is examined to see if it is close to -1.0

    if (c<-0.99 && dd<=epsilon) || beta<0.005|| iter>maxiter
        if verbose
            disp(['Stopping criteria met']);
            disp(['   c    = ' num2str(c)]);
            disp(['   beta = ' num2str(beta)]);
            disp(['   iter = ' num2str(iter)]);
        end
        stop_criteria=true;
    end

    if computeL2
        geo.offOrigin=offOrigin;
        geo.offDetector=offDetector;
        resL2(ii)=im3Dnorm(proj-Ax(f,geo,angles,'gpuids',gpuids),'L2');                       % Compute error norm2 of b-Ax
        % If the error is not minimized.
        if  iter~=1 && resL2(ii)>resL2(ii-1)
            if verbose
                disp(['Convergence criteria met, exiting on iteration number:', num2str(iter)]);
            end
            return;
        end

    end

    if (iter==1 && verbose==1)
        expected_time=toc*maxiter;
        disp('OS_AwPCSD');
        disp(['Expected duration  :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end

end
end

function [beta,beta_red,f0,ng,verbose,epsilon,delta,block_size,OrderStrategy,QualMeasOpts,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,argin)
opts=     {'lambda','lambda_red','init','tviter','verbose','maxl2err','delta','blocksize','orderstrategy','qualmeas','nonneg','gpuids','redundancy_weighting','groundtruth'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('TIGRE:OS_AwPCSD:InvalidInput','Invalid number of inputs')
end

% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('TIGRE:OS_AwPCSD:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option isnot default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('TIGRE:OS_AwPCSD:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end
    % parse inputs
    switch opt
        % Verbose
        %  =========================================================================
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('TIGRE:Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
            % Lambda
            %  =========================================================================
        case 'lambda'
            if default
                beta=1;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:OS_AwPCSD:InvalidInput','Invalid lambda')
                end
                beta=val;
            end
            % Lambda reduction
            %  =========================================================================
        case 'lambda_red'
            if default
                beta_red=0.99;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:OS_AwPCSD:InvalidInput','Invalid lambda')
                end
                beta_red=val;
            end
            % Initial image
            %  =========================================================================
        case 'init'
            if default || strcmp(val,'none')
                f0=zeros(geo.nVoxel','single');

            else
                if strcmp(val,'FDK')
                    f0=FDK(proj, geo, angles);
                else
                    error('TIGRE:MLEM:InvalidInput','Invalid init')
                end
            end
            % Number of iterations of TV
            %  =========================================================================
        case 'tviter'
            if default
                ng=20;
            else
                ng=val;
            end
            %  Maximum L2 error to have a "good image"
            %  =========================================================================
        case 'maxl2err'
            if default
                epsilon=im3Dnorm(FDK(proj,geo,angles),'L2')*0.2; %heuristic
            else
                epsilon=val;
            end
            %  =========================================================================
            %  Parameter to control the amount of smoothing for pixels at the
            %  edges
            %  =========================================================================
        case 'delta'
            if default
                delta=-0.005;
            else
                delta=val;
            end
            %  =========================================================================
            %  Block size for OS-SART
            %  =========================================================================
        case 'blocksize'
            if default
                block_size=20;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:OS_AwASD_POCS:InvalidInput','Invalid BlockSize')
                end
                block_size=val;
            end
            %  Order strategy
            %  =========================================================================
        case 'orderstrategy'
            if default
                OrderStrategy='random';
            else
                OrderStrategy=val;
            end
            %  =========================================================================
            %  Image Quality Measure
            %  =========================================================================
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('TIGRE:OS_AwPCSD:InvalidInput','Invalid quality measurement parameters');
                end
            end
            %  Non negative
            %  =========================================================================
        case 'nonneg'
            if default
                nonneg=true;
            else
                nonneg=val;
            end
            %  GPU Ids
            %  =========================================================================
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'redundancy_weighting'
            if default
                redundancy_weights = true;
            else
                redundancy_weights = val;
            end
        case 'groundtruth'
            if default
                gt=nan;
            else
                gt=val;
            end
        otherwise
            error('TIGRE:OS_AwPCSD:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in OS_AwPCSD()']);

    end
end

end


function [Q,Z,decomp,Den] = findsos(P,flag,options)
% FINDMATRIXSOS --- Find a sum of squares decomposition of a given matrix polynomial.
%
% [Q,Z,decomp,Den] = findsos(P,flag,options)
%
% P is a symmetric polynomial matrix.
%
% FLAG is an optional argument which, if set to 'rational', returns a
% sum of squares decomposition with rational coefficients.
%
% OPTIONS is an optional argument which specifies the solver and the
% solver-specific parameters on its fields as
%   options.solver
%   options.params
% the default value for options.solver is 'SeDuMi'. The solver parameters
% are fields of options.params as, for instance, options.params.tol = 1e-9.
%
% A positive semidefinite Q and a symbolic monomial vector Z will be
% computed such that
%
%    (Ir kron Z)' * Q * (Ir kron Z) = P(x)
%
% If P is not a sum of squares, the function will return empty Q and Z.
%
% If P is a polynomial with integer coefficients and is represented as a
% symbolic object, then [Q,Z] = findsos(P,'rational') will compute a rational
% matrix Q such that Z'*Q*Z = P.

% This file is part of SOSTOOLS - Sum of Squares Toolbox ver 3.00.
%
% Copyright (C)2002, 2004, 2013  A. Papachristodoulou (1), J. Anderson (1),
%                                G. Valmorbida (1), S. Prajna (2),
%                                P. Seiler (3), P. A. Parrilo (4)
% (1) Department of Engineering Science, University of Oxford, Oxford, U.K.
% (2) Control and Dynamical Systems - California Institute of Technology,
%     Pasadena, CA 91125, USA.
% (3) Aerospace and Engineering Mechanics Department, University of
%     Minnesota, Minneapolis, MN 55455-0153, USA.
% (4) Laboratory for Information and Decision Systems, M.I.T.,
%     Massachusetts, MA 02139-4307
%
% Send bug reports and feedback to: sostools@cds.caltech.edu
%
% 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 should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
%

% 12/27/01 - SP
% 03/27/02 - SP

if nargin == 2
    if ~strcmp(flag,'rational')
        options=flag;
        flag = 'abcdefgh';
    else
        %options.solver='sedumi';
        options.solver='sdpt3';
        flag = 'abcdefgh';
    end
elseif nargin==1
    %options.solver='sedumi';
    options.solver='sdpt3';
end

if isa(P,'sym')

    dimp = size(P);
    if dimp(1)~=dimp(2) ;
        disp(['The polynomial matrix is not square, it cannot be a sum of squares']);
        Q = [];
        Z = [];
        return;
    end;

    P = expand(P);
    vars = findsym(P);
    vars = sym(['[',vars,']']);

    nvars = size(vars,2) ;

    for var = 1:nvars;
        for j = 1:dimp(1)
            if rem(double(feval(symengine,'degree',P(j,j))),2) ;
                disp(['Degree in ' char(vars(var)) ' is not even for the diagonal elements. The polynomial matrix cannot be a sum of squares']);
                Q = [];
                Z = [];
                return;
            end;
        end;
    end;

    if isequal(P,P.')~=1;
        disp(['The polynomial matrix is not symmetric, it can not be a sum of squares']);
        Q = [];
        Z = [];
        return;
    end;

    prog = sosprogram(vars);
    prog = sosineq(prog,P);
    [prog,info] = sossolve(prog,options);

    if strcmp(options.solver,'SDPNAL')
        disp('findsos function currently not supported for SDPNAL solver');
        Q = [];
        Z = [];
        if nargout >= 3
            decomp = [];
            Den = [];
        end
        return;
    elseif (info.dinf==1)||(info.pinf==1)
        disp('No sum of squares decomposition is found.');
        Q = [];
        Z = [];
        if nargout >= 3
            decomp = [];
            Den = [];
        end
        return;
    else
        Q = reshape(prog.solinfo.RRx,sqrt(length(prog.solinfo.RRx)),sqrt(length(prog.solinfo.RRx)));
        if isa(P,'sym');
            Z = mysympower(vars,prog.extravar.Z{1});
        else
            pvar Z
            coefftemp = speye(size(prog.extravar.Z{1},1));
            Z = polynomial(coefftemp,prog.extravar.Z{1},prog.vartable,[size(prog.extravar.Z{1},1),1]);
        end;
    end;

    if nargin > 1 & strcmp(flag,'rational')

        A = (full(prog.expr.At{1})') ;
        B = (full(prog.expr.b{1})) ;
        % Round x0
        N = 1 ; % This is the tolerance in the rounding
        xx = prog.solinfo.x;

        kmax = 10 ;
        pd = 0 ;
        while kmax ~= 0 ;
            kmax = kmax - 1 ;
            x0 = round(N*xx);
            [Qflat,NN] = proj3(x0,A,B,N);
            n = sqrt(length(Qflat));
            Qr = reshape(Qflat,n,n);
            % Qr should be PSD (should really check symbolically)
            if min(eig(Qr/NN))>-1e-14 ; kmax=0 ; pd = 1 ; end
            % Increase N, and try again
            N = 2*N ;
        end

        % Experimental, no good error checking yet, so we check that
        % expand(NN*P - Z.'*Qr*Z) is zero!

        if (expand(NN*P-Z.'*Qr*Z) ~= 0) | (pd == 0);
            Qr=[];Z=[];NN=[];
            disp('Could not compute a rational SOS!');
        end

        if nargout == 4
            Q = Qr;
            Den = NN;
        else
            if isa(P,'sym')
                Q = sym(Qr/NN);
            else
                Q = Qr/NN;
                disp('To obtain an exact rational solution, run the function with three output arguments.');
            end;
        end;

    end;

    L = sqrtm(double(Q));
    decomp   = L*(kron(eye(dimp(1)),Z));

else

    % PJS -- Handle polynomial variables
    % Most of this code simply mimics the symbolic case and hence the
    % overlap can be reduced.

    dimp = size(P);
    if dimp(1)~=dimp(2) ;
        disp(['The polynomial matrix is not square, it cannot be a sum of squares']);
        Q = [];
        Z = [];
        return;
    end;

    nvars = P.nvar;
    vars = polynomial(zeros(nvars,1));
    for j=1:nvars
        vars(j) = pvar(P.varname{j});
    end

    for var = 1:nvars;
        for j = 1:dimp(1)
            Pjj = P(j,j);
            maxdeg = max(Pjj.degmat(:,var));
            if rem(maxdeg,2)
                disp(['Degree in ' char(vars(var)) ' is not even for the diagonal elements. The polynomial matrix cannot be a sum of squares']);
                Q = [];
                Z = [];
                return;
            end;
        end;
    end;

    tmp = isequal(P,P.');
    if ~all(tmp(:))
        disp(['The polynomial matrix is not symmetric, it can not be a sum of squares']);
        Q = [];
        Z = [];
        return;
    end;

    prog = sosprogram(vars);
    prog = sosineq(prog,P);
    [prog,info] = sossolve(prog);

    if 'solver' == 'SDPNAL'
        disp('findsos function currently not supported for SDPNAL solver');
        Q = [];
        Z = [];
        if nargout >= 3
            decomp = [];
            Den = [];
        end
        return;
    elseif (info.dinf==1)||(info.pinf==1)
        disp('No sum of squares decomposition is found.');
        Q = [];
        Z = [];
        if nargout >= 3
            decomp = [];
            Den = [];
        end
        return;
    else
        Q = reshape(prog.solinfo.RRx,sqrt(length(prog.solinfo.RRx)),sqrt(length(prog.solinfo.RRx)));
        if isa(P,'sym');
            Z = mysympower(vars,prog.extravar.Z{1});
        else
            pvar Z
            coefftemp = speye(size(prog.extravar.Z{1},1));
            Z = polynomial(coefftemp,prog.extravar.Z{1},prog.vartable,[size(prog.extravar.Z{1},1),1]);
        end;
    end;

    if nargin > 1 & strcmp(flag,'rational')

        A = (full(prog.expr.At{1})') ;
        B = (full(prog.expr.b{1})) ;
        % Round x0
        N = 1 ; % This is the tolerance in the rounding
        xx = prog.solinfo.x;

        kmax = 10 ;
        pd = 0 ;
        while kmax ~= 0 ;
            kmax = kmax - 1 ;
            x0 = round(N*xx);
            [Qflat,NN] = proj3(x0,A,B,N);
            n = sqrt(length(Qflat));
            Qr = reshape(Qflat,n,n);
            % Qr should be PSD (should really check symbolically)
            if min(eig(Qr/NN))>-1e-14 ; kmax=0 ; pd = 1 ; end
            % Increase N, and try again
            N = 2*N ;
        end

        % Experimental, no good error checking yet, so we check that
        % expand(NN*P - Z.'*Qr*Z) is zero!

        if (expand(NN*P-Z.'*Qr*Z) ~= 0) | (pd == 0);
            Qr=[];Z=[];NN=[];
            disp('Could not compute a rational SOS!');
        end

        if nargout == 4
            Q = Qr;
            Den = NN;
        else
            if isa(P,'sym')
                Q = sym(Qr/NN);
            else
                Q = Qr/NN;
                disp('To obtain an exact rational solution, run the function with three output arguments.');
            end;
        end;

    end;

    L = sqrtm(double(Q));
    decomp   = L*(kron(eye(dimp(1)),Z));

    % PJS -- Comment out original code in this section
%     varname = P.var;
%     vars = [];
%     for i = 1:size(varname,1)
%         pvar(varname{i});
%         vars = [vars eval(varname{i})];
%     end;
%
%     deg = mod(max(P.degmat,[],1),2);
%     if sum(deg)~=0
%         i = find(deg~=0);
%         disp(['Degree in ' varname{i(1)} ' is not even. The polynomial cannot be a sum of squares']);
%         Q = [];
%         Z = [];
%         return;
%     end;

end;


function headmodel = ft_headmodel_openmeeg(bnd, varargin)

% FT_HEADMODEL_OPENMEEG creates a volume conduction model of the
% head using the boundary element method (BEM). This function takes
% as input the triangulated surfaces that describe the boundaries and
% returns as output a volume conduction model which can be used to
% compute leadfields.
%
% This function implements
%   Gramfort et al. OpenMEEG: opensource software for quasistatic
%   bioelectromagnetics. Biomedical engineering online (2010) vol. 9 (1) pp. 45
%   http://www.biomedical-engineering-online.com/content/9/1/45
%   doi:10.1186/1475-925X-9-45
% and
%   Kybic et al. Generalized head models for MEG/EEG: boundary element method
%   beyond nested volumes. Phys. Med. Biol. (2006) vol. 51 pp. 1333-1346
%   doi:10.1088/0031-9155/51/5/021
%
% This link with FieldTrip is derived from the OpenMEEG project
% with contributions from Daniel Wong and Sarang Dalal, and uses external
% command-line executables. See http://openmeeg.github.io/
%
% Use as
%   headmodel = ft_headmodel_openmeeg(bnd, ...)
%
% Optional input arguments should be specified in key-value pairs and can
% include
%   conductivity     = vector, conductivity of each compartment
%   tissue           = tissue labels for each compartment
%
% See also FT_PREPARE_VOL_SENS, FT_COMPUTE_LEADFIELD, FT_SYSMAT_OPENMEEG,
%          FT_SENSINTERP_OPENMEEG

%$Id$

ft_hastoolbox('openmeeg', 1);  % add to path (if not yet on path)
openmeeg_license;              % show the license (only once)
prefix = om_checkombin;        % check the installation of the binaries
if(~ispc) % if Linux/Mac, set number of threads
    omp_num_threads = feature('numCores');
    prefix = ['export OMP_NUM_THREADS=' num2str(omp_num_threads) ' && ' prefix];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% the first part is largely shared with the dipoli and bemcp implementation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% get the optional arguments
conductivity    = ft_getopt(varargin, 'conductivity');
tissue          = ft_getopt(varargin, 'tissue');

% copy the boundaries from the mesh into the volume conduction model
if isfield(bnd, 'bnd')
    bnd = bnd.bnd;
end

% rename pnt into pos
bnd = fixpos(bnd);

% start with an empty volume conductor
headmodel = [];

% determine the number of compartments
numboundaries = length(bnd);

% determine the desired nesting of the compartments
order = surface_nesting(bnd, 'outsidefirst');

% rearrange boundaries and conductivities
if numel(bnd)>1
    fprintf('reordering the boundaries to: ');
    fprintf('%d ', order);
    fprintf('\n');
    % update the order of the compartments
    bnd = bnd(order);
end

if isempty(conductivity)
    ft_warning('No conductivity is declared, using default values\n')
    if numboundaries == 1
        conductivity = 1;
    elseif numboundaries == 3
        % skin/skull/brain
        conductivity = [0.33 0.0042 0.33]
    elseif numboundaries == 4
        conductivity = [0.33 0.0042 1 0.33]
    else
        ft_error(['Conductivity values are required for ' num2str(numboundaries) ' shells'])
    end
    headmodel.cond = conductivity;
else
    if numel(conductivity)~=numboundaries
        ft_error('a conductivity value should be specified for each compartment');
    end
    % update the order of the compartments
    headmodel.cond = conductivity(order);
end

% assign default tissue labels if none provided
if(isempty(tissue))
    switch(numboundaries)
        case 3
            tissue = {'scalp','skull','brain'};
        case 4
            tissue = {'scalp','skull','CSF','brain'};
        otherwise
            tissue = strcat({'domain'},num2str((1:numboundaries)'));
    end
else
    tissue = tissue(order);
end
headmodel.tissue = tissue;

headmodel.skin_surface = 1;
headmodel.source = numboundaries;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% this uses an implementation that was contributed by INRIA Odyssee Team
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

workdir = fullfile(tempdir,['ft_om_' datestr(now,'ddmmyyHHMMSSFFF')]);
mkdir(workdir);

try
    % Write the triangulations to file, named after tissue type.
    % OpenMEEG v2.3 and up internally adjusts the convention for surface
    % normals, but OpenMEEG v2.2 expects surface normals to point inwards;
    % this checks and corrects if needed
    bndfile = fullfile(workdir,strcat(tissue,'.tri'));
    for ii=1:length(bnd)
        ok = checknormals(bnd(ii));
        if ~ok
            % Flip faces for openmeeg convention (inwards normals)
            bndtmp = bnd(ii);
            bndtmp.tri = fliplr(bnd(ii).tri);

            ok = checknormals(bndtmp);
            if(ok)
                fprintf('flipping normals for OpenMEEG''s convention\n')
                bnd(ii).tri = bndtmp.tri;
            else
                % if still not ok after flip, throw a warning
                ft_warning('neither orientation of surface normals passes check... leaving as is.')
            end
            clear bndtmp
        end

        om_save_tri(bndfile{ii}, bnd(ii).pos, bnd(ii).tri);
    end

    % retain surfaces in headmodel structure (after possible normal flip)
    headmodel.bnd = bnd;

    condfile  = fullfile(workdir,'om.cond');
    geomfile  = fullfile(workdir,'om.geom');
    hmfile    = fullfile(workdir,'hm.bin');
    hminvfile = fullfile(workdir,'hminv.bin');

    % write conductivity and mesh files
    bndlabel = {};
    for i=1:length(bnd)
        [dum,bndlabel{i}] = fileparts(bndfile{i});
    end

    om_write_geom(geomfile,bndfile,bndlabel);
    om_write_cond(condfile,headmodel.cond,bndlabel);

    om_status = system([prefix 'om_assemble -HM ' geomfile ' ' condfile ' ' hmfile]);
    if(om_status ~= 0) % status = 0 if successful
        ft_error('Aborting OpenMEEG pipeline due to above error.');
    end

    headmodel.mat = inv(om_load_sym(hmfile,'binary'));

    rmdir(workdir,'s'); % remove workdir with intermediate files
catch
    disp(lasterr);
    rmdir(workdir,'s'); % remove workdir with intermediate files
    ft_error('an error occurred while running OpenMEEG');
end

% remember the type of volume conduction model
headmodel.type = 'openmeeg';

function ok = checknormals(bnd)
points = bnd.pos;
faces = bnd.tri;

% FIXME: this method is rigorous only for star shaped surfaces
ok = 0;
% translate to the center
org = mean(points,1);
points(:,1) = points(:,1) - org(1);
points(:,2) = points(:,2) - org(2);
points(:,3) = points(:,3) - org(3);

w = sum(solid_angle(points, faces));

if w<0 && (abs(w)-4*pi)<1000*eps
    ok = 0;
    disp('Your surface normals are outwards oriented...')
elseif w>0 && (abs(w)-4*pi)<1000*eps
    ok = 1;
    %   ft_warning('your normals are inwards oriented')
else
    ft_error('your surface probably is irregular\n')
    ok = 0;
end