function [rootVals,exitCode]=polyRootsMultiDim(polyCoeffMats,maxDegIncreases,useMotzkinNull)
%%POLYROOTSMULTIDIM Find the roots of a system of simultaneous multivariate
%                   polynomials. Only the affine roots are found (which are
%                   generally the only ones desired), not the roots at
%                   infinity. The algorithm used transforms the
%                   multivariate root finding problem into a generalized
%                   eigenvalue problem involving the nullspace of a
%                   Macaulay matrix. Due to finite precision errors and the
%                   combinational complexity of the algorithm, it is
%                   generally best if the maximum number of variables is
%                   <=3 and the maximum degree of the polynomials is <=3.
%                   Sparser polynomials are much easier than dense ones
%                   (where all possible monomial coefficients are nonzero).
%                   When dealing with dense polynomials, it is generally
%                   best if the maximum number of variables is 2, where the
%                   maximum degree can get as high as sixth order without
%                   notable problems.
%
%INPUTS: polyCoeffMats An nX1 or 1Xn cell array containing n hypermatrices
%               of polynomial coefficients for a multivariate polynomial. A
%               hypermatrix of the coefficients for a multivariate
%               polynomial contains elements arranged such that
%               coeffs(a1,a2,a3...an) corresponds to the coefficient of an
%               x1^(a1-1)*x2^(a2-1)*x3^(a3-1)...xn^(an-1) term.  Thus, the
%               number of indices coeffs takes is equal to the
%               dimensionality of x (not counting singleton dimensions at
%               the end of coeffs). Note that this ordering is the reverse
%               that used in the 1D polyval function that is built into
%               Matlab. The number of elements for each index in coeffs is
%               the maximum order of that dimension +1.
% maxDegIncreases An optional parameter specifying the maximum number
%               of degree increases of the Motzkin matrix to use when
%               trying to solve the multivariate polynomial system. If
%               maxDegIncreases is too small, then the polynomial root
%               solver will fail. This parameter is provided as an option
%               to help identify when solving large systems would be very
%               slow. The default if this parameter is omitted or an empty
%               matrix is passed is 10*n. If one wishes there to be no
%               limit, then one can set maxDegIncreases to Inf and degree
%               increases will be allowed until Matlab runs out of memory.
% useMotzkinNull A key step in the algorithm is the determination of
%               nullspaces. By defauly, the nullspace function is used.
%               However, if desired, the Motzkin algorithm described in [1]
%               can be used. The Motzkin algorithm is generally not as
%               numerically stable, but it can be useful if one wishes to
%               step through the code and compare the results to the values
%               in [1].
%
%OUTPUTS: rootVals An nXnumSol matrix of the numSol zeros of the polynomial
%                  system found, or an empty matrix if exitCode does not
%                  equal zero.
%         exitCode A parameter indicating how the algorithm terminated.
%                  Possible values:
%                  0 The algorithm was a success; the roots were found.
%                  1 The maximum number of degree increases elapsed.
%                  2 A finite precision error occured either causing the
%                    nullity of the Macaulay matrix to change after
%                    stabilization, or to decrease when the degree was
%                    increased.
%
%This function implements Algorithm 3 of [1], an affine null space based
%root finding method for multivariate poylnomials. The polynomials in
%polyCoeffMats are converted into term matrices. The degree negative
%lexicographic ordering in the paper, for a fixed degree, coincides with
%the ordering of the terms provided by unrankTComposition with
%firstElMostSig=true. Thus, ranking and unranking of compositions is used
%to keep track of the monomials in the columns of the Macaulay matrix.
%
%The algorithm requires that a "shift function" be chosen (g(x) in [1]).
%The shift function was arbitrarily chosen to be the sum_{i=1}^n i*xi,
%where xi is the ith unknown variable. That is, the sum of all first degree
%terms multiplied by their position.
%
%The use of the Motzkin algorithm for finding the nullspace computation
%(Sections 3.2.1, 3.2.2 and 6.2.5 of [1]) allows one to avoid computation
%of degree-augmented Macaulay matrices, and provides a basis for the
%nullspace havign the nice sparse structure as used in [1]. However,
%Section 6.2.3 demonstrated that the choice of basis for the nullspace does
%not change the final solution. Thus, the option to use the (possibly more
%stable) null command (which uses singular value decomposition) is offered,
%though it does require keeping track of the entire Macaulay matrix.
%
%Though the Macaulay matrix has a sparse quasi-Toeplitz structure, the
%sparsity was not taken advantage of in the implementation of this
%algorithm.
%
%Roots at infinity are not "true" solutions to the problem; they only arise
%when the system of polynomials has been homogenized (all monomial terms in
%each equation made to have the same total degree) through the introduction
%of an additional variable. Section 2.3 of [1] discusses what they are.
%This algorithm does not find them.
%
%EXAMPLE 1:
%An example is the third-order polynomial system on page 94 of [1]. The
%equations are
%0=x1^2+5*x1*x2+4*x2*x3-10
%0=x2^3+3*x1^2*x2-12
%0=x3^3+4*x1*x2*x3-8
%Thus, the solution is
% p=zeros(4,4,4);%First Equation
% p(2+1,0+1,0+1)=1;
% p(1+1,1+1,0+1)=5;
% p(0+1,1+1,1+1)=4;
% p(0+1,0+1,0+1)=-10;
%
% q=zeros(4,4,4);%Second Equation
% q(0+1,3+1,0+1)=1;
% q(2+1,1+1,0+1)=3;
% q(0+1,0+1,0+1)=-12;
%
% k=zeros(4,4,4);%Third Equation
% k(0+1,0+1,3+1)=1;
% k(1+1,1+1,1+1)=4;
% k(0+1,0+1,0+1)=-8;
%
% polyCoeffMats=cell(3,1);
% polyCoeffMats{1}=p;
% polyCoeffMats{2}=q;
% polyCoeffMats{3}=k;
% rootVals=polyRootsMultiDim(polyCoeffMats)
%Where 18 solutions are found. Two are real; the rest complex.
%
%EXAMPLE 2:
%This is the system of equations used as an example in Section 2.1. of [1].
%0=-x1^2+2*x1*x2+x2^2+5*x1-3*x2-4
%0=x1^2+2*x1*x2+x2^2-1
%Thus, the solution is
% p=zeros(2+1,2+1);
% p(0+1,0+1)=-4;
% p(2+1,0+1)=-1;
% p(1+1,1+1)=2;
% p(0+1,2+1)=1;
% p(1+1,0+1)=5;
% p(0+1,1+1)=-3;
%
% q=zeros(3+1,3+1);
% q(0+1,0+1)=-1;
% q(2+1,0+1)=1;
% q(1+1,1+1)=2;
% q(0+1,2+1)=1;
% polyCoeffMats=cell(2,1);
% polyCoeffMats{1}=p;
% polyCoeffMats{2}=q;
% rootVals=polyRootsMultiDim(polyCoeffMats)
%In this instance, all four roots of the system are real. The solutions are
%(4,-5), (1,0), (3,-2), (0,-1).
%
%REFERENCES:
%[1] P. Dreesen, "Back to the roots: Polynomial system solving using linear
%    algebra," Ph.D. dissertation, Katholieke Universiteit Leuven, Leuven,
%    Flanders, Belgium, Sep. 2013.
%
%January 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The number polynomials and the number of variables.
n=length(polyCoeffMats);

if(nargin<2||isempty(maxDegIncreases))
    maxDegIncreases=10*n;
end

if(nargin<3||isempty(useMotzkinNull))
    useMotzkinNull=false;
end

%Determine the number of variables. Not all polynomials have to have all of
%the same variables, so
termMats=cell(n,1);

%Holds the degrees of each polynomial.
d=zeros(n,1);

%Convert the polynomial hypermatrices to term matrices. The order of the
%monomials in the term matrix does not matter here (though the ordering of
%the monomials when added to the Motzkin matrix is important).
for curPoly=1:n
    termMat=multiDimPolyMat2Terms(polyCoeffMats{curPoly},2,n);

    %Normalize the equation so that the highest coefficient has magnitude
    %one. This helps reduce finite precision problems.
    maxVal=max(abs(termMat(1,:)));
    termMat(1,:)=termMat(1,:)/maxVal;

    termMats{curPoly}=termMat;

    %The total degree of the current polynomial.
    d(curPoly)=max(sum(termMat(2:end,:),1));
end

%The maximum total degree of all of the polynomials.
d0=max(d);

%This keeps track of the number of columns of the Macaulay matrix  before
%the first column containing a certain degree monomial term (the
%number of monomial terms before the first monomial term of the given
%degree. The array numBeforeDeg is extended each time another degree block
%is added to the Macaulay matrix. numBeforeDeg(i+1) is the number of items
%before degree i, starting with degree 0, where numBeforeDeg(0+1)=0.
numBeforeDeg=getNumElsBeforeDeg(n,d0);

%Construct the initial Macaulay matrix of degree d0, which is the highest
%total degree of all of the polynomials going in.
[N,numBeforeDeg]=buildInitialMacaulayMat(termMats,d,d0,numBeforeDeg);
p=size(N,1);
q=size(N,2);

%Compute the inital basis for the nullspace
if(useMotzkinNull)
    Z=MotzkinMatrix(N);
else
    Z=nullspace(N);
end
N=[];%N is no longer needed.

%We keep track of the nullity to determine when the size of the nullspace
%has stabilized. Only after it has stabilized do we consider whether we can
%identify where any solutions at infinity are (so as to eliminate them).
nullity=size(Z,2);

dG=[];
dCur=d0;

nullityStabilized=false;
for curDegIncrease=1:maxDegIncreases
    %Get the new rows that would have to be augmented.
    [NRows,numBeforeDeg]=newRows4MacaulayMatrix(p,numBeforeDeg,termMats,d,dCur);
    qNew=size(NRows,2);

    %Compute the augmented nullspace for the Macaulay matrix using
    %Motzkin's algorithm (Algorithm 4). This used the block method of
    %Section 6.2.5 to expand the nullspace.
    N1=NRows(:,1:q);
    N2=NRows(:,(q+1):qNew);
    if(useMotzkinNull)
        XY=MotzkinMatrix([N1*Z,N2]);
    else
        XY=nullspace([N1*Z,N2]);
    end
    numZPrev=size(Z,2);
    X=XY(1:numZPrev,:);
    Y=XY((numZPrev+1):end,:);
    Z=[Z*X;Y];

    nullityNew=size(Z,2);

    dCur=dCur+1;
    p=size(N,1);
    q=qNew;

    %If we have found the degree for which the nullity no longer increases.
    if(nullityNew==nullity)
        nullityStabilized=true;

        %Check whether we are at dG -the degree where we can detect the
        %affine basis set (and thus know that there are ma solutions).
        [isAtdG,ma,degOfGap]=checkFordG(Z,numBeforeDeg,dCur);

        if(isAtdG)
            dG=dCur;
            break;
        end
    elseif(nullityStabilized==true||nullityNew<nullity)
        %The nullity has stabilized and then become unstabilized or the
        %nullity decreased. Neither should happen and both are indicative
        %of finite precision problems.
        rootVals=[];
        exitCode=2;
        return;
    else
        nullity=nullityNew;
    end
end

if(isempty(dG))
    rootVals=[];
    exitCode=1;%The maximum number of iterations elapsed.
    return;
end

%S1 will select all monomials up to one less than the degree of the gap. We
%will use all monomials in the nullspace up to the degree of the gap.
k=numBeforeDeg(degOfGap+2);
S1=eye(numBeforeDeg(degOfGap+1),k);

%Perform column compression on Z to get W11 using Theorem 6.9
[~,~,Q]=svd(Z(1:k,:));
W=Z*Q;
W11=W(1:k,1:ma);

Sg=constructSg(n,degOfGap,numBeforeDeg);

%The generalized eigenvalue problem to solve is S1*W11*V11*D=Sg*W11*V11
%However, eig cannot solve that as it is a rectangular problem. We use the
%method of Section 6.2.2 to transform it into a square eigenvalue problem.
[V11,~]=eig(lsqminnorm(S1*W11,Sg*W11));

%Using Corollary 6.11 to extract the actual solutions.
Ka1=W11*V11;
Ka1=bsxfun(@rdivide,Ka1,Ka1(1,:));
rootVals=Ka1(2:(n+1),:);

exitCode=0;%Successful return

end

function Sg=constructSg(n,maxDeg,numBeforeDeg)
%This function constructs Sg where g is the shift function, chosen to be
%the sum of all first order monomial terms weighted by their index.
%That is x1+2*x2+3*x3+...+n*xn
%These values are defined through Proposition 6.3. Our chose of sg was
%arbitrary.

termMats{1}=[(1:n);eye(n)];

Sg=buildInitialMacaulayMat(termMats,1,maxDeg,numBeforeDeg);
end

function [dGHasBeenReached,ma,degOfGap]=checkFordG(Z,numBeforeDeg,maxDeg)
%This implements Corollary 6.12 to determine whether we have reached a
%point where we can identify the affine basis set.

    curRank=1;%The rank of the degree-0 block.

    for curDeg=1:maxDeg
        selRows=(1:numBeforeDeg(curDeg+2));
        newRank=rank(Z(selRows,:));
        if(newRank==curRank)
            dGHasBeenReached=true;
            ma=curRank;%The number of affine bases.
            degOfGap=curDeg;
            return;
        end
        curRank=newRank;
    end
    dGHasBeenReached=false;%No gap detected.
    ma=[];
    degOfGap=[];
end

function W=MotzkinRow(b,epsVal)
%Chapter 3.2.1. A subroutine in the Motzkin method for finding the null
%space of a matrix. This implements the row-subroutine used by the
%MotzkinMatrix function.

n=length(b);
W=zeros(n,n-1);

%Explicitly zero values that are too small in magnitude.
b(abs(b)<epsVal)=0;

%Step 1: Find the rightmost pivot. This is the rightmost nonzero value.
ip=find(b,1,'last');

%If there is no nonzero element.
if(isempty(ip))
    W=eye(n,n);
    return;
end

%Step 2: Rescale b
b=b/b(ip);

%Step 3
e=zeros(n,1);
for i=(ip+1):n
    e(i)=1;
    W(:,i-1)=e;
    e(i)=0;
end

%Step 4
for i=(ip-1):-1:1
    W(i,i)=1;
    W(ip,i)=-b(i);
end
end

function H=MotzkinMatrix(A)
%Chapter 3.2.2 to find the Motzkin (canonical) nullspace of a matrix.
%An example with A=[0,2,1,0,2,0;1,0,3,2,0,1;4,0,3,2,1,0] in the paper.

m=size(A,1);

%A value for declaring elements of b zero for purposes of the Motzkin row
%algorithm.
epsVal=max(size(A))*eps(norm(A));

b=A(1,:);

H=MotzkinRow(b,epsVal);
for i=2:m
   b=A(i,:)*H;
   H=H*MotzkinRow(b,epsVal);
end
end

function [MRows,numBeforeDeg]=newRows4MacaulayMatrix(p,numBeforeDeg,termMats,d,dCur)
%To enlarge the Macaulay matrix to one degree higher, new rows must be
%appended. This computes those rows. The Macaulay matrix is defined in
%Section 5.1.2. This function only computes the rows of a particular new
%degree. When increasing the degree of the Macaulay matrix, it is not
%necessary to respect the same row ordering as in a Macaulay matrix built
%to a particular degree from scratch.

s=length(termMats);%The number of polynomials
n=size(termMats{1},1)-1;%The number of variables

%The new maximum degree if these rows are appended.
d0=dCur+1;

%Update offsets with the current degree.
numCompositionsInDeg=binomial(dCur+1+n-1,n-1);
numBeforeDeg(end+1)=numBeforeDeg(end)+numCompositionsInDeg;

[pNew,qNew]=MacaulayMatrixSize(d0,d);

%Allocate space for the new rows.
MRows=zeros(pNew-p,qNew);

curRow=1;
for i=1:s%Going through all the polynomials
    termMat=termMats{i};%The current polynomial being manipulated.
    numTerms=size(termMat,2);

    degree=d0-d(i);

    %For each value n, all possible monomials are given by all
    %compositions of the degree into n parts (as n is the number of
    %variables). Of course, these compositions are defined such that
    %the minimum value in each element is 1 and we want it to be zero,
    %so we have to adjust for that.

    numMonomials=binomial(degree+n-1,n-1);
    for j=0:(numMonomials-1)
        curMonomial=unrankTComposition(j,n,degree+n,true)-1;

        %Add the polynomial multiplied by the current monomial.
        for curTerm=1:numTerms
            monomial2Add=curMonomial+termMat(2:end,curTerm);

            %The degree of the term to add.
            deg=sum(monomial2Add);
            offset=numBeforeDeg(deg+1);

            idx=rankTComposition(monomial2Add+1,true)+offset+1;
            MRows(curRow,idx)=termMat(1,curTerm);
        end
        curRow=curRow+1;
    end
end
end

function numBeforeDeg=getNumElsBeforeDeg(n,dMax)
%A composition of n into d parts is the number of ways of splitting the
%integer n into the sum of d nonzero values. Compositions can also be
%offset to split n into d values, which are >=0. The lexicographic rank of
%a composition (starting from 0) is the offset within graded lexicographic
%ordering of a value with a particular fixed degree. We are going to have
%to keep track of these offsets to be able to index vectors.

numBeforeDeg=zeros(dMax+1,1);
numBeforeDeg(0+1)=0;
numBeforeDeg(1+1)=1;
for degree=1:dMax
    %The number of compositions of degree into n parts, where the parts can
    %be zero. We are including d0 as the function checkFordG will need it
    %even though this function only needs it up to degree d0-1.
    numCompositionsInDeg=binomial(degree+n-1,n-1);

    numBeforeDeg(degree+2)=numBeforeDeg(degree+1)+numCompositionsInDeg;
end

end

function [M,numBeforeDeg]=buildInitialMacaulayMat(termMats,d,d0,numBeforeDeg)
%%BUILDINITIASLMACAULAYMAT Construct the minimum-sized Macaulay matrix for
%             the system given. The degree of the matrix is the maximum
%             total degree of all of the polynomials. The Macaulay matrix
%             is defined in Section 5.1.2. This function is also used as a
%             convenience function to construct Sg.

s=length(termMats);%The number of polynomials
n=size(termMats{1},1)-1;%The number of variables

%Initially a Macaulay matrix of degree d0 is constructed. The number of
%rows p and columns q of the matrix are given
[p,q]=MacaulayMatrixSize(d0,d);

%Allocate space for the d0th order Macaulay matrix.
M=zeros(p,q);

%Now, fill in the Macaulay matrix of order d0. Here, each polynomial is
%multiplied by all monomials of degree <=d0-d(i) and added. This includes
%the zeroth-degree, which contains
curRow=1;
for i=1:s%Going through all the polynomials
    termMat=termMats{i};%The current polynomial being manipulated.

    %First, add the zeroth-order term (the polynomial itself.
    numTerms=size(termMat,2);
    for curTerm=1:numTerms
        %The degree of the term to add.
        deg=sum(termMat(2:end,curTerm));
        offset=numBeforeDeg(deg+1);

        idx=rankTComposition(termMat(2:end,curTerm)+1,true)+offset+1;
        M(curRow,idx)=termMat(1,curTerm);
    end
    curRow=curRow+1;

    %Now, add all other terms associated with this polynomial.
    for degree=1:(d0-d(i))%All the orders for the current polynomial.
        %For each order, all possible monomials are given by all
        %compositions of degree into n parts (as n is the number of
        %variables). Of course, these compositions are defined such that
        %the minimum value in each element is 1 and we want it to be zero,
        %so we have to adjust for that

        numMonomials=binomial(degree+n-1,n-1);
        for j=0:(numMonomials-1)
            curMonomial=unrankTComposition(j,n,degree+n,true)-1;

            %Add the polynomial multiplied by the current monomial.
            for curTerm=1:numTerms
                monomial2Add=curMonomial+termMat(2:end,curTerm);

                %The degree of the term to add.
                deg=sum(monomial2Add);
                offset=numBeforeDeg(deg+1);

                idx=rankTComposition(monomial2Add+1,true)+offset+1;
                M(curRow,idx)=termMat(1,curTerm);
            end
            curRow=curRow+1;
        end
    end
end
end

function [p,q]=MacaulayMatrixSize(d0,d)
%%%MACAULAYMATRIXSIZE The size of a d0th order Macaulay matrix when the
%                   polynomials have degree d(i). This implements Lemma 5.8
%                   in Section 5.2.1 of [1].

%The number of polynomials.
n=length(d);

q=binomial(n+d0,d0);

p=0;
for i=1:n
   p=p+binomial(n+d0-d(i),d0-d(i));
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [u,c]=getEllipsHarmAxes(pointHarmon,E)
%%GETELLIPSHARMAXES Find the Cartesian unit vectors corresponding to the
%                   normalized  gradients of the components of a point in
%                   ellipsoidal harmonic coordinates. Also, get the
%                   magnitudes of the gradients.
%
%INPUTS: pointHarmon A point given in terms of ellipsoidal harmonic reduced
%                    latitude and longitude in radians and a semi-major
%                    axis in meters.
%                  E The linear eccentricity defining the ellipsoidal
%                    harmonic coordinate system. If this parameter is
%                    omitted, then the linear eccentricity of the WGS84
%                    reference ellipsoid is used.
%
%OUTPUTS: u u(:,1), u(:,2) and u(:,3) are the unit vectors in the
%           respective directions of the respective gradients of reduced
%           latitude, longitude and the semi-major axis.
%         c c(1), c(2) and c(3) are the respective magnitudes of the
%           derivative of the Cartesian position with respect to reduced
%           latitude, longitude and the semi-major axis.
%
%The ellipsoidal harmonic coordinate system is described in Chapter 1.15
%of [1] and is an orthogonal coordinate system.
%
%REFERENCES:
%[1] B. Hofmann-Wellenhof and H. Moritz, Physical Geodesy, 2nd ed.
%    SpringerWienNewYork, 2006.
%
%January 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(E))
    a=Constants.WGS84SemiMajorAxis;
    f=Constants.WGS84Flattening;
    b=a*(1 - f);
    E=sqrt(a^2-b^2);
end

    beta=pointHarmon(1);
    lambda=pointHarmon(2);
    u=pointHarmon(3);

dBeta=[-sqrt(E^2+u^2)*sin(beta)*cos(lambda);
       -sqrt(E^2+u^2)*sin(beta)*sin(lambda);
        u*cos(beta)];
c1=norm(dBeta);
dBeta=dBeta/c1;

du=[u*cos(beta)*cos(lambda)/sqrt(E^2+u^2);
    u*cos(beta)*sin(lambda)/sqrt(E^2+u^2);
    sin(beta)];
c3=norm(du);
du=du/c3;

dLambda=[-sqrt(E^2+u^2)*cos(beta)*sin(lambda);
          sqrt(E^2+u^2)*cos(beta)*cos(lambda);
          0];
c2=norm(dLambda);

%If the point is too close to the poles, then it is possible that c2 is
%nearly equal to zero. However, a normalized dLambda can just be found by
%orthogonality: it is orthogonal to dBeta and du.
dLambda=cross(dBeta,du);

u=[dBeta,dLambda,du];
c=[c1;c2;c3];
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function R = functionRlocalscattering3D(M_H, M_V, d_H, d_V, varphi, stdphi, theta, stdtheta, dist)
%Generate the spatial correlation matrix for the 3D local scattering model,
%defined in (7.17) for different angular distributions.
%
%INPUT:
% M_H           = Number of antennas per horizontal row, i.e., # of columns
% M_V           = Number of horizontal rows
% d_H           = Horizontal antenna spacing in multiples of wavelength
% d_V           = Vertical antenna spacing in multiples of wavelength
% varphi        = Azimuth angle in radians
% stdphi        = Azimuth angular spread (standard deviation or limit)
% theta         = Elevation angle in radians
% stdtheta      = Elevation angular spread (standard deviation or limit)
% dist          = 'Gaussian' or 'Laplace' or 'Uniform'
%
%OUTPUT:
% R             = M x M channel covariance matrix, where M = M_H x M_V
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

% Index mapping functions
i = @(m) mod(m-1, M_H);
j = @(m) floor((m-1)/M_H);

%Create the correlation matrix
M = M_H*M_V;
R = zeros(M);

% Define angle distributions as functions
if (strcmp(dist,'Laplace'))
    f1 = @(x) exp(-sqrt(2)*abs(x-varphi)/stdphi)/(sqrt(2)*stdphi);
    f2 = @(x) exp(-sqrt(2)*abs(x-theta)/stdtheta)/(sqrt(2)*stdtheta);
elseif (strcmp(dist,'Gaussian'))
    f1 = @(x) exp(-(x-varphi).^2/(2*stdphi^2))/(sqrt(2*pi)*stdphi);
    f2 = @(x) exp(-(x-theta).^2/(2*stdtheta^2))/(sqrt(2*pi)*stdtheta);
elseif (strcmp(dist,'Uniform'))
    f1 =  @(x)1/(2*stdphi);
    f2 =  @(x) 1/(2*stdtheta);
else
    error('Please provide a valid angle distribution')
end

%% Compute correlation matrix
LOOKUP = zeros(2*M_H-1,M_V); %Define a lookup table to speedup the computation process
for m = 1:M
    for l = 1:m
        indi = i(l)-i(m) + M_H;
        indj = j(l)-j(m) + M_V;
        if (LOOKUP(indi,indj)==0)
            integrand = @(p,t) exp(1j*2*pi*d_V*(j(l)-j(m)).*sin(t)) .* exp(1j*2*pi*d_H*(i(l)-i(m)).*cos(t).*sin(p)) .* f1(p) .* f2(t);
            if (strcmp(dist,'Uniform'))
                LOOKUP(indi,indj) = integral2(integrand, varphi-stdphi, varphi+stdphi, theta-stdtheta, theta+stdtheta);
            else
                LOOKUP(indi,indj) = integral2(integrand, varphi-20*stdphi, varphi+20*stdphi, theta-20*stdtheta, theta+20*stdtheta);
            end
        end
        R(m,l) = LOOKUP(indi,indj);
        R(l,m) = R(m,l)';
    end
end


% Chapter 10 - Limit Cycles.
% Programs_10b - Phase Portrait (Fig. 10.2).
% Copyright Birkhauser 2013. Stephen Lynch.

% Limit cycle of a van der Pol system.
% IMPORTANT - Programs_10a.m is vectorfield.m.
clear
hold on
% sys=inline('[x(2);-x(1)-5*x(2)*((x(1))^2-1)]','t', 'x');
sys = @(t,x) [x(2);-x(1)-5*x(2)*((x(1))^2-1)];
vectorfield(sys,-3:.3:3,-10:1.3:10);
[t,xs] = ode45(sys,[0 30],[2 1]);
plot(xs(:,1),xs(:,2))
hold off
axis([-3 3 -10 10])
fsize=15;
set(gca,'XTick',-3:1:3,'FontSize',fsize)
set(gca,'YTick',-10:5:10,'FontSize',fsize)
xlabel('x(t)','FontSize',fsize)
ylabel('y(t)','FontSize',fsize)
hold off

% End of Programs_10b.


function sparse_test01 ( )

%*****************************************************************************80
%
%% SPARSE_TEST01 demonstrates a simple use of the SPARSE matrix facility.
%
%  Discussion:
%
%    This unrealistic example sets up a tiny matrix:
%
%    11  12   0   0  15
%     0  22  23   0   0
%    31   0  33  34  35
%
%    Note that we assume here that the entire matrix is available
%    at once, so we only need a single call to SPARSE to define the matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 August 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    George Lindfield and John Penny,
%    Numerical Methods Using MATLAB,
%    Prentice Hall, 1999
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPARSE_TEST01:\n' );
  fprintf ( 1, '  Demonstrate the use of MATLAB''s SPARSE facility\n' );
  fprintf ( 1, '  to define a tiny sparse matrix.\n' );

  i = [  1,  1,  1,  2,  2,  3,  3,  3,  3 ];
  j = [  1,  2,  5,  2,  3,  1,  3,  4,  5 ];
  s = [ 11, 12, 15, 22, 23, 31, 33, 34, 35 ];
  m = 3;
  n = 5;
  nzmax = 9;

  a = sparse ( i, j, s, m, n, nzmax );

  a

  return
end


function timer_etime_test02 ( )

%*****************************************************************************80
%
%% TEST02 times the vectorized EXP routine.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 December 2005
%
%  Author:
%
%    John Burkardt
%
  n_log_min = 12;
  n_log_max = 22;
  n_min = 2^n_log_min;
  n_max = 2^n_log_max;
  n_rep = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02\n' );
  fprintf ( 1, '  Time vectorized operations:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    y(1:n) =        x(1:n)  \n' );
  fprintf ( 1, '    y(1:n) = PI *   x(1:n)  \n' );
  fprintf ( 1, '    y(1:n) = sqrt ( x(1:n) )\n' );
  fprintf ( 1, '    y(1:n) = exp  ( x(1:n) )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Data vectors will be of minimum size %d\n', n_min );
  fprintf ( 1, '  Data vectors will be of maximum size %d\n', n_max );
  fprintf ( 1, '  Number of repetitions of the operation: %d\n', n_rep );

  for func = 1 : 4

    for i_rep = 1 : n_rep

      for n_log = n_log_min : n_log_max

        n = 2^n_log;

        x = rand(n,1);

        time1 = clock;

        if ( func == 1 )
          y(1:n) = x(1:n);
        elseif ( func == 2 )
          y(1:n) = pi * x(1:n);
        elseif ( func == 3 )
          y(1:n) = sqrt ( x(1:n) );
        elseif ( func == 4 )
          y(1:n) = exp ( x(1:n) );
        end

        time2 = clock;

        delta(n_log,i_rep) = etime ( time2, time1 );

      end

    end

    fprintf ( 1, '\n' );
    fprintf ( 1, 'Test02 Results:\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'Vector Size  Rep #1        Rep #2        ' );
    fprintf ( 1, 'Rep #3        Rep #4        Rep #5\n' );
    fprintf ( 1, '\n' );

    for n_log = n_log_min : n_log_max
      n = 2^n_log;
      fprintf ( 1, '%10d', n );
      for j = 1 : n_rep
        fprintf ( 1, '%14f', delta(n_log,1:n_rep) );
      end
      fprintf ( 1, '\n' );
    end

  end

  return
end


function [dim] = pos2dim(pos)

% POS2DIM reconstructs the volumetric dimensions from an ordered list of
% positions.
%
% Use as
%   [dim] = pos2dim(pos)
% where pos is an ordered list of positions.
%
% The output dim is a 3-element vector which correspond to the 3D
% volumetric dimensions
%
% See also POS2TRANSFORM

% Copyright (C) 2009, Jan-Mathijs Schoffelen

if isstruct(pos)
  % the input is a FieldTrip data structure
  pos = pos.pos;
end

% this part depends on the assumption that the list of positions is describing a full 3D volume in
% an ordered way which allows for the extraction of a transformation matrix, i.e. slice by slice
npos = size(pos, 1);
dpos = zscore(abs(diff(pos, [], 1)));

dim        = nan(1, 3);
[tmp, ind] = max(dpos, [], 2);
dim(1)     = find(tmp>1.5, 1, 'first');
dpos       = dpos(dim:dim:npos-1, :);
[tmp, ind] = max(dpos(:, setdiff(1:3, ind(dim(1)))), [], 2);
dim(2)     = find(tmp>1.1*min(tmp), 1, 'first'); % this threshold seems to work on what I tried out
dim(3)     = npos./prod(dim(1:2));


%OUTEROP calculate an outer operation on two vectors
%   function y=outerop(A,B,OPERATOR)
%
%   Calculates resultant matrix when the OPERATOR is applied
%   to all combinations of the elements of vector A and the
%   elements of vector B e.g. the outer product of A and B
%   is outerop(A,B,'*'), the outer sum of A and B
%   is outerop(A,B,'+')
%
%   If OPERATOR is omitted '+' is assumed
%
%   This function is equivalent to the
%   APL language's circle.dot operation. e.g.
%   in APL Ao.*B is the outer product
%   of A and B % and Ao.+B is the outer sum.
%   Ideally it would work on matrices but in practice
%   I've only ever needed to use it with vectors.
%
%   Copyright Murphy O'Brien 2005
%   all rights unreserved
%
function y=outerop(a,b,operator)

if nargin<3
    operator='+';                       % for only two arguments assume outerproduct
end

if isequal(operator,'*')                % common operator
    y=a(:)*b(:)';
else
  outera=a(:)*ones(1,length(b));        % these are the matrices that
  outerb=ones(length(a),1)*b(:).';      % meshgrid(A,B) would generate
  functionHandle=str2func(operator);  % new R14 functionality
  y=functionHandle(outera,outerb);    % allows faster/neater method
end


function [Fitness, extreme] = CalFitness(PopObj)
% Calculate the fitness of each solution
% F(x) = f(x) + d(x) Algorithm 3

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Li Li

    [N,M]  = size(PopObj); %N  No. of solutions, M No. of Objectives

   %% q
    %Zmax = max(PopObj,[],1); % Identify the max point
    Zmin = min(PopObj,[],1); % Identify the ideal point
    % Identify the extreme points
    W = zeros(M) + 1e-6;
    W(logical(eye(M))) = 1;
    ASF = zeros(N,M);
    for i = 1 : M
        ASF(:,i) = max((PopObj-repmat(Zmin,N,1))./repmat(W(i,:),N,1),[],2);
    end
    [~,extreme] = min(ASF,[],1); %'extreme' is the extreme solutions
    % Calculate the intercepts
    Hyperplane = PopObj(extreme,:)\ones(M,1); % linear equation X=A\B???A*X=B???hyperplane is the solution of linear equation PopObj(extreme,:)x Hyperplane = ones(M,1)
    a = (1./Hyperplane)';
    if any(isnan(a))
        a = max(PopObj,[],1);
    end
    % Normalization
    PopObj = (PopObj-repmat(Zmin,N,1))./repmat(a-Zmin,N,1);

    %---------------w_nad
    w_nad = a; % set a as the nadir point

    Zmin = min(PopObj,[],1); % Identify the ideal point
    for i = 1 : M
        ASF_q(:,i) = max((PopObj-repmat(Zmin,N,1))./repmat(w_nad,N,1),[],2);
    end
    [~,key_point] = min(ASF_q,[],1);
    %q
    q = sqrt(sum(PopObj(key_point).^2,2)) * sqrt(M);
    if q <= 1
        fx = sum(PopObj-repmat(Zmin,N,1),2);
    else
        fx = max(PopObj-repmat(Zmin,N,1),[],2);
    end

    %% Calculate the DMD between each two solutions
    Distance = inf(N);
    for i = 1 : N
        for j = [1:i-1,i+1:N]
            denominator = 1 + min(PopObj(i,:),PopObj(j,:));
            numerator = max(PopObj(i,:),PopObj(j,:)) - min(PopObj(i,:),PopObj(j,:));
            dis = numerator./ denominator;
            Distance(i,j) = sum(dis);
        end
    end

    %% Calculate D(i)
    Distance = sort(Distance,2);
    k = floor(sqrt(N)); %k-NN
    d = 1./(Distance(:,k)+2); % dimensionality margin distance

    %% Calculate the fitnesses
    Fitness = fx+d;
end


function out = SY_Trend(y)
% SY_Trend  Quantifies various measures of trend in a time series.
%
%---INPUT:
% y, the input time series.
%
%---OUTPUTS:
% Linearly detrends the time series using detrend, and returns the ratio of
% standard deviations before and after the linear detrending. If a strong linear
% trend is present in the time series, this operation should output a low value.
%
% Also fits a line and gives parameters from that fit, as well as statistics on
% a cumulative sum of the time series.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

if ~BF_iszscored(y)
    warning('The input time series should be z-scored')
end

N = length(y);

% Ratio of std before and after linear detrending:
out.stdRatio = std(detrend(y)) / std(y);

% Linear fit:
[out.gradient,out.intercept] = LinearFit(1:N,y);

% Stats on the cumulative sum:
yC = cumsum(y);
out.meanYC = mean(yC);
out.stdYC = std(yC);
[out.gradientYC,out.interceptYC] = LinearFit(1:N,yC);

% Mean cumsum in first and second half of the time series:
out.meanYC12 = mean(yC(1:floor(N/2)));
out.meanYC22 = mean(yC(floor(N/2)+1:end));

% ------------------------------------------------------------------------------
function [m,b] = LinearFit(xData,yData)
    if size(xData,1) ~= size(yData,1);
        yData = yData';
    end
    coeff = polyfit(xData,yData,1);
    m = coeff(1); b = coeff(2);
end
% ------------------------------------------------------------------------------

end


function M = spherefactory(n, m)
% Returns a manifold struct to optimize over unit-norm vectors or matrices.
%
% function M = spherefactory(n)
% function M = spherefactory(n, m)
%
% Manifold of n-by-m real matrices of unit Frobenius norm.
% By default, m = 1, which corresponds to the unit sphere in R^n. The
% metric is such that the sphere is a Riemannian submanifold of the space
% of nxm matrices with the usual trace inner product, i.e., the usual
% metric.
%
% See also: obliquefactory spherecomplexfactory

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

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

    if m == 1
        M.name = @() sprintf('Sphere S^%d', n-1);
    else
        M.name = @() sprintf('Unit F-norm %dx%d matrices', n, m);
    end

    M.dim = @() n*m-1;

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

    M.norm = @(x, d) norm(d, 'fro');

    M.dist = @(x, y) real(acos(x(:).'*y(:)));

    M.typicaldist = @() pi;

    M.proj = @(x, d) d - x*(x(:).'*d(:));

    M.tangent = M.proj;

    % For Riemannian submanifolds, converting a Euclidean gradient into a
    % Riemannian gradient amounts to an orthogonal projection.
	M.egrad2rgrad = M.proj;

	M.ehess2rhess = @ehess2rhess;
	function rhess = ehess2rhess(x, egrad, ehess, u)
        rhess = M.proj(x, ehess) - (x(:)'*egrad(:))*u;
	end

    M.exp = @exponential;

    M.retr = @retraction;

    M.log = @logarithm;
    function v = logarithm(x1, x2)
        v = M.proj(x1, x2 - x1);
        di = M.dist(x1, x2);
        % If the two points are "far apart", correct the norm.
        if di > 1e-6
            nv = norm(v, 'fro');
            v = v * (di / nv);
        end
    end

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

    M.rand = @() random(n, m);

    M.randvec = @(x) randomvec(n, m, x);

    M.lincomb = @matrixlincomb;

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

    M.transp = @(x1, x2, d) M.proj(x2, d);

    M.pairmean = @pairmean;
    function y = pairmean(x1, x2)
        y = x1+x2;
        y = y / norm(y, 'fro');
    end

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

end

% Exponential on the sphere
function y = exponential(x, d, t)

    if nargin == 2
        t = 1;
    end

    td = t*d;

    nrm_td = norm(td, 'fro');

    if nrm_td > 4.5e-8
        y = x*cos(nrm_td) + td*(sin(nrm_td)/nrm_td);
    else
        % If the step is too small to accurately evaluate sin(x)/x,
        % then sin(x)/x is almost indistinguishable from 1.
        y = x + td;
        y = y / norm(y, 'fro');
    end

end

% Retraction on the sphere
function y = retraction(x, d, t)

    if nargin == 2
        t = 1;
    end

    y = x + t*d;
    y = y / norm(y, 'fro');

end

% Uniform random sampling on the sphere.
function x = random(n, m)

    x = randn(n, m);
    x = x/norm(x, 'fro');

end

% Random normalized tangent vector at x.
function d = randomvec(n, m, x)

    d = randn(n, m);
    d = d - x*(x(:).'*d(:));
    d = d / norm(d, 'fro');

end


%% Quiver calculations
% These are calculations for the quiver dimensions as implemented in MATLAB
% (HG1) as in the |quiver.m| function.
%
% For HG2 and Octave, the situation might be different.
%
% A single quiver is defined as:
%
%                   C
%                    \
%                     \
%  A ----------------- B
%                     /
%                    /
%                   D
%
% To know the dimensions of the arrow head, MATLAB defines the quantities
%  alpha = beta = 0.33 that determine the coordinates of C and D as given below.

clc;
clear variables;
close all;

%% Parameters
try
    syms x y z u v w alpha beta epsilon real
catch
    warning('Symbolic toolbox not found. Interpret the values with care!');
    x = randn(); y = randn(); z = randn();
    u = randn(); v = randn(); w = randn();
end
alpha = 0.33;
beta = alpha;
epsilon = 0;
is2D = true;

%% Coordinates as defined in MATLAB
% Note that in 3D, the arrow head is oriented in a weird way. Let' just ignore
% that and only focus on 2D and use the same in 3D. Due to the lack
% of [u,v,w]-symmetry in those equations, the angle is bound to depend on the
% length of |delta|, i.e. something we don't know beforehand.
A = [x y z].';
delta = [u v w].';
B = A + delta;
C = B - alpha*[u+beta*(v+epsilon);
               v-beta*(u+epsilon)
               w];
D = B - alpha*[u-beta*(v+epsilon);
               v+beta*(u+epsilon)
               w];

if is2D
    A = A(1:2);
    B = B(1:2);
    C = C(1:2);
    D = D(1:2);
    delta = delta(1:2);
end

%% Calculating the angle of the arrowhead
% Calculate the cos(angle) using the inner product
unitVector = @(v) v/norm(v);
cosAngleBetween = @(a,b,c) unitVector(a-b).' * unitVector(c-b);

cosTwiceTheta = cosAngleBetween(C,B,D);
if isa(cosTwiceTheta, 'sym')
    cosTwiceTheta = simplify(cosTwiceTheta);
end

theta = acos(cosTwiceTheta) / 2

radToDeg = @(rads) (rads * 180 / pi);

thetaVal = radToDeg(theta)
try
    thetaVal = double(thetaVal)
end

% For the MATLAB parameters alpha=beta=0.33, we get theta = 18.263 degrees.


function s=phon2sone(p)
%PHON2SONE convert PHON loudness values to SONEs s=(p)
%Inputs:    p is a matrix of phon values
%
%Outputs:   s is a matrix, the same size as p, of sone values
%
% The phon scale measures perceived loudness in dB; at 1 kHz it is identical to dB SPL
% relative to 20e-6 Pa sound pressure. The sone scale is proportional to apparent loudness
% and, by definition, equals 1 at 40 phon. The form of the loudness curve is taken from [1].
% The hearing threshold at 1 kHz for 18 to 25 year olds with normal hearing is taken from [2].
%
% Refs: [1]	J. Lochner and J. Burger. Form of the loudness function in the presence of masking noise.
%           The Journal of the Acoustical Society of America, 33: 1705, 1961.
%       [2]	ISO/TC43. Acoustics  normal equal-loudness-level contours.
%           Standard ISO 226:2003, Aug. 2003.

%      Copyright (C) Mike Brookes 2012-2013
%      Version: $Id: phon2sone.m 3295 2013-08-02 14:03:11Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b d
if isempty(a)
    b=log(10)*0.1*0.27; % 0.27 is the exponent from [1] and [2]
    d=exp(b*2.4); % 2.4 dB is teh hearing threshold from [2]
    a=1/(exp(b*40)-d); % scale factor to make p=40 give s=1
end
if nargout>0
    s=a*(exp(b*p)-d);
else
    if nargin<1 || isempty(p)
        pp=linspace(5,90,100)'; % phon values
    else
        pp=p;
    end
    semilogy(pp,phon2sone(pp));
    axisenlarge(-1);
    yticksi;
    xlabel('phon = dB SPL @ 1 kHz');
    ylabel('sone');
end


function d = Point2LineDistance(P, A, B)

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Annibale Panichella

    d = zeros(size(P,1),1);
    for i = 1:size(P,1)
        pa = P(i,:) - A;
        ba = B - A;
        t = dot(pa, ba)/dot(ba, ba);
        d(i,1) = norm(pa - t * ba,2);
    end
end


function [xVals,tVals,dxdtVals,exitCode,nextStepSize]=RKAdaptiveOverRange(xStart,tSpan,f,initStepSize,numRefinedSteps,order,solutionChoice,RelTol,AbsTol,maxSteps)
%%RKADAPTIVEOVERRANGE Perform explicit Runge-Kutta integration over a
%                     given range of values using an adaptive step size.
%                     Runge-Kutta methods are derivative-free techniques
%                     for solving ordinary differential equations. That is,
%                     integrating dx/dt=f(x,t) given initial conditions
%                     (xStart,tSpan(1)). More information on available
%                     algorithms for the steps is given in the comments to
%                     the function RungeKStep.
%
%INPUTS: xStart The NX1 state vector at time tSpan(1).
%         tSpan A 2X1 or 1X2 vector where tSpan(1) is the starting time and
%               tSpan2 is the desired stopping time for the integration.
%             f The function handle for f(x,t)=dxdt over which integration
%               is to be performed. The output is NX1-dimensional.
%  initStepSize An optional initial step size (in t) to use for the
%               integration. If omitted or an empty matrix is passed, an
%               ad-hoc method described below is used to find an initial
%               step size.
% numRefinedSteps An optional parameter specifying the number of steps to
%               interpolate between each normal step. With high-order
%               formulas, the steps produced might not be very nice to
%               use for plotting, so setting this to a value above 0 adds
%               more intermediate values (using the polynomial returned by
%               the RKInterpPolys function) so that plotted data can look
%               nicer. If omitted or an empty matrix is passed, the default
%               value of 0 (no extra/ interpolated steps) is used.
%               Interpolation is done at the main order of the Runge-Kutta
%               method.
% order,solutionChoice  A pair of optional parameters that specify the
%               highest order of the embedded Runge-Kutta pair to use as
%               well as the specific algorithm to use. Details are given in
%               the comments to the RungeKStep function. If omitted or
%               empty matrices are passed, the default order of 5 is used
%               and the default solutionChoice of 0 is used.
%        RelTol The maximum relative error tolerance allowed, a
%               positive scalar (its use is explained in more detail
%               below). If omitted or an empty matrix is passed, the
%               default value of 1e-3 is used.
%        AbsTol The absolute error tolerance allowed, a positive scalar, of
%               the same for all components of x, or a positive NX1 vector.
%               If omitted or an empty matrix is passed, the default value
%               of 1e-6 is used.
%      maxSteps The maximum allowable number of steps to perform the
%               integration. If omitted, the default of 1024 is used.
%
%OUTPUTS: xVals The NXnumSteps set of values of x along the path.
%               xVals(:,1) is xStart and xVals(:,end) is the value at final
%               time tSpan(2). If the Runge-Kutta integration failed, e.g.
%               due to encountering a NaN or being unable to get a
%               sufficient step size, an empty matrix is returned.
%         tVals A numStepsX1 vector of values of t corresponding to the
%               values of x in xVals. tVals(1) is equal to tSpan(1) and
%               tVals(end) is equal to tSpan(2). If integration fails, an
%               empty matrix is returned.
%      dxdtVals A numStepsXN array containing derivatives of x evaluated at
%               the times in tVals. The derivatives are just the result of
%               evaluating f(x,t) at each point in (xVals,tVals). This
%               combined with xVals and tVals could be used in functions to
%               perform Hermite interpolation. If integration fails, an
%               empty matrix is returned.
%      exitCode A code indicating how the algorithm terminated. Possible
%               values are
%               0: Integration was successful.
%               1: Unable to get a small enough step size.
%               2: Maximum number of steps reached without completion.
%               3: Non-finite number encountered.
%  nextStepSize The step size (in t) that would be used for the next
%               integration step. If empty, then the integration ended
%               because the step size was too small.
%
%The basic idea behind adaptive stepsize control can be ascertained from
%Chapter 5.2 of [3]. However, the implementation here has a number of
%changes. For example, unlike the basic algorithm of [3], depending on the
%user's choice of order and solutionChoice, the main Runge-Kutta formula,
%which is the one whose result is saved, might have a higher or lower order
%than the subsidiary formula, which is only used to check the error to
%determine how the stepsize should change. The smallest order of the two is
%the order that is used in formulae for adaptively changing the step size.
%
%The algorithm needs an initial stepsize to start. If the user does not
%provide an intial stepsize, then the initial stepsize is just set to the
%width of the integration region in t divided by the maximum number of
%steps. That is, initStepSize = (tSpan(2)-tSpan(1))/maxSteps;
%
%Once an initial stepsize has been determined, the function RungeKStep is
%used to perform a single step of the Runge-Kutta algorithm. Then one must
%determine whether the stepsize was small enough. If the stepsize is small
%enough, then the stepsize should still be adaptively changed for the nest
%step. The procedure for updating the stepsize is essentially the procedure
%for non-stiff equations from [2], Equation 2.3). The method has been
%slightly modified in that the error being adjusted is not just the
%absolute error, as explained below. Also, the maximum stepsize is limited
%to 1/5 the integration region and increases in the stepsize are limited
%so as not to be too large.
%
%We would like the error in each step to be less than the absolute and the
%relative error tolerances. For an absolute error tolerance, this means
%that
%abs(xPredMain-xPredSubsid)<AbsTol
%for all components of the predicted value xPred in a particular step of
%size deltaT. For a relative error tolerance, this means that
%abs(xPredMain-xPredSubsid)./max(max(abs(xPred),abs(xCur)),RelTol)<RelTol
%for all components. The extra RelTol in the denominator on the left helps
%deal with issues when the true value is close to zero.
%
%The requirements for absolute and relative tolerances can be combined such
%that the absolute tolerance only comes into play when the numbers are
%sufficiently small that the relative tolerance becomes problematic. The
%two lines
% normFactor=max(max(abs(xPred),abs(xHatPred)),AbsTol/RelTol);
% theError=max(abs((xPred-xHatPred)./normFactor));
%Produce a single quantity, theError, which, if less than RelTol, means
%that the stepsize was too large. If the denominator of the normFactor is
%too small, then it is just AbsTol/RelTol, in which case the RelTol values
%on both sides of the equal sign cancel. And the comparison is just the
%comparison for the absolute tolerance (looking only at the largest
%error component). Thus theError term uses a threshold factor of
%AbsTol/RelTol. This is used as a similar threshold ratio when creating an
%ad-hoc initial estimate.
%
%REFERENCES:
%[1] I. Gladwell, L. F. Shampine, and R. W. Brankin, "Automatic selection
%    of the initial step size for an ODE solver," Journal of Computational
%    and Applied Mathematics, vol. 18, no. 2, pp. 175-192, May 1987.
%[2] G. Hall, "A new stepsize strategy for explicit Runge-Kutta codes,"
%    Advances in Computational Mathematics, vol. 3, no. 4, pp. 343-352, May
%    1995.
%[3] R. L. Burden and J. D. Faires, Numerical Analysis, 9th ed. Boston, MA:
%    Brooks/ Cole, 2011.
%
%February 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<10)
    maxSteps=1024;
end

if(nargin<9||isempty(AbsTol))
    AbsTol=1e-6;
end

if(nargin<8||isempty(RelTol))
   RelTol=1e-3;
end

if(nargin<7||isempty(solutionChoice))
   solutionChoice=0;
end

if(nargin<6||isempty(order))
    order=5;
end

if(nargin<5||isempty(numRefinedSteps))
    numRefinedSteps=0;
end

xDim=size(xStart,1);
tStart=tSpan(1);
tEnd=tSpan(2);
tDiff=diff(tSpan);
tDiffMag=abs(tDiff);
deltaTSign=sign(tDiff);
%The sign is separated out so that one can use the algorithm running
%backwards as well as forwards in time.

%The maximum step size is arbitrarily set to 1/3 the total distance.
deltaTMaxMag=tDiffMag/3;

if(nargin<4||isempty(initStepSize))
    %If no initial step size is given, then just use the smallest uniform
    %step size for the given value of maxSteps.
    initStepSize=tDiffMag/maxSteps;
elseif(initStepSize<eps(tStart))
   error('The initial step size provided is too small.')
end

%Allocate the maximum possible space needed for the return parameters.
numSpaces=1+maxSteps*(numRefinedSteps+1);
xVals=zeros(xDim,numSpaces);
dxdtVals=zeros(xDim,numSpaces);
tVals=zeros(numSpaces,1);

xVals(:,1)=xStart;
tVals(1)=tStart;
dxdtVals(:,1)=f(xStart,tStart);
lastStep=1;

deltaT=deltaTSign*initStepSize;
for curStep=2:(maxSteps+1)
    xCur=xVals(:,lastStep);
    tCur=tVals(lastStep);
    dxdtCur=dxdtVals(:,lastStep);

    %The absolute value of the minimum allowable step size. It is set so
    %that the step must makes something of a difference compared to the
    %numerical precision.
    deltaTMinMag=2^4*eps(tCur);

    %If we would have overstepped the end, change deltaT to be the end.
    %Then, we can terminate at the end.
    if(deltaTSign*(tCur+deltaT)>tEnd*deltaTSign)
        deltaTMag=abs(tEnd-tCur);
        deltaT=deltaTMag*deltaTSign;
        %Allow the last step to be very small.
        deltaTMinMag=min(deltaTMinMag,deltaTMag);
    end

    [deltaTNew,xNew,tNew,k,dxdtCur,exitCode]=performOneAdaptiveRKStep(xCur,tCur,f,deltaT,deltaTMinMag,deltaTMaxMag,dxdtCur,order,solutionChoice,AbsTol,RelTol);

    %If an error occurred, then just return.
    if(exitCode~=0)
        xVals=[];
        tVals=[];
        dxdtVals=[];
        nextStepSize=abs(deltaTNew);
        return
    end

    dxdtVals(:,lastStep)=dxdtCur;

    if(numRefinedSteps~=0)
        %If extra interpolation between the steps is to be performed.
        %Get a Hermite interpolating polynomial over the step.
        [interpPolyA,interpPolyC]=RKInterpPolys(xCur,tCur,xNew,tNew,f,order,solutionChoice,k);
        %The interpolating polynomials take stapes parameterized by
        %a value between zero and 1 representing the distance from
        %tCur to tNew.
        deltaTTaken=tNew-tCur;

        deltaFraction=1/(numRefinedSteps+1);
        curStepFrac=deltaFraction;
        for curRStep=1:numRefinedSteps
            lastStep=lastStep+1;
            tVals(lastStep)=tVals(lastStep-1)+deltaFraction*deltaTTaken;

            xVals(:,lastStep)=polyValNewton(curStepFrac,interpPolyA,interpPolyC);
            dxdtVals(:,lastStep)=f(xVals(:,lastStep),tVals(lastStep));
            curStepFrac=curStepFrac+deltaFraction;
        end
    end

    lastStep=lastStep+1;
    xVals(:,lastStep)=xNew;
    tVals(lastStep)=tNew;
    dxdtVals(:,lastStep)=dxdtCur;
    deltaT=deltaTNew;

    if(tVals(lastStep)==tEnd)
        %If integration completed, reshape the outputs to match the actual
        %number of steps.
        xVals=xVals(:,1:lastStep);
        tVals=tVals(1:lastStep);
        dxdtVals=dxdtVals(:,1:lastStep);
        exitCode=0;
        nextStepSize=deltaTMag;
        return
    end
end

%If we get here, then the maximum number of iterations was reached without
%reaching the end.
xVals=[];
dxdtVals=[];
tVals=[];
exitCode=2;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function R = quatToMat(b, c, d, qx, qy, qz, qfac, pixdim)
%
% R = quatToMat(qb, qc, qd, qx, qy, qz, qfac, pixdim)
%
% Convert a quaternion to a 4x4 rotation/scaling/translation matrix. Based on the
% nifti1 spec implemented in nifti1_io.c.
%
% 2008.10.01 RFD: wrote it.
%

if(isstruct(b))
    c = b.quatern_c;
    d = b.quatern_d;
    qx = b.quatern_x;
    qy = b.quatern_y;
    qz = b.quatern_z;
    qfac = b.qfac;
    pixdim = [b.dx b.dy b.dz];
    b = b.quatern_b;
end

R = zeros(4);

R(4,:) = [0 0 0 1];

% compute a from b,c,d
a = 1.0 - (b*b + c*c + d*d);
if( a < 1.e-7 )                     % special case
     a = 1.0 / sqrt(b*b+c*c+d*d);
     b = b*a; c = c*a; d = d*a;     % normalize (b,c,d) vector
     a = 0.0;                       % a = 0 ==> 180 degree rotation
else
     a = sqrt(a);                   % angle = 2*arccos(a)
end

% build rotation matrix, including scaling factors for voxel sizes
if length(pixdim) == 3
    xd = pixdim(1); yd = pixdim(2); zd = pixdim(3);
else
    xd = pixdim(1); yd = pixdim(2); zd = 1;
end
if(xd<=0.0), xd = 1.0; end
if(yd<=0.0), yd = 1.0; end
if(zd<=0.0), zd = 1.0; end

if(qfac < 0.0), zd = -zd; end       % left handedness?

R(1,1) =       (a*a+b*b-c*c-d*d) * xd ;
R(1,2) = 2.0 * (b*c-a*d        ) * yd ;
R(1,3) = 2.0 * (b*d+a*c        ) * zd ;
R(2,1) = 2.0 * (b*c+a*d        ) * xd ;
R(2,2) =       (a*a+c*c-b*b-d*d) * yd ;
R(2,3) = 2.0 * (c*d-a*b        ) * zd ;
R(3,1) = 2.0 * (b*d-a*c        ) * xd ;
R(3,2) = 2.0 * (c*d+a*b        ) * yd ;
R(3,3) =       (a*a+d*d-c*c-b*b) * zd ;

% load offsets

R(1:3,4) = [qx;qy;qz];

return;


% --------------------------------------------
% demo_rand: random low-rank and sparse
% --------------------------------------------
% This demo tests the matlab code lmafit_sms.m
% for solving sparse matrix separation problem.
% July 25, 2010. Yin Zhang

function demo_rand(Solver,m,n,r,impulse,tol)

if nargin < 1; Solver = 1; end
if nargin < 2; m = 400; end
if nargin < 3; n = m; end
if nargin < 4; r = round(.1*min(m,n)); end
if nargin < 5; impulse = 0.1; end
if nargin < 6; tol = 1e-4; end

if ~exist('PROPACK','dir');
   path(path,genpath(pwd));
   warning off all;
end

sigma = 0e-3; % noise STD
tol = max(tol, .1*sigma);
tol = min(1e-1,tol);
maxit = 200;

% lmafit parameter setup
opts.tol = tol;
opts.maxit = maxit;
opts.est_rank = 1;
opts.print = 0;
% initial rank estimate
k = round(0.25*min(m,n));

% random low-rank and sparse
XY0 = randn(m,r)*randn(r,n);            % random low-rank
Z0 = rand(m,n) < impulse;               % random sparse pattern
Z0 = Z0.*randn(m,n);                    % Gaussian values
%disp(max(abs(XY0(:)))/max(abs(Z0(:))))

% data matrix D (with possible noise)
D = XY0 + Z0;
D = D + sigma*randn(m,n);

beta = []; %beta = 1.2.^(0:20);
t = 1;

fprintf('\n[m n r k] = [%i, %i, %i, %i]\n',m,n,r,k);
fprintf('noise sigma: %6.2e\n',sigma);
fprintf('---------------------------------\n')

% call solvers
for sol = Solver
    tic
    switch sol
        case 1; fprintf('   ... LMafit_sms ...\n')
            [X,Y,Z,out] = lmafit_sms_v1(D,k,opts,beta);
            XY = X*Y; iter = out.iter; rk = size(X,2);
        case 2; fprintf('   ... IALM_rpca  ...\n')
            lambda = t/sqrt(m);
            fprintf('lambda = %.2f/sqrt(m)\n',t);
            [XY,Z,iter] = inexact_alm_rpca(D,lambda,tol/10,maxit);
            rk = rank(XY);
    end
    toc
    % error calculation function
    ferr = @(U,V) norm(U-V,'fro')/norm(V,'fro');
    fprintf('XY RelErr = %8.3e\n',ferr(XY,XY0));
    fprintf('Z  RelErr = %8.3e\n',ferr( Z,Z0 ));
    fprintf('A  RelErr = %8.3e\n',ferr(XY+Z,D));
    fprintf('iter %i: final rank = %i\n',iter,rk);
    fprintf('---------------------------------\n')
end
fprintf('\n\n')


% TRIMMED_GRASSMANN_AVERAGE    Robustly estimate average subspace spanned by data by element-wise trimmed averages.
%
%     TRIMMED_GRASSMANN_AVERAGE(X, P) returns a basis vector for the
%     average one-dimensional subspace spanned by the data X. This is
%     a N-by-D matrix containing N observations in R^D. Robustness is
%     achieved by element-wise trimming at P percent.
%
%     TRIMMED_GRASSMANN_AVERAGE(X, P, K) returns a D-by-K matrix of
%     orthogonal basis vectors spanning a K-dimensional average subspace.
%
%     Reference:
%     "Grassmann Averages for Scalable Robust PCA".
%     S. Hauberg, A. Feragen and M.J. Black. In CVPR 2014.
%     http://ps.is.tue.mpg.de/project/Robust_PCA

%    Grassmann Averages
%    Copyright (C) 2014  Søren Hauberg
%
%    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/>.

function vectors = trimmed_grassmann_average(X, percent, K)

  %% Check input
  if (nargin < 2)
    error ('trimmed_grassmann_average: not enough input arguments');
  end % if
  if (nargin < 3)
    K = 1;
  end % if
  if (percent < 0 || percent > 50)
    error('trimmed_grassmann_average: trimming percentage must be between 0 and 50');
  end % if

  %% Initialisation
  [N, D] = size(X);
  vectors = NaN(D, K, class(X));

  %% Options
  epsilon = 1e-5;

  %% Create output structure
  for k = 1:K
    %% Compute k'th principal component
    mu = rand(D, 1) - 0.5;
    mu = mu / norm(mu);

    %% Initialize using a few EM iterations
    for iter = 1:3
      dots = X * mu; % Nx1
      mu = dots.' * X; % 1xD
      mu = mu(:) / norm(mu); % Dx1
    end % for
    dots = []; % clear up memory

    %% Now the Grassmann average
    for iter = 1:N
      prev_mu = mu;

      %% Compute angles and flip
      dot_signs = sign(X * mu); % Nx1

      %% Compute weighted Grassmannian mean
      mu = trimmed_mean(bsxfun(@times, X, dot_signs), percent); % 1xD
      mu = mu(:) / norm(mu); % Dx1

      %% Check for convergence
      if (max(abs(mu - prev_mu)) < epsilon)
        iter
        break;
      end % if
    end % for
    dot_signs = []; % clear up memory
    prev_mu = []; % clear up memory

    %% Store the estimated vector (and possibly subtract it from data, and perform reorthonormalisation)
    if (k == 1)
      vectors(:, k) = mu;
      X = X - (X * mu) * mu.';
    elseif (k < K)
      mu = reorth(vectors(:, 1:k-1), mu, 1);
      mu = mu / norm(mu);
      vectors(:, k) = mu;

      X = X - (X * mu) * mu.';
    else % k == K
      mu = reorth(vectors(:, 1:k-1), mu, 1);
      mu = mu / norm(mu);
      vectors(:, k) = mu;
    end % if
  end % for
end % function


function [model, L] = mlpReg(X, y, k, lambda)
% Train a multilayer perceptron neural network for regression with backpropagation
% tanh activation function is used
% Input:
%   X: d x n data matrix
%   y: 1 x n real value response vector
%   k: T x 1 vector to specify number of hidden nodes in each layer
%   lambda: regularization parameter
% Ouput:
%   model: model structure
%   L: (regularized least square) loss
% Written by Mo Chen (sth4nth@gmail.com).
if nargin < 4
    lambda = 1e-2;
end
eta = 1e-5;
tol = 1e-5;
maxiter = 50000;
L = inf(1,maxiter);

k = [size(X,1);k(:);size(y,1)];
T = numel(k)-1;
W = cell(T,1);
b = cell(T,1);
for t = 1:T
    W{t} = randn(k(t),k(t+1));
    b{t} = randn(k(t+1),1);
end
R = cell(T,1);
Z = cell(T+1,1);
Z{1} = X;
for iter = 2:maxiter
%     forward
    for t = 1:T-1
        Z{t+1} = tanh(W{t}'*Z{t}+b{t});             % 5.10 5.113
    end
    Z{T+1} = W{T}'*Z{T}+b{T};                       % 5.114

%     loss
    E = Z{T+1}-y;
    Wn = cellfun(@(x) dot(x(:),x(:)),W);            % |W|^2
    L(iter) = dot(E(:),E(:))+lambda*sum(Wn);
    if abs(L(iter)-L(iter-1)) < tol*L(iter-1); break; end

%     backward
    R{T} = E;
    for t = T-1:-1:1
        df = 1-Z{t+1}.^2;    % h'(a)
        R{t} = df.*(W{t+1}*R{t+1});    % 5.66
    end

%     gradient descent
    for t=1:T
        dW = Z{t}*R{t}'+lambda*W{t};    % 5.67
        db = sum(R{t},2);
        W{t} = W{t}-eta*dW;             % 5.43
        b{t} = b{t}-eta*db;
    end
end
L = L(2:iter);
model.W = W;
model.b = b;


% This is a sample code from Appendix A.4 of the following book:
%
%    Zhuang Jiao, YangQuan Chen, Igor Podlubny,
%    Distributed-Order Dynamic Systems: Stability, Simulation,
%    Applications and Perspectives (Springer, London, 2012),
%    ISBN ISBN 978-1-4471-2851-9,
%    http://www.springer.com/engineering/control/book/978-1-4471-2851-9
%
% (C) Igor Podlubny, 2011

clear all

% Check if the FEX package 36574 is on your MATLAB path,
% and if not, then suggest downloading the FEX submission #36574
% "Demos for investigating distributed-order linear time-invariant systems"
% by Zhuang Jiao, which contains the Matlab code from Appendices 1-3
% of the same book
Explanation = ['You may be interested in downloading ' ...
'Zhuang Jiao''s FEX submission #36574' sprintf('\n')...
'(Demos for investigating distributed-order linear ' ...
'time-invariant systems)', sprintf('\n')...
'which contains the Matlab code from Appendices 1-3 to the same book.' ];
if ~(exist('stabbound_dods', 'file') == 2)
    P = suggestFEXpackage(36574, Explanation);
end

% Check if the FEX package 22071 is on your MATLAB path,
% and if it is not there, then require the FEX packages
% 31069 ("requireFEXpackage") and 22071 ("Matrix approach
% to discretization of ODEs and PDEs of arbitrary real order"):
if ~(exist('ban', 'file') == 2 && exist('fan', 'file') == 2 && exist('eliminator', 'file') == 2)
    P = requireFEXpackage(31069); % "requireFEXpackage"
    P = requireFEXpackage(22071);  % "Matrix approach..."
end

% (1) Prepare constants and nodes
%     (this is the longest part of the script):
h = 0.01;           % step of discretization
t = 0:h:5;          % discretization nodes
N = length(t) + 1;  % number of nodes
B = 0.1;            % coefficient of the equation
f = '0 + 0*t';      % right-hand-side
M = zeros(N,N);     % pre-allocate matrix M for the system

% (2) Write the discretization matrix:
M = doban('6*alf.*(1-alf)', [0 1], 0.01, N-1, h) + B*eye(N-1,N-1);

% (3) Compute the right-hand side at discretization nodes:
F = eval ([f '-B'], t)';

% (4) Utilize zero initial condition:
M = eliminator(N-1,[1])*M*eliminator(N-1, [1])';
F = eliminator(N-1,[1])*F;

% (5) And solve the system MY=F:
Y = M\F;

% (6) Pre-pend the zero initial value
Y0 = [0; Y];

% and plot the solution:
U = Y0 + 1;
plot(t, U, 'k')


function trans = localToGlobal3d(varargin)
%LOCALTOGLOBAL3D Transformation matrix from local to global coordinate system
%
%   TRANS = localToGlobal3d(CENTER, THETA, PHI, PSI)
%   Compute the transformation matrix from a local (or modelling)
%   coordinate system to the global (or world) coordinate system.
%   This is a low-level function, used by several drawing functions.
%
%   The transform is defined by:
%   - CENTER: the position of the local origin into the World coordinate
%       system
%   - THETA: colatitude, defined as the angle with the Oz axis (between 0
%       and 180 degrees), positive in the direction of the of Oy axis.
%   - PHI: azimut, defined as the angle of the normal with the Ox axis,
%       between 0 and 360 degrees
%   - PSI: intrinsic rotation, corresponding to the rotation of the object
%       around the direction vector, between 0 and 360 degrees
%
%   The resulting transform is obtained by applying (in that order):
%   - Rotation by PSI   around he Z-axis
%   - Rotation by THETA around the Y-axis
%   - Rotation by PHI   around the Z-axis
%   - Translation by vector CENTER
%   This corresponds to Euler ZYZ rotation, using angles PHI, THETA and
%   PSI.
%
%   The 'eulerAnglesToRotation3d' function may better suit your needs as
%   it is more 'natural'.
%
%   Example
%   localToGlobal3d
%
%   See also
%   transforms3d, createEulerAnglesRotation
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2009-06-19,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

%   HISTORY
%   19/08/2009 fix bug in parsing center with 4 args
%   2011-06-21 use degrees

% extract the components of the transform
if nargin == 1
    % all components are bundled in  the first argument
    var     = varargin{1};
    center  = var(1:3);
    theta   = var(4);
    phi     = var(5);
    psi     = 0;
    if length(var) > 5
        psi = var(6);
    end

elseif nargin == 4
    % arguments = center, then the 3 angles
    center  = varargin{1};
    theta   = varargin{2};
    phi     = varargin{3};
    psi     = varargin{4};

elseif nargin > 4
    % center is given in 3 arguments, then 3 angles
    center  = [varargin{1} varargin{2} varargin{3}];
    theta   = varargin{4};
    phi     = varargin{5};
    psi     = 0;
    if nargin > 5
        psi = varargin{6};
    end
end

% conversion from degrees to radians
k = pi / 180;

% rotation around normal vector axis
rot1    = createRotationOz(psi * k);

% colatitude
rot2    = createRotationOy(theta * k);

% longitude
rot3    = createRotationOz(phi * k);

% shift center
tr      = createTranslation3d(center);

% create final transform by concatenating transforms
trans   = tr * rot3 * rot2 * rot1;


function c = l1nn_cholesky ( n, h )

%*****************************************************************************80
%
%% L1NN_CHOLESKY computes the Cholesky factor of the 1D NN Laplacian.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%    N must be at least 3.
%
%    Input, real H, the spacing between points.
%
%    Output, real C(N,N), the Cholesky factor.
%
  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'L1NN_CHOLESKY - Fatal error!\n' );
    fprintf ( 1, '  N < 3.\n' );
    error ( 'L1NN_CHOLESKY - Fatal error!' );
  end

  c = zeros ( n, n );

  for i = 1 : n - 1
    c(i,i)   = + 1.0;
    c(i,i+1) = - 1.0;
  end

  c(1:n,1:n) = c(1:n,1:n) / h;

  return
end


function EVAL = Evaluate(ACTUAL,PREDICTED)
% This fucntion evaluates the performance of a classification model by
% calculating the common performance measures: Accuracy, Sensitivity,
% Specificity, Precision, Recall, F-Measure, G-mean.
% Input: ACTUAL = Column matrix with actual class labels of the training
%                 examples
%        PREDICTED = Column matrix with predicted class labels by the
%                    classification model
% Output: EVAL = Row matrix with all the performance measures

idx = (ACTUAL()==1);

p = length(ACTUAL(idx));
n = length(ACTUAL(~idx));
N = p+n;

tp = sum(ACTUAL(idx)==PREDICTED(idx));
tn = sum(ACTUAL(~idx)==PREDICTED(~idx));
fp = n-tn;
fn = p-tp;

tp_rate = tp/p;
tn_rate = tn/n;

accuracy = (tp+tn)/N;
sensitivity = tp_rate;
specificity = tn_rate;
precision = tp/(tp+fp);
recall = sensitivity;
f_measure = 2*((precision*recall)/(precision + recall));
gmean = sqrt(tp_rate*tn_rate);

EVAL = [accuracy sensitivity specificity precision recall f_measure gmean];


function scores = ras(pop)
% shortcut to rastriginsfcn()

scores = 10.0 * size(pop,2) + sum(pop .^2 - 10.0 * cos(2 * pi .* pop),2);


function p = predict(theta, X)
%PREDICT Predict whether the label is 0 or 1 using learned logistic
%regression parameters theta
%   p = PREDICT(theta, X) computes the predictions for X using a
%   threshold at 0.5 (i.e., if sigmoid(theta'*x) >= 0.5, predict 1)

m = size(X, 1); % Number of training examples

% You need to return the following variables correctly
p = zeros(m, 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters.
%               You should set p to a vector of 0's and 1's
%
v=sigmoid(X*theta);
for i=1:m
	if v(i,1)>=0.5
		p(i,1)=1;
	else
		p(i,1)=0;
end

% =========================================================================

end


function [ x, seed ] = extreme_values_sample ( a, b, seed )

%*****************************************************************************80
%
%% EXTREME_VALUES_SAMPLE samples the Extreme Values PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < B.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ cdf, seed ] = r8_uniform_01 ( seed );

  x = extreme_values_cdf_inv ( cdf, a, b );

  return
end


function sparse_grid_open_test05 ( dim_num, level_max )

%*****************************************************************************80
%
%% TEST05 tests LEVELS_OPEN_INDEX to make a Gauss Patterson grid.
%
%  Discussion:
%
%    This routine gets the sparse grid indices and determines the
%    corresponding sparse grid abscissas for a Gauss Patterson scheme.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST05:\n' );
  fprintf ( 1, '  Make a sparse Gauss Patterson grid.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEVELS_OPEN_INDEX returns all grid indexes\n' );
  fprintf ( 1, '  whose level value satisfies\n' );
  fprintf ( 1, '    0 <= LEVEL <= LEVEL_MAX.\n' );
  fprintf ( 1, '  Here, LEVEL is the sum of the levels of the 1D rules,\n' );
  fprintf ( 1, '  and the order of the rule is 2^(LEVEL+1) - 1.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now we demonstrate how to convert grid indices\n' );
  fprintf ( 1, '  into physical grid points.  In this case, we\n' );
  fprintf ( 1, '  want points on [-1,+1]^DIM_NUM.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEVEL_MAX = %d\n', level_max );
  fprintf ( 1, '  Spatial dimension DIM_NUM = %d\n', dim_num );

  point_num = sparse_grid_ofn_size ( dim_num, level_max );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of unique points in the grid = %d\n', point_num );
%
%  Compute the orders and points.
%
  grid_index = levels_open_index ( dim_num, level_max, point_num );
%
%  Now we're done.  Print the merged grid data.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Grid index:\n' );
  fprintf ( 1, '\n' );
  for j = 1 : point_num
    fprintf ( 1, '  %4d', j );
    for dim = 1 : dim_num
      fprintf ( 1, '%6d', grid_index(dim,j) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Convert index information to physical information.
%
  grid_point = zeros ( dim_num, point_num );

  order_max = 2^( level_max + 1 ) - 1;

  for j = 1 : point_num
    for dim = 1 : dim_num
      grid_point(dim,j) = gp_abscissa ( order_max, grid_index(dim,j) );
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Grid points:\n' );
  fprintf ( 1, '\n' );
  for j = 1 : point_num
    fprintf ( 1, '  %4d', j );
    for dim = 1 : dim_num
      fprintf ( 1, '%10f', grid_point(dim,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function edge_length = triangle_edge_length_2d ( t )

%*****************************************************************************80
%
%% TRIANGLE_EDGE_LENGTH_2D returns edge lengths of a triangle in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the triangle vertices.
%
%    Output, real EDGE_LENGTH(3,1), the length of the edges.
%
  for j1 = 1 : 3
    j2 = i4_wrap ( j1 + 1, 1, 3 );
    edge_length(j1,1) = sqrt ( ( t(1,j2) - t(1,j1) )^2 ...
                             + ( t(2,j2) - t(2,j1) )^2 );
  end

  return
end


function value = c8_sqrt ( c )

%*****************************************************************************80
%
%% C8_SQRT returns the principal square root of a C8.
%
%  Discussion:
%
%    A C8 is a complex double precision value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, complex double precision C, the number whose square root is desired.
%
%    Output, complex double precision VALUE, the square root of X.
%
  argument = c8_arg ( c );
  magnitude = c8_mag ( c );

  if ( magnitude == 0.0 )

    value = 0.0;

  else

    value = sqrt ( magnitude ) ...
      * ( cos ( argument / 2.0 ) + i * sin ( argument / 2.0 ) );

  end

  return
end


function d=sqdist(a,b)
% SQDIST - computes squared Euclidean distance matrix
%          computes a rectangular matrix of pairwise distances
% between points in A (given in columns) and points in B

% NB: very fast implementation taken from Roland Bunschoten

aa = sum(a.*a,1); bb = sum(b.*b,1); ab = a'*b;
d = abs(repmat(aa',[1 size(bb,2)]) + repmat(bb,[size(aa,2) 1]) - 2*ab);


function A_xyz = computeVelCoeffsMtx(sectorOrder)
%COMPUTEVELCOEFFSMTX Compute the generating matrices of products of
%beampatterns with dipoles
%
%   This routine computes the matrices that generate the coefficients of
%   the beampattern of order (sectorOrder+1) that is essentially the
%   product of a pattern of order=sectorOrder and a dipole. It is used in
%   beamWeightsVelocityPatterns.m. For the derivation of the matrices see:
%
%       Politis, A. and Pulkki, V., 2016.
%       Acoustic intensity, energy-density and diffuseness estimation in a directionally-constrained region.
%       arXiv preprint arXiv:1609.03409.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% COMPUTEVELCOEFFSMTX.M - 7/06/2015
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% orders of sector and resulting velocity patterns
Ns = sectorOrder;
Nxyz = Ns+1;

% Complex SH coefficients of velocity x,y,z
% X = cos\phi sin\theta
% Y = sin\phi sin\theta
% Z = cos\theta
% q = 1 -> Yq = Y_{1(-1)}
% q = 2 -> Yq = Y_{10}
% q = 3 -> Yq = Y_{11}
% x_q != 0 only for q=1,3
% y_q != 0 only for q=1,3
% z_q != 0 only for q=2
x1 = sqrt(2*pi/3);
x3 = -sqrt(2*pi/3);
y1 = 1i*sqrt(2*pi/3);
y3 = 1i*sqrt(2*pi/3);
z2  = sqrt(4*pi/3);

A_xyz = zeros((Nxyz+1)^2, (Ns+1)^2, 3);

G_mtx = gaunt_mtx(Ns, 1, Nxyz);

for ii=1:(Nxyz+1)^2
    for jj=1:(Ns+1)^2

        A_xyz(ii,jj,1) = x1*G_mtx(jj,2,ii) + x3*G_mtx(jj,4,ii);
        A_xyz(ii,jj,2) = y1*G_mtx(jj,2,ii) + y3*G_mtx(jj,4,ii);
        A_xyz(ii,jj,3) = z2*G_mtx(jj,3,ii);
    end
end


function [ cc_est, r_min, r_max ] = cube_random_centralize ( m, n, oc, cc, cr )

%*****************************************************************************80
%
%% CUBE_RANDOM_CENTRALIZE centralizes random data from a cube surface.
%
%  Discussion:
%
%    We generate N points uniformly at random on the surface of the
%    M dimensional cube, of center CC and radius R.
%
%    The cube is aligned with the coordinate axes.
%
%    We seek to estimate the value of CC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 May 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of random points to generate.
%
%    Input, real OC(M,1), the observation point.
%
%    Input, real CC(M,1), the center of the cube.
%
%    Input, real CR, the radius of the cube.
%
%    Output, real CC_EST(M,1), the estimated cube center.
%
%    Output, real R_MIN, R_MAX, the minimum and maximum distances
%    between the observation point and sample points on the cube surface.
%

%
%  Destroy all row vectors!
%
  cc = cc(:);
  oc = oc(:);
%
%  Compute sample points on the cube surface.
%
  x = cube_surface_sample ( m, n, cc, cr );
%
%  The center estimate is simply the average.
%
  cc_est(1:m,1) = sum ( x, 2 ) / n;
%
%  For each point on the cube surface, the "radius" ROC is the norm of X - OC.
%
  roc = x - repmat ( oc, 1, n );
  roc = roc.^2;
  roc = sum ( roc, 1 );
  roc = sqrt ( roc );
%
%  Return the minimum and maximum radiuses.
%
  r_min = min ( roc );
  r_max = max ( roc );

  return
end


function r = sobol_dataset ( m, n, skip )

%*****************************************************************************80
%
%% SOBOL_DATASET generates a Sobol dataset and writes it to a file.
%
%  Usage:
%
%    r = sobol_dataset ( m, n, skip )
%
%    where
%
%    * M, the spatial dimension,
%    * N, the number of points to generate,
%    * SKIP, the number of initial values to skip.
%    * R is the M by N array created.
%
%    creates an M by N Sobol dataset and writes it to the
%    file "sobol_M_N.txt".
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 December 2009
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SOBOL_DATASET\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Generate a Sobol dataset.\n' );
%
%  Get the spatial dimension.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    m = input ( '  Enter the spatial dimension M: ' );
  else
    m = str2num ( m );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension M = %d\n', m );
%
%  Get the number of points.
%
  if ( nargin < 2 )
    fprintf ( 1, '\n' );
    n = input ( '  Enter the number of points N: ' );
  else
    n = str2num ( n );
  end

  fprintf ( 1, '  Number of points N = %d\n', n );
%
%  Get SKIP.
%
  if ( nargin < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  SKIP is the number of points to skip.\n' );
    fprintf ( 1, '  0 is a common choice.\n' );
    fprintf ( 1, '  Another is the least power of 2 greater than or equal to N.\n' );
    skip = input ( '  Enter SKIP: ' );
  else
    skip = str2num ( skip );
  end

  fprintf ( 1, '  SKIP = %d\n', skip );
%
%  Compute the data.
%
  r = i4_sobol_generate ( m, n, skip );
%
%  Write it to a file.
%
  output_filename = ...
    strcat ( 'sobol_', num2str ( m ), '_', num2str ( n ), '.txt' );

  r8mat_write ( output_filename, m, n, r );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The data was written to the file "%s".\n', ...
    output_filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SOBOL_DATASET:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function bit = i4_bit_hi1 ( n )

%*****************************************************************************80
%
%% I4_BIT_HI1 returns the position of the high 1 bit base 2 in an integer.
%
%  Example:
%
%       N    Binary     BIT
%    ----    --------  ----
%       0           0     0
%       1           1     1
%       2          10     2
%       3          11     2
%       4         100     3
%       5         101     3
%       6         110     3
%       7         111     3
%       8        1000     4
%       9        1001     4
%      10        1010     4
%      11        1011     4
%      12        1100     4
%      13        1101     4
%      14        1110     4
%      15        1111     4
%      16       10000     5
%      17       10001     5
%    1023  1111111111    10
%    1024 10000000000    11
%    1025 10000000001    11
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the integer to be measured.
%    N should be nonnegative.  If N is nonpositive, the value will always be 0.
%
%    Output, integer BIT, the number of bits base 2.
%
  i = floor ( n );
  bit = 0;

  while ( 1 )

    if ( i <= 0 )
      break;
    end

    bit = bit + 1;
    i = floor ( i / 2 );

  end

  return
end

function bit = i4_bit_lo0 ( n )

%*****************************************************************************80
%
%% I4_BIT_LO0 returns the position of the low 0 bit base 2 in an integer.
%
%  Example:
%
%       N    Binary     BIT
%    ----    --------  ----
%       0           0     1
%       1           1     2
%       2          10     1
%       3          11     3
%       4         100     1
%       5         101     2
%       6         110     1
%       7         111     4
%       8        1000     1
%       9        1001     2
%      10        1010     1
%      11        1011     3
%      12        1100     1
%      13        1101     2
%      14        1110     1
%      15        1111     5
%      16       10000     1
%      17       10001     2
%    1023  1111111111     1
%    1024 10000000000     1
%    1025 10000000001     1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the integer to be measured.
%    N should be nonnegative.
%
%    Output, integer BIT, the position of the low 1 bit.
%
  bit = 0;
  i = floor ( n );

  while ( 1 )

    bit = bit + 1;
    i2 = floor ( i / 2 );

    if ( i == 2 * i2 )
      break;
    end

    i = i2;

  end

  return
end
function [ quasi, seed ] = i4_sobol ( dim_num, seed )

%*****************************************************************************80
%
%% I4_SOBOL generates a new quasirandom Sobol vector with each call.
%
%  Discussion:
%
%    The routine adapts the ideas of Antonov and Saleev.
%
%    Thanks to Francis Dalaudier for pointing out that the range of allowed
%    values of DIM_NUM should start at 1, not 2!  17 February 2009.
%
%    This function was modified to use PERSISTENT variables rather than
%    GLOBAL variables, 13 December 2009.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Bennett Fox.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Antonov, Saleev,
%    USSR Computational Mathematics and Mathematical Physics,
%    Volume 19, 1980, pages 252 - 256.
%
%    Paul Bratley, Bennett Fox,
%    Algorithm 659:
%    Implementing Sobol's Quasirandom Sequence Generator,
%    ACM Transactions on Mathematical Software,
%    Volume 14, Number 1, pages 88-100, 1988.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Ilya Sobol,
%    USSR Computational Mathematics and Mathematical Physics,
%    Volume 16, pages 236-242, 1977.
%
%    Ilya Sobol, Levitan,
%    The Production of Points Uniformly Distributed in a Multidimensional
%    Cube (in Russian),
%    Preprint IPM Akad. Nauk SSSR,
%    Number 40, Moscow 1976.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the number of spatial dimensions.
%    DIM_NUM must satisfy 1 <= DIM_NUM <= 40.
%
%    Input/output, integer SEED, the "seed" for the sequence.
%    This is essentially the index in the sequence of the quasirandom
%    value to be generated.  On output, SEED has been set to the
%    appropriate next value, usually simply SEED+1.
%    If SEED is less than 0 on input, it is treated as though it were 0.
%    An input value of 0 requests the first (0-th) element of the sequence.
%
%    Output, real QUASI(DIM_NUM), the next quasirandom vector.
%
  persistent atmost;
  persistent dim_max;
  persistent dim_num_save;
  persistent initialized;
  persistent lastq;
  persistent log_max;
  persistent maxcol;
  persistent poly;
  persistent recipd;
  persistent seed_save;
  persistent v;

  if ( isempty ( initialized ) )
    initialized = 0;
    dim_num_save = -1;
  end

  if ( ~initialized | dim_num ~= dim_num_save )

    initialized = 1;

    dim_max = 40;
    dim_num_save = -1;
    log_max = 30;
    seed_save = -1;
%
%  Initialize (part of) V.
%
    v(1:dim_max,1:log_max) = zeros(dim_max,log_max);

    v(1:40,1) = [ ...
      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, ...
      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, ...
      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, ...
      1, 1, 1, 1, 1, 1, 1, 1, 1, 1 ]';

    v(3:40,2) = [ ...
            1, 3, 1, 3, 1, 3, 3, 1, ...
      3, 1, 3, 1, 3, 1, 1, 3, 1, 3, ...
      1, 3, 1, 3, 3, 1, 3, 1, 3, 1, ...
      3, 1, 1, 3, 1, 3, 1, 3, 1, 3 ]';

    v(4:40,3) = [ ...
               7, 5, 1, 3, 3, 7, 5, ...
      5, 7, 7, 1, 3, 3, 7, 5, 1, 1, ...
      5, 3, 3, 1, 7, 5, 1, 3, 3, 7, ...
      5, 1, 1, 5, 7, 7, 5, 1, 3, 3 ]';

    v(6:40,4) = [ ...
                     1, 7, 9,13,11, ...
      1, 3, 7, 9, 5,13,13,11, 3,15, ...
      5, 3,15, 7, 9,13, 9, 1,11, 7, ...
      5,15, 1,15,11, 5, 3, 1, 7, 9 ]';

    v(8:40,5) = [ ...
                           9, 3,27, ...
     15,29,21,23,19,11,25, 7,13,17, ...
      1,25,29, 3,31,11, 5,23,27,19, ...
     21, 5, 1,17,13, 7,15, 9,31, 9 ]';

    v(14:40,6) = [ ...
              37,33, 7, 5,11,39,63, ...
     27,17,15,23,29, 3,21,13,31,25, ...
      9,49,33,19,29,11,19,27,15,25 ]';

    v(20:40,7) = [ ...
                                         13, ...
     33,115, 41, 79, 17, 29,119, 75, 73,105, ...
      7, 59, 65, 21,  3,113, 61, 89, 45,107 ]';

    v(38:40,8) = [ ...
                                7, 23, 39 ]';
%
%  Set POLY.
%
    poly(1:40)= [ ...
        1,   3,   7,  11,  13,  19,  25,  37,  59,  47, ...
       61,  55,  41,  67,  97,  91, 109, 103, 115, 131, ...
      193, 137, 145, 143, 241, 157, 185, 167, 229, 171, ...
      213, 191, 253, 203, 211, 239, 247, 285, 369, 299 ];

    atmost = 2^log_max - 1;
%
%  Find the number of bits in ATMOST.
%
    maxcol = i4_bit_hi1 ( atmost );
%
%  Initialize row 1 of V.
%
    v(1,1:maxcol) = 1;

  end
%
%  Things to do only if the dimension changed.
%
  if ( dim_num ~= dim_num_save )
%
%  Check parameters.
%
    if ( dim_num < 1 | dim_max < dim_num )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'I4_SOBOL - Fatal error!\n' );
      fprintf ( 1, '  The spatial dimension DIM_NUM should satisfy:\n' );
      fprintf ( 1, '    1 <= DIM_NUM <= %d\n', dim_max );
      fprintf ( 1, '  But this input value is DIM_NUM = %d\n', dim_num );
      return
    end

    dim_num_save = dim_num;
%
%  Initialize the remaining rows of V.
%
    for i = 2 : dim_num
%
%  The bits of the integer POLY(I) gives the form of polynomial I.
%
%  Find the degree of polynomial I from binary encoding.
%
      j = poly(i);
      m = 0;

      while ( 1 )

        j = floor ( j / 2 );

        if ( j <= 0 )
          break;
        end

        m = m + 1;

      end
%
%  Expand this bit pattern to separate components of the logical array INCLUD.
%
      j = poly(i);
      for k = m : -1 : 1
        j2 = floor ( j / 2 );
        includ(k) = ( j ~= 2 * j2 );
        j = j2;
      end
%
%  Calculate the remaining elements of row I as explained
%  in Bratley and Fox, section 2.
%
      for j = m + 1 : maxcol
        newv = v(i,j-m);
        l = 1;
        for k = 1 : m
          l = 2 * l;
          if ( includ(k) )
            newv = bitxor ( newv, l * v(i,j-k) );
          end
        end
        v(i,j) = newv;
      end
    end
%
%  Multiply columns of V by appropriate power of 2.
%
    l = 1;
    for j = maxcol-1 : -1 : 1
      l = 2 * l;
      v(1:dim_num,j) = v(1:dim_num,j) * l;
    end
%
%  RECIPD is 1/(common denominator of the elements in V).
%
    recipd = 1.0 / ( 2 * l );

    lastq(1:dim_num) = 0;

  end

  seed = floor ( seed );

  if ( seed < 0 )
    seed = 0;
  end

  if ( seed == 0 )

    l = 1;
    lastq(1:dim_num) = 0;

  elseif ( seed == seed_save + 1 )
%
%  Find the position of the right-hand zero in SEED.
%
    l = i4_bit_lo0 ( seed );

  elseif ( seed <= seed_save )

    seed_save = 0;
    l = 1;
    lastq(1:dim_num) = 0;

    for seed_temp = seed_save : seed - 1
      l = i4_bit_lo0 ( seed_temp );
      for i = 1 : dim_num
        lastq(i) = bitxor ( lastq(i), v(i,l) );
      end
    end

    l = i4_bit_lo0 ( seed );

  elseif ( seed_save + 1 < seed )

    for seed_temp = seed_save + 1 : seed - 1
      l = i4_bit_lo0 ( seed_temp );
      for i = 1 : dim_num
        lastq(i) = bitxor ( lastq(i), v(i,l) );
      end
    end

    l = i4_bit_lo0 ( seed );

  end
%
%  Check that the user is not calling too many times!
%
  if ( maxcol < l )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_SOBOL - Fatal error!\n' );
    fprintf ( 1, '  Too many calls!\n' );
    fprintf ( 1, '  MAXCOL = %d\n', maxcol );
    fprintf ( 1, '  L =      %d\n', l );
    return
  end
%
%  Calculate the new components of QUASI.
%
  for i = 1 : dim_num
    quasi(i) = lastq(i) * recipd;
    lastq(i) = bitxor ( lastq(i), v(i,l) );
  end

  seed_save = seed;
  seed = seed + 1;

  return
end
function r = i4_sobol_generate ( m, n, skip )

%*****************************************************************************80
%
%% I4_SOBOL_GENERATE generates a Sobol dataset.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 December 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points to generate.
%
%    Input, integer SKIP, the number of initial points to skip.
%
%    Output, real R(M,N), the points.
%
  seed = skip;

  for j = 1 : n
    [ r(1:m,j), seed ]  = i4_sobol ( m, seed );
  end

  return
end
function r8mat_write ( output_filename, m, n, table )

%*****************************************************************************80
%
%% R8MAT_WRITE writes an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the output filename.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real TABLE(M,N), the points.
%

%
%  Open the file.
%
  output_unit = fopen ( output_filename, 'wt' );

  if ( output_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_WRITE - Error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'R8MAT_WRITE - Error!' );
    return;
  end
%
%  Write the data.
%
%  For smaller data files, and less precision, try:
%
%     fprintf ( output_unit, '  %14.6f', table(i,j) );
%
  for j = 1 : n
    for i = 1 : m
      fprintf ( output_unit, '  %24.16f', table(i,j) );
    end
    fprintf ( output_unit, '\n' );
  end
%
%  Close the file.
%
  fclose ( output_unit );

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


clear all; clc;

%% moudle DIP5
syms  b1 b2 b3 real
syms  g1 g2 g3 real
syms  v1 v2 v3 real
syms  lambda real
syms norm_h norm_g real
B = [b1 b2 b3]';
V = [v1 v2 v3]';
G = [g1 g2 g3]';
% %G'*(V - B) - CONST

 f1= (V - B)' * (V - B) - norm_h^2;
 j1 = jacobian(f1, [B' norm_h lambda]);

 f2 = G' * (V - B) - norm_g * norm_h * lambda;
 j2 = jacobian(f2, [B' norm_h lambda]);

f1
j1

f2
j2