classdef GaussianD
%%GAUSSIAND Functions to handle the scalar and multivariate Gaussian
%           distribution.
%Implemented methods are: mean,cov, PDF, PDFI, PDFIDerivs (for multivariate
%                         derivatives of the PDF), PDFS, logPDFS,
%                         PDFSGradHessVechS (for the gradient and Hessian
%                         of the elements of a lower-triangular square-root
%                         of the covariance matrix), CDF (for scalar
%                         distributions), invCDF (for scalar
%                         distributions), normProdDist, normConvDist (for
%                         scalar distributions), momentGenFun
%                         (multivariate, including derivatives), cumGenFun
%                         (multivariate, including derivatives), rand,
%                         randS, integralOverRegion, entropy
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

methods(Static)

function val=mean(mu)
%%MEAN Obtain the mean of the Gaussian distribution.
%
%INPUTS: mu The mean of the PDF. If the PDF is multivariate, then this is
%           a numDimX1 column vector.
%
%OUTPUTS: val The numDimX1 mean of the Gaussian distribution.
%
%The Gaussian distribution is parameterized by its mean and covariance
%matrix. Thus, this function just returns the mean it is given.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    val=mu;
end

function val=cov(Sigma)
%%COV Obtain the covariance matrix of the Gaussian distribution (the
%     variance if scalar).
%
%INPUTS: Sigma The variance (if scalar) or covariance matrix (if
%               multidimensional) of the PDF. The variance cannot be zero
%               and the covariance matrix cannot be singular.
%
%OUTPUTS: val The covariance matrix of the Gaussian distribution.
%
%The Gaussian distribution is parameterized by its mean and covariance
%matrix. Thus, this function just returns the covariance matrix it is
%given.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    val=Sigma;
end

function vals=PDF(z,mu,Sigma)
%%PDF Evaluate a scalar or multivariate Gaussian (normal) PDF at specified
%     points given the mean and the covariance matrix.
%
%INPUTS: z The points at which the PDF should be evaluated. If the PDF is
%          multivariate, then this is a column vector. If evaluation at
%          multiple points are desired, then this is a numDimXN matrix with
%          each column being the a point (a vector).
%       mu The mean of the PDF. If the PDF is multivariate, then this is a
%          numDimX1 column vector. If omitted or an empty matrix is passed,
%          a zero mean is used.
%    Sigma The variance (if scalar) or numDimXnumDim covariance matrix
%          (if multidimensional) of the PDF. The variance cannot be zero
%          and the covariance matrix cannot be singular. If omitted or an
%          empty matrix is passed, the identity matrix is used as the
%          covariance matrix.
%
%OUTPUTS: vals The scalar values of the normal PDF with mean mu and
%              covariance matrix Sigma evaluated at the points in z. If
%              multiple points are passed (z is a matrix), then val is a
%              row vector.
%
%September 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    numDim=size(z,1);
    if(nargin<2||isempty(mu))
        mu=zeros(numDim,1);
    end

    if(nargin<3||isempty(Sigma))
       Sigma=eye(numDim,numDim);
    end

    diff=bsxfun(@minus,z,mu);
    vals=(2*pi)^(-numDim/2)*(det(Sigma))^(-1/2)*exp(-0.5*invSymQuadForm(diff,Sigma));
end

function vals=logPDF(z,mu,Sigma)
%%LOGPDF Evaluate the natural logarithm of a scalar or multivariate
%        Gaussian (normal) PDF at a certain points given the mean and the
%        covariance matrix.
%
%INPUTS: z The points at which the PDF should be evaluated. If the PDF is
%          multivariate, then this is a column vector. If evaluation at
%          multiple points are desired, then this is a numDimXN matrix with
%          each column being the a point (a vector).
%       mu The mean of the PDF. If the PDF is multivariate, then this is a
%          numDimX1 column vector. If omitted or an empty matrix is passed,
%          a zero mean is used.
%    Sigma The variance (if scalar) or numDimXnumDim covariance matrix
%          (if multidimensional) of the PDF. The variance cannot be zero
%          and the covariance matrix cannot be singular. If omitted or an
%          empty matrix is passed, the identity matrix is used as the
%          covariance matrix.
%
%OUTPUTS: vals The scalar values of the natural logarithm of the normal PDF
%              with mean mu and covariance matrix Sigma evaluated at the
%              points in z. If multiple points are passed (z is a matrix),
%              then val is a row vector.
%
%March 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

    numDim=size(z,1);
    if(nargin<2||isempty(mu))
        mu=zeros(numDim,1);
    end

    if(nargin<3||isempty(Sigma))
       Sigma=eye(numDim,numDim);
    end

    diff=bsxfun(@minus,z,mu);
    vals=-(1/2)*log(det(2*pi*Sigma))-(1/2)*invSymQuadForm(diff,Sigma);
end

function vals=PDFI(z,mu,SigmaInv,SigmaInvDet)
%%PDFI Evaluate a scalar or multivariate Gaussian (normal) PDF at specified
%      points given the mean and the inverse of the covariance matrix.
%
%INPUTS: z The points at which the PDF should be evaluated. If the PDF is
%          multivariate, then this is a column vector. If evaluations at
%          multiple points are desired, then this is a numDimXN matrix with
%          each column being the a point (a vector).
%       mu The mean of the PDF. If the PDF is multivariate, then this is a
%          numDimX1 column vector. If omitted or an empty matrix is passed,
%          a zero mean is used.
% SigmaInv The inverse variance (if scalar) or numDimXnumDim inverse
%          covariance matrix (if multidimensional) of the PDF. SigmaInv can
%          be singular. If omitted or an empty matrix is passed, the
%          identity matrix is used as the covariance matrix.
% SigmaInvDet Optionally, a length-N set of determinants of the matrices
%          in SigmaInv can be passed so as to speed up the computation. If
%          omitted or an empty matrix is passed, determinants will be taken
%          as needed.
%
%OUTPUTS: val The scalar value of the normal PDF with mean mu and inverse
%             covariance matrix SigmaInv evaluated at the point z. If
%             multiple points are passed (z is a matrix), then val is a row
%             vector.
%
%September 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    numPoints=size(z,2);
    vals=zeros(1,numPoints);
    n=size(z,1);
    if(nargin<2||isempty(mu))
        mu=zeros(numDim,1);
    end

    if(nargin<3||isempty(SigmaInv))
       SigmaInv=eye(numDim,numDim);
    end

    if(nargin<4||isempty(SigmaInvDet))
        constVal=sqrt(det(SigmaInv)/(2*pi)^(n));
    else
        constVal=sqrt(SigmaInvDet/(2*pi)^(n));
    end
    for curPoint=1:numPoints
        diff=z(:,curPoint)-mu;
        %Note that det(A^(-1))=1/det(A) and that det(a*A)=a^n*det(A), where
        %a is a scalar and A is an nXn matrix.

        vals(curPoint)=constVal*exp(-0.5*(diff'*SigmaInv*diff));
    end
end

function [PDFDerivVal,coeffPolyPart]=PDFIDerivs(mu,SigmaInv,numDerivs,x)
%%PDFIDERIVS Compute derivatives of the multivariate Gaussian normal PDF at
%            given the mean and the inverse of the covariance matrix.
%            Derivatives are taken with respect to components of the
%            argument of the PDF, x.
%
%INPUTS: mu The mean of the PDF. If the PDF is multivariate, then this is a
%           numDimX1 column vector.
%  SigmaInv The inverse variance (if scalar) or numDimXnumDim inverse
%           covariance matrix (if multidimensional) of the PDF.
%           SigmaInv can be singular.
% numDerivs A numDimX1 or 1XnumDim vector indicating the number of
%           derivatives to take with respect to each of the dimensions of
%           the state.numDerivs>=0.
%         x The numDimXnumPoints argument of the PDF at which the
%           derivatives of the PDF should be evaluated. If this parameter
%           is omitted or an empty matrix is passed, then a default of
%           x=zeros(numDim,1) is used.
%
%OUTPUTS: PDFDerivVal A numPointsX1 vector of the values of the derivatives
%                   of the PDF function given at the points in x or at x=0
%                   if x is omitted.
%     coeffPolyPart A hypermatrix taking numDim indices that can be
%                   evaluated using the polyValMultiDim at different values
%                   of x to get the polynomial coefficient that multiplies
%                   the exponential term in the given set of derivatives
%                   of the PDF.
%
%All derivatives of the multivariate normal distribution have the form
%sqrt((2*pi)^(-2)*det(SigmaInv))*exp(-1/2*((x-mu)'*SigmaInv*(x-mu))*...
%(polynomial)
%The polynomial can be found using the chain rule each time a derivative is
%taken. The derivative of the exponential term with respect to x(i), the
%ith component of the state is
%sqrt((2*pi)^(-2)*det(SigmaInv))*exp(-1/2*((x-mu)'*SigmaInv*(x-mu)) times
%SigmaInv(i,:)*mu-SigmaInv(i,:)*x.
%This function returns coeffPolyPart, the coefficients of the multivariate
%polynomial that multiplies
%sqrt((2*pi)^(-2)*det(SigmaInv))*exp(-1/2*((x-mu)'*SigmaInv*(x-mu)) when
%computing the derivatives in addition to returning the value of the PDF at
%any desired points.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

numIdx=size(mu,1);

coeffPolyPart=1;

%i is the current dimension we are differentiating.
for i=1:numIdx
    %basePoly shall be the multivariate polynomial that is added every time
    %the exponential term in the moment generating function is
    %differentiated with respect to the ith index. This makes basePoly the
    %derivative with respect to the ith element of t of the argument of the
    %exponent in the multivariate normal PDF.

    %Allocate space for the polynomial and make it have the correct shape.
    basePoly=reshape(zeros(2^numIdx,1),2*ones(1,numIdx));

    %The additive term
    basePoly(1)=SigmaInv(i,:)*mu;

    %The term multiplied by x consists of elements from the inverse
    %covariance matrix. This just consists of terms in the ith row of the
    %inverse covariance matrix (the matrix is symmetric).
    idxVec=ones(numIdx,1);
    %The dimensionalities of all of the variables for the nDim2Index
    %function.
    dims=2*ones(numIdx,1);
    for curDim=1:numIdx
        idxVec(curDim)=2;
        basePoly(nDim2Index(dims,idxVec))=-SigmaInv(i,curDim);
        idxVec(curDim)=1;
    end

    %Now, we enter into a loop to evaluate derivatives of the PDF with
    %respect to the current dimension.
    for derivsLeft=numDerivs(i):-1:1
        %The chain rule means that there are two terms to consider (both of
        %which are multiplied by the same exponential term).
        %The first term is the current coeffPolyPart times the derivative
        %of the exponential term with respect to the ith dimensions. This
        %is the product of coeffPolyPart and basePoly. The second term is
        %the exponential term times the derivative of coeffPolyPart. The
        %two terms then must be added.
        term1=convn(basePoly,coeffPolyPart);%Multiply the polynomials.
        term2=polyDerMultiDim(coeffPolyPart,i);

        %Add the multivariate polynomials.
        coeffPolyPart=polySumMultiDim(term1,term2);
    end
end

%Get rid of redundant parts that arose due to the multiplication.
coeffPolyPart=shrinkMultiDimPoly2Fit(coeffPolyPart);

%coeffPolyPart now contains the multivariate polynomial that is multiplied
%by the exponential term. If no value of t is given, then just evaluate it
%at x=0. In this instance, the exponential term is zero and only the
%constant term from the polynomial appears.
if(nargin<4||isempty(x))
    PDFDerivVal=GaussianD.PDFI([0;0],mu,SigmaInv)*coeffPolyPart(1);
else
    numPoints=size(x,2);
    PDFDerivVal=zeros(numPoints,1);
    for curPoint=1:numPoints
        xCur=x(:,curPoint);
        PDFDerivVal(curPoint)=GaussianD.PDFI(xCur,mu,SigmaInv)*polyValMultiDim(coeffPolyPart,xCur);
    end
end
end

function val=PDFS(z,mu,S)
%%PDFS Evaluate a scalar or multivariate Gaussian (normal) PDF at specifed
%      points given the mean and the lower-triangular square root of the
%      covariance matrix.
%
%INPUTS: z The points at which the PDF should be evaluated. If the PDF is
%          multivariate, then this is a column vector. If evaluations at
%          multiple points are desired, then this is a numDimXN matrix with
%          each column being the a point (a vector).
%       mu The mean of the PDF. If the PDF is multivariate, then this is a
%          numDimX1 column vector.
%        S The square root of the variance (if scalar) or the numDimXnumDim
%          lower-triangular square root of the covariance matrix (if
%          multidimensional) of the PDF such that S*S'=Sigma, where Sigma
%          is the covariance matrix. S cannot be a singular matrix. If
%          omitted or an empty matrix is passed, the identity matrix is
%          used.
%
%OUTPUTS: val The scalar value(s) of the normal PDF with mean mu and square
%             root covariance matrix S evaluated at the points in z. If
%             multiple points are passed (z is a matrix), then val is a row
%             vector.
%
%September 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<2||isempty(mu))
        mu=zeros(numDim,1);
    end

    if(nargin<3||isempty(S))
    	S=eye(numDim,numDim);
    end

%Note that (S*S')^(-1)=(S')^(-1)*S^(-1)
    diff=S\bsxfun(@minus,z,mu);
%Note that det(S*S')=det(S)*det(S') and that det(S)=det(S') so
%det(S*S')=det(S)^2. Also, det(a*S)=a^ndet(S), where a is a scalar and S is
%an nXn matrix. Thus,
%det(2*pi*S*S')=det(sqrt(2*pi)*S)^2=(2*pi)^n*det(S)^2
    n=size(z,1);
    %The abs in the determinant is necessary if the main diagonal of S has
    %negative terms. S can still be such that S*S'=Sigma as the sign of
    %those terms is not unique due to the squaring.
    val = (1/((2*pi)^(n/2)*abs(det(S))))*exp(-0.5*sum(diff.*diff,1));
end

function val=logPDFS(z,mu,S)
%%LOGPDFS Evaluate the natural logarithm of a scalar or multivariate
%         Gaussian (normal) PDF at specified points given the mean and the
%         lower-triangular square root of the covariance matrix.
%
%INPUTS: z The points at which the PDF should be evaluated. If the PDF is
%          multivariate, then this is a column vector. If evaluations at
%          multiple points are desired, then this is a numDimXN matrix with
%          each column being the a point (a vector).
%       mu The mean of the PDF. If the PDF is multivariate, then this is a
%          numDimX1 column vector.
%        S The square root of the variance (if scalar) or the numDimXnumDim
%          lower-triangular square root of the covariance matrix (if
%          multidimensional) of the PDF such that S*S'=Sigma, where Sigma
%          is the covariance matrix. S cannot be a singular matrix. If
%          omitted or an empty matrix is passed, the identity matrix is
%          used.
%
%OUTPUTS: val The scalar value(s) of the natural logarithm of the normal
%             PDF with mean mu and square root covariance matrix S
%             evaluated at the points in z. If multiple points are passed
%             (z is a matrix), then val is a row vector.
%
%March 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<2||isempty(mu))
        mu=zeros(numDim,1);
    end

    if(nargin<3||isempty(S))
    	S=eye(numDim,numDim);
    end

%Note that (S*S')^(-1)=(S')^(-1)*S^(-1)
    diff=S\bsxfun(@minus,z,mu);
%Note that det(S*S')=det(S)*det(S') and that det(S)=det(S') so
%det(S*S')=det(S)^2. Also, det(a*S)=a^ndet(S), where a is a scalar and S is
%an nXn matrix. Thus,
%det(2*pi*S*S')=det(sqrt(2*pi)*S)^2=(2*pi)^n*det(S)^2
    n=size(z,1);
    %The abs in the determinant is necessary if the main diagonal of S has
    %negative terms. S can still be such that S*S'=Sigma as the sign of
    %those terms is not unique due to the squaring.
    % val=-(n/2)*log((2*pi))-log(abs(det(S)))-0.5*sum(diff.*diff,1);
    val=log((1/((2*pi)^(n/2)*abs(det(S))))*exp(-0.5*sum(diff.*diff,1)));
end

function [grad,CDetGrad,Hess,CDetHess]=PDFSGradHessVechS(x,mu,C)
%%PDFGRADHESSVECHS Find the gradient and (if requested Hessian) of the
%            normal (Gaussian) probability density function (PDF), taken
%            with respect to the vech(C), where C is the lower-triangular
%            square root of the covariance matrix of the distribution.
%
%INPUTS: x The dXnumPoints points at which the normal PDF is considered.
%       mu The dX1 mean of the normal PDF.
%        C The dXd lower-triangular square root covariance matrix of the
%          normal PDF. This cannot be singular.
%
%OUTPUTS: grad The gradient of the normal PDF with respect to vech(C)
%              (vector of first partial derivatives  with respect to the
%              elements of vech(C)). This is an (n*(n+1)/2)XnumPoints set
%              of vectors.
%     CDetGrad The (n*(n+1)/2)X1 gradient of 1/sqrt(C*C') with respect to
%              vech(C). This term is needed to compute grad and Hess and is
%              often needed in algorithms using grad and Hess.
%         Hess The Hessian of the normal PDF with respect to vech(C)
%              (vector of second partial derivatives  with respect to the
%              elements of vech(C)). This is an
%              (n*(n+1)/2)X(n*(n+1)/2)XnumPoints set of symmetric matrices.
%              Element i,j in a matrix is the second derivative with
%              respect to elements i and j of vech(C).
%     CDetHess The (n*(n+1)/2)X1 Hessian of 1/sqrt(C*C') with respect to
%              vech(C). This term is needed to compute grad and Hess and is
%              often needed in algorithms using grad and Hess.
%
%The gradient and Hessian provided by this function play a role in
%multivariate kernel bandwidth estimation algorithms, such as the
%cross-validation bandwidth estimation algorithms of [1], which optimize
%over the components of a lower-triangular square root matrix to avoid
%obtaining invalid covariance matrix estimates.
%
%This function is implemented based on the rules of matrix calculus.
%
%A 5-dimensional example.
% c=[17;23;4;10;11;5;6;12;18;13;19;25;21;2;9];
% C=vech2Mat(c,0);
% x=[5;-10;33;24;-18];
% mu=[0;0;0;0;0];
% [grad,Hess]=GaussianD.PDFSGradHessVechS(x,mu,C)
%
%REFERENCES:
%[1] T. Duong and M. L. Hazelton, "Cross-validation bandwidth matrices for
%    multivariate kernel density estimation," Scandinavian Journal of
%    Statistics, vol. 32, no. 3, pp. 485-506, Sep. 2005.
%
%January 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Since the derivatives are not with respect to x, it does not matter if the
%distribution is not zero-mean; just shift the value.
x=bsxfun(@minus,x,mu);

%Problem dimensionality
d=size(x,1);
numPoints=size(x,2);

vechC=vech(C);
numVechEls=length(vechC);

CDetRoot=1/sqrt(det(C*C'));

%Find the gradient of 1/sqrt(C*C') with respect to vech(C). Only the
%derivatives corresponding to the diagonals are non-zero.
CDetGrad=vech(diag(-CDetRoot./diag(C)));

CInv=inv(C);

%The exponential term
expTerm=exp(-0.5*invSymQuadForm(x,C,1));

%Find the gradient of C^(-1) with respect to vech(C).
CInvGrad=zeros(d,d,numVechEls);
CDeriv=zeros(d,d);
curEl=1;
for j=1:d
    for i=j:d
        CDeriv(i,j)=1;

        CInvGrad(:,:,curEl)=CInv*CDeriv*CInv;
        curEl=curEl+1;
        CDeriv(i,j)=0;
    end
end

%Find the gradient of (C*C')^(-1) with respect to vech(C).
CCpInvGrad=zeros(d,d,numVechEls);
for i=1:numVechEls
    temp=CInv'*CInvGrad(:,:,i);
    CCpInvGrad(:,:,i)=temp+temp';
end

%Find the gradient of the exponential term with respect to vech(C).
expTermGrad=zeros(numVechEls,numPoints);
for i=1:numVechEls
    expTermGrad(i,:)=(1/2)*expTerm.*sum(bsxfun(@times,x,CCpInvGrad(:,:,i)*x),1);
end

%Now, find the derivatives of the PDF with respect to every element of
%of vech(C).
grad=zeros(numVechEls,numPoints);
for i=1:numVechEls
    grad(i,:)=CDetGrad(i)*expTerm+CDetRoot*expTermGrad(i,:);
end
grad=grad/(2*pi)^(d/2);

if(nargout>2)%If the Hessian is desired.
    Hess=zeros(numVechEls,numVechEls,numPoints);
    CDetHess=zeros(numVechEls,numVechEls);
    for m=1:numVechEls
        CDerivM=zeros(d,d);
        [i,j]=vechInd2Sub(d,m);%--derivative indices
        CDerivM(i,j)=1;

        %This term is used in the Hessian of exp((1/2)*x'*inv(C*C')*x)
        term1H=sum(bsxfun(@times,x, CCpInvGrad(:,:,m)*x),1);
        for n=m:numVechEls
            CDerivN=zeros(d,d);
            [k,l]=vechInd2Sub(d,n);%--derivative indices
            CDerivN(k,l)=1;

            %The Hessian of inv(C) with respect to indices i,j and k,l.
            CInvHess=CInv*(CDerivN*CInv*CDerivM+CDerivM*CInv*CDerivN)*CInv;

            term1=CInv'*CInvHess;
            term2=CInvGrad(:,:,m)'*CInvGrad(:,:,n);
            %The Hessian of inv(C*C') with respect to indices i,j and k,l.
            CCpInvHess=term1+term1'+term2+term2';
            %The Hessian of exp((1/2)*x'*inv(C*C')*x)
            term2=sum(bsxfun(@times,x, CCpInvGrad(:,:,n)*x),1);
            term3=sum(bsxfun(@times,x,CCpInvHess*x),1);
            expTermHess=(expTerm/2).*((1/2)*term1H.*term2-term3);

            Hess(m,n,:)=expTermGrad(m,:)*CDetGrad(n)+expTermGrad(n,:)*CDetGrad(m)+CDetRoot*expTermHess;
            if(i==j&&l==k)%The second derivative term of the Hessian
                CDetHess(m,n)=CDetRoot/(C(i,i)*C(k,k));
                if(i==k)
                    CDetHess(m,n)=CDetHess(m,n)*2;
                end
                %Due to the symmetry of the Hessian.
                CDetHess(n,m)=CDetHess(m,n);

                Hess(m,n,:)=Hess(m,n,:)+reshape(expTerm*CDetHess(m,n),1,1,numPoints);
            end

            Hess(m,n,:)=Hess(m,n,:)*(2*pi)^(-d/2);
            %Due to the symmetry of the Hessian, the upper triangular part
            %is also known.
            Hess(n,m,:)=Hess(m,n,:);
        end
    end
end

end

function val=CDF(z,mu,varVal)
%%CDF Evaluate cumulative distribution function (CDF) of a a scalar
%     Gaussian (normal) distribution at a specified points given the mean
%     and the variance, or for a normal(0,1) distribution if the mean and
%     variance are omitted.
%
%INPUTS: z A matrix of the point(s) at which the CDF should be evaluated.
%       mu The mean of the distribution. If omitted or an empty matrix is
%          passed, a mean of 0 is used.
%   varVal The variance of the distribution. If omitted or an empty matrix
%          is passed, a variance of 1 is used.
%
%OUTPUTS: val The scalar value(s) of the normal CDF with mean mu and
%             variance varVal evaluated at the point(s) z.
%
%This just uses the relation between the normal CDF and the error function
%along with the erf function in Matlab.
%
%September 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

	if(nargin<2||isempty(mu))
        mu=0;
	end

	if(nargin<3||isempty(varVal))
        varVal=1;
	end

    x=(z-mu)/sqrt(varVal);
    val=(1+erf(x/sqrt(2)))/2;
end

function val=invCDF(prob,mu,varVal)
%%CDF Evaluate the inverse CDF of a scalar Gaussian (normal) distribution
%     at a given point given the mean and the variance, or for a
%     normal(0,1) distribution if the mean and variance are omitted. When
%     considering a normal(0,1) distribution, this is also known as the
%     probit function.
%
%INPUTS: prob The probability or probabilities (0<=prob<=1) at which the
%             argument of the CDF is desired.
%          mu The mean of the distribution. If omitted or an empty matrix
%             is passed, a mean of 0 is used.
%      varVal The variance of the distribution. If omitted or an empty
%             matrix is passed, a variance of 1 is used.
%
%OUTPUTS: val The argument(s) of the CDF that would give the probability or
%             probabilities in prob.
%
%This just uses the relation between the normal CDF and the error function
%along with the erfinv (inverse error function) command in Matlab.
%
%June 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<2||isempty(mu))
       mu=0;
    end

    if(nargin<3||isempty(varVal))
       varVal=1;
    end

    val=sqrt(2*varVal)*erfinv(2*prob-1)+mu;
end

function [mu,SigmaInv,multiConst]=normProdDist(mu1,SigmaInv1,mu2,SigmaInv2)
%%NORMPRODDIST The product of two multivariate normal distributions is an
%              unnormalized Gaussian distribution. This finds the
%              parameters of the product distribution.
%
%INPUTS: mu1,SigmaInv1 The mean and inverse of the covariance matrix
%                      (inverse of the variance for a scalar distribution)
%                      of the first normal distribution.
%        mu2,SigmaInv2 The mean and inverse of the covariance matrix
%                      (inverse of the variance for a scalar distribution)
%                      of the second normal distribution.
%
%OUTPUTS: mu, SigmaInv The mean and inverse covariance matrix of the
%                      product distribution.
%           multiConst The product distribution is not normalized. This is
%                      the multiplicative constant that is multiplied by a
%                      normalized distribution.
%
%The derivation of the product of two normal PDFs is a standard exercise in
%many statistics classes. The product distribution is also given
%explicitly in [1].
%
%Note that while SigmaInv1 and SigmaInv2 can each be singular, the sum must
%be non-singular. Note that for products involving distributions with
%distant means and small variances, multiConst might be numerically zero.
%
%REFERENCES:
%[1] K. B. Petersen and M. S. Pedersen, "The matrix cookbook," Technical
%    University of Denmark, Tech. Rep., 15 Nov. 2012. [Online]. Available:
%    http://www2.imm.dtu.dk/pubdb/views/publication details.php?id=3274
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

SigmaInv=SigmaInv1+SigmaInv2;
mu=SigmaInv\(SigmaInv1*mu1+SigmaInv2*mu2);

multiConst=GaussianD.PDFI(mu1,mu2,SigmaInv);
end

function [mu,Sigma]=normConvDist(mu1,Sigma1,mu2,Sigma2)
%%NORMCONVDIST The convolution of two scalar normal distributions is also a
%              normal distribution. This provides the parameters for the
%              convoluted distribution.
%
%INPUTS: mu1,Sigma1 The mean and variance of the first scalar normal
%                   distribution.
%        mu2,Sigma2 The mean and variance of the second scalar normal
%                   distribution.
%
%OUTPUTS: mu, SigmaInv The mean and variance of the product distribution.
%
%Note that the product distribution is normalized. The derivation of the
%product distribution is straightforward nothing that the Fourier transform
%of a normal distribution is also a fully-normalized (integrates to one)
%normal distribution.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

mu=mu1+mu2;
Sigma=Sigma1+Sigma2;

end

function [momentVal,coeffPolyPart]=momentGenFun(mu,Sigma,numDerivs,t)
%%MOMENTGENFUN Evaluate the moment generating function (or one of its
%              derivatives) of the multivariate normal distribution. Taking
%              the ith, jth, kth... derivative of the moment generating
%              function with respect to the first, second, third...
%              components of the argument and evaluating it at t=0 provides
%              the noncentral moment of the multivariate normal
%              distribution involving the ith, jth, kth power of the first
%              second, third... components of the random vector.
%
%INPUTS: mu The mean of the PDF. If the PDF is multivariate, then this is a
%           numDimX1 column vector.
%     Sigma The variance (if scalar) or numDimXnumDim covariance matrix
%           (if multidimensional) of the PDF.
% numDerivs A numDimX1 or 1XnumDim vector indicating the number of
%           derivatives to take with respect to each of the components of
%           the argument of the moment generating function. numDerivs>=0.
%         t The numDimXnumPoints argument of the moment generating
%           function at which the derivatives of the moment generating
%           function should be evaluated. If this parameter is omitted or
%           an empty matrix is passed, then a default of
%           t=zeros(numDim,1) is used.
%
%OUTPUTS: momentVal A numPointsX1 vector of the values of the derivatives
%                   of the moment generating function given at the points
%                   in t or at t=0 if t is omitted.
%     coeffPolyPart A hypermatrix taking numDim indices that can be
%                   evaluated using the polyValMultiDim at different values
%                   of t to get the polynomial coefficient that multiplies
%                   the exponential term in the given set of derivatives
%                   of the moment generating function.
%
%The moment generating function of a random vector is defined to be
%E(exp(t'*x)) where E is the expected value operator, x is the random
%variable and t is a real parameter having the same dimensionality as the
%random variable. It can be shown that the moment generating function of a
%multivariate normal distribution is
%E(exp(t'*x))=exp(t'*mu+(1/2)*t'*Sigma*t)
%Derivatives of this can be evaluated systematically. First,
%differentiating the exponential with respect to the ith component of t
%leads to the original exponential term times
%mu(i)+sum(Sigma(:,i).*t(:))
%Thus, all derivatives include the original exponential term times a
%multivariate polynomial. This function keeps track of the polynomial
%through all of the derivatives, using the chain rule, the fact that
%multivariate polynomial multiplication can be performed using the convn
%function, and using polyDerMultiDim and polySumMultiDim for
%multidimensional polynomial differentiation and addition.
%
%As an example, consider finding the noncentral third moment E(x1*x2*x3) of
%a 3D multivariate normal distribution. This can be done using
%If one solves for it by hand, one gets
%mu(1)*Sigma(2,3)+mu(2)*Sigma(1,3)+mu(3)*Sigma(1,2)+prod(mu)
%However, the moment can also be found by evaluating the derivatives of the
%moment generating function with respect to the first, second and third
%variables at t=[0;0;0]. For example, consider
% mu=[1;2;3];
% Sigma=[8, 3, 2;
%        3,11, 9;
%        2, 9,18];
% numDerivs=[1;1;1];
% momentVal=GaussianD.momentGenFun(mu,Sigma,numDerivs)
%One will find the value of the noncentral moment to be 28, which is
%correct.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

%The number of dimensions.
numIdx=length(mu);

%Derivative of the moment generating function produce a polynomial times
%the original exponential term. We shall keep track of that polynomial
%while differentiating. The loops below take successive derivatives with
%respect to the different dimensions of the argument vector of the moment
%generating function. Each component is differentiated for the required
%number of derivatives. coeffPolyPart holds the accumulated polynomial that
%multiplies the exponential part of the differentiated moment generating
%function. Initially, coeffPolyPart=1 to signify that no derivatives have
%been taken. It is initially implemented with more elements than necessary
%to simplify the addition of the terms after the use of the polyDerMultiDim
%function below.
coeffPolyPart=1;

%i is the current dimension we are differentiating.
for i=1:numIdx
    %basePoly shall be the multivariate polynomial that is added every time
    %the exponential term in the moment generating function is
    %differentiated with respect to the ith index. This makes basePoly the
    %derivative with respect to the ith element of t of the argument of the
    %exponent in the multivariate normal moment generating function

    %Allocate space for the polynomial and make it have the correct shape.
    if(numIdx>1)
        basePoly=reshape(zeros(2^numIdx,1),2*ones(1,numIdx));
    else
        basePoly=zeros(2^numIdx,1);
    end

    %The additive term is the component of the mean that was multiplied by
    basePoly(1)=mu(i);

    %The term multiplied by x consists of elements from the covariance
    %matrix. This just consists of terms in the ith row of the
    %covariance matrix (the matrix is symmetric).
    idxVec=ones(numIdx,1);
    %The dimensionalities of all of the variables for the nDim2Index
    %function.
    dims=2*ones(numIdx,1);
    for curDim=1:numIdx
        idxVec(curDim)=2;
        basePoly(nDim2Index(dims,idxVec))=Sigma(i,curDim);
        idxVec(curDim)=1;
    end

    %Now, we enter into a loop to evaluate derivatives of the moment
    %generating function with respect to the current dimension.
    for derivsLeft=numDerivs(i):-1:1
        %The chain rule means that there are two terms to consider (both of
        %which are multiplied by the same exponential term).
        %The first term is the current coeffPolyPart times the derivative
        %of the exponential term with respect to the ith dimensions. This
        %is the product of coeffPolyPart and basePoly. The second term is
        %the exponential term times the derivative of coeffPolyPart. The
        %two terms then must be added.
        term1=convn(basePoly,coeffPolyPart);%Multiply the polynomials.
        term2=polyDerMultiDim(coeffPolyPart,i);

        %Add the multivariate polynomials.
        coeffPolyPart=polySumMultiDim(term1,term2);
    end
end

%coeffPolyPart now contains the multivariate polynomial that is multiplied
%by the exponential term. If no value of t is given, then just evaluate it
%at t=0. In this instance, the exponential term is zero and only the
%constant term from the polynomial appears.
if(nargin<4||isempty(t))
    momentVal=coeffPolyPart(1);
else
    numPoints=size(t,2);
    momentVal=zeros(numPoints,1);
    for curPoint=1:numPoints
        tCur=t(:,curPoint);
        momentVal(curPoint)=exp(tCur'*mu+tCur'*Sigma*tCur)*polyValMultiDim(coeffPolyPart,tCur);
    end
end

end

function cumVal=cumGenFun(mu,Sigma,numDerivs,t)
%%CUMGENFUN Evaluate the cumulant generating function (or one of its
%           derivatives) of the multivariate normal distribution. Taking
%           the ith, jth, kth... derivative of the cumulant generating
%           function with respect to the first, second, third...
%           components of the argument and evaluating it at t=0 provides
%           the cumulant of the multivariate normal distribution involving
%           the ith, jth, kth power of the first second, third...
%           components of the random vector. The cumulant generating
%           function is the natural logarithm of the moment generating
%           function.
%
%INPUTS: mu The mean of the PDF. If the PDF is multivariate, then this
%           is a numDimX1 column vector.
%     Sigma The variance (if scalar) or numDimXnumDim covariance matrix
%           (if multidimensional) of the PDF.
% numDerivs A numDimX1 or 1XnumDim vector indicating the number of
%           derivatives to take with respect to each of the components of
%           the argument of the cumulant generating function.
%           numDerivs>=0.
%         t The numDimXnumPoints argument of the cumulant generating
%           function at which the derivatives of the cumulant generating
%           function should be evaluated. If this parameter is omitted or
%           an empty matrix is passed, then a default of
%           t=zeros(numDim,1) is used.
%
%OUTPUTS: cumVal A numPointsX1 vector of the values of the derivatives
%                of the cumulant generating function given at the points
%                in t or at t=0 if t is omitted.
%
%Cumulants are useful in interpolating probability distributions through
%the use of, for example, an Edgeworth series. The cumulant generating
%function is defined as the natural logarithm of the moment generating
%function. It can be shown that the moment generating function of the
%multivariate normal distribution is
%E(exp(t'*x))=exp(t'*mu+(1/2)*t'*Sigma*t)
%Thus, the cumulant generating function is just
%t'*mu+(1/2)*t'*Sigma*t
%Consequently, for derivatives higher than two, the cumulant generating
%function (and hence the cumulants) of the multivariate normal distribution
%are all zero.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

if(nargin<4||isempty(t))
    numDim=length(mu);
    t=zeros(numDim,1);
end

numPoints=size(t,2);
cumVal=zeros(numPoints,1);

sumVal=sum(numDerivs);
switch(sumVal)
    case 0%No derivatives.
        for curPoint=1:numPoints
            tCur=t(:,curPoint);
            cumVal(curPoint)=tCur'*mu+(1/2)*tCur'*Sigma*tCur;
        end
    case 1
        %Find the nonzero term.
        derivIdx=find(numDerivs);
        for curPoint=1:numPoints
            tCur=t(:,curPoint);
            cumVal(curPoint)=mu(derivIdx)+sum(Sigma(:,derivIdx).*tCur(:));
        end
    case 2
        derivIdx=find(numDerivs);
        cumVal(:)=Sigma(derivIdx(1),derivIdx(2));%The same for all t.
    otherwise%No third or higher order cumulants.
       cumVal(:)=0;
end
end

function x=rand(N,mu,P)
%%RAND Generate multivariate Gaussian random variables with a given mean
%      vector and covariance matrix.
%
%INPUTS: N The number of random variables to generate.
%       mu The xDim X1 mean of the multivariate Gaussian to generate.
%        P The xDim X xDim positive definite covariance matrix of the
%          multivariate Gaussian to generate. If this parameter is omitted
%          or an empty matrix is passed, then the identity matrix will be
%          used.
%
%OUTPUTS: x An xDimXN matrix of random instances of the multivariate
%           Gaussian distribution.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    xDim=size(mu,1);
    if(nargin<3||isempty(P))
        P=zeros(xDim,xDim);
    end

    xDim=size(mu,1);
    x=bsxfun(@plus,mu,chol(P,'lower')*randn(xDim,N));
end

function x=randS(N,mu,S)
%%RANDS Generate multivariate Gaussian random variables with a given mean
%       vector and lower-triangular square root covariance matrix.
%
%INPUTS: N The number of random variables to generate.
%       mu The xDim X1 mean of the multivariate Gaussian to generate.
%        S The xDim X xDim lower triangular square root covariance matrix
%          of the multivariate Gaussian to generate.
%
%OUTPUTS: x An xDimXN matrix of random instances of the multivariate
%           Gaussian distribution.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    xDim=size(mu,1);
    x=bsxfun(@plus,mu,S*randn(xDim,N));
end

function [P,error,curIter]=integralOverRegion(mu, Sigma,minVals,maxVals,epsVal,alpha,maxIter)
%%INTEGRALOPVERREGION Compute the probability of a Gaussian probability
%                  density function (PDF) within a (hyper-)rectangular
%                  region. The probability is computed using a transformed
%                  Monte Carlo method designed for this specific integral
%                  that converges significantly faster than generic,
%                  textbook Monte Carlo integration techniques.
%
%INPUTS:  mu The NX1 mean vector of the multivariate normal distribution.
%      Sigma The NXN positive definite covariance matrix of the
%            distribution.
%    minVals An NX1 or 1XN vector of the lower integration bounds for all
%            of the dimensions.
%    maxVals An NX1 or 1XN vector of the upper integration bounds. Note
%            that minVals(i)<maxVals(i) for all elements.
%     epsVal The desired error tolerance for the probability computation.
%            If omitted or an empty matrix is passed, the defaut of 1e-6
%            (1e-4%) is used.
%      alpha The confidence factor for the standard error tolerance. Thus,
%            for the estimate of epsVal used to determine termination to be
%            correct 99% of the time, one should use
%            alpha=GaussianD.invCDF(0.99); If this parameter is omitted or
%            an empty matrix is passed, the default value of 3.5 is used.
%    maxIter The maximum number of iterations to use. If this parameter is
%            omitted or an empty matrix is passed, the default value of
%            1000 is used.
%
%OUTPUTS: P The approximate probability within the region (0-1).
%     error The estimated error in P, with a confidence interval determined
%           by alpha.
%   curIter The number of the last iteration performed before termination.
%
%The algorithm is that of [1]. Various transformations are applied to the
%function to achieve better convergence than when performing typical Monte
%Carlo integration.
%
%EXAMPLE:
%This is the 3D example in the paper. The probability should be about
%0.8279.
% minBounds=[-Inf;-Inf;-Inf];
% maxBounds=[1;4;2];
% R=[1,3/5,1/3;
%    3/5,1,11/15;
%    1/3,11/15,1];
% mu=[0;0;0];
% PApprox=GaussianD.integralOverRegion(mu,R,minBounds,maxBounds)
%
%REFERENCES:
%[1] A.Genz, "Numerical computation of multivariate normal probabilities,"
%    Journal of Computational and Graphical Statistics, vol. 1, no. 2, pp.
%    141-149, June 1992.
%
%November 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<7||isempty(maxIter))
       maxIter=1000;
    end

    if(nargin<6||isempty(alpha))
       alpha=3.5;
    end

    if(nargin<5||isempty(epsVal))
       epsVal=1e-6;
    end

    %Recenter things for a zero-mean distribution.
    minVals=minVals-mu;
    maxVals=maxVals-mu;

    %First, we reorder the variables so that the largest integration
    %regions come last. This is suggested in [1] to make the algorithm
    %faster.
    [~,idxSort]=sort(maxVals-minVals,'ascend');
    a=minVals(idxSort);
    b=maxVals(idxSort);
    C2=Sigma(idxSort,idxSort);

    C=chol(C2,'lower');

    numDim=size(C,1);

    intSum=0;
    varSum=0;

    %Allocate space
    d=zeros(numDim,1);
    e=zeros(numDim,1);
    f=zeros(numDim,1);

    d(1)=GaussianD.CDF(a(1)/C(1,1));
    e(1)=GaussianD.CDF(b(1)/C(1,1));
    f(1)=e(1)-d(1);

    y=zeros(numDim-1,1);%Allocate space.
    for curIter=0:(maxIter-1)

        w=rand(numDim-1,1);

        for i=2:numDim
            y(i-1)=GaussianD.invCDF(d(i-1)+w(i-1)*(e(i-1)-d(i-1)));
            deltaVal=C(i,1:(i-1))*y(1:(i-1));

            if(a(i)==-Inf)
                d(i)=0;
            else
                d(i)=GaussianD.CDF((a(i)-deltaVal)/C(i,i));
            end

            if(b(i)==Inf)
                e(i)=1;
            else
                e(i)=GaussianD.CDF((b(i)-deltaVal)/C(i,i));
            end
            f(i)=(e(i)-d(i))*f(i-1);
        end

        N=curIter+1;

        delta=(f(numDim)-intSum)/N;
        intSum=intSum+delta;
        varSum=(N-2)*varSum/N+delta^2;
        error=alpha*sqrt(varSum);

        if(error<epsVal)
            break;
        end
    end

    %The probability estimate.
    P=min(1,max(0,intSum));
end

function entropyVal=entropy(Sigma)
%%ENTROPY Obtain the differential entropy of the multivariate Gaussian
%         distribution given in nats. The differential entropy of a
%         continuous distribution is entropy=-int_x p(x)*log(p(x)) dx where
%         the integral is over all values of x. Units of nats mean that the
%         natural logarithm is used in the definition. Unlike the Shannon
%         entropy for discrete variables, the differential entropy of
%         continuous variables can be both positive and negative.
%
%INPUTS: Sigma The NXN positive definite covariance matrix of the
%              distribution.
%
%OUTPUTS: entropyVal The value of the differential entropy in nats.
%
%Differential entropy is defined in Chapter 8 of [1].
%
%REFERENCES:
%[1] T. M. Cover and J. A. Thomas, Elements of Information Theory, 2nd ed.
%    Hoboken, NJ: Wiley-Interscience, 2006.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

    entropyVal=(1/2)*log(det(2*pi*exp(1)*Sigma));
end

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 j_polynomial_plot ( n_vec, alpha_vec, beta_vec, filename )

%*****************************************************************************80
%
%% J_POLYNOMIAL_PLOT plots Jacobi polynomials J(n,a,b,x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N_VEC(*), the orders of 1 or more polynomials
%    to be plotted together.
%
%    Input, integer ALPHA_VEC(*), BETA_VEC(*), the alpha and beta values
%    for each polynomial.
%
%    Input, string FILENAME, the name into which the graphics information is
%    to be stored.  Note that the PNG format will be used.
%
  a = -1.0;
  b = +1.0;
  m = 501;
  x = linspace ( a, b, m );
  x = x';
  vec_num = length ( n_vec );

  clf
  hold on
  for i = 1 : vec_num
    n = n_vec(i);
    alpha = alpha_vec(i);
    beta = beta_vec(i);
    y = j_polynomial ( m, n, alpha, beta, x );
    plot ( x, y(:,n+1), 'LineWidth', 2 );
  end
  grid on
  xlabel ( '<--- X --->' )
  ylabel ( '<--- J(n,a,b,x) --->' )
  title ( 'Jacobi polynomials J(n,a,b,x)' )
  hold off
  print ( '-dpng', filename )

  return
end


function varargout = levi13(X)
% Levi function, #13
%
%   LEVI13([x1, x2]) returns the value of the value of the 13th Levi
%   function at the specified points. [x1] and [x2] may be vectors.
%   The search domain is
%
%               -10 < x_i < 10
%
%   The global minimum is
%
%               f(x1, x2) = f(1, 1) = 0.

% Author: Rody P.S. Oldenhuis
% Delft University of Technology
% E-mail: oldenhuis@dds.nl
% Last edited 20/Jul/2009

    % if no input is given, return dimensions, bounds and minimum
    if (nargin == 0)
        varargout{1} = 2;  % # dims
        varargout{2} = [-10, -10]; % LB
        varargout{3} = [+10, +10]; % UB
        varargout{4} = [1,1]; % solution
        varargout{5} = 0; % function value at solution

    % otherwise, output function value
    else

        % keep values in teh search domain
        X(X < -10) = inf;       X(X > 10) = inf;

        % split input vector X into x1, x2
        if size(X, 1) == 2
            x1 = X(1, :);        x2 = X(2, :);
        else
            x1 = X(:, 1);        x2 = X(:, 2);
        end

        % output function value
        varargout{1} = sin(3*pi*x1).^2 + (x1-1).^2.*(1 + sin(3*pi*x2).^2) + ...
            (x2-1).^2.*(1 + sin(2*pi*x2).^2);

    end

end


function [M,MU,N,B,C,prm,iprm] = msns_pre(M,N,B,C)
%
%  Preprocessing of the system
%      .
%    M x  =  N x + B u
%                                                             (1)
%      y  =  C x,
%
%  where both M and N are REAL, SYMMETRIC and SPARSE. Moreover, M must be
%  positive definite and N must be negative definite.
%
%  The preprocessing consists of a double transformation of the state:
%
%    x <-- MU * P * x .
%
%  The first transformation with the permutation matrix P for bandwidth
%  reduction results in "overwriting" the system matrices as
%
%    M <-- P * M * P',  N <-- P * N * P',  B <-- P * B,  C <-- C * P'.
%
%  The bandwidth of the reordered matrices M and N is often much smaller
%  than that of the original matrices.
%
%  By the second transformation, the generalized system (1) is transformed
%  into a standard system
%      .
%      x  =  A x + B u
%                                                             (2)
%      y  =  C x.
%
%  To this end, the Cholesky factorization of M is computed: M = MU'*MU,
%  where MU is upper triangular. This results in:
%
%    A := inv(MU')*N*inv(MU),  B <-- inv(MU')*B,  C <-- C*inv(MU).
%
%  The matrix A, which is dense in general, is not formed explicitely.
%  It is implicitely given by N and MU.
%
%  Note that the systems (1) and (2) have an identical input-output
%  mapping.
%
%  Calling sequence:
%
%    [M,MU,N,B,C,prm,iprm] = msns_pre(M,N,B,C)
%
%  Input:
%
%    M, N      n-x-n system matrices;
%    B         n-x-m system matrix;
%    C         q-x-n system matrix.
%
%  Output:
%
%    M         permuted matrix M;
%    MU        Cholesky factor of (permuted) matrix M;
%    N         permuted matrix N;
%    B, C      transformed system matrices;
%    prm       the permutation that has been used in the first
%              transformation step;
%    iprm      the inverse permutation (needed to re-reorder certain data
%              in postprocessing).
%
%
%  LYAPACK 1.0 (Thilo Penzl, May 1999)

% Input data not completely checked!

if any(any(imag(M))) | any(any(imag(N))) | any(any(imag(B))) | ...
    any(any(imag(C)))
  disp('WARNING in ''msns_pre'': M, N, B, and C must be real matrices.');
  pause(10);
end

if norm(M-M','fro')~=0
  error('M is not symmetric!');
end

if norm(N-N','fro')~=0
  error('N is not symmetric!');
end

[prm,iprm] = lp_prm(M,N);

M = M(prm,prm);
N = N(prm,prm);

[MU,t] = chol(M);

if t~=0
  error('M is not (numerically) positive definite!');
end

if length(B)
  B = MU'\B(prm,:);
end

if length(C)
  C = C(:,prm)/MU;
end


function [xPred, PPred] = KalmanFilterX_PredictState(x,P,F,Q,u,B,Qu)
% KALMANFILTERX_PREDICTSTATE Perform the discrete-time KF state prediction
% step, under the assumption of additive process noise.
%
% Parameters
% ----------
% x: column vector
%   The (xDim x 1) state estimate at the previous time-step.
% P: matrix
%   The (xDim x xDim) state covariance matrix at the previous
%   time-step.
% F: matrix
%   An (xDim x xDim) state transition matrix.
% Q: matrix
%   The (xDim x xDim) process noise covariance matrix.
% u: column vector, optional
%   An optional (xDim x 1) control input.
%   If omitted, no control input is used.
% B: matrix, optional
%   An optional (xDim x xDim) control gain matrix.
%   If omitted, B is assumed to be 1.
% O: matrix, optional
%   An optional (xDim x xDim) control noise covariance
%   matrix. If omitted, Q is assumed to be 0.
%
% Returns
% -------
% xPred: column vector
%   The (xDim x 1) predicted state estimate.
% PPred: matrix
%   The (xDim x xDim) predicted state covariance matrix.
%
%October 2017 Lyudmil Vladimirov, University of Liverpool.

    switch(nargin)
        case(4)
            u  = 0;
            B  = 0;
            Qu = 0;
        case(5)
            B  = 1;
            Qu = 0;
        case(6)
            Qu = 0;
    end

    % Compute predicted state mean and covariance
    xPred = F*x + B*u;
    PPred =F*P*F' + Q + B*Qu*B';
end


function H=MDexact(M,x0,y0,z0,x,y,z,er,mr,sigma,f)
%
% function H=MDexact(M,x0,y0,z0,x,y,z,er,mr,sigma,f)
%
% Function MDEXACT calculates the magnetic field in position (x,y,z) for a
% impulsive source M=I*Am placed in the point (x0,y0,z0). The source is
% inserted in a medium characterized by dielectric permettivity er,
% magnetic permeability mr and conductivity sigma. The temporal rule is
% exp(-i*omega*t).
%
% INPUT
%   M = M*l, impulsive value of the magnetic dipole [A*m^2]
%   x0,y0,z0 = coordinates of the dipole [m]
%   x,y,z = point where the field is calculated [m]
%   er = relative dialectric permettivity
%   mr = relative magnetic permeability
%   sigma = conductivity [S/m]
%   f = frequency [Hz]
%
% OUTPUT
%   H(1:3,1) = Hx, Hy and Hz with dipole directed along x
%   H(1:3,2) = Hx, Hy and Hz with dipole directed along y
%   H(1:3,3) = Hx, Hy and Hz with dipole directed along z

% change the reference system (index 0 refers to the system in which the
% dipole is placed in the origin): the script was originally written for a
% dipole placed in (0,0,0).
x=x-x0;
y=y-y0;
z=z-z0;

% constant
v0=2.997925e8;
mu0=pi*4e-7;
eps0=1/(v0^2*mu0);
eps=eps0*er;
mu=mu0*mr;
omega=2*pi*f;
gammaq=j*omega*mu.*(sigma+j*omega*eps);
gamma=sqrt(gammaq);

H=zeros(3,3);

% dipole along x
rho=sqrt(y^2+z^2);
r=sqrt(rho^2+x^2);
if rho<1e-10,
    cosphi=1;
    sinphi=0;
else
    cosphi=z/rho;
    sinphi=-y/rho;
end
costheta=x/r;
sintheta=sqrt(1-costheta^2);

Hr_exact=-j*(M/(2*pi*omega*mu*r^3))*(1+gamma*r).*exp(-gamma*r)*costheta;
Htheta_exact=-j*(M/(4*pi*omega*mu*r^3))*(1+gamma*r+gamma.^2*r^2).*exp(-gamma*r)*sintheta;
Hrho_exact=Hr_exact*sintheta+Htheta_exact*costheta;

Hx_exact=Hr_exact*costheta-Htheta_exact*sintheta;
Hz_exact=Hrho_exact*cosphi;
Hy_exact=-Hrho_exact*sinphi;

H(1,1)=Hx_exact;
H(2,1)=Hy_exact;
H(3,1)=Hz_exact;

% dipole along y
rho=sqrt(x^2+z^2);
r=sqrt(rho^2+y^2);
if rho<1e-10,
    cosphi=1;
    sinphi=0;
else
    cosphi=x/rho;
    sinphi=z/rho;
end
costheta=y/r;
sintheta=sqrt(1-costheta^2);

Hr_exact=(M/(2*pi*r^3))*(1+gamma*r).*exp(-gamma*r)*costheta;
Htheta_exact=(M/(4*pi*r^3))*(1+gamma*r+gamma.^2*r^2).*exp(-gamma*r)*sintheta;
Hrho_exact=Hr_exact*sintheta+Htheta_exact*costheta;

Hy_exact=Hr_exact*costheta-Htheta_exact*sintheta;
Hx_exact=Hrho_exact*cosphi;
Hz_exact=Hrho_exact*sinphi;

H(1,2)=Hx_exact;
H(2,2)=Hy_exact;
H(3,2)=Hz_exact;

% dipole along z
rho=sqrt(x^2+y^2);
r=sqrt(rho^2+z^2);
if rho<1e-10,
    cosphi=1;
    sinphi=0;
else
    cosphi= x/rho;
    sinphi= y/rho;
end
costheta=z/r;
sintheta=sqrt(1-costheta^2);

Hr_exact=(M/(2*pi*r^3))*(1+gamma*r).*exp(-gamma*r)*costheta;
Htheta_exact=(M/(4*pi*r^3))*(1+gamma*r+gamma.^2*r^2).*exp(-gamma*r)*sintheta;
Hrho_exact=Hr_exact*sintheta+Htheta_exact*costheta;

Hz_exact=Hr_exact*costheta-Htheta_exact*sintheta;
Hx_exact=Hrho_exact*cosphi;
Hy_exact=Hrho_exact*sinphi;

H(1,3)=Hx_exact;
H(2,3)=Hy_exact;
H(3,3)=Hz_exact;

% switch to exp(-i*omega*t)
H = conj(H);


function fx2 = p19_fx2 ( x )

%*****************************************************************************80
%
%% P19_FX2 evaluates the second derivative of the function for problem 19.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(*), the point at which F is to be evaluated.
%
%    Output, real FX2(*), the value of the second derivative at X.
%
  arg = - ( 10.0 - 30.0 * x ).^2;

  fx2 = - 10000.0 * cos ( 100.0 * x ) ...
    + 14400.0 * exp ( arg ) * ( 10.0 - 30.0 * x ) / sqrt ( pi );

  return
end


function [ element_area, mesh_area ] = area_q4_mesh ( node_num, element_num, ...
  node_xy, element_node )

%*****************************************************************************80
%
%% AREA_Q4_MESH computes areas of elements in a Q4 mesh.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, real NODE_XY(2,NODE_NUM), the node coordinates.
%
%    Input, integer ELEMENT_NODE(4,ELEMENT_NUM), lists the
%    nodes that make up each element, in counterclockwise order.
%
%    Output, real ELEMENT_AREA(ELEMENT_NUM), the element areas.
%
%    Output, real MESH_AREA, the mesh area.
%
  for element = 1 : element_num
    for node = 1 : 4
      q4(1:2,node) = node_xy(1:2,element_node(node,element));
    end
    element_area(element) = area_quad ( q4 );
  end

  mesh_area = sum ( element_area(1:element_num) );

  return
end


function [lambda_vec, error_train, error_val] = ...
    validationCurve(X, y, Xval, yval)
%VALIDATIONCURVE Generate the train and validation errors needed to
%plot a validation curve that we can use to select lambda
%   [lambda_vec, error_train, error_val] = ...
%       VALIDATIONCURVE(X, y, Xval, yval) returns the train
%       and validation errors (in error_train, error_val)
%       for different values of lambda. You are given the training set (X,
%       y) and validation set (Xval, yval).
%

% Selected values of lambda (you should not change this)
lambda_vec = [10000]';

% You need to return these variables correctly.
error_train = zeros(length(lambda_vec), 1);
error_val = zeros(length(lambda_vec), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return training errors in
%               error_train and the validation errors in error_val. The
%               vector lambda_vec contains the different lambda parameters
%               to use for each calculation of the errors, i.e,
%               error_train(i), and error_val(i) should give
%               you the errors obtained after training with
%               lambda = lambda_vec(i)
%
% Note: You can loop over lambda_vec with the following:
%
%       for i = 1:length(lambda_vec)
%           lambda = lambda_vec(i);
%           % Compute train / val errors when training linear
%           % regression with regularization parameter lambda
%           % You should store the result in error_train(i)
%           % and error_val(i)
%           ....
%
%       end
%
%
for i = 1:length(lambda_vec)
		lambda = lambda_vec(i)
		[theta] = trainLinearReg(X, y, lambda);
		error_train(i) = linearRegCostFunction(X, y, theta, 0);
		error_val(i) = linearRegCostFunction(Xval, yval, theta, 0);
end

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

end


%Author: Moeti Ncube
% This code uses the Schwartz-Smith model to calibrate and simulate multiple assets such that:
%
% 1. The Calibration and simulation is consistent with the observable forward curve, adjusted for seasonality, at any given date
% 2. The Calibration and simulation is consistent with the observable ATM volatility at a given date
% 3. The Calibration and simulation is consistent with the observable correlation structure between the forward curve vectors at
% a given date
%
%
% In this example, I use four assets: 5x16,2x16,7x8 PJM forward prices and ATM volatilities along with natural gas forward prices
% and ATM volatilities
%
% Calibration of the parameters is done in Excel. By inputing the vectors of Forward and ATM volatilites for each commodity, I
% can compute the theoretical Schwartz Smith Forward Prices and Standard Deviations for a given maturity. I then use Excel solver
% to minimize the difference between the observed market values and their theoretical values to obtain the Schwartz-Smith
% parameters for each commodity. Note this methodology is drastically different from the one described in "Short Term Variation
% and Long-Term Dynamics in Commodity Prices" in which Schwartz and Smith calibrate their model to historical futures prices.
% Here I calibrate the model to the current forward and volatility curve as well as adjust for seasonality. This procedure is
% much more practical pricing methodology.
%
% The more difficult step was insuring that the correlation between and  asset(i) and asset(j) at maturity (t) was consistent
% with the implied correlation between the forward price vectors. This was done by adjusting the correlation between the
% short-term factors of commodities at each maturity. The matlab code factors in this adjustment and simulates the 4 commodities
% in this example to show that the theoretical prices, volatilities, and correlations match up with the observed market data.
%
% There is not, to my knowledge, a commodities methodology that incoporates so many market factors across multible commodities
% into one simulation. The advantages of such a model allows for more accurate modeling of spark spreads and pricing of deals
% that are dependent on multiple commodities prices. I have included all files, including excel, associated with this calibration
% and simulation.

clear all; close all

filename = 'SchwartzSmithCalibration.xls';

sim=1000;
str={'Fit 5x16';'Fit 2x16';'Fit 7x8';'Fit Fuel'};
iter=length(str);

for i=1:iter
data{i} = xlsread(filename, str{i}, 'B3:M86');
fwd(:,i)=data{i}(:,1);

kappa(i)=data{i}(1,end);
sigmax(i)=data{i}(3,end);
sigmae(i)=data{i}(4,end);
pxe(i)=data{i}(7,end);
T(:,i)=data{i}(:,3);
x0(i)=data{i}(8,end);
e0(i)=data{i}(9,end);
for j=1:length(T(:,i))
ve(j,i)=sigmae(i)^2*T(j,i);
vx(j,i)=(1-exp(-2*kappa(i)*T(j,i)))*(.5*sigmax(i)^2)/kappa(i);
covxe(j,i)=(1-exp(-kappa(i)*T(j,i)))*pxe(i)*sigmax(i)*sigmae(i)/kappa(i);
end
corxet(:,i)=covxe(:,i)./sqrt(ve(:,i).*vx(:,i));
end

%Compute Correlation matrix from observed forward curves
cmatrix=corrcoef(log(fwd));

%Find correlation structures needed to keep observed correlation structure
%during simulation
rho(:,1:1)=ones(length(T(:,1)),1);
for i=2:iter
for j=1:length(T(:,i))
atop1(j,1)=(1-exp(-(kappa(1)+kappa(i))*T(j,i)))*sigmax(1)*sigmax(i)/(kappa(1)+kappa(i));
atop2(j,1)=(1-exp(-kappa(1)*T(j,i)))*pxe(i)*sigmax(1)*sigmae(i)/kappa(1);
atop3(j,1)=(1-exp(-kappa(i)*T(j,i)))*pxe(1)*sigmax(i)*sigmae(1)/kappa(i);
atop4(j,1)=pxe(1)*pxe(i)*sigmae(1)*sigmae(i)*T(j,i);
abot1(j,1)=sqrt(vx(j,1)+ve(j,1)+2*covxe(j,1));
abot2(j,1)=sqrt(vx(j,i)+ve(j,i)+2*covxe(j,i));
acorxy(j,i)=(atop1(j,1)+atop2(j,1)+atop3(j,1)+atop4(j,1))/(abot1(j,1)*abot2(j,1));
end
rho(:,i)=cmatrix(1,i)./acorxy(:,i);
rho=min(rho,1);
end

iter2=1;
rmatrix0=randn(sim,length(data{1}(:,1)));
for j=1:iter

for k=1:length(data{1}(:,1))
    rmatrix{j}(:,k)=rho(k,j)*rmatrix0(:,k)+sqrt(1-rho(k,j)^2)*randn(sim,1);
end

[tspath,tmpath,tstdpath,tefwd,testdfwd,xpath1,epath1,r1,r2]=schwartzsmithsim(data{j}(22,end),data{j}(1:9,end),data{j}(10:21,end),data{j}(:,3),rmatrix{j},sim);
spath{j}=tspath;
mpath{j}=tmpath;
stdpath{j}=tstdpath;
efwd{j}=tefwd;
estdfwd{j}=testdfwd;
epath{j}=epath1;
xpath{j}=xpath1;
lnpath{j}=epath1+xpath1;
r1path{j}=r1;
r2path{j}=r2;

subplot(length(str),2,iter2)
title('Market Fwd (blue) vs Sim Fwd (red)')
hold on
plot(fwd(:,j))
plot(mpath{j},'r')

subplot(length(str),2,iter2+1)
title('Market Vol (blue) vs Sim Vol (red)')
hold on
plot(estdfwd{j})
plot(stdpath{j},'r')
iter2=iter2+2;
end

%Estimate empirical correlation matrix
for j=2:iter
for i=1:length(data{1}(:,1))
c(i)=corr(log(spath{1}(:,i)),log(spath{j}(:,i)));
end
empc(j)=mean(c);
end

MarketCorrelations=cmatrix(1,2:end)
SimCorrelations=empc(2:end)


function points = randomPointInBox3d(box, N, varargin)
%RANDOMPOINTINBOX3D Generate random point(s) within a 3D box.
%
%   PTS = randomPointInBox3d(BOX)
%   Generate a random point within the 3D box BOX. The result is a 1-by-3
%   row vector.
%
%   PTS = randomPointInBox3d(BOX, N)
%   Generates N points within the box. The result is a N-by-3 array.
%
%   BOX has the format:
%   BOX = [XMIN XMAX YMIN YMAX ZMIN ZMAX].
%
%   Example
%     % draw points within a box
%     box = [10 40 20 60 30 50];
%     pts =  randomPointInBox3d(box, 500);
%     figure(1); hold on;
%     drawBox3d(box);
%     drawPoint3d(pts, '.');
%     axis('equal');
%     axis([0 100 0 100 0 100]);
%     view(3);
%
%   See also
%   points3d, boxes3d

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2011-06-27, using Matlab 7.9.0.529 (R2009b)
% Copyright 2011-2022 INRA - Cepia Software Platform

if nargin < 2
    N = 1;
end

% extract box bounds
xmin = box(1);
ymin = box(3);
zmin = box(5);

% compute size of box
dx = box(2) - xmin;
dy = box(4) - ymin;
dz = box(6) - zmin;

% compute point coordinates
points = [rand(N, 1)*dx+xmin , rand(N, 1)*dy+ymin , rand(N, 1)*dz+zmin];


function ng = tetrahedron_grid_count ( n )

%*****************************************************************************80
%
%% TETRAHEDRON_GRID_COUNT counts the grid points inside a tetrahedron.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of subintervals.
%
%    Output, integer NG, the number of grid points.
%
  ng = ( ( n + 1 ) * ( n + 2 ) * ( n + 3 ) ) / 6;

  return
end


function idx = findClosestCentroids(X, centroids)
%FINDCLOSESTCENTROIDS computes the centroid memberships for every example
%   idx = FINDCLOSESTCENTROIDS (X, centroids) returns the closest centroids
%   in idx for a dataset X where each row is a single example. idx = m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set K
K = size(centroids, 1);

% You need to return the following variables correctly.
idx = zeros(size(X,1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every example, find its closest centroid, and store
%               the index inside idx at the appropriate location.
%               Concretely, idx(i) should contain the index of the centroid
%               closest to example i. Hence, it should be a value in the
%               range 1..K
%
% Note: You can use a for-loop over the examples to compute this.
%

m = size(X  , 1);

for i = 1 : m,
	T = [];
	for j = 1 : K,
		T = [T ; X(i,:)];
	end
	[Max , idx(i)] = min(sum((T - centroids).^2 , 2));
end

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

end


function geometry_test0765 ( )

%*****************************************************************************80
%
%% TEST0765 tests POLYGON_AREA_2D_2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2009
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  test_num = 2;
  area_exact_test = [ 2.0, 6.0 ];
  n_test = [ 4, 8 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0765\n' );
  fprintf ( 1, '  For a polygon in 2D:\n' );
  fprintf ( 1, '  POLYGON_AREA_2D_2 computes the area.\n' );

  for test = 1 : 2

    n = n_test(test);
    area_exact = area_exact_test(test);

    if ( test == 1 )

      v = [ ...
        1.0, 0.0; ...
        2.0, 1.0; ...
        1.0, 2.0; ...
        0.0, 1.0 ]';

    elseif ( test == 2 )

      v = [ ...
        0.0, 0.0; ...
        3.0, 0.0; ...
        3.0, 3.0; ...
        2.0, 3.0; ...
        2.0, 1.0; ...
        1.0, 1.0; ...
        1.0, 2.0; ...
        0.0, 2.0 ]';

    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Number of polygonal vertices = %d\n', n );

    r8mat_transpose_print ( dim_num, n, v, '  The polygon vertices:' );

    area = polygon_area_2d_2 ( n, v );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Exact area is        %f\n', area_exact );
    fprintf ( 1, '  The computed area is %f\n', area );

  end

  return
end


function [cost grad] = svmCost(w, X, y, lambda)
% cost = HingeLoss^2 + lambda*||w||^2
% 1 2 3 4 5 step
yp = X*w;
idx = find(yp.*y<1);
err = yp(idx)-y(idx);
cost = err'*err + lambda*w'*w;
grad = 2*X(idx,:)'*err + 2*lambda*w;
end


function cub = CubatureVolumeMesh2D(CubatureOrder)

% function cub = CubatureVolumeMesh2D(CubatureOrder)
% purpose: build cubature nodes, weights and geometric factors for all elements

Globals2D;

% set up cubature nodes
[cub.R,cub.S,cub.W, cub.Ncub] = Cubature2D(CubatureOrder);

% evaluate generalized Vandermonde of Lagrange interpolant functions at cubature nodes
cub.V  = InterpMatrix2D(cub.R, cub.S);

% evaluate local derivatives of Lagrange interpolants at cubature nodes
[cub.Dr,cub.Ds] = Dmatrices2D(N,cub.R,cub.S,V);

% evaluate the geometric factors at the cubature nodes
[cub.rx,cub.sx,cub.ry,cub.sy,cub.J] = GeometricFactors2D(x,y, cub.Dr,cub.Ds);

% custom mass matrix per element
cub.mmCHOL = zeros(Np, Np, K); cub.mm = zeros(Np, Np, K);
for k=1:K
  cub.mm(:,:,k)     = cub.V'*diag(cub.J(:,k).*cub.W(:))*cub.V;
  cub.mmCHOL(:,:,k) = chol(cub.mm(:,:,k));
end

% incorporate weights and Jacobian
cub.w = cub.W; cub.W = cub.W*ones(1,K); cub.W = cub.W.*cub.J;

% compute coordinates of cubature nodes
cub.x = cub.V*x; cub.y = cub.V*y;
return


function [lambda_vec, error_train, error_val] = ...
    validationCurve(X, y, Xval, yval)
%VALIDATIONCURVE Generate the train and validation errors needed to
%plot a validation curve that we can use to select lambda
%   [lambda_vec, error_train, error_val] = ...
%       VALIDATIONCURVE(X, y, Xval, yval) returns the train
%       and validation errors (in error_train, error_val)
%       for different values of lambda. You are given the training set (X,
%       y) and validation set (Xval, yval).
%

% Selected values of lambda (you should not change this)
lambda_vec = [0 0.001 0.003 0.01 0.03 0.1 0.3 1 3 10]';

% You need to return these variables correctly.
error_train = zeros(length(lambda_vec), 1);
error_val = zeros(length(lambda_vec), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return training errors in
%               error_train and the validation errors in error_val. The
%               vector lambda_vec contains the different lambda parameters
%               to use for each calculation of the errors, i.e,
%               error_train(i), and error_val(i) should give
%               you the errors obtained after training with
%               lambda = lambda_vec(i)
%
% Note: You can loop over lambda_vec with the following:
%
%       for i = 1:length(lambda_vec)
%           lambda = lambda_vec(i);
%           % Compute train / val errors when training linear
%           % regression with regularization parameter lambda
%           % You should store the result in error_train(i)
%           % and error_val(i)
%           ....
%
%       end
%
%

for i = 1:length(lambda_vec)
    lambda = lambda_vec(i);
    theta = trainLinearReg(X, y, lambda);
    error_train(i) = linearRegCostFunction(X, y, theta, 0);
    error_val(i) = linearRegCostFunction(Xval, yval, theta, 0);
end

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

end


function center = triangle_incenter_2d ( t )

%*****************************************************************************80
%
%% TRIANGLE_INCENTER_2D computes the incenter of a triangle in 2D.
%
%  Discussion:
%
%    The incenter of a triangle is the center of the inscribed circle.
%
%    The inscribed circle of a triangle is the largest circle that can
%    be drawn inside the triangle.
%
%    The inscribed circle is tangent to all three sides of the triangle.
%
%    The angle bisectors of the triangle intersect at the center of the
%    inscribed circle.
%
%    In geometry, the incenter is often represented by "I".
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Adrian Bowyer and John Woodwark,
%    A Programmer's Geometry,
%    Butterworths, 1983.
%
%  Parameters:
%
%    Input, real T(2,3), the triangle vertices.
%
%    Output, real CENTER(2,1), the incenter.
%
  dim_num = 2;
%
%  Compute the length of each side.
%
  a = sqrt ( sum ( ( t(1:dim_num,1) - t(1:dim_num,2) ).^2 ) );
  b = sqrt ( sum ( ( t(1:dim_num,2) - t(1:dim_num,3) ).^2 ) );
  c = sqrt ( sum ( ( t(1:dim_num,3) - t(1:dim_num,1) ).^2 ) );

  perimeter = a + b + c;

  if ( perimeter == 0.0 )
    center(1:dim_num,1) = t(1:dim_num,1);
  else
    center(1:dim_num,1) = ( b * t(1:dim_num,1) ...
                          + c * t(1:dim_num,2) ...
                          + a * t(1:dim_num,3) ) / perimeter;
  end

  return
end


function A = bezout(p,q)
%BEZOUT       Bezout matrix of two univariate polynomials p and q
%
%   A = bezout(p,q)
%
%Both polynomials must be univariate with the same independent variable.
%The (symmetric) Bezout matrix is singular iff p and q have a root in common.
%For p or q being interval polynomials, A is an interval matrix.
%The call
%
%   A = bezout(p)
%
%is the same as   A = bezout(p,p') .
%The definition follows Fiedler, Special matrices and their applications.
%

% written  02/03/04     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if nargin==1
    q = p';
  end

  if ( size(p.v)>1 ) | ( size(q.v)>1 )
    error('both polynomials must be univariate')
  end
  if p.v~=q.v
    error('both polynomials must depend on the same variable')
  end

  np = p.e;
  nq = q.e;
  n = max(np,nq);

  A = toeplitz([p.c(np+1) zeros(1,n-1)],[fliplr(p.c) zeros(1,2*n-np-1)]);
  B = toeplitz([q.c(nq+1) zeros(1,n-1)],[fliplr(q.c) zeros(1,2*n-nq-1)]);
  A = flipud( A(:,n+1:end)*B(:,1:n) - B(:,n+1:end)*A(:,1:n) );

  if rndold
    setround(rndold)
  end


function crf = tapas_physio_crf(t)
% cardiac response function, as described in
%
% Chang, Catie, and Gary H. Glover. �Effects of Model-based Physiological
% Noise Correction on Default Mode Network Anti-correlations and Correlations.�
% NeuroImage 47, no. 4 (October 1, 2009): 1448�1459. doi:10.1016/j.neuroimage.2009.05.012.
%
% following:
% Chang, C., Cunningham, J.P., Glover, G.H., 2009. Influence of heart rate
% on the BOLD signal: the cardiac response function. Neuroimage 44, 857�869.%
%
%   crf = tapas_physio_crf(t)
%
% IN
%   t       vector of timepoints
% OUT
%   crf     cardiac response function at sampled time points
%
% EXAMPLE
%   % just for visualization
%   t = 0:0.1:100;
%   crf = tapas_physio_crf(t);
%   figure;plot(t,crf);
%
%   See also tapas_physio_rrf

% Author: Lars Kasper
% Created: 2013-07-26
% Copyright (C) 2013 TNU, Institute for Biomedical Engineering, University of Zurich and ETH Zurich.
%
% This file is part of the physIO toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

crf = 0.6*t.^2.7.*exp(-t/1.6) - 16/sqrt(2*pi*9).*exp(-1/2.*(t-12).^2/9);


function [lambda_vec, error_train, error_val] = ...
    validationCurve(X, y, Xval, yval)
%VALIDATIONCURVE Generate the train and validation errors needed to
%plot a validation curve that we can use to select lambda
%   [lambda_vec, error_train, error_val] = ...
%       VALIDATIONCURVE(X, y, Xval, yval) returns the train
%       and validation errors (in error_train, error_val)
%       for different values of lambda. You are given the training set (X,
%       y) and validation set (Xval, yval).
%

% Selected values of lambda (you should not change this)
lambda_vec = [0 0.001 0.003 0.01 0.03 0.1 0.3 1 3 10]';

% You need to return these variables correctly.
error_train = zeros(length(lambda_vec), 1);
error_val = zeros(length(lambda_vec), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return training errors in
%               error_train and the validation errors in error_val. The
%               vector lambda_vec contains the different lambda parameters
%               to use for each calculation of the errors, i.e,
%               error_train(i), and error_val(i) should give
%               you the errors obtained after training with
%               lambda = lambda_vec(i)
%
% Note: You can loop over lambda_vec with the following:
%
%       for i = 1:length(lambda_vec)
%           lambda = lambda_vec(i);
%           % Compute train / val errors when training linear
%           % regression with regularization parameter lambda
%           % You should store the result in error_train(i)
%           % and error_val(i)
%           ....
%
%       end
%
%

for i = 1:length(lambda_vec),
    lambda = lambda_vec(i);
    theta = trainLinearReg(X, y, lambda);
    error_train(i) = linearRegCostFunction(X, y, theta, 0);
    error_val(i) = linearRegCostFunction(Xval, yval, theta, 0);
end;

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

end


function R = rotx(alpha)

    % ROTX computes a rotation along the x axis (rotation matrix).
    %
    % FORMAT: R = rotx(alpha)
    %
    % INPUT:  - alpha = angle in radians
    %
    % OUTPUT: - R = [3 * 3] rotation matrix along x axis
    %
    % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava
    %
    %          all authors are with the Italian Istitute of Technology (IIT)
    %          email: name.surname@iit.it
    %
    % Genoa, Dec 2017
    %

    %% --- Initialization ---

   R =  [1, 0, 0;
         0, cos(alpha), -sin(alpha);
         0, sin(alpha), cos(alpha)];

end


function [newnodes, len]=polylinesimplify(nodes, minangle)
%
% [newnodes, len]=polylinesimplify(nodes, minangle)
%
% Calculate a simplified polyline by removing nodes where two adjacent
% segment have an angle less than a specified limit
%
% author: Qianqian Fang (q.fang at neu.edu)
%
% input:
%    node: an N x 3 array defining each vertex of the polyline in
%          sequential order
%    minangle:(optional) minimum segment angle in radian, if not given, use
%          0.75*pi
%
% output:
%    newnodes: the updated node list; start/end will not be removed
%    len: the length of each segment between the start and the end points
%
%
% -- this function is part of brain2mesh toolbox (http://mcx.space/brain2mesh)
%    License: GPL v3 or later, see LICENSE.txt for details
%

if(nargin<2)
    minangle=0.75*pi;
end

v=segvec(nodes(1:end-1,:), nodes(2:end,:));
ang=acos(max(min(sum(-v(1:end-1,:).*(v(2:end,:)),2),1),-1));

newnodes=nodes;
newv=v;
newang=ang;

idx=find(newang<minangle);

while(~isempty(idx))
    newnodes(idx+1,:)=[];
    newv(idx+1,:)=[];
    newang(idx)=[];
    idx=unique(idx-(0:(length(idx)-1))');
    idx1=idx(idx<size(newnodes,1));
    newv(idx1,:)  =segvec(newnodes(idx1,:),newnodes(idx1+1,:));
    idx1=idx(idx<size(newv,1));
    newang(idx1)  =acos(sum(-newv(idx1,:).*(newv(idx1+1,:)),2));
    idx0=idx(idx>1);
    newang(idx0-1)=acos(sum(-newv(idx0-1,:).*(newv(idx0,:)),2));
    idx=find(newang<minangle);
end

if(nargout>1)
    len=newnodes(1:end-1,:) - newnodes(2:end,:);
    len=sqrt(sum(len.*len,2));
end

function v=segvec(n1, n2)

v=n2-n1;
normals=sqrt(sum(v.*v,2));
v=v./repmat(normals,1,size(v,2));


function [ x, seed ] = sphere_unit_sample_3d_2 ( seed )

%*****************************************************************************80
%
%% SPHERE_UNIT_SAMPLE_3D_2 is a BAD method for sampling the unit sphere in 3D.
%
%  Discussion:
%
%    The unit sphere in 3D satisfies:
%
%      X * X + Y * Y + Z * Z = 1
%
%    Points on the unit sphere have coordinates ( PHI, THETA ) where
%    PHI varies from 0 to PI, and THETA from 0 to 2 PI, so that:
%
%    x = cos ( theta ) * sin ( phi )
%    y = sin ( theta ) * sin ( phi )
%    z =                 cos ( phi )
%
%    This routine implements a sampling of the sphere that simply
%    picks PHI and THETA uniformly at random from their ranges.
%    This is a uniform sampling on the cylinder, but it is NOT
%    a uniform sampling on the sphere.  I implement it here just
%    so I can run some tests against the code in SPHERE_UNIT_SAMPLE_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X(3), the sample point.
%
%    Output, integer SEED, a seed for the random number generator.
%
  dim_num = 3;

  [ phi, seed ] = r8_uniform_01 ( seed );
  phi = pi * phi;

  [ theta, seed ] = r8_uniform_01 ( seed );
  theta = 2.0 * pi * theta;

  x(1) = cos ( theta ) * sin ( phi );
  x(2) = sin ( theta ) * sin ( phi );
  x(3) = cos ( phi );

  return
end


function Ic = adjacency2incidence(A)

% adjacency2incidence - convert an adjacency matrix to an incidence matrix
%
%   Ic = adjacency2incidence(A);
%
%   A(i,j) = 1 iff (i,j) is an edge of the graph.
%   For each edge number k of the graph linking (i,j)
%       Ic(i,k)=1 and Ic(j,k)=-1
%
%   Ic is a sparse matrix.
%   Ic is also known as the graph gradient.
%
%   Copyright (c) 2006 Gabriel Peyre

%% compute list of edges
[i,j,s] = find(sparse(A));
I = find(i<=j);
i = i(I);
j = j(I);
% number of edges
n = length(i);
% number of vertices
nverts = size(A,1);

%% build sparse matrix
s = [ones(n,1); -ones(n,1)];
is = [(1:n)'; (1:n)'];
js = [i(:); j(:)];
Ic = sparse(is,js,s,n,nverts);
Ic = Ic';

% fix self-linking problem (0)
a = find(i==j);
if not(isempty(a))
    for t=a'
        Ic(i(t),t) = 1;
    end
end


%% Example 4.5: Solution of the Ornstein–Uhlenbeck process
%
% Copyright:
%   2018 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

%% Simulate trajectories from the OU process

  if exist('rng') % Octave doesn't have rng
      rng(10,'twister')
  else
      randn('state',1)
  end

  lambda = 0.5;
  q = 1;
  dt = 0.01;
  T = (0:dt:1);
  x0 = 4;
  M = exp(-lambda*T)*x0;
  P = q/(2*lambda)*(1 - exp(-2*lambda*T));

  XX = zeros(50,length(T));
  for n=1:size(XX,1)
    x = x0;
    for k=1:length(T)
      XX(n,k) = x;
      x = x - lambda * x * dt + sqrt(dt)*randn;
    end
  end

  figure(1); clf; hold on

    % Shade the 95% quantiles
    fill([T fliplr(T)],[M+1.96*sqrt(P) fliplr(M-1.96*sqrt(P))],1, ...
      'FaceColor',[.9 .9 .9],'EdgeColor',[.9 .9 .9])

    % Plot realizations
    h1 = plot(T,XX,'-','Color',[.5 .5 .5],'LineWidth',0.5);

    % Plot mean and quantiles
    h2 = plot(T,M,'k-','LineWidth',1);
    h34 = plot(T,M+1.96*sqrt(P),'--k', ...
               T,M-1.96*sqrt(P),'--k','LineWidth',0.7);

    %set(h(1:3),'Linewidth',2)

    legend([h2(1) h34(1) h1(1)],'Mean','95\% quantiles','Realizations')
    xlabel('Time, $t$'); ylabel('$x(t)$')

    ylim([0 5])
    box on
    set(gca,'Layer','Top')


close all; clear all; clc;
rng('default');
resize_images = true;

target.psnr = 35;
% target mean square error
target.mse = 255^2 * 10^(-target.psnr/10);
image_width = 42;
image_height = 48;
num_pixels = image_width * image_height;
% target sum of squared errors
target.sse = num_pixels * target.mse;
% target residual norm
target.rnorm = sqrt(target.sse);
fprintf('Target PSNR: %.2f dB\n', target.psnr);
fprintf('Target Mean Square Error: %.2f\n', target.mse);
fprintf('Target Root Mean Square Error: %.2f\n', sqrt(target.mse));
fprintf('Target Residual Norm: %.2f\n', target.rnorm);

N =  num_pixels;
redundancy_factor = 8;
D = redundancy_factor * N;

A = spx.dict.simple.gaussian_mtx(N, D);
dc_atom = (1/sqrt(N)) * ones(N, 1);
% throw away one original atom and replace with DC.
A = [dc_atom A(:, 1:D-1)];
dictionary = spx.dict.MatrixOperator(A);

yf = spx.data.image.YaleFaces();
yf.load();
fprintf('\n Resizing images:\n');
tstart = tic;
yf.resize_all(42, 48);
images = yf.ImageData;
[~, S] = size(images);
%images = yf.get_subject_images_resized(1, 42, 48);
elapsed = toc(tstart);
fprintf('Time taken: %.2f seconds \n', elapsed);
representations = zeros(D, S);
elapsed_times = zeros(1, S);
support_sizes = zeros(1, S);
res_norms = zeros(1, S);
psnrs = zeros(1, S);
fprintf('Total images: %d\n', S);
for s=1:S
    Y = images(:, s);
    solver = spx.pursuit.single.OrthogonalMatchingPursuit(dictionary);
    solver.MaxResNorm = target.rnorm;
    solver.Verbose = true;
    fprintf('sparsifying image: %d\n', s);
    tstart = tic;
    result = solver.solve_qr(Y);
    elapsed = toc(tstart);
    fprintf('Time taken: %.2f seconds \n', elapsed);
    fprintf('Support size: %d\n', result.iterations);
    support_sizes(s) = result.iterations;
    elapsed_times(s) = elapsed;
    representations(:, s) = result.z;
    res_norms(s) = result.rnorm;
    sse = result.rnorm^2;
    mse =  sse / num_pixels;
    psnr = 10 * log10(255^2 / mse);
    psnrs(s) = psnr;
    fprintf('PSNR : %.3f dB\n', psnr);
    if mod(s, 100) == 0
        % We wish to save intermediate data too
        save('omp_representations', 'representations');
        save('omp_stats', 'elapsed_times', 'support_sizes', 'res_norms', 'psnrs', 's');
    end
end
% final saving of all data.
save('bin/omp_dictionary', 'dictionary', 'N', 'D');
save('bin/omp_images', 'images');
save('bin/omp_representations', 'representations');
save('bin/omp_stats', 'elapsed_times', 'support_sizes', 'res_norms', 'psnrs', 'S');


function [x, funVal, ValueL]=LeastC(A, y, z, opts)
%
%%
% Function LeastC
%      Least Squares Loss with the L1 ball constraint (also known as Lasso)
%
%% Problem
%
%  min  1/2 || A x - y||^2 + 1/2 rsL2 * ||x||_2^2
%  s.t. ||x||_1 <= z
%
%  By default, rsL2=0.
%  When rsL2 is nonzero, this correspons the well-know elastic net.
%
%% Input parameters:
%
%  A-         Matrix of size m x n
%                A can be a dense matrix
%                         a sparse matrix
%                         or a DCT matrix
%  y -        Response vector (of size mx1)
%  z -        Radius of the L1 ball (z >0)
%  opts-      Optional inputs (default value: opts=[])
%
%% Output parameters:
%  x-         Solution
%  funVal-    Function value during iterations
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified on February 18, 2010.
%
%% Related papers
%
% [1]  Jun Liu and Jieping Ye, Efficient Euclidean Projections
%      in Linear Time, ICML 2009.
%
% [2]  Jun Liu and Jieping Ye, Sparse Learning with Efficient Euclidean
%      Projections onto the L1 Ball, Technical Report ASU, 2008.
%
%% Related functions
%
%  sll_opts, initFactor, pathSolutionLeast,
%  LeastR, nnLeastR, nnLeastC,
%  eplb
%
%%

%% Verify and initialize the parameters
%%

% Verify the number of input parameters
if (nargin <3)
    error('\n Inputs: A, y and z should be specified!\n');
elseif (nargin==3)
    opts=[];
end

% Get the size of the matrix A
[m,n]=size(A);

% Verify the length of y
if (length(y) ~=m)
    error('\n Check the length of y!\n');
end

% Verify the value of z
if (z<=0)
    error('\n z should be positive!\n');
end

% run sll_opts to set default values (flags)
opts=sll_opts(opts);

%% Detailed initialization
%% Normalization

% Please refer to sll_opts for the definitions of mu, nu and nFlag
%
% If .nFlag =1, the input matrix A is normalized to
%                     A= ( A- repmat(mu, m,1) ) * diag(nu)^{-1}
%
% If .nFlag =2, the input matrix A is normalized to
%                     A= diag(nu)^{-1} * ( A- repmat(mu, m,1) )
%
% Such normalization is done implicitly
%     This implicit normalization is suggested for the sparse matrix
%                                    but not for the dense matrix
%

if (opts.nFlag~=0)
    if (isfield(opts,'mu'))
        mu=opts.mu;
        if(size(mu,2)~=n)
            error('\n Check the input .mu');
        end
    else
        mu=mean(A,1);
    end

    if (opts.nFlag==1)
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=n)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,1)/m).^(0.5); nu=nu';
        end
    else % .nFlag=2
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=m)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,2)/n).^(0.5);
        end
    end

    ind_zero=find(abs(nu)<= 1e-10);    nu(ind_zero)=1;
    % If some values in nu is typically small, it might be that,
    % the entries in a given row or column in A are all close to zero.
    % For numerical stability, we set the corresponding value to 1.
end

if (~issparse(A)) && (opts.nFlag~=0)
    fprintf('\n -----------------------------------------------------');
    fprintf('\n The data is not sparse or not stored in sparse format');
    fprintf('\n The code still works.');
    fprintf('\n But we suggest you to normalize the data directly,');
    fprintf('\n for achieving better efficiency.');
    fprintf('\n -----------------------------------------------------');
end

%% Starting point initialization

% compute AT y
if (opts.nFlag==0)
    ATy=A'*y;
elseif (opts.nFlag==1)
    ATy=A'*y - sum(y) * mu';  ATy=ATy./nu;
else
    invNu=y./nu;              ATy=A'*invNu-sum(invNu)*mu';
end

% L2 norm regularization
if isfield(opts,'rsL2')
    rsL2=opts.rsL2;
    if (rsL2<0)
        error('\n opts.rsL2 should be nonnegative!');
    end
else
    rsL2=0;
end

% initialize a starting point
if opts.init==2
    x=zeros(n,1);
else
    if isfield(opts,'x0')
        x=opts.x0;
        if (length(x)~=n)
            error('\n Check the input .x0');
        end
    else
        x=ATy;  % if .x0 is not specified, we use ratio*ATy,
        % where ratio is a positive value
    end
end

% compute A x
if (opts.nFlag==0)
    Ax=A* x;
elseif (opts.nFlag==1)
    invNu=x./nu; mu_invNu=mu * invNu;
    Ax=A*invNu -repmat(mu_invNu, m, 1);
else
    Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
end

if (opts.init==0) % If .init=0, we set x=ratio*x by "initFactor"
    % Please refer to the function initFactor for detail

    x_norm=sum(abs(x)); % L1 norm of x
    x_2norm=x'*x;       % squared two norm of x
    if x_norm>=1e-6
        ratio=initFactor(x_norm, Ax, y, z,'LeastC', rsL2, x_2norm);
        x=ratio*x;    Ax=ratio*Ax;
    end
end

%% The main program

bFlag=0; % this flag tests whether the gradient step only changes a little

%% The Armijo Goldstein line search schemes
if (opts.lFlag==0)

    L=1 + rsL2;
    % We assume that the maximum eigenvalue of A'A is over 1

    % assign xp with x, and Axp with Ax.
    % xxp=x - xp
    xp=x; Axp=Ax; xxp=zeros(n,1);

    % alphap and alpha are used for computing the weight in forming search point
    alphap=0; alpha=1;

    % lambda0 is a guess of the root in the Euclidean projection
    lambda0=0;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp;

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute the gradient (g) at s
        As=Ax + beta* (Ax-Axp);

        % compute AT As
        if (opts.nFlag==0)
            ATAs=A'*As;
        elseif (opts.nFlag==1)
            ATAs=A'*As - sum(As) * mu';  ATAs=ATAs./nu;
        else
            invNu=As./nu;                ATAs=A'*invNu-sum(invNu)*mu';
        end

        % obtain the gradient g
        g=ATAs-ATy + rsL2 * s;

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and project v onto the L1 ball
            v=s-g/L;

            % projection
            [x, lambda, zf_step]=eplb(v, n, z, lambda0);
            lambda0=lambda;

            v=x-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute A x
            if (opts.nFlag==0)
                Ax=A* x;
            elseif (opts.nFlag==1)
                invNu=x./nu; mu_invNu=mu * invNu;
                Ax=A*invNu -repmat(mu_invNu, m, 1);
            else
                Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./nu;
            end

            Av=Ax -As;
            r_sum=v'*v; l_sum=Av'*Av;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is ||Av||_2^2 <= (L - rsL2) * ||v||_2^2
            if(l_sum <= r_sum * (L-rsL2))
                break;
            else
                L=max(2*L, l_sum/r_sum + rsL2);
                % fprintf('\n L=%5.6f',L);
            end
        end

        ValueL(iterStep)=L;

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        xxp=x-xp;   Axy=Ax-y;
        funVal(iterStep)=Axy' * Axy/2 + rsL2/2 * x'*x;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp);    norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end % end of .lFlag=0
end

%% Adaptive Line Search

% we set gamma_0 to the L that is appropriate for the starting point
% opts.x0

if (opts.lFlag==1)

    L=1 + rsL2;
    % We assume that the maximum eigenvalue of A'A is over 1

    lambda0=0;
    % lambda0 is a guess of the root in the Euclidean projection

    gamma=1;
    % we shall set the value of gamma = L,
    % and L is appropriate for the starting point

    xp=x; Axp=Ax;
    % store x and Ax
    xxp=zeros(n,1);
    % the difference of x and xp

    % compute AT Ax
    if (opts.nFlag==0)
        ATAx=A'*Ax;
    elseif (opts.nFlag==1)
        ATAx=A'*Ax - sum(Ax) * mu';  ATAx=ATAx./nu;
    else
        invNu=Ax./nu;                ATAx=A'*invNu-sum(invNu)*mu';
    end

    % We begin the adaptive line search in the following
    %
    % Note that, in the line search, L and beta are changing

    for iterStep=1:opts.maxIter

        ATAxp=ATAx;
        % store ATAx to ATAxp

        if (iterStep~=1)
            % compute AT Ax
            if (opts.nFlag==0)
                ATAx=A'*Ax;
            elseif (opts.nFlag==1)
                ATAx=A'*Ax - sum(Ax) * mu';  ATAx=ATAx./nu;
            else
                invNu=Ax./nu;                ATAx=A'*invNu-sum(invNu)*mu';
            end
        end

        %--------- Line Search for L begins
        while (1)
            if (iterStep~=1)
                alpha= (-gamma+ sqrt(gamma*gamma + 4* L * gamma)) / (2*L);
                beta= (gamma - gamma* alphap) / (alphap * gamma + alphap* L * alpha);
                % beta is the coefficient for generating search point s

                s=x + beta* xxp;
                As=Ax + beta* (Ax-Axp);
                ATAs=ATAx + beta* (ATAx-ATAxp);
                % compute the search point s, A * s, and A' * A * s
            else
                alpha= (-1+ sqrt(5)) / 2;
                beta=0; s=x;  As=Ax; ATAs=ATAx;
            end

            g=ATAs-ATy + rsL2 * s;
            % compute the gradient g

            v=s-g/L;
            % a gradient step based on the search point s

            % projection
            [xnew, lambda, zf_step]=eplb(v, n, z, lambda0);
            lambda0=lambda;

            v=xnew-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute A xnew
            if (opts.nFlag==0)
                Axnew=A* xnew;
            elseif (opts.nFlag==1)
                invNu=xnew./nu; mu_invNu=mu * invNu;
                Axnew=A*invNu -repmat(mu_invNu, m, 1);
            else
                Axnew=A*xnew-repmat(mu*xnew, m, 1);     Axnew=Axnew./nu;
            end

            Av=Axnew -As;
            r_sum=v'*v; l_sum=Av'*Av + v'*v * rsL2;

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is ||Av||_2^2  + rsL2 * ||v||_2^2<= L* ||v||_2^2
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end
        %--------- Line Search for L ends

        gamma=L* alpha* alpha;    alphap=alpha;
        % update gamma, and alphap

        ValueL(iterStep)=L;
        % store values for L

        tao=L * r_sum / l_sum;
        if (tao >=5)
            L=L*0.8;
        end
        % decrease the value of L

        xp=x;   x=xnew;  xxp=x-xp;
        Axp=Ax; Ax=Axnew;
        % update x and Ax with xnew and Axnew

        Axy=Ax-y;
        funVal(iterStep)=Axy' * Axy/2 + rsL2/2 * x'*x;
        % compute function value

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=sqrt(xp'*xp);    norm_xxp=sqrt(xxp'*xxp);
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end