classdef Optimizer_MMA < Optimizer

    properties (GetAccess = public, SetAccess = protected)
        type = 'MMA'
    end

    properties (Access = private)
        kkttol
        maxoutit
        x
        xold1
        xold2
        xmin
        xmax
        low
        outit = 0;
        outeriter = 0;
        upp
        m
        c
        d
        a0
        a
        n
        f0val
        df0dx
        fval
        dfdx
        upperBound
        lowerBound
        hasFinished
        incrementalScheme
        hasConverged
        historicalVariables
       % targetParameters
        KKTnorm
    end

    methods (Access = public)

        function obj = Optimizer_MMA(cParams)
            obj.initOptimizer(cParams);
            obj.init(cParams);
            obj.outputFunction.monitoring.create(cParams);
%             obj.upperBound = cParams.uncOptimizerSettings.ub;
%             obj.lowerBound = cParams.uncOptimizerSettings.lb;
            obj.maxoutit = 1e4;
        end

       function solveProblem(obj)
            % obj.cost.computeFunctionAndGradient();
            % obj.constraint.computeFunctionAndGradient();
%             obj.printOptimizerVariable();
            obj.hasFinished = false;
            obj.printOptimizerVariable();
            while ~obj.hasFinished
                obj.update();
                obj.updateIterInfo();
                obj.updateMonitoring();
                obj.printOptimizerVariable();
            end
%             obj.printOptimizerVariable();
%             obj.printHistory();
            obj.hasConverged = 0;
%             obj.printHistoryFinalValues();
       end

        function update(obj)
            x = obj.designVariable.value;
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();
            obj.checkInitial(x);
            obj.outit = obj.outit+1;
            obj.outeriter = obj.outeriter+1;
            %%%% The MMA subproblem is solved at the point xval:
            [xmma,ymma,zmma,lam,xsi,eta,mu,zet,s,obj.low,obj.upp] = ...
                obj.mmasub(obj.m,obj.n,obj.outeriter,x,obj.xmin,obj.xmax,obj.xold1,obj.xold2, ...
                obj.f0val,obj.df0dx,obj.fval,obj.dfdx,obj.low,obj.upp,obj.a0,obj.a,obj.c,obj.d);
            %%%% Some vectors are updated:
            obj.xold2 = obj.xold1;
            obj.xold1 = x;
            x = xmma;
            %%%% The user should now calculate function values and gradients
            %%%% of the objective- and constraint functions at xval.
            %%%% The results should be put in f0val, df0dx, fval and dfdx.
            obj.designVariable.update(x);
            obj.cost.computeFunctionAndGradient();
            obj.constraint.computeFunctionAndGradient();

            [obj.f0val,obj.df0dx,obj.fval,obj.dfdx] = obj.funmma();
            %%%% The residual vector of the KKT conditions is calculated:
            [~,kktnorm] = obj.kktcheck(obj.m,obj.n,xmma,ymma,zmma,lam,xsi,eta,mu,zet,s, ...
                obj.xmin,obj.xmax,obj.df0dx,obj.fval,obj.dfdx,obj.a0,obj.a,obj.c,obj.d);

            obj.historicalVariables.kktnorm = kktnorm;
            obj.dualVariable.value = lam;
            obj.updateConvergenceStatus();
            obj.KKTnorm     = kktnorm;
        end

    end

    methods (Access = private)

        function updateMonitoring(obj)
            s.hasFinished          = obj.hasFinished;
            s.nIter                = obj.nIter;
            s.KKTnorm              = obj.KKTnorm;
            s.outitFrac            = obj.outit/obj.maxoutit;
            obj.outputFunction.monitoring.compute(s);
        end

        function init(obj,cParams)
            obj.outputFunction         = cParams.outputFunction.monitoring;
            obj.upperBound             = cParams.uncOptimizerSettings.ub;
            obj.lowerBound             = cParams.uncOptimizerSettings.lb;
            obj.incrementalScheme      = cParams.incrementalScheme;
            obj.hasConverged           = false;
            obj.constraintCase         = cParams.constraintCase;
            obj.targetParameters       = cParams.targetParameters;
        end

        function [f,df,c,dc] = funmma(obj)
            f  = obj.cost.value;
            df = obj.cost.gradient;
            c  = obj.constraint.value;
            dc = obj.constraint.gradient;
            dc = dc';

            [c,dc] = obj.checkConstraintCase(c,dc);

            %% Re-scale constraints
            % In many applications, the constraints are on the form yi(x) < =  ymaxi
            % The user should then preferably scale the constraints in such a way that 1 < =  ymaxi < =  100 for each i
            % (and not ymaxi = 10^10 for example).

            % kconstr = 100;
            kconstr = 1;
            cconstr = 0;
            c = kconstr*c;
            c(c > 0) = c(c > 0) + cconstr;
            c(c < 0) = c(c < 0) - cconstr;
            % dc = kconstr*dc;

            %% Re-scale objective function
            % The objective function f(x) should preferably be scaled such that
            % 1 < =  f0(x) < =  100 for reasonable values on the variables.
            kfun = 1;
            cfun = 0;
            f = kfun*f + cfun;
            df = kfun*df;
        end

        function checkInitial(obj,x0)
            if isempty(obj.x)
                obj.x = x0;
                obj.xold1 = obj.x;
                obj.xold2 = obj.xold1;
                obj.xmin = obj.lowerBound*ones(length(x0),1);
                obj.xmax = obj.upperBound*ones(length(x0),1);
                % obj.low = obj.xmin;
                obj.low = zeros(length(x0),1);
                % obj.upp = obj.xmax;
                obj.upp = ones(length(x0),1);
                [obj.f0val,obj.df0dx,obj.fval,obj.dfdx] = obj.funmma();
                obj.m = length(obj.fval);
                obj.c = 1000*ones(obj.m,1);
                obj.d = 0*ones(obj.m,1);
                obj.a0 = 1;
                obj.a = 0*ones(obj.m,1);
                obj.n = length(obj.x);
            end
        end

        function [xmma,ymma,zmma,lam,xsi,eta,mu,zet,s,low,upp] = ...
                mmasub(obj,m,n,iter,xval,xmin,xmax,xold1,xold2, ...
                ~,df0dx,fval,dfdx,low,upp,a0,a,c,d)
            epsimin = sqrt(m+n)*10^(-9); %10^(-7);
            raa0 = 1e-5;
            move = 1.0;
            albefa = 0.1;
            asyinit = 0.1 ; %0.2 % 0.5;
            asyincr = 1.1; % 1.2;
            asydecr = 0.65; % 0.7
            eeen = ones(n,1);
            eeem = ones(m,1);
            zeron = zeros(n,1);

            % Calculation of the asymptotes low and upp :
            if iter < 2.5
                low = xval - asyinit*(xmax-xmin);
                upp = xval + asyinit*(xmax-xmin);
            else
                zzz = (xval-xold1).*(xold1-xold2);
                factor = eeen;
                factor(find(zzz > 0)) = asyincr;
                factor(find(zzz < 0)) = asydecr;
                low = xval - factor.*(xold1 - low);
                upp = xval + factor.*(upp - xold1);
                lowmin = xval - 10*(xmax-xmin);
                lowmax = xval - 0.01*(xmax-xmin);
                uppmin = xval + 0.01*(xmax-xmin);
                uppmax = xval + 10*(xmax-xmin);
                low = max(low,lowmin);
                low = min(low,lowmax);
                upp = min(upp,uppmax);
                upp = max(upp,uppmin);
            end

            % Calculation of the bounds alfa and beta :

            zzz1 = low + albefa*(xval-low);
            zzz2 = xval - move*(xmax-xmin);
            zzz  = max(zzz1,zzz2);
            alfa = max(zzz,xmin);
            zzz1 = upp - albefa*(upp-xval);
            zzz2 = xval + move*(xmax-xmin);
            zzz  = min(zzz1,zzz2);
            beta = min(zzz,xmax);

            % Calculations of p0, q0, P, Q and b.

            xmami = xmax-xmin;
            xmamieps = 1e-5*eeen;
            xmami = max(xmami,xmamieps);
            xmamiinv = eeen./xmami;
            ux1 = upp-xval;
            ux2 = ux1.*ux1;
            xl1 = xval-low;
            xl2 = xl1.*xl1;
            uxinv = eeen./ux1;
            xlinv = eeen./xl1;
            %
            p0 = zeron;
            q0 = zeron;
            p0 = max(df0dx,0);
            q0 = max(-df0dx,0);
            %p0(find(df0dx > 0)) = df0dx(find(df0dx > 0));
            %q0(find(df0dx < 0)) = -df0dx(find(df0dx < 0));
            pq0 = 0.001*(p0 + q0) + raa0*xmamiinv;
            p0 = p0 + pq0;
            q0 = q0 + pq0;
            p0 = p0.*ux2;
            q0 = q0.*xl2;
            %
            dfdx = sparse(dfdx);
            P = max(dfdx,0);
            Q = max(-dfdx,0);
            %P(find(dfdx > 0)) = dfdx(find(dfdx > 0));
            %Q(find(dfdx < 0)) = -dfdx(find(dfdx < 0));
            PQ = 0.001*(P + Q) + raa0*(eeem*xmamiinv');
            %PQ = 0.001*(P + Q) + raa0*eeem*xmamiinv;
            P = P + PQ;
            Q = Q + PQ;
            P = P * spdiags(ux2,0,n,n);
            Q = Q * spdiags(xl2,0,n,n);
            b = P*uxinv + Q*xlinv - fval ;
            %
            %%% Solving the subproblem by a primal-dual Newton method
            [xmma,ymma,zmma,lam,xsi,eta,mu,zet,s] = ...
                obj.subsolv(m,n,epsimin,low,upp,alfa,beta,p0,q0,P,Q,a0,a,b,c,d);
        end

        function updateConvergenceStatus(obj)
            kktnorm = obj.historicalVariables.kktnorm;
            has_not_converged = kktnorm > obj.kkttol && obj.outit < obj.maxoutit;
            obj.hasConverged = ~has_not_converged;
        end

        function [c,dc] = checkConstraintCase(obj,c,dc)
            if strcmp(obj.constraintCase,'EQUALITY')
                c  = [c;-c];
                dc = [dc;-dc];
            end
        end

        function updateIterInfo(obj)
            obj.increaseIter();
            obj.updateStatus();
        end

        function increaseIter(obj)
            obj.nIter = obj.nIter + 1;
        end

        function updateStatus(obj)
            obj.hasFinished = obj.hasConverged || obj.hasExceededStepIterations();
        end

        function itHas = hasExceededStepIterations(obj)
            iStep = obj.incrementalScheme.iStep;
            nStep = obj.incrementalScheme.nSteps;
            itHas = obj.nIter >= obj.maxIter*(iStep/nStep);
        end

    end

    methods (Access = private, Static)

        function [xmma,ymma,zmma,lamma,xsimma,etamma,mumma,zetmma,smma] = ...
                subsolv(m,n,epsimin,low,upp,alfa,beta,p0,q0,P,Q,a0,a,b,c,d)
            %
            % This function subsolv solves the MMA subproblem:
            %
            % minimize   SUM[ p0j/(uppj-xj) + q0j/(xj-lowj) ] + a0*z +
            %          + SUM[ ci*yi + 0.5*di*(yi)^2 ],
            %
            % subject to SUM[ pij/(uppj-xj) + qij/(xj-lowj) ] - ai*z - yi < =  bi,
            %            alfaj < =   xj < =   betaj,  yi > =  0,  z > =  0.
            %
            % Input:  m, n, low, upp, alfa, beta, p0, q0, P, Q, a0, a, b, c, d.
            % Output: xmma,ymma,zmma, slack variables and Lagrange multiplers.
            %
            een = ones(n,1);
            eem = ones(m,1);
            epsi = 1;
            epsvecn = epsi*een;
            epsvecm = epsi*eem;
            x = 0.5*(alfa+beta);
            y = eem;
            z = 1;
            lam = eem;
            xsi = een./(x-alfa);
            xsi = max(xsi,een);
            eta = een./(beta-x);
            eta = max(eta,een);
            mu  = max(eem,0.5*c);
            zet = 1;
            s = eem;
            itera = 0;
            while epsi > epsimin
                epsvecn = epsi*een;
                epsvecm = epsi*eem;
                ux1 = upp-x;
                xl1 = x-low;
                ux2 = ux1.*ux1;
                xl2 = xl1.*xl1;
                uxinv1 = een./ux1;
                xlinv1 = een./xl1;
                plam = p0 + P'*lam ;
                qlam = q0 + Q'*lam ;
                gvec = P*uxinv1 + Q*xlinv1;
                dpsidx = plam./ux2 - qlam./xl2 ;
                rex = dpsidx - xsi + eta;
                rey = c + d.*y - mu - lam;
                rez = a0 - zet - a'*lam;
                relam = gvec - a*z - y + s - b;
                rexsi = xsi.*(x-alfa) - epsvecn;
                reeta = eta.*(beta-x) - epsvecn;
                remu = mu.*y - epsvecm;
                rezet = zet*z - epsi;
                res = lam.*s - epsvecm;
                residu1 = [rex' rey' rez]';
                residu2 = [relam' rexsi' reeta' remu' rezet res']';
                residu = [residu1' residu2']';
                residunorm = sqrt(residu'*residu);
                residumax = max(abs(residu));
                ittt = 0;
                while residumax > 0.9*epsi & ittt < 200 % 100
                    ittt = ittt + 1;
                    itera = itera + 1;
                    ux1 = upp-x;
                    xl1 = x-low;
                    ux2 = ux1.*ux1;
                    xl2 = xl1.*xl1;
                    ux3 = ux1.*ux2;
                    xl3 = xl1.*xl2;
                    uxinv1 = een./ux1;
                    xlinv1 = een./xl1;
                    uxinv2 = een./ux2;
                    xlinv2 = een./xl2;
                    plam = p0 + P'*lam ;
                    qlam = q0 + Q'*lam ;
                    gvec = P*uxinv1 + Q*xlinv1;
                    GG = P*spdiags(uxinv2,0,n,n) - Q*spdiags(xlinv2,0,n,n);
                    dpsidx = plam./ux2 - qlam./xl2 ;
                    delx = dpsidx - epsvecn./(x-alfa) + epsvecn./(beta-x);
                    dely = c + d.*y - lam - epsvecm./y;
                    delz = a0 - a'*lam - epsi/z;
                    dellam = gvec - a*z - y - b + epsvecm./lam;
                    diagx = plam./ux3 + qlam./xl3;
                    diagx = 2*diagx + xsi./(x-alfa) + eta./(beta-x);
                    diagxinv = een./diagx;
                    diagy = d + mu./y;
                    diagyinv = eem./diagy;
                    diaglam = s./lam;
                    diaglamyi = diaglam+diagyinv;
                    if m < n
                        blam = dellam + dely./diagy - GG*(delx./diagx);
                        bb = [blam' delz]';
                        Alam = spdiags(diaglamyi,0,m,m) + GG*spdiags(diagxinv,0,n,n)*GG';
                        AA = [Alam     a
                            a'    -zet/z ];
                        solut = AA\bb;
                        dlam = solut(1:m);
                        dz = solut(m+1);
                        dx = -delx./diagx - (GG'*dlam)./diagx;
                    else
                        diaglamyiinv = eem./diaglamyi;
                        dellamyi = dellam + dely./diagy;
                        Axx = spdiags(diagx,0,n,n) + GG'*spdiags(diaglamyiinv,0,m,m)*GG;
                        azz = zet/z + a'*(a./diaglamyi);
                        axz = -GG'*(a./diaglamyi);
                        bx = delx + GG'*(dellamyi./diaglamyi);
                        bz  = delz - a'*(dellamyi./diaglamyi);
                        AA = [Axx   axz
                            axz'  azz ];
                        bb = [-bx' -bz]';
                        solut = AA\bb;
                        dx  = solut(1:n);
                        dz = solut(n+1);
                        dlam = (GG*dx)./diaglamyi - dz*(a./diaglamyi) + dellamyi./diaglamyi;
                    end
                    %
                    dy = -dely./diagy + dlam./diagy;
                    dxsi = -xsi + epsvecn./(x-alfa) - (xsi.*dx)./(x-alfa);
                    deta = -eta + epsvecn./(beta-x) + (eta.*dx)./(beta-x);
                    dmu  = -mu + epsvecm./y - (mu.*dy)./y;
                    dzet = -zet + epsi/z - zet*dz/z;
                    ds   = -s + epsvecm./lam - (s.*dlam)./lam;
                    xx  = [ y'  z  lam'  xsi'  eta'  mu'  zet  s']';
                    dxx = [dy' dz dlam' dxsi' deta' dmu' dzet ds']';
                    %
                    stepxx = -1.01*dxx./xx;
                    stmxx  = max(stepxx);
                    stepalfa = -1.01*dx./(x-alfa);
                    stmalfa = max(stepalfa);
                    stepbeta = 1.01*dx./(beta-x);
                    stmbeta = max(stepbeta);
                    stmalbe  = max(stmalfa,stmbeta);
                    stmalbexx = max(stmalbe,stmxx);
                    stminv = max(stmalbexx,1);
                    steg = 1/stminv;
                    %
                    xold   =   x;
                    yold   =   y;
                    zold   =   z;
                    lamold =  lam;
                    xsiold =  xsi;
                    etaold =  eta;
                    muold  =  mu;
                    zetold =  zet;
                    sold   =   s;
                    %
                    itto = 0;
                    resinew = 2*residunorm;
                    while resinew > residunorm & itto < 50
                        itto = itto+1;
                        x   =   xold + steg*dx;
                        y   =   yold + steg*dy;
                        z   =   zold + steg*dz;
                        lam = lamold + steg*dlam;
                        xsi = xsiold + steg*dxsi;
                        eta = etaold + steg*deta;
                        mu  = muold  + steg*dmu;
                        zet = zetold + steg*dzet;
                        s   =   sold + steg*ds;
                        ux1 = upp-x;
                        xl1 = x-low;
                        ux2 = ux1.*ux1;
                        xl2 = xl1.*xl1;
                        uxinv1 = een./ux1;
                        xlinv1 = een./xl1;
                        plam = p0 + P'*lam ;
                        qlam = q0 + Q'*lam ;
                        gvec = P*uxinv1 + Q*xlinv1;
                        dpsidx = plam./ux2 - qlam./xl2 ;
                        rex = dpsidx - xsi + eta;
                        rey = c + d.*y - mu - lam;
                        rez = a0 - zet - a'*lam;
                        relam = gvec - a*z - y + s - b;
                        rexsi = xsi.*(x-alfa) - epsvecn;
                        reeta = eta.*(beta-x) - epsvecn;
                        remu = mu.*y - epsvecm;
                        rezet = zet*z - epsi;
                        res = lam.*s - epsvecm;
                        residu1 = [rex' rey' rez]';
                        residu2 = [relam' rexsi' reeta' remu' rezet res']';
                        residu = [residu1' residu2']';
                        resinew = sqrt(residu'*residu);
                        steg = steg/2;
                    end
                    residunorm = resinew;
                    residumax = max(abs(residu));
                    steg = 2*steg;
                end
                if ittt > 198
                    epsi;
                    ittt;
                end
                epsi = 0.1*epsi;
            end
            xmma   =   x;
            ymma   =   y;
            zmma   =   z;
            lamma =  lam;
            xsimma =  xsi;
            etamma =  eta;
            mumma  =  mu;
            zetmma =  zet;
            smma   =   s;
        end

        function [residu,residunorm,residumax] = ...
                kktcheck(~,~,x,y,z,lam,xsi,eta,mu,zet,s, ...
                xmin,xmax,df0dx,fval,dfdx,a0,a,c,d)

            rex   = df0dx + dfdx'*lam - xsi + eta;
            rey   = c + d.*y - mu - lam;
            rez   = a0 - zet - a'*lam;
            relam = fval - a*z - y + s;
            rexsi = xsi.*(x-xmin);
            reeta = eta.*(xmax-x);
            remu  = mu.*y;
            rezet = zet*z;
            res   = lam.*s;
            %
            residu1 = [rex' rey' rez]';
            residu2 = [relam' rexsi' reeta' remu' rezet res']';
            residu = [residu1' residu2']';
            residunorm = sqrt(residu'*residu);
            residumax = max(abs(residu));
        end

    end

    methods

        function kkttol = get.kkttol(obj)
            kkttol = obj.targetParameters.optimality_tol;
        end

    end
end


function out = tangentspace(A, B)

% Get tangent space between A and B

for i = 1:size(A, 3)
    out(:, :, i) = sqrtm(B) * logm(B ^ (-1 / 2) * A(:, :, i) * B ^ (-1 / 2)) * sqrtm(B);
end


function [Cbe_new, Ve_new, ecef_new]=strapdown_ecef_dcm(Cbe, Ve, ecef, a, w, dt)
%Gravity (most time consuming part of ecef implementations)
Llh=ecef2geo_v000(ecef,0);
[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(Llh);
Cne=pos2Cne_v000(Llh(1), Llh(2));
ge=-Cne(:,3)*g;

%Update attitude
rot=w*dt;
rot_norm=norm(rot);
sr_a=1-(rot_norm^2/6)+(rot_norm^4/120);
sr_b=(1/2)-(rot_norm^2/24)+(rot_norm^4/720);
mx_a=eye(3)+sr_a*skew(rot)+sr_b*skew(rot)*skew(rot);

rot=-([0;0;WIE_E])*dt;
rot_norm=norm(rot);
sr_a=1-(rot_norm^2/6)+(rot_norm^4/120);
sr_b=(1/2)-(rot_norm^2/24)+(rot_norm^4/720);
mx_b=eye(3)+sr_a*skew(rot)+sr_b*skew(rot)*skew(rot);

Cbe_new=mx_b*Cbe*mx_a;

%%Update Velocity
%Update Vel
vel_inc1=Cbe*a*dt;
vel_inc2=(-ge+2*cross(Ve,[0;0;WIE_E]))*dt;
Ve_new=Ve+vel_inc1+vel_inc2;

%Update_pos
ecef_new=ecef+Ve*dt;


% Data File DTX2
% Free damped vibrations
% of a particle
  m    = 'm';    % mass of the particle
  Fx   = '-k*v - c*x'; % projections of forces on x
  x0   = '0.1';  % initial coordinate
  v0   = '10';   % initial velocity
  Tend = 20;     % upper bound of integration
  eps  = 1e-10;  % desirable accuracy
  np   = 3;      % number of parameters
  P{1} = 'm';    % mass of the particle
  P{2} = 'k';    % coefficient of resistance
  P{3} = 'c';    % spring stiffness


function [ x, qraux, ipvt ] = cqrdc ( x, ldx, n, p, ipvt, job )

%*****************************************************************************80
%
%% CQRDC computes the QR factorization of an N by P complex matrix.
%
%  Discussion:
%
%    CQRDC uses Householder transformations to compute the QR factorization
%    of an N by P matrix X.  Column pivoting based on the 2-norms of the
%    reduced columns may be performed at the user's option.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex X(LDX,P), the matrix whose decomposition
%    is to be computed.
%
%    Input, integer LDX, the leading dimension of X.  N <= LDX.
%
%    Input, integer N, the number of rows of the matrix.
%
%    Input, integer P, the number of columns in the matrix X.
%
%    Input, integer IPVT(P), integers that control the
%    selection of the pivot columns.  The K-th column X(K) of X is placed
%    in one of three classes according to the value of IPVT(K):
%      IPVT(K) > 0, then X(K) is an initial column.
%      IPVT(K) == 0, then X(K) is a free column.
%      IPVT(K) < 0, then X(K) is a final column.
%    Before the decomposition is computed, initial columns are moved to the
%    beginning of the array X and final columns to the end.  Both initial
%    and final columns are frozen in place during the computation and only
%    free columns are moved.  At the K-th stage of the reduction, if X(K)
%    is occupied by a free column it is interchanged with the free column
%    of largest reduced norm.
%
%    Input, integer JOB, initiates column pivoting.
%    0, no pivoting is done.
%    nonzero, pivoting is done.
%
%    Output, complex X(LDX,P); the upper triangle contains the upper
%    triangular matrix R of the QR factorization.  Below its diagonal, X
%    contains information from which the unitary part of the decomposition
%    can be recovered.  If pivoting has been requested, the decomposition is
%    not that of the original matrix X, but that of X with its columns
%    permuted as described by IPVT.
%
%    Output, complex QRAUX(P), further information required to recover
%    the unitary part of the decomposition.
%
%    Output, integer IPVT(P); the index of the column of the
%    original matrix that has been interchanged into
%    the K-th column, if pivoting was requested.
%    IPVT is not referenced if JOB == 0.
%
  pl = 1;
  pu = 0;

  if ( job ~= 0 )
%
%  Pivoting has been requested.  Rearrange the columns according to IPVT.
%
    for j = 1 : p

      swapj = ( 0 < ipvt(j) );
      negj = ( ipvt(j) < 0 );

      if ( negj )
        ipvt(j) = -j;
      else
        ipvt(j) = j;
      end

      if ( swapj )

        if ( j ~= pl )
          temp      = x(1:n,pl);
          x(1:n,pl) = x(1:n,j);
          x(1:n,j)  = temp;
        end

        ipvt(j) = ipvt(pl);
        ipvt(pl) = j;
        pl = pl + 1;

      end

    end

    pu = p;

    for jj = 1 : p

      j = p - jj + 1;

      if ( ipvt(j) < 0 )

        ipvt(j) = -ipvt(j);

        if ( j ~= pu )

          temp      = x(1:n,pu);
          x(1:n,pu) = x(1:n,j);
          x(1:n,j)  = temp;

          i        = ipvt(pu);
          ipvt(pu) = ipvt(j);
          ipvt(j)  = i;

        end

        pu = pu - 1;

      end

    end

  end
%
%  Compute the norms of the free columns.
%
  for j = pl : pu
    qraux(j) = scnrm2 ( n, x(1:n,j), 1 );
    work(j) = qraux(j);
  end
%
%  Perform the Householder reduction of X.
%
  lup = min ( n, p );

  for l = 1 : lup
%
%  Locate the column of largest norm and bring it
%  into the pivot position.
%
    if ( pl <= l & l < pu )

      maxnrm = 0.0;
      maxj = l;

      for j = l : pu
        if ( maxnrm < real ( qraux(j) ) )
          maxnrm = real ( qraux(j) );
          maxj = j;
        end
      end

      if ( maxj ~= l )

        temp        = x(1:n,l);
        x(1:n,l)    = x(1:n,maxj);
        x(1:n,maxj) = temp;

        qraux(maxj) = qraux(l);
        work(maxj)  = work(l);

        i          = ipvt(maxj);
        ipvt(maxj) = ipvt(l);
        ipvt(l)    = i;

      end

    end

    qraux(l) = 0.0;

    if ( l ~= n )
%
%  Compute the Householder transformation for column L.
%
      nrmxl = scnrm2 ( n-l+1, x(l:n,l), 1 );

      if ( cabs1 ( nrmxl ) ~= 0.0 )

        if ( cabs1 ( x(l,l) ) ~= 0.0 )
          nrmxl = csign2 ( nrmxl, x(l,l) );
        end

        x(l:n,l) = x(l:n,l) / nrmxl;
        x(l,l) = 1.0 + x(l,l);
%
%  Apply the transformation to the remaining columns,
%  updating the norms.
%
        for j = l+1 : p

          t = - ( conj ( transpose ( x(l:n,l) ) ) * x(l:n,j) ) / x(l,l);
          x(l:n,j) = x(l:n,j) + t * x(l:n,l);

          if ( j < pl | pu < j )
            continue
          end

          if ( cabs1 ( qraux(j) ) == 0.0 )
            continue
          end

          tt = 1.0 - ( abs ( x(l,j) ) / real ( qraux(j) ) )^2;
          tt = max ( tt, 0.0 );
          t = tt;
          tt = 1.0 + 0.05 * tt * ( real ( qraux(j) ) / real ( work(j) ) )^2;

          if ( tt ~= 1.0 )
            qraux(j) = qraux(j) * sqrt ( t );
          else
            qraux(j) = scnrm2 ( n-l, x(l+1:n,j), 1 );
            work(j) = qraux(j);
          end

        end
%
%  Save the transformation.
%
        qraux(l) = x(l,l);
        x(l,l) = -nrmxl;

      end

    end

  end

  return
end


% op_freqrange.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% out=op_freqrange(in,ppmmin,ppmmax);
%
% DESCRIPTION:
% Output only a specified frequency range of the input spectrum.
%
% INPUTS:
% in         = input data in matlab structure format.
% ppmmin     = minimum extent of frequency range in ppm.
% ppmmax     = maximum extent of frequency range in ppm.
%
% OUTPUTS:
% out        = Output following frequency range selection.

function out=op_freqrange(in,ppmmin,ppmmax);

%Calculate Specs using fft
fullspecs=fftshift(ifft(in.fids,[],in.dims.t),in.dims.t);

%now take only the specified range of the spectrum
specs=fullspecs(in.ppm>ppmmin & in.ppm<ppmmax,:,:);

%convert back to time domain
%if the length of Fids is odd, then you have to do a circshift of one to
%make sure that you don't introduce a small frequency shift into the fids
%vector.
if mod(size(specs,in.dims.t),2)==0
    %disp('Length of vector is even.  Doing normal conversion');
    fids=fft(fftshift(specs,in.dims.t),[],in.dims.t);
else
    %disp('Length of vector is odd.  Doing circshift by 1');
    fids=fft(circshift(fftshift(specs,in.dims.t),1),[],in.dims.t);
end

%calculate the size;
sz=size(fids);

%calculate the ppm scale
ppm=in.ppm(in.ppm>ppmmin & in.ppm<ppmmax);

%calculate the new spectral width and dwelltime:
dppm=abs(ppm(2)-ppm(1));
ppmrange=abs((ppm(end)-ppm(1)))+dppm;
spectralwidth=ppmrange*in.Bo*42.577;
dwelltime=1/spectralwidth;

%calculate the time scale
t=[0:dwelltime:(sz(1)-1)*dwelltime];

%FILLING IN DATA STRUCTURE
out=in;
out.fids=fids;
out.specs=specs;
out.sz=sz;
out.ppm=ppm;
out.t=t;
out.spectralwidth=spectralwidth;
out.dwelltime=dwelltime;

%FILLING IN THE FLAGS
out.flags=in.flags;
out.flags.writtentostruct=1;
out.flags.freqranged=1;


function [qua, DCMbn, euler] = att_update(wb, DCMbn, qua, omega_ie_n, omega_en_n, dt, att_mode)
% att_update: updates attitude using quaternion or DCM.
%
% INPUT
%   wb,         3x1 incremental turn-rates in body-frame (rad/s).
%   DCMbn,      3x3 body-to-nav DCM.
%   qua,        4x1 quaternion.
%   omega_ie_n, 3x3 skew-symmetric Earth rate matrix (rad/s).
%   omega_en_n, 3x3 skew-symmetric transport rate (rad/s).
%   dt,         1x1 IMU sampling interval (s).
%	att_mode,   attitude mode (string).
%      'quaternion': attitude updated as quaternion. Default value.
%             'dcm': attitude updated as Direct Cosine Matrix.
%
% OUTPUT
%   qua,      4x1 updated quaternion.
%   DCMbn,    3x3 updated body-to-nav DCM.
%   euler,    3x1 updated Euler angles (rad).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% Reference:
%
%	Titterton, D.H. and Weston, J.L. (2004). Strapdown
% Inertial Navigation Technology (2nd Ed.). Institution
% of Engineering and Technology, USA.
%
%	Crassidis, J.L. and Junkins, J.L. (2011). Optimal Esti-
% mation of Dynamic Systems, 2nd Ed. Chapman and Hall/CRC, USA.
% Eq. 7.39, p. 458.
%
% Version: 004
% Date:    2022/03/06
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

if nargin < 7, att_mode  = 'quaternion'; end

%% Gyros output correction for Earth and transport rates

om_ie_n = skewm_inv(omega_ie_n);
om_en_n = skewm_inv(omega_en_n);
wb = (wb - DCMbn' * (om_ie_n + om_en_n));  % Titterton, Eq. 3.29, p. 32

if strcmp(att_mode, 'quaternion')
%% Quaternion update

    qua   = qua_update(qua, wb, dt);    % Quaternion update
    qua   = qua / norm(qua);            % Brute-force normalization
    DCMbn = qua2dcm(qua);               % DCM update
    euler = qua2euler(qua);             % Euler angles update

elseif strcmp(att_mode, 'dcm')
%% DCM update

    delta_theta = wb * dt;                  % Incremental Euler angles
    DCMbn = dcm_update(DCMbn, delta_theta); % DCM update
    euler = dcm2euler(DCMbn);               % Euler angles update
    qua   = euler2qua(euler);               % Quaternion update
    qua   = qua / norm(qua);                % Brute-force normalization

else
    error('att_update: no attitude update mode defined. Check the attitude update mode selected.')
end

end


% V = est_calcInvCovMatFourierPDC(Rinv,E,foi,fs,N,p, verb)
%
% Obtain the frequency domain transform of the inverse covariance matrix of
% an M-variate VAR[p] process
% This is needed for calculating the analytic statistics for the PDC [1]
%
% Inputs:
%
%     Rinv: inverse process covariance matrix obtained from est_calcInvCovMat
%     E:    noise covariance matrix
%     foi:  frequencies of interest (Hz)
%     fs:   sampling rate
%     N:    # chans
%     p:    model order
%     verb: verbosity level. 0 = no output, 1=text.
%
% Outputs:
%
%     V:    Frequency-domain transform of the inverse covariance matrix
%
% References:
%
% [1] Schelter et al, (2009). Testing for directed influences among neural
% signals using partial directed coherence. J. Neuroscience Methods. 152:210-9.
%
% see also: est_calcInvCovMat(), est_calcInvCovMatFourier()
%
% Author: Tim Mullen 2010, SCCN/INC, UCSD
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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 3 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function V = est_calcInvCovMatFourierPDC(Rinv,E,foi,fs,N,p,verb,DEBUG)

if nargin<8
    DEBUG = false;
end

%% extract the diagonal elements of H=Rinv
% structure of Hd is (e.g., for p=2):
% [diag(H(1,1)), diag(H(2,1)), diag(H(1,2)), diag(H(2,2))]
% where diag(H(u,v)) is the column vector formed by the diagonal of
% submatrix H(u,v) of H
Hd = zeros(N,p^2);
cnt=1;
for v=1:p
    for u=1:p
        Hd(:,cnt)=diag(Rinv((u-1)*N+1:u*N,(v-1)*N+1:v*N));
        cnt=cnt+1;
    end
end

if DEBUG
    for j=1:N
        try chol(reshape(Hd(j,:),[p p])); catch; fprintf('Hj<0: j=%d - ',j); keyboard; end;
    end
end

%% create u,v index vectors
% us = [1 2 ... p 1 2 ... p ... ]'   (p^2 length)
% vs = [1 1 ... 1 2 2 ... 2 ... ]'   (p^2 length)
us = repmat(1:p,1,p)';
vs = zeros(p^2,1);
for ii=1:p
    vs((ii-1)*p+1:(ii-1)*p+p)=ones(p,1)*ii;
end

%% construct the V matrix for all freqs
fi=0;
COS=zeros(p^2,1);
freqs=(2*pi*foi)/fs;
V = zeros(length(freqs),N,N); % NOTE: V will end up (N,N,freqs)
if verb, h=waitbar(0,'calculating V^-1...'); end
for f=freqs
    fi=fi+1;

    %% construct cosine matrix
    COS(:,1) = cos(us*f).*cos(vs*f)+sin(us*f).*sin(vs*f);
%     COS(:,2) = sin(us*f).*sin(vs*f);
%     COS(:,3) = cos(us*f).*sin(vs*f);
%     COS(:,4) = sin(us*f).*sin(vs*f);

    %% multiply Hd and COS matrices to get matrix where row j is
    % [sum_{u,v=1 : p} Hjj(u,v)COS_11(u,v), ...
    %  sum_{u,v=1 : p} Hjj(u,v)COS_21(u,v), ...
    %  sum_{u,v=1 : p} Hjj(u,v)COS_12(u,v), ...
    %  sum_{u,v=1 : p} Hjj(u,v)COS_22(u,v)]
    %
    % where COSab(u,v) is the a,bth element of the sine-cos transform
    % matrix evaluated at u,v.
    %
    % NOTE: reshaping row j to 2x2 yeilds a matrix proportional to V_ij(f)
    % for some specific i
    Vm = Hd*COS;

    % now we multiply in the variances of the i's (E(i,i)) to generate the
    % full V matrix
    Vm = kron(diag(E)',Vm);

    % next, reshape to desired structure (NxNx2x2)
    V(fi,:,:) = Vm';

    if verb, waitbar(fi/length(freqs),h); end
end % for freqs

% permute V: (chs,chs,freqs,2,2)
V = permute(V,[2 3 1]);
if verb, close(h); end


function [LL, LLS, ht] = igarch_likelihood(parameters, epsilon, fepsilon, p, q, errorType, igarchType, constant, backCast, T, estimFlag)
% Log likelihood for IGARCH(P,Q) estimation
%
% USAGE:
%   [LL, LLS, HT] = igarch_likelihood(PARAMETERS,EPSILON,FEPSILON,P,Q,ERRORTYPE,IGARCHTYPE,CONSTANT,BACKCAST,T,ESTIMFLAG)
%
% INPUTS:
%   PARAMETERS   - A vector of IGARCH process parameters
%                    [omega alpha beta [nu lambda]]
%   DATA         - Vector of mean zero residuals
%   EPSILON      - A column of mean zero data
%   FEPSILON     - Either abs(EPSILON) or EPSILON.^2, depending on IGARCHTYPE
%   P            - Positive, scalar integer representing the number of
%                    symmetric innovations
%   Q            - Non-negative, scalar integer representing the number
%                    of lags of conditional variance (0 for ARCH)
%   ERRORTYPE    - [OPTIONAL] The error distribution used, valid types are:
%                    'NORMAL'    - Gaussian Innovations [DEFAULT]
%                    'STUDENTST' - T distributed errors
%                    'GED'       - Generalized Error Distribution
%                    'SKEWT'     - Skewed T distribution
%   IGARCHTYPE   - [OPTIONAL] The type of variance process, either
%                    1 - Model evolves in absolute values
%                    2 - Model evolves in squares [DEFAULT]
%   CONSTANT     - [OPTIONAL] Logical value indicating whether model
%                    should include a constant.  Default is true (include).
%   BACKCAST     - The value used for variance recursion
%   T            - Length of data
%   ESTIMFLAG    - [OPTIONAL] Flag (0 or 1) to indicate if the function
%                    is being used in estimation.  If it is 1, then the parameters are
%                    transformed from unconstrained values to constrained by standard
%                    garch model constraints
%
% OUTPUTS:
%   LL             - Minus 1 times the log likelihood
%   LLS            - Time series of log likelihoods (Also multiplied by -1)
%   HT             - Time series of conditional variances
%
% COMMENTS:
%   See also IGARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 7/12/2009

if nargin==11 && estimFlag
    %If for estimation, transform the parameters
    [parameters,nu,lambda]=igarch_itransform(parameters,p,q,errorType,constant);
else
    %Otherwise the parameters simply must be parsed
    if errorType==2 || errorType==3
        %Seperate nu from the remaning parameters
        nu=parameters(p+q+constant);
        parameters=parameters(1:constant+p+q-1);
    elseif errorType==4
        lambda=parameters(p+q+constant+1);
        nu=parameters(p+q+constant);
        parameters=parameters(1:constant+p+q-1);
    end
end

%Backcast length
m  =  max([p q]);
%Compute the conditional variances
ht=igarch_core(fepsilon,parameters,backCast,p,q,m,T,igarchType,constant);
%Indices for the relevant opservations
t  = (m + 1):T;
ht = ht(t);
%Compute the log likelihoods
switch errorType
    case 1
        [LL, LLS] = normloglik(epsilon,0,ht);
        LLS = -LLS;
        LL = -LL;
    case 2
        [LL, LLS] = stdtloglik(epsilon,0,ht,nu);
        LLS = -LLS;
        LL = -LL;
    case 3
        [LL, LLS] = gedloglik(epsilon,0,ht,nu);
        LLS = -LLS;
        LL = -LL;
    case 4
        [LL, LLS] = skewtloglik(epsilon,0,ht,nu,lambda);
        LLS = -LLS;
        LL = -LL;
end


function combo_test30 ( )

%*****************************************************************************80
%
%% COMBO_TEST30 tests PRUEFER_*.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
  n = 4;

  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST30\n' );
  fprintf ( 1, '  Pruefer codes:\n' );
  fprintf ( 1, ' \n' );
  fprintf ( 1, '  PRUEFER_ENUM enumerates,\n' );
  fprintf ( 1, '  PRUEFER_RANK ranks,\n' );
  fprintf ( 1, '  PRUEFER_SUCCESSOR lists,\n' );
  fprintf ( 1, '  PRUEFER_UNRANK unranks.\n' );
%
%  Enumerate.
%
  ncode = pruefer_enum ( n );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  For N = %d\n', n );
  fprintf ( 1, '  the number of Pruefer codes is %d\n', ncode );
  fprintf ( 1, ' \n' );
%
%  List
%
  p = [];
  rank = -1;

  while ( 1 );

    rank_old = rank;

    [ p, rank ] = pruefer_successor ( n, p, rank );

    if ( rank <= rank_old )
      break
    end

    fprintf ( 1, '  %3d  ', rank );
    for i = 1 : n - 2
      fprintf ( 1, '%5d', p(i) );
    end
    fprintf ( 1, '\n' );

  end
%
%  Unrank.
%
  rank = floor ( ncode / 2 );

  p = pruefer_unrank ( rank, n );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  The element of rank %d:\n', rank );
  fprintf ( 1, ' \n' );
  for i = 1 : n - 2
    fprintf ( 1, '%5d', p(i) );
  end
  fprintf ( 1, '\n' );
%
%  Rank.
%
  rank = pruefer_rank ( n, p );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '  The rank of the element is computed as %d:\n', rank );

  return
end


function M = stiefelstackedfactory(m, d, k)
% Stiefel(k, d)^m, represented as matrices of size m*d-by-k.
%
% function M = stiefelstackedfactory(m, d, k)
%
% Points on this manifold are matrices Y of size n x k, with n = m*d.
% Y is thought of as m matrices of size d x k each, stacked on top of each
% other. Call them Y1, ..., Ym. Each Yi is an orthonormal matrix, that is,
% its d rows are unit norm and are orthogonal to each other. Thus, this
% geometry is a product of Stiefel manifolds.
%
% To easily transform matrices Y to 3D arrays Y3 of size d x k x m such
% that each slice Y3(:, :, i) corresponds to one of the matrices Yi, use
% the functions
%
%    Y3 = M.to3D(Y)   and   Y = M.to2D(Y3).
%
% The ambient space R^(nxk) is endowed with the usual inner product
% <A, B> = trace(A'*B). This inner product is restricted to the tangent
% spaces of the present manifold, thus making it a Riemannian submanifold
% of the Euclidean space R^(nxk). Tangent vectors are represented as
% matrices of the same size as Y, and can likewise be converted to 3D
% arrays and back using to3D() and to2D().
%
% In dealing with this geometry, especially when dealing with the 3D array
% representations of points and tangent vectors, the tools multiprod,
% multitransp, multitrace, multiscale etc. available in Manopt are often
% useful.
%
% See also: stiefelfactory obliquefactory multiprod multitransp

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, May 4, 2015.
% Contributors:
% Change log:

    assert(k >= d, 'k must be at least as large as d.');

    n = m*d;

    M.name = @() sprintf('Manifold of %d orthonormal matrices of size %dx%d, stacked', m, d, k);

    M.dim = @() m*(k*d - .5*d*(d+1));

    M.size = @() [m, d, k];

    M.inner = @(x, d1, d2) d1(:).'*d2(:);

    M.norm = @(x, d) norm(d(:));

    M.dist = @(x, y) error('stiefelstackedfactory.dist not implemented yet.');

    M.typicaldist = @() sqrt(M.dim());

    % Convert a dxkxm matrix to an nxk matrix
    M.to2D = @to2D;
    function A2 = to2D(A3)
        A2 = reshape(multitransp(A3), [k, m*d])';
    end

    % Convert an nxk matrix to a dxkxm matrix
    M.to3D = @to3D;
    function A3 = to3D(A2)
        A3 = multitransp(reshape(A2', [k, d, m]));
    end

    % Given 2 3D matrices A and B of size dxkxm, returns a 3D matrix C of
    % size dxdxm such that each slice C(:, :, i) is the symmetric part of
    % the product A(:, :, i) * B(:, :, i)'. The name is short for
    % "symmetric-block-diagonal", because if A and B were transformed to
    % their 2D equivalents via to2D, then the output would contain the
    % symmetric parts of the diagonal blocks of A*B'.
    M.symbdiag = @symbdiag;
    function C = symbdiag(A, B)
        C = multisym(multiprod(A, multitransp(B)));
    end

    % Orthogonal projection from the ambient space R^(nxk) to the tangent
    % space at X.
    M.proj = @projection;
    function Zt = projection(Y, Z)
        Y3 = to3D(Y);
        Z3 = to3D(Z);
        Lambda = symbdiag(Y3, Z3);
        Zt3 = Z3 - multiprod(Lambda, Y3);
        Zt = to2D(Zt3);
    end

    M.tangent = M.proj;

    M.egrad2rgrad = M.proj;

    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(Y, egrad, ehess, Ydot)
        Y3 = to3D(Y);
        Ydot3 = to3D(Ydot);
        egrad3 = to3D(egrad);
        C = symbdiag(Y3, egrad3);
        CYdot = to2D(multiprod(C, Ydot3));
        rhess = projection(Y, ehess - CYdot);
    end

    M.retr = @retraction;
    function Y = retraction(Y, U, t)
        if nargin < 3
            t = 1.0;
        end
        Y = Y + t*U;
        Y3 = to3D(Y);
        for i = 1 : m
            % Orthonormalize the rows of Y3(:, :, i):
            [u, s, v] = svd(Y3(:, :, i), 'econ'); %#ok<ASGLU>
            Y3(:, :, i) = u*v';
            % Alternatively, one could also use qr_unique as retraction.
        end
        Y = to2D(Y3);
    end

    M.exp = @exponential;
    function Y = exponential(Y, U, t)
        if nargin == 2
            t = 1;
        end
        tU3 = multitransp(to3D(t*U));
        Y3 = multitransp(to3D(Y));
        % From a formula by Ross Lippert, Example 5.4.2 in AMS08.
        for i = 1 : m
            X = Y3(:, :, i);
            Z = tU3(:, :, i);
            Y3(:, :, i) = [X, Z] * ...
                          expm([  X'*Z , -Z'*Z ; eye(d) , X'*Z]) * ...
                          [ expm(-X'*Z) ; zeros(d) ];
            % We may loose orthonormality here. Just to be sure:
            [u, s, v] = svd(Y3(:, :, i), 'econ'); %#ok<ASGLU>
            Y3(:, :, i) = u*v';
        end
        Y = to2D(multitransp(Y3));
    end

    M.hash = @(Y) ['z' hashmd5(Y(:))];

    M.rand = @random;
    function Y = random()
        Y3 = zeros(d, k, m);
        for i = 1 : m
            [Q, unused] = qr(randn(k, d), 0); %#ok<ASGLU>
            Y3(:, :, i) = Q';
        end
        Y = to2D(Y3);
    end

    M.randvec = @randomvec;
    function U = randomvec(Y)
        U = projection(Y, randn(n, k));
        U = U / M.norm(Y, U);
    end

    M.lincomb = @matrixlincomb;

    M.zerovec = @(x) zeros(n, k);

    M.transp = @(x1, x2, u) projection(x2, u);

    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, [n, k]);
    M.vecmatareisometries = @() true;

end


function MeshStat = fem_meshstats(FemFile)
% FEM_MESHSTATS: Computes and display FEM mesh volume (tetrahedral only).
%
% INPUTS:
%    - FemFile : Relative file path to a Braistorm tetrahedral FEM mesh
% OUTPUTS:
%    - MeshStat : Mtlab structure that contains all FEM mesh stqtistics (edge length, elem volum and mesh quality)
%
% DEPENDENCIES:
%    This function require the iso2mesh toolbox

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

% Install/load iso2mesh plugin
isInteractive = 1;
[isInstalled, errInstall] = bst_plugin('Install', 'iso2mesh', isInteractive);
if ~isInstalled
    error('Plugin "iso2mesh" not available.');
end

% Get data in database
bst_progress('start', 'Mesh volume', 'Loading file...');
FemFullFile = file_fullpath(FemFile);
femmat = load(FemFullFile);
% Check type of mesh: accept only tetrahedral
if (size(femmat.Elements,2) ~= 4)
    error('This menu is available for tetrahedral meshes only.');
end
% Display results in figures if no variable in output
isDisplay = (nargout == 0);
hFig = [];

% Convert to millimeter for convenience
femmat.Vertices = 1000 .* femmat.Vertices;

% Loop over the tissues
TissueID = unique(femmat.Tissue);
for iTissue = 1:length(TissueID)
    iTissueID = find(femmat.Tissue == TissueID(iTissue));

    % 1. Edges length
    bst_progress('text', sprintf('Computing edges length...  [%d/%d]', iTissue, length(TissueID)));
    Edges = meshedge(femmat.Elements(iTissueID,:));
    n1 = femmat.Vertices(Edges(:,1),:);
    n2 = femmat.Vertices(Edges(:,2),:);
    EdgeLength = sqrt((n1(:,1)- n2(:,1)).^2 + (n1(:,2)- n2(:,2)).^2 + (n1(:,3)- n2(:,3)).^2);

    tstat.EdgeLengthMax = max(EdgeLength);
    tstat.EdgeLengthMin = min(EdgeLength);
    tstat.EdgeLengthStd = std(EdgeLength);
    tstat.EdgeLengthMean = mean(EdgeLength);
    tstat.EdgeLengthRMS = rms(EdgeLength);

    % 2. Mesh quality: Joe-Liu mesh quality metric (0-1)
    %  quality: a vector of the same length as size(elem,1), with
    %            each element being the Joe-Liu mesh quality metric (0-1) of
    %            the corresponding element. A value close to 1 represents
    %            higher mesh quality (1 means equilateral tetrahedron);
    %            a value close to 0 means nearly degenerated element.
    bst_progress('text', sprintf('Computing mesh quality...  [%d/%d]', iTissue, length(TissueID)));
    quality = 100 .*meshquality(femmat.Vertices, femmat.Elements(iTissueID,:));
    tstat.MeshQualityMax = max(quality);
    tstat.MeshQualityMin = min(quality);
    tstat.MeshQualityStd = std(quality);
    tstat.MeshQualityMean = mean(quality);

    % 3. Volume of elem
    bst_progress('text', sprintf('Computing volume of elements...  [%d/%d]', iTissue, length(TissueID)));
    voli = elemvolume(femmat.Vertices, femmat.Elements(iTissueID,:));
    tstat.MeshVolumeMax = max(voli);
    tstat.MeshVolumeMin = min(voli);
    tstat.MeshVolumeStd = std(voli);
    tstat.MeshVolumeMean = mean(voli);
    tstat.MeshVolumeSum = sum(voli);

    MeshStat.(femmat.TissueLabels{iTissue}) = tstat;

    % Visualization
    if isDisplay
        bst_progress('text', sprintf('Visualisation... [%d/%d]', iTissue, length(TissueID)));
        hFig(end+1) = figure('Name', ['Mesh stat: ' femmat.TissueLabels{iTissue}], 'NumberTitle', 'off');

        nbins = 30;
        subplot(3,1,1)
        histogram(EdgeLength,nbins);
        xlabel(sprintf('Edge length (mm):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f', tstat.EdgeLengthMean, tstat.EdgeLengthStd, tstat.EdgeLengthMin, tstat.EdgeLengthMax))
        drawnow

        subplot(3,1,2)
        histogram(quality,nbins);
        xlabel(sprintf('Mesh quality (%%):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f', tstat.MeshQualityMean, tstat.MeshQualityStd, tstat.MeshQualityMin, tstat.MeshQualityMax))
        drawnow

        subplot(3,1,3)
        histogram(voli,nbins);
        xlabel(sprintf('Element volume (mm3):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f | sum=%1.2f', tstat.MeshVolumeMean, tstat.MeshVolumeStd, tstat.MeshVolumeMin, tstat.MeshVolumeMax, tstat.MeshVolumeSum))
        drawnow
    end
end

% For all the full Model
% 1. Edges length
bst_progress('text', 'Computing edges length...');
Edges = meshedge(femmat.Elements);
n1 = femmat.Vertices(Edges(:,1),:);
n2 = femmat.Vertices(Edges(:,2),:);
EdgeLength = sqrt((n1(:,1)- n2(:,1)).^2 + (n1(:,2)- n2(:,2)).^2 + (n1(:,3)- n2(:,3)).^2);

MeshStat.FullModel.EdgeLengthMax = max(EdgeLength);
MeshStat.FullModel.EdgeLengthMin = min(EdgeLength);
MeshStat.FullModel.EdgeLengthStd = std(EdgeLength);
MeshStat.FullModel.EdgeLengthMean = mean(EdgeLength);
MeshStat.FullModel.EdgeLengthRMS = rms(EdgeLength);

% 2. Mesh quality: Joe-Liu mesh quality metric (0-100)
bst_progress('text', 'Computing mesh quality...');
quality = 100 .* meshquality(femmat.Vertices, femmat.Elements);
MeshStat.FullModel.MeshQualityMax = max(quality);
MeshStat.FullModel.MeshQualityMin = min(quality);
MeshStat.FullModel.MeshQualityStd = std(quality);
MeshStat.FullModel.MeshQualityMean = mean(quality);

% 3. Volume of elem
bst_progress('text', 'Computing volume of elements...');
voli = elemvolume(femmat.Vertices, femmat.Elements);
MeshStat.FullModel.MeshVolumeMax = max(voli);
MeshStat.FullModel.MeshVolumeMin = min(voli);
MeshStat.FullModel.MeshVolumeStd = std(voli);
MeshStat.FullModel.MeshVolumeMean = mean(voli);
MeshStat.FullModel.MeshVolumeSum = sum(voli);

% Visualization
if isDisplay
    bst_progress('text', 'Visualisation...');
    hFig(end+1) = figure('Name', 'Mesh stat: all tissues combined', 'NumberTitle', 'off');

    nbins = 30;
    subplot(3,1,1)
    histogram(EdgeLength,nbins);
    xlabel(sprintf('Edge length (mm):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f', MeshStat.FullModel.EdgeLengthMean, MeshStat.FullModel.EdgeLengthStd, MeshStat.FullModel.EdgeLengthMin, MeshStat.FullModel.EdgeLengthMax))
    drawnow

    subplot(3,1,2)
    histogram(quality,nbins);
    xlabel(sprintf('Mesh quality (%%):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f', MeshStat.FullModel.MeshQualityMean, MeshStat.FullModel.MeshQualityStd, MeshStat.FullModel.MeshQualityMin, MeshStat.FullModel.MeshQualityMax))
    drawnow

    subplot(3,1,3)
    histogram(voli,nbins);
    xlabel(sprintf('Element volume (mm3):   mean=%1.2f | std=%1.2f | min=%1.2f | max=%1.2f | sum=%1.2f', MeshStat.FullModel.MeshVolumeMean, MeshStat.FullModel.MeshVolumeStd, MeshStat.FullModel.MeshVolumeMin, MeshStat.FullModel.MeshVolumeMax, MeshStat.FullModel.MeshVolumeSum))

    % Close all the figures at once
    set(hFig, 'DeleteFcn', @(h,ev)delete(setdiff(hFig,h)));
end

bst_progress('stop');


function [mv1, mv2, mv3] = specificIntMeanCurvDetails(img, varargin)
%SPECIFICINTMEANCURVDETAILS Ohser's Integral of Mean Curvature with details
%
%   this version is just for debugging.
%   It allows to extact contribution from each directions.
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 16/02/2005.
%

%   HISTORY

img = img~=0;

delta = [1 1 1];

% square faces of the unit cell
kr = [...
    1 2 3 4;...
    1 2 5 6;...
    1 3 5 7;...
    1 2 7 8;...
    3 5 4 6;...
    1 6 3 8;...
    2 4 5 7;...
    2 3 6 7;...
    1 5 3 8];

% triangle faces of the unit cell
kt = [...
    1 7 6;...
    2 5 8;...
    4 7 6;...
    3 5 8;...
    2 3 8;...
    4 1 7;...
    2 3 5;...
    4 1 6];

% unit surface for each cell
c1 = 0.045778;
c2 = 0.036981;
c3 = 0.035196;
c = [c1 c1 c1   c2 c2 c2   c2 c2 c2   c3 c3 c3 c3];

% distances between pixels
d1 = delta(1);
d2 = delta(2);
d3 = delta(3);
d12  = sqrt(delta(1)*delta(1) + delta(2)*delta(2));
d13  = sqrt(delta(1)*delta(1) + delta(3)*delta(3));
d23  = sqrt(delta(2)*delta(2) + delta(3)*delta(3));
s = (d12 + d23 + d13)/2;
a123 = 2*sqrt(s*(s-d12)*(s-d13)*(s-d23));

a = [d1*d2 d1*d3 d2*d3   d3*d12 d3*d12 d2*d13 d2*d13 d1*d23 d1*d23  a123 a123 a123 a123];

% compute gray-tone histogram of the image
h = grayHist(img);

mv1 = 0;
mv2 = 0;
mv3 = 0;

% for each type of configuration
for l=1:256

    v = l-1;
    b(1) = bitand(v,1)~=0;
    b(2) = bitand(v,2)~=0;
    b(3) = bitand(v,4)~=0;
    b(4) = bitand(v,8)~=0;
    b(5) = bitand(v,16)~=0;
    b(6) = bitand(v,32)~=0;
    b(7) = bitand(v,64)~=0;
    b(8) = bitand(v,128)~=0;

    % for each square face
    for nu=1:3

        b1 = b(kr(nu, 1));
        b2 = b(kr(nu, 2));
        b3 = b(kr(nu, 3));
        b4 = b(kr(nu, 4));

        s = sum([b1 b2 b3 b4]);
        if s==1
            mv1 = mv1 + h(l)*c(nu)/4/a(nu);
        elseif s==3
            mv1 = mv1 - h(l)*c(nu)/4/a(nu);
        end
    end

    % for each square diagonal face
    for nu = 4:9

        b1 = b(kr(nu, 1));
        b2 = b(kr(nu, 2));
        b3 = b(kr(nu, 3));
        b4 = b(kr(nu, 4));

        s = sum([b1 b2 b3 b4]);
        if s==1
            mv2 = mv2 + h(l)*c(nu)/4/a(nu);
        elseif s==3
            mv2 = mv2 - h(l)*c(nu)/4/a(nu);
        end
    end

    % for each triangular face
    for nu=10:13

        b1 = b(kt(nu-9, 1));
        b2 = b(kt(nu-9, 2));
        b3 = b(kt(nu-9, 3));
        b4 = b(kt(nu-5, 1));
        b5 = b(kt(nu-5, 2));
        b6 = b(kt(nu-5, 3));

        s1 = sum([b1 b2 b3]);
        s2 = sum([b4 b5 b6]);
        if s1==1
            mv3 = mv3 + h(l)*c(nu)/3/a(nu);
        end
        if s2==2
            mv3 = mv3 - h(l)*c(nu)/3/a(nu);
        end
    end
end

%imc = 4*pi*mv/sum(h(:));


function [u,p,info] =  tripreMaxwellsaddle(A,G,f,g,node,elem,bdFlag,M,grad,option)
%% tripreMaxwellsaddle solve the Maxwell system with divgence free condition,
%
%         [A  G] [u]  = [f]
%         [G' O] [p]  = [g]
%
% where  G = M_e*grad with the mass matrix for the edge element Me.
%
% This system can be rewritten as
%         [A+G*DMinv*G'       G] [u]  = f +G*DMinv*g0
%         [G'                 O] [p]  = g0
%
% where DMinv is the inverse of the diagonal matrix of the mass. Then
%
%  [A+G*DMinv*G'  G] [I    grad]                = [Abar   O  ]
%  [G'            O] [0  -Dminv*grad'*Me*grad ] = [G'     Ap]
%
%  We solve the system by GMRES with  the preconditioner
%
%       [I   grad                ]  [Abar O   ]^{-1}
%       [O  -Dminv*grad'*M_e*grad]  [G'   A_p ]
%
% where we compute the inverse Abar by mgHodgeLapE and Ap by mg.
%
% Created by Long chen and Jie Zhou on Aug,2015.

Nf = length(f);
Ng = length(g);
t = cputime;

%% Parameters
if ~exist('option','var'), option = []; end
Ndof = Nf + Ng;
option = mgoptions(option,Ndof);    % parameters
x0 = option.x0;
tol = option.tol;
maxIt = option.solvermaxit;
printlevel = option.printlevel;
d = size(node,2);

%% Set up auxiliary matrices
if d == 2
    area = simplexvolume(node,elem);
    Mvlump = accumarray([elem(:,1);elem(:,2);elem(:,3)],[area;area;area]/3,...
                        [max(elem(:)),1]);
elseif d == 3
    volume = abs(simplexvolume(node,elem)); % uniform refine in 3D is not orientation presereved
    Mvlump = accumarray([elem(:,1);elem(:,2);elem(:,3);elem(:,4)],...
                        [volume;volume;volume;volume]/4,[max(elem(:)),1]);
end
DMinv = spdiags(1./Mvlump(option.isFreeNode),0,Ng,Ng);
f = f + G*(DMinv*g);  % add second equation to the first one
Abar = A + G*DMinv*G'; % Hodge Laplacian
Ap = grad'*M*grad;    % scalar Laplacian

%% Set up matrices for multigrid
setupOption.solver = 'NO';
setupOption.isFreeEdge = option.isFreeEdge;
setupOption.freeDof = option.isFreeNode;
[~,~,Ai_N,Bi_N,BBi_N,Res_N,Pro_N] = mg(Ap,g,elem,setupOption);
[x,info,Ai,Bi,BBi,Res,Pro] = mgHodgeLapE(Abar,f,node,elem,bdFlag,setupOption); %#ok<*ASGLU>

%% Form a big matrix equation
bigA = [Abar G; G' sparse(Ng,Ng)];
bigF = [f; g];

%% Preconditioned GMRES
% options for V-cycle of Schur complement
if strcmp(option.solver,'CG') % change default set up in mgoption
   option.solver = 'Vcycle';
end
if isfield(option,'Vit')
   option.solvermaxIt = option.Vit;
else
   option.solvermaxIt = 1;
end
option.setupflag = false;
option.x0 = zeros(Nf,1);
option.printlevel = 0;
Apmgoption.printlevel = 0;
Apmgoption.printlevel = 0;
Apmgoption.setupflag = false;
Apmgoption.maxIt     = 1;
% minres for the saddle point system
[x,flag,stopErr,itStep,err] = gmres(bigA,bigF,20,tol,maxIt,@tripre,[],x0);
itStep = (itStep(1)-1)*20 + itStep(2); % total iteration
u = x(1:Nf);
p = x(Nf+1:end);

%% Output
time = cputime - t;
if printlevel >= 1
    fprintf('Triangular Preconditioned GMRES \n');
    fprintf('#dof: %8.0u,  #nnz: %8.0u, V-cycle: %2.0u, iter: %2.0u,   err = %8.2e,   time = %4.2g s\n',...
                 Ndof, nnz(bigA), option.solvermaxIt, itStep, stopErr, time)
end
if (flag == 1) && (printlevel>0)
   fprintf('NOTE: the iterative method does not converge! \n');
end
info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',stopErr);

%% Preconditioner
    function e  = tripre(r)
        r1 = r(1:Nf);
        r2 = r(Nf+1:end);
        e1 = mgHodgeLapE(Abar,r1,node,elem,bdFlag,option,Ai,Bi,BBi,Res,Pro);
        e2 = mg(Ap,r2-G'*e1,elem,Apmgoption,Ai_N,Bi_N,BBi_N,Res_N,Pro_N);
        e  = [e1 + grad*e2; -DMinv*(Ap*e2)];
    end
end


function x = nsctrec(y, dfilt, pfilt)
% NSCTREC   Nonsubsampled Contourlet Reconstruction
%
%	x = nsscrec(y, [dfilt, pfilt] )
%
% INPUT:
%   y:  a cell vector of length length(nlevs) + 1, where except y{1} is
%       the lowpass subband, each cell corresponds to one pyramidal
%       level and is a cell vector that contains bandpass directional
%       subbands from the DFB at that level.
%   dfilt:
%       a string, filter name for the directional decomposition step.
%       It is optional with default value 'dmaxflat7'. See dfilters.m for all
%       available filters.
%   pfilt:
%       a string, filter name for the pyramidal decomposition step.
%       It is optional with default value 'maxflat'. See atrousfilters.m for
%       all available filters.
%
% OUTPUT:
%   x:
%       a matrix, reconstructed image
%
% See also: ATROUSFILTERS, DFILTERS, NSCTDEC, NSFBREC, NSDFBREC

%
% HISTORY:
%     02/17/04 Created by Jianping Zhou.
%     08/07/04 Modified by Jianping Zhou. Incorporte the fast implementation
%              of convolution algorithm by Jason Laska.
%     08/30/04 Modified by Arthur. L. Cunha, added more pyramid filters.
%     10/17/04 Modified by Arthur. L. Cunha, replaced periodic with symmetric extension
%     01/24/05 Modified by Jianping Zhou, changed function names and corrected a bug.
%     10/31/05 Modified by Arthur L Cunha, corrected a bug in the pyramid decomposition (a wrong index)

% Check input
if ~exist('dfilt', 'var')
    dfilt = 'dmaxflat7' ;
end;

if ~exist('pfilt', 'var')
    pfilt = 'maxflat' ;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Get fan filters, parallelogram filters, and pyramid filters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Get the directional filters for the critically sampled DFB.
filters = cell(4) ;
[h1, h2] = dfilters(dfilt, 'r');
% A scale is required for the nonsubsampled case.
h1 = h1./sqrt(2) ;
h2 = h2./sqrt(2) ;

% Generate the first-level fan filters by modulations.
filters{1} = modulate2(h1, 'c');
filters{2} = modulate2(h2, 'c');

% Obtain the parallelogram filters from the diamond filters
[filters{3}, filters{4}] = parafilters( h1, h2 ) ;

% Currently only one filter by Arthur Cunha
% It has been normalized.
[h1, h2, g1, g2] = atrousfilters(pfilt) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nonsubsampled Contourlet transform with tree structure filter banks
% Nonsubsampled pyramids make multiresolution decomposition.
% Nonsubsampled directional filter banks make directional decomposition.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n = length(y) - 1;
xlo = y{1};
nIndex = n-1;
for i=1:n

    % Process the detail subbands
    if iscell( y{i+1} )
        % Nonsubsampled DFB reconstruction
        xhi = nsdfbrec( y{i+1}, filters );
    else
        % No DFB decomposition, copy directly
        xhi = y{i+1};
    end

    % Nonsubsampled Pyramid reconstruction
    x = nsfbrec(xlo, xhi, g1, g2, nIndex);

    % Prepare for the next level
    xlo = x ;
    nIndex = nIndex -1;
end


%% FUNCTION Least_msmtfl_capL1
%   Multi-Stage Multi-Task Feature Learning
%
%% OBJECTIVE
%   min_W ||XW - Y||_F^2 + lambda*\sum_i \min{||W^i||_1,theta}
%
%   It's a nonconvex optimization problem, which can be relaxed into a Multi-Stage Convex optimization:
%   min_W ||XW - Y||_F^2 + \sum_i{gamma_i*||W^i||_1}
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   lambda: regularized parameter (vector)
%   theta: theresholding paramter
%
%   (Optional)
%   opts.lFlag: estimate the upper bound of Lipschitz constant if nonzero, zero otherwise
%
%% OUTPUT
%   W: output weight
%   fun: function values
%
%% LICENSE
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as pubtolwlished by
%   the Free Software Foundation, either version 3 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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Pinghua Gong and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on Dec 18, 2012.
%
%
%% RELATED PAPERS
%
% [1] Pinghua Gong, Jieping Ye, Changshui Zhang. Multi-Stage Multi-Task
%     Feature Learning. The 26th Annual Conference on Neural Information
%     Processing Systems (NIPS 2012), Lake Tahoe, Nevada, USA,
%     December 3-6, 2012.
%
%% RELATED FUNCTIONS
%  init_opts, combine_input (utils)

function [W,funcVal] = Least_msmtfl_capL1(X, Y, lambda, theta, opts)

if nargin <4
    error('\n Inputs: X, Y, and lambda1, and lambda2 should be specified!\n');
end
if nargin <5
    opts = [];
end

% initialize options.
opts=init_opts(opts);

% initial Lipschiz constant.
if isfield(opts, 'lFlag')
    lFlag = opts.lFlag;
else
    lFlag = false;
end

task_num = length(X);
[X, y, ~, samplesize] = combine_input(X, Y);
dimension = size(X, 2);

% initialize a starting point
if opts.init==2
    W0 = zeros(dimension, task_num);
elseif opts.init == 0
    W0 = randn(dimension, task_num);
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .P0');
        end
    else
        W0=randn(dimension, task_num);
    end
end

% Set an array to save the objective value
funcVal = [];

W = W0;

[d,m] = size(W); % d: dimension, m: the number of tasks
X = diagonalize(X,samplesize);
XtX = X'*X; Xty = X'*y;

L1norm = max(sum(abs(X),1)); Linfnorm = max(sum(abs(X),2));

if lFlag
    % Upper bound for largest eigenvalue of Hessian matrix
    L = 2*min([L1norm*Linfnorm; size(X,1)*Linfnorm*Linfnorm; size(X,2)*L1norm*L1norm; size(X,1)*size(X,2)*max(abs(X(:)))]);
else
    % Lower bound for largest eigenvalue of Hessian matrix
    L = 2*max(L1norm*L1norm/size(X,1),Linfnorm*Linfnorm/size(X,2));
end
% Initial function value
funcVal = cat(1, funcVal, norm(X*W(:) - y)^2 + lambda*(sum(min(sum(abs(W),2),theta))));

tolw = 1e-5; % precision for inner iterations.

for iter = 1:opts.maxIter
    if iter == 1
        weight = lambda*ones(d,m);
    else
        weight = lambda*(repmat(sum(abs(W),2) < theta,1,m));
    end
    [W,~,iterw] = wLassomtl(X,y,XtX,Xty,weight,W,tolw,100,L,lFlag);

    if iterw == 1
        tolw = tolw/4;
    end

    funcVal = cat(1, funcVal, norm(X*W(:) - y)^2 + lambda*(sum(min(sum(abs(W),2),theta))));

    % stopping condition
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

end


%DEMO_DERIVATIVEOBS  Regression problem demonstration with derivative
%                    observations
%
%  Description
%    The regression problem consist of a data with one input variable,
%    two output variables with Gaussian noise; observations and
%    derivative observations. The constructed model is full GP with
%    Gaussian likelihood.
%
%    The covariance matrix K includes now also covariances between
%    derivative observations and between derivative and latent
%    observations. With derivative observations, the K matrix is a
%    block matrix with following blocks:
%
%        K = [K_ll K_Dl'; K_Dl K_DD]
%
%    Where D refers to derivative and l to latent observation and
%       K_ll = k(x_i, x_j | th)
%       K_Dl = d k(x_i, x_j | th) / dx_i
%       K_DD = d^2 k(x_i, x_j | th) / dx_i dx_j
%
%    To include derivative observations in the inference:
%
%       - provide partial derivative observations in the
%       observation vector after output observations
%       y=[y;dy_1;...;dy_n]; for ex. if size(x)=[10 2] ->
%       size(y)=[30 1]
%
%       - gp_set(gp, 'derivobs', 'on')
%
%   The demo is organised in two parts:
%     1) data analysis without derivative observations
%     2) data analysis with derivative observations
%
%  See also  DEMO_REGRESSION1
%

% Copyright (c) 2010 Tuomas Nikoskinen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% Create the data
tp=9;                                  %number of training points -1
x=[-2:4/tp:2]';
y=sin(x).*cos(x).^2;                   % The underlying process
dy=cos(x).^3 - 2*sin(x).^2.*cos(x);    % Derivative of the process
ns=0.06;                               % noise standard deviation

% Add noise
y=y + ns*randn(size(y));
% derivative observations are also noisy
dy=dy + ns*randn(size(dy));
% observation vector with derivative observations
y2=[y;dy];

% test points
xt=[-3:0.05:3]';
nt=length(xt);

%========================================================
% PART 1 GP model without derivative obs
%========================================================
disp('GP model without derivative obs')

% Covariance function
pl = prior_t();
pm = prior_sqrtt();
gpcf = gpcf_sexp('lengthScale', 0.5, 'magnSigma2', .5, ...
                 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
% Use default Gaussian likelihood
gp = gp_set('cf', gpcf);

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3,'DerivativeCheck','on');
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);
% Do the prediction
[Eft, Varft] = gp_pred(gp, x, y, xt);

% PLOT THE DATA

figure
%m=shadedErrorBar(p,Eft(1:size(xt)),2*sqrt(Varft(1:size(xt))),{'k','lineWidth',2});
subplot(2,1,1)
m=plot(xt,Eft,'k','lineWidth',2);
hold on
plot(xt,Eft+2*sqrt(Varft),'k--')
hold on
m95=plot(xt,Eft-2*sqrt(Varft),'k--');
hold on
hav=plot(x, y(1:length(x)), 'ro','markerSize',7,'MarkerFaceColor','r');
hold on
h=plot(xt,sin(xt).*cos(xt).^2,'b--','lineWidth',2);
%legend([m.mainLine m.patch h hav],'prediction','95%','f(x)','observations');
legend([m m95 h hav],'prediction','95%','f(x)','observations');
title('GP without derivative observations')
xlabel('input x')
ylabel('output y')

%========================================================
% PART 2 GP model with derivative obs
%========================================================
disp('GP model with derivative obs')

% Option derivobs set so that the derivatives are in use
gp = gp_set('cf', gpcf, 'derivobs', 'on');

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3,'DerivativeCheck','on');
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y2,'opt',opt);
% Do the prediction
[Eft2, Varft2] = gp_pred(gp, x, y2, xt);
% Use predictions for function values only
Eft2=Eft2(1:nt);Varft2=Varft2(1:nt);

% PLOT THE DATA
% plot lines indicating the derivative

subplot(2,1,2)
m=plot(xt,Eft2,'k','lineWidth',2);
hold on
plot(xt,Eft2+2*sqrt(Varft2),'k--')
hold on
m95=plot(xt,Eft2-2*sqrt(Varft2),'k--');
hold on
hav=plot(x, y(1:length(x)), 'ro','markerSize',7,'MarkerFaceColor','r');
hold on
h=plot(xt,sin(xt).*cos(xt).^2,'b--','lineWidth',2);

xlabel('input x')
ylabel('output y')
title('GP with derivative observations')

i1=0;
a=0.1;
ddx=zeros(2*length(x),1);
ddy=zeros(2*length(x),1);
for i=1:length(x)
  i1=i1+1;
  ddx(i1)=x(i)-a;
  ddy(i1)=y(i)-a*dy(i);
  i1=i1+1;
  ddx(i1)=x(i)+a;
  ddy(i1)=y(i)+a*dy(i);
end

for i=1:2:length(ddx)
  hold on
  dhav=plot(ddx(i:i+1), ddy(i:i+1),'r','lineWidth',2);
end
legend([m m95 h hav dhav],'prediction','95%','f(x)','observations','der. obs.');


function [d] = triangle2distance(tri, pos, s, maxinitdist)

% TRIANGLE2DISTANCE computes the geodesic distance (across the edges) on a
% mesh, using Dijkstra's algorithm. The Dijkstra code is an efficient
% vectorized version of a function from MIT's graphtool toolbox, operating
% on an adjacency matrix.
%
% Use as
%   d = triangle2distance(tri, pos, s)
%
% Input arguments:
%   tri = Mx3 matrix describing the triangles
%   pos = Nx3 matrix describing the position of the vertices
%   s   = (can be empty), scalar or vector with indices for the points for
%         which the distance (to all other points) will be computed. If
%         empty or not defined, all points will be considered.
%
% Output argument:
%   d   = Nxnumel(s) distance matrix

% Copyright (C) 2015, Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id:$

adj = triangle2connectivity(tri, pos);
n   = length(adj);
if nargin<3 || isempty(s)
  s  = 1:n;
end
if nargin<4
  maxinitdist = inf;
end
ns = length(s);

d    = inf*ones(n,ns); % distance s-all nodes
for k = 1:ns
  d(s(k),k) = 0; % s-s distance
end

for k = 1:ns
  if mod(k,10)==0,
    fprintf('computing distance between node %d and nodes 1:%d\n', s(k), n);
  end
  T = 1:n;    % node set with shortest paths not found

  if isfinite(maxinitdist)
    initd = sqrt(sum((pos-pos(s(k),:)).^2,2));
    T(initd>=maxinitdist) = [];
  end

  %while ~isempty(T) %%% a for-loop goes ~10% faster, and we know how often
  %we need to iterate
  for m = 1:n
    if mod(m,1000)==0,
      fprintf('looping across vertices %d/%d\n', m, n);
    end
    [dmin,ind] = min(d(T,k));

    adj_ = adj(T,T(ind));
    d_   = d(T,k);

    % logic: shrink the distance if there's an edge (adj_>0) AND if it's
    % shorter to travel through this edge
    criterion = adj_ > 0 & d_ > d(T(ind),k)+adj_;
    d(T(criterion),k) = d(T(ind),k) + adj_(criterion);

    T(ind) = [];
  end
end

% the below code is from MIT's graphtool toolbox. the above code that
% computes the distance based on an adjacency matrix is taken from there,
% but the vectorized functionality is ~10 times as fast.

% Implements a simple version of the Dijkstra shortest path algorithm
% Returns the distance from a single vertex to all others, doesn't save the path
% INPUTS: adjacency matrix (adj), start node (s)
% OUTPUTS: shortest path length from start node to all other nodes
% Note: works with a weighted/directed matrix
% GB, Last Updated: December 13, 2004

% Copyright (c) 2011, Massachusetts Institute of Technology.
% All rights reserved.
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%
%    Redistributions of source code must retain the above copyright notice, this list
%    of conditions and the following disclaimer.
%    Redistributions in binary form must reproduce the above copyright notice, this list
%    of conditions and the following disclaimer in the documentation and/or other materials
%    provided with the distribution.
%    Neither the name of the Massachusetts Institute of Technology nor the names of its
%    contributors may be used to endorse or promote products derived from this software without
%    specific prior written permission.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
% IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
% DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
% DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
% IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
% THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

% function d = simple_dijkstra(adj,s)
%
% n=length(adj);
% d = inf*ones(1,n); % distance s-all nodes
% d(s) = 0;    % s-s distance
% T = 1:n;    % node set with shortest paths not found
%
% while not(isempty(T))
%     [dmin,ind] = min(d(T));
%     for j=1:length(T)
%         if adj(T(ind),T(j))>0 & d(T(j))>d(T(ind))+adj(T(ind),T(j))
%             d(T(j))=d(T(ind))+adj(T(ind),T(j));
%         end
%     end
%     T = setdiff(T,T(ind));
%
% end


function beta = lars(X, y, method, stop, useGram, Gram, Cardi, bSparse, trace)
% This function is provided at
% http://www2.imm.dtu.dk/pubdb/views/publication_details.php?id=3897
% I have made some small modifications  -- Deng Cai, Feb/2008

% LARS  The LARS algorithm for performing LAR or LASSO.
%    BETA = LARS(X, Y) performs least angle regression on the variables in
%    X to approximate the response Y. Variables X are assumed to be
%    normalized (zero mean, unit length), the response Y is assumed to be
%    centered.
%    BETA = LARS(X, Y, METHOD), where METHOD is either 'LARS' or 'LASSO'
%    determines whether least angle regression or lasso regression should
%    be performed.
%    BETA = LARS(X, Y, METHOD, STOP) with nonzero STOP will perform least
%    angle or lasso regression with early stopping. If STOP is negative,
%    STOP is an integer that determines the desired number of variables. If
%    STOP is positive, it corresponds to an upper bound on the L1-norm of
%    the BETA coefficients.
%    BETA = LARS(X, Y, METHOD, STOP, USEGRAM) specifies whether the Gram
%    matrix X'X should be calculated (USEGRAM = 1) or not (USEGRAM = 0).
%    Calculation of the Gram matrix is suitable for low-dimensional
%    problems. By default, the Gram matrix is calculated.
%    BETA = LARS(X, Y, METHOD, STOP, USEGRAM, GRAM) makes it possible to
%    supply a pre-computed Gram matrix. Set USEGRAM to 1 to enable. If no
%    Gram matrix is available, exclude argument or set GRAM = [].
%    BETA = LARS(X, Y, METHOD, STOP, USEGRAM, GRAM, TRACE) with nonzero
%    TRACE will print the adding and subtracting of variables as all
%    LARS/lasso solutions are found.
%    Returns BETA where each row contains the predictor coefficients of
%    one iteration. A suitable row is chosen using e.g. cross-validation,
%    possibly including interpolation to achieve sub-iteration accuracy.
%
% Author: Karl Skoglund, IMM, DTU, kas@imm.dtu.dk
% Reference: 'Least Angle Regression' by Bradley Efron et al, 2003.

%% Input checking
% Set default values.
if nargin < 9
    trace = 0;
end
if nargin < 8
    bSparse = 1;
end
if nargin < 7
    Cardi = [];
end
if nargin < 6
    Gram = [];
end
if nargin < 5
    useGram = 0;
end
if nargin < 4
    stop = 0;
end
if nargin < 3
    method = 'lasso';
end
if strcmpi(method, 'lasso')
    lasso = 1;
else
    lasso = 0;
end

if isempty(X)
    error('The code has been updated. Please input the X');
end

%% LARS variable setup
[n p] = size(X);
% nvars = min(n-1,p); %
nvars = p; %

maxk = 512*nvars; % Maximum number of iterations

if isempty(Cardi)
    if stop == 0
        if bSparse
            beta = sparse(p,2*nvars);
        else
            beta = zeros(p,2*nvars);
        end
    elseif stop < 0
        if bSparse
            beta = sparse(p,2*round(-stop));
        else
            beta = zeros(p,2*round(-stop));
        end
    else
        if bSparse
            beta = sparse(p,100);
        else
            beta = zeros(p,100);
        end
    end
else
    Cardi = unique(Cardi);
    Cardi(Cardi>nvars) = [];
    stop = -max(Cardi);
    if bSparse
        beta = sparse(p,length(Cardi));
    else
        beta = zeros(p,length(Cardi));
    end
    betak = zeros(p,1);
end

mu = zeros(n, 1); % current "position" as LARS travels towards lsq solution
I = 1:p; % inactive set
A = []; % active set

% Calculate Gram matrix if necessary
if isempty(Gram) && useGram
    error('The code has been updated. Please input the Gram');
%     clear Gram;
%     global Gram;
    %   Gram = X'*X; % Precomputation of the Gram matrix. Fast but memory consuming.
end

if ~useGram
    R = []; % Cholesky factorization R'R = X'X where R is upper triangular
end

lassocond = 0; % LASSO condition boolean
stopcond = 0; % Early stopping condition boolean
k = 0; % Iteration count
vars = 0; % Current number of variables

if trace
    disp(sprintf('Step\tAdded\tDropped\t\tActive set size'));
end

% TimeLoop = zeros(2*nvars,1);
tmpT = cputime;

%% LARS main loop
while vars < nvars && ~stopcond && k < maxk
    k = k + 1;
    c = X'*(y - mu);
    [C j] = max(abs(c(I)));
    j = I(j);

    if ~lassocond % if a variable has been dropped, do one iteration with this configuration (don't add new one right away)
        if ~useGram
            diag_k = X(:,j)'*X(:,j); % diagonal element k in X'X matrix
            if isempty(R)
                R = sqrt(diag_k);
            else
                col_k = X(:,j)'*X(:,A); % elements of column k in X'X matrix
                R_k = R'\col_k'; % R'R_k = (X'X)_k, solve for R_k
                R_kk = sqrt(diag_k - R_k'*R_k); % norm(x'x) = norm(R'*R), find last element by exclusion
                R = [R R_k; [zeros(1,size(R,2)) R_kk]]; % update R
            end
        end
        A = [A j];
        I(I == j) = [];
        vars = vars + 1;
        if trace
            disp(sprintf('%d\t\t%d\t\t\t\t\t%d', k, j, vars));
        end
    end

    s = sign(c(A)); % get the signs of the correlations

    if useGram
        if vars <= 200
            R = chol(Gram(A,A));
        elseif lassocond
            if (rJ <= 200) & vars <= 1000
                R = chol(Gram(A,A));
            else
                R(:,rJ) = []; % remove column j
                tmpn = size(R,2);
                for tmpk = rJ:tmpn
                    tmpp = tmpk:tmpk+1;
                    [G,R(tmpp,tmpk)] = planerot(R(tmpp,tmpk)); % remove extra element in column
                    if tmpk < tmpn
                        R(tmpp,tmpk+1:tmpn) = G*R(tmpp,tmpk+1:tmpn); % adjust rest of row
                    end
                end
                R(end,:) = []; % remove zero'ed out row
            end
        else
            R_k = R'\Gram(A(1:end-1),j);
            R_kk = sqrt(Gram(j,j)-R_k'*R_k);
            R = [R R_k; [zeros(1,size(R,2)) R_kk]]; % update R
        end
        GA1 = R\(R'\s);
        AA = 1/sqrt(sum(GA1.*s));
        w = AA*GA1;
    else
        GA1 = R\(R'\s);
        AA = 1/sqrt(sum(GA1.*s));
        w = AA*GA1;
    end
    u = X(:,A)*w; % equiangular direction (unit vector)

    if vars == nvars % if all variables active, go all the way to the lsq solution
        gamma = C/AA;
    else
        a = X'*u; % correlation between each variable and eqiangular vector
        temp = [(C - c(I))./(AA - a(I)); (C + c(I))./(AA + a(I))];
        gamma = min([temp(temp > 0); C/AA]);
    end

    % LASSO modification
    if lasso
        lassocond = 0;
        if isempty(Cardi)
            temp = -beta(A,k)./w;
        else
            temp = -betak(A)./w;
        end
        [gamma_tilde] = min([temp(temp > 0); gamma]);
        j = find(temp == gamma_tilde);
        if gamma_tilde < gamma,
            gamma = gamma_tilde;
            lassocond = 1;
        end
    end

    mu = mu + gamma*u;
    if isempty(Cardi)
        if size(beta,2) < k+1
            if bSparse
                beta = [beta sparse(p,size(beta,1))];
            else
                beta = [beta zeros(p,size(beta,1))];
            end
        end
        beta(A,k+1) = beta(A,k) + gamma*w;
    else
        tmpbetak = betak(A) + gamma*w;
        betak = zeros(p,1);
        betak(A) = tmpbetak;
        idx = find(Cardi==vars);
        if ~isempty(idx)
            beta(:,idx) = betak;
        end
    end

    % Early stopping at specified bound on L1 norm of beta
    if isempty(Cardi)
        if stop > 0
            t2 = sum(abs(beta(:,k+1)));
            if t2 >= stop
                t1 = sum(abs(beta(:,k)));
                s = (stop - t1)/(t2 - t1); % interpolation factor 0 < s < 1
                beta(:,k+1) = beta(:,k) + s*(beta(:,k+1) - beta(:,k));
                stopcond = 1;
            end
        end
    end

    % If LASSO condition satisfied, drop variable from active set
    if lassocond == 1
        if ~useGram
            R(:,j) = []; % remove column j
            tmpn = size(R,2);
            for tmpk = j:tmpn
                tmpp = tmpk:tmpk+1;
                [G,R(tmpp,tmpk)] = planerot(R(tmpp,tmpk)); % remove extra element in column
                if tmpk < tmpn
                    R(tmpp,tmpk+1:tmpn) = G*R(tmpp,tmpk+1:tmpn); % adjust rest of row
                end
            end
            R(end,:) = []; % remove zero'ed out row
        end
        rJ = j;
        I = [I A(j)];
        A(j) = [];
        vars = vars - 1;
        if trace
            disp(sprintf('%d\t\t\t\t%d\t\t\t%d', k, j, vars));
        end
    end

    % Early stopping at specified number of variables
    if stop < 0
        stopcond = vars >= -stop;
    end

%     TimeLoop(k) = cputime - tmpT;
%     tmpT = cputime;

%     if vars < 1000
%         if mod(vars,500) == 0
%             tmpT = cputime - tmpT;
%             disp(['LARS: ',num2str(vars),' features selected. Time: ',num2str(tmpT)]);
%             tmpT = cputime;
%         end
%     elseif vars < 2000
%         if mod(vars,200) == 0
%             tmpT = cputime - tmpT;
%             disp(['LARS: ',num2str(vars),' features selected. Time: ',num2str(tmpT)]);
%             tmpT = cputime;
%         end
%     elseif vars < 3000
%         if mod(vars,100) == 0
%             tmpT = cputime - tmpT;
%             disp(['LARS: ',num2str(vars),' features selected. Time: ',num2str(tmpT)]);
%             tmpT = cputime;
%         end
%     else
%         if mod(vars,50) == 0
%             tmpT = cputime - tmpT;
%             disp(['LARS: ',num2str(vars),' features selected. Time: ',num2str(tmpT)]);
%             tmpT = cputime;
%         end
%     end
end

if isempty(Cardi)
    % trim beta
    if size(beta,2) > k+1
        beta(:,k+2:end) = [];
    end
end

if k == maxk
    disp('LARS warning: Forced exit. Maximum number of iteration reached.');
end

%% To do
%
% There is a modification that turns least angle regression into stagewise
% (epsilon) regression. This has not been implemented.


function [amppts1,timepts,okflag] = ISM_RIRpow_approx(aa,room,cc,timepts,rt_type,rt_val)
%ISM_RIRpow_approx  Approximation of ISM RIR power (Lehmann & Johansson's method)
%
% [P_VEC,T_VEC,OK_FLAG] = ISM_RIRpow_approx(ALPHA,ROOM,C,T_VEC,RT_TYPE,RT_VAL)
%
% This function returns the predicted values of RIR power in P_VEC (as
% would result from ISM simulations) estimated by means of the EDC
% approximation method described in: "Prediction of energy decay in room
% impulse responses simulated with an image-source model", J. Acoust. Soc.
% Am., vol. 124(1), pp. 269-277, July 2008. The values of P_VEC are
% computed for the time points given as input in T_VEC (in sec), which is
% assumed to contain increasing values of time. The vector T_VEC (and
% corresponding vector P_VEC) will be cropped if the numerical computation
% limits are reached for the higher time values in T_VEC (for which NaNs
% are generated in P_VEC), in which case the output parameter OK_FLAG will
% be set to 0 (1 otherwise).
%
% The environmental setting is defined via the following input parameters:
%
%    ALPHA: 1-by-6 vector, corresponding to each wall's absorption
%           coefficient: [x1 x2 y1 y2 z1 z2]. Index 1 indicates wall closest
%           to the origin. E.g.: [0.5 0.5 0.45 0.87 0.84 0.32].
%  RT_TYPE: character string, measure of reverberation time used for the
%           definition of the coefficients in ALPHA. Set to either 'T60' or
%           'T20'.
%   RT_VAL: scalar, value of the reverberation time (in seconds) defined by
%           RT_TYPE. E.g.: 0.25.
%     ROOM: 1-by-3 vector, indicating the rectangular room dimensions
%           (in m): [x_length y_length z_length]. E.g.: [4 4 3].
%        C: scalar (in m/s), propagation speed of sound waves. E.g.: 343.

% Release date: November 2009
% Author: Eric A. Lehmann, Perth, Australia (www.eric-lehmann.com)
%
% Copyright (C) 2009 Eric A. Lehmann
%
% 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 3 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/>.

numradpts = length(timepts);
radpts = cc * timepts;              % radius values corresponding to time points

bxx = ( sqrt(1-aa(1))*sqrt(1-aa(2)) )^(1/room(1));
byy = ( sqrt(1-aa(3))*sqrt(1-aa(4)) )^(1/room(2));
bzz = ( sqrt(1-aa(5))*sqrt(1-aa(6)) )^(1/room(3));

if bxx==byy && byy==bzz,
    intcase = 1;
elseif bxx==byy && bxx~=bzz,
    intcase = 2;
elseif byy==bzz && bzz~=bxx,
    if bzz<bxx,     % coordinate swap x<->z
        foo = bxx; bxx = bzz; bzz = foo;
        intcase = 2;
    else
        intcase = 3;
    end
elseif bxx==bzz && bzz~=byy,
    if bzz<byy,     % coordinate swap y<->z
        foo = byy; byy = bzz; bzz = foo;
        intcase = 2;
    else
        intcase = 4;
    end
else
    intcase = 5;
    if bxx>bzz && bxx>byy,      % coordinate swap x<->z
        foo = bxx; bxx = bzz; bzz = foo;
    elseif byy>bzz && byy>bxx,	% coordinate swap y<->z
        foo = byy; byy = bzz; bzz = foo;
    end
end

amppts1 = zeros(1,numradpts);
for ss=1:numradpts,    % compute amplitude/energy estimates
    Bx = bxx^(radpts(ss)); Bx(Bx==0) = eps;
    By = byy^(radpts(ss)); By(By==0) = eps;
    Bz = bzz^(radpts(ss)); Bz(Bz==0) = eps;
    switch intcase
        case 1
            int2 = Bx;
        case 2
            int2 = (Bx-Bz) / log(Bx/Bz);
        case 3
            n1 = log(Bz/Bx);
            int2 = Bz*( expint(n1) + log(n1) + 0.5772156649 ) / n1;
        case 4
            n1 = log(Bz/By);
            int2 = Bz*( expint(n1) + log(n1) + 0.5772156649 ) / n1;
        otherwise
            n1 = log(Bz/By);
            n2 = log(Bz/Bx);
            int2 = Bz*(log(n1/n2) + expint(n1) - expint(n2)) / log(Bx/By);
    end
    amppts1(ss) = int2/radpts(ss);      % 'propto' really...
end

okflag = 1;
foo = find(isnan(amppts1),1);
if ~isempty(foo),
    amppts1 = amppts1(1:foo-1);
    timepts = timepts(1:foo-1);
    okflag = 0;
end

if nargin==6,
    switch lower(rt_type)  % offset correction
        case 't60', sl = exp(3.05*exp(-1.85*rt_val));
        case 't20', sl = exp(3.52*exp(-7.49*rt_val));
    end
    amppts1 = amppts1 ./ exp(sl*(timepts-timepts(1)));
end


function kernel = create_kernel(kernel_type, pars, nMax, lb, ub, bound_pars)
%% create convolution kernel
%% inputs:
%   kernel_type: string, convolution kernel type. now support {'exp',
%       'exp2', 'vector'}
%   pars: parameters for the selected kernel type
%   nMax: length of the kernel
%   lb:     lower bound for each parameter
%   ub:     upper bound for each parameter
%   bound_pars: logical variable, bound the parameters or not {1, 0}
%% outputs
%   kernel: struct variable

%% Author: Pengcheng Zhou, Carnegie Mellon University, 2016

%% kernel size
if ~exist('nMax', 'var') || isempty(nMax)
    nMax = 50;
end
kernel.nMax = nMax;

%% initialize kernel
if ~exist('kernel_type', 'var') || isempty(kernel_type)
    kernel_type = 'exp2';
end
if strcmpi(kernel_type, 'exp')
    % single exponential function: ~ exp(-t/tau)
    kernel.type = 'exp';
    % parameter
    if ~exist('pars', 'var') || isempty(pars)
        kernel.pars = [5, .1];
    else
        kernel = kernel.pars;
    end
    % function handle
    kernel.fhandle = @(pars, t) exp(-t/pars) * (1-exp(-1/pars)) ...
        / (1-exp(-nMax/pars));
elseif strcmpi(kernel_type, 'vector')
    % single vector
    kernel.type = 'vector';
    % parameter
    if ~exist('pars', 'var') || isempty(pars)
        kernel.pars = exp(-(1:nMax)/10);
    else
        kernel.pars = pars;
    end
    % function handle
    kernel.fhandle = @(pars, t) pars/sum(pars);
else
    % differencing of two exponential function:
    % ~  exp(-t/tau_d)-exp(-t/tau_r)
    kernel.type = 'exp2';

    % parameters
    if ~exist('pars', 'var') || isempty(pars)
        kernel.pars = [10, 1];
    else
        kernel.pars = pars;
    end
    % function handle
    kernel.fhandle = @(pars, t) (exp(-t/pars(1)) - exp(-t/pars(2)))  ...
        /( (1-exp(-nMax/pars(1)))/(1-exp(-1/pars(1))) ...
        - (1-exp(-nMax/pars(2)))/(1-exp(-1/pars(2))));
end

% lower and upper bounds for parameters
if ~exist('lb', 'var') || isempty(lb)
    kernel.lb = 0.5*kernel.pars;
else
    kernel.lb = lb;
end
if ~exist('ub', 'var') || isempty(ub)
    kernel.ub = 2*kernel.pars;
else
    kernel.ub = ub;
end

% bound the parameters of not
if ~exist('bound_pars', 'var')||isempty(bound_pars)
    kernel.bound_pars = false;
else
    kernel.bound_pars = bound_pars;
end


classdef TirePolynomial < VehicleDynamicsLateral.Tire
    % TirePolynomial Polynomial tire model
    %
    % It inherits methods from Tire.

    methods
        % Constructor
        function self = TirePolynomial()
            self.k1 = 115000;
            self.k2 = 560000;
        end

        function p = PlotTire(self)
            % Returns the handle of the curve
            alpha = (0:0.1:15)*pi/180;
            Fy = - self.Characteristic(alpha);
            p = plot(alpha*180/pi,Fy);
            grid on; box on;
            xlabel('Slip angle [deg]')
            ylabel('Lateral force [N]')
        end

        function Fy = Characteristic(self, alpha, varargin)
            % Lateral force
            Fy = - (self.k1 * alpha - self.k2 * alpha.^3);
        end
    end

    properties
        k1 % 1st polynomial coefficient, cornering stiffness [N/rad]
        k2 % 2nd polynomial coefficient [N/rad^3]
    end
end

%% See Also
%
% <../../index.html Home>
%


function [M,P] = ghkf_predict(M,P,f,Q,f_param,p)
% GHKF_PREDICT - Gauss-Hermite Kalman filter prediction step
%
% Syntax:
%   [M,P] = GHKF_PREDICT(M,P,[f,Q,f_param,p])
%
% In:
%   M - Nx1 mean state estimate of previous step
%   P - NxN state covariance of previous step
%   f - Dynamic model function as a matrix A defining
%       linear function f(x) = A*x, inline function,
%       function handle or name of function in
%       form f(x,param)                   (optional, default eye())
%   Q - Process noise of discrete model   (optional, default zero)
%   f_param - Parameters of f               (optional, default empty)
%   p - Degree of approximation (number of quadrature points)
%
% Out:
%   M - Updated state mean
%   P - Updated state covariance
%
% Description:
%   Perform additive form Gauss-Hermite Kalman Filter prediction step.
%
%   Function f(.) should be such that it can be given a
%   DxN matrix of N sigma Dx1 points and it returns
%   the corresponding predictions for each sigma
%   point.
%
% See also:
%   GHKF_UPDATE, GHRTS_SMOOTH, GH_TRANSFORM

% History:
%   Aug 5,  2010 - Renamed from 'gh_predict' to 'ghkf_predict' (asolin)

% Copyright (C) 2009 Hartikainen, Särkkä, Solin
%
% $Id: gh_predict.m,v 1.2 2009/07/01 06:34:40 ssarkka Exp $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.
%%

  %
  % Check which arguments are there
  %
  if nargin < 2
    error('Too few arguments');
  end
  if nargin < 3
    f = [];
  end
  if nargin < 4
    Q = [];
  end
  if nargin < 5
    f_param = [];
  end
  if nargin < 6
     p = [];
  end

  %
  % Apply defaults
  %
  if isempty(f)
    f = eye(size(M,1));
  end
  if isempty(Q)
    Q = zeros(size(M,1));
  end
  if isempty(p)
    p = 10;
  end

  %
  % Do transform and add process noise
  %
  tr_param = {p};
  [M,P] = gh_transform(M,P,f,f_param,tr_param);
  P = P + Q;