function [V,gradV,HessianV]=spherHarmonicSetEval(C,S,point,a,c,systemType,spherDerivs,scalFactor,algorithm)
%%SPHERHARMONICSETEVAL Evaluate a set of real or complex potentials,
%                   gradients and Hessians when the potentials are
%                   expressed in terms of a set of real or complex
%                   spherical harmonic coefficients. This function is very
%                   similar to the spherHarmonicEval function, but it
%                   allows C and S to be matrices, where each column
%                   contains a set of spherical harmonic coefficients. The
%                   evaluation of multiple sets at once is faster than
%                   making multiple calls to spherHarmonicEval. The
%                   ordering of the elements of the outputs of this
%                   function is different than that used in
%                   spherHarmonicEval.
%
%INPUTS: C An ((M+2)*(M+1)/2)XnumSets matrix holding numSets real or
%          complex coefficient terms that are multiplied by cosines in the
%          harmonic expansion. The coefficients must be fully normalized
%          using the type of full normalization that is used in the EGM2008
%          model. Their normalization type can be changed using the
%          changeSpherHarmonicNorm function If given to a
%          CountingClusterSet class, C(n+1,m+1) is the 1XnumSets vector of
%          the coefficient of degree n and order m for each set. When a
%          maximum degree of M is used, all C must have values for all n
%          from 0 to M and for all m from 0 to n for each n. If
%          coefficients are not present for certain degrees, then insert a
%          0. It is assumed that M>=3.
%        S An ((M+2)*(M+1)/2)XnumSets matrix holding numSets sets of
%          coefficient terms that are multiplied by sines in the harmonic
%          expansion. The requirements on S are the same as those on C.
%    point The 3XN set of N real points at which the potential and/or
%          gradient should be evaluated given in SPHERICAL coordinates
%          consisting of [r;azimuth;elevation]; When evaluating points on a
%          grid, the algorithm will be fastest if the points are provided
%          presorted by range and then by azimuth. This reduces the amount
%          of recomputation of certain values. Alternatively, if C and S
%          are for evaluating terrain heights, then points are 2XN having
%          the format [azimuth;elevation] and it is best if the points are
%          sorted by azimuth. Azimuth is measured counterclockwise from the
%          x-axis in the x-y plane. Elevation is measured up from the x-y
%          plane (towards the z-axis). Angles must be given in radians.
%        a The real or complex numerator in the (a/r)^n term in the
%          spherical harmonic sum. Normally, this is some type of a
%          reference radius. For example, when using most gravitational
%          models, a is the semi-major axis of the reference ellipsoid. If
%          this parameter is omitted, it is assumed that one is using the
%          spherical harmonics with something like the National Geospatial
%          Intelligence Agency's (NGA's) EGM96 or EGM2008 models, in which
%          case a=Constants.EGM2008SemiMajorAxis is used unless point is
%          2D, in which case c=1 is used.
%        c The real or complex constant value by which the spherical
%          harmonic series is multiplied. For example, for gravitational
%          potentials, the value is usually GM where G is the universal
%          gravitational constant and  M is the mass of the Earth. When
%          using the International Geomagnetic Reference Field (IGRF), the
%          constant is a^2, where a is the same as the numerator in the a/r
%          term. If this parameter is omitted, it is assumed that one is
%          using the spherical harmonics with something like the NGA's
%          EGM96 or EGM2008 models, in which case c=Constants.EGM2008GM is
%          used unless point is 2D, in which case c=1 is used.
% systemType An optional parameter specifying the axis from which the
%           angles are measured in radians. Possible values are
%           0 (The default if omitted) Azimuth is measured
%             counterclockwise from the x-axis in the x-y plane. Elevation
%             is measured up from the x-y plane (towards the z-axis). This
%             is consistent with common spherical coordinate systems for
%             specifying longitude (azimuth) and geocentric latitude
%             (elevation).
%           2 This is the same as 0 except instead of being given
%             elevation, one is given the angle away from the z-axis, which
%             is (pi/2-elevation).
% spherDerivs This parameter specifies whether the gradient and Hessian
%          terms should be returned in spherical coordinates rather than
%          Cartesian coordinates. Possible values are true and false. The
%          default if this parameter is omitted or an empty matrix is
%          passed is false. The spherical coordinate system used by the
%          gradient and Hessian will match that specified by the systemType
%          input.
% scalFactor An optional real scale factor used in computing the normalized
%          associated Legendre polynomials. Generally, the default value
%          (if the scalFactor parameter is omitted) of 10^(-280) that is
%          suggested in [1] is sufficient. When very high-order models are
%          used, this scale factor prevents overflows.
% algorithm An optional parameter that selects which algorithm to use.
%          Possible values are:
%          0 (The default if omitted or an empty matrix is passed) If only
%            the potential is desired, always use Legendre's algorithm.
%            Otherwise, for points above 88 degrees spherical latitude, use
%            Pines' algorithm and for all other points use Legendre's
%            algorithm.
%          1 Only use Legendre's algorithm regardless of the location of
%            the points. Note that the gradient is singular at the poles,
%            so numerical problems will arise.
%          2 Only use Pines' algorithm.
%
%OUTPUTS: V The numSetsXN scalar potentials as obtained from the spherical
%           harmonic series for each set and point. This is real is all of
%           the inputs are real.
%     gradV The 3XnumSetsXN set of gradients of the potential in Cartesian
%           coordinates for each set and point as obtained using the
%           spherical harmonic series. The derivatives are in the order
%           [dV/dx;dV/dy;dV/dz] for Cartesian values and in the order
%           [dV/dr;dV/dAz;dV/dEl] for spherical values.
%     HessV The 3X3XnumSetsXN collection of Hessian matrices of the
%           potential for each set and point.  If Cartesian derivatives are
%           used, then the ordering is
%           [d2/(dxdx),d2/(dxdy),d2/(dxdz);
%            d2/(dydx),d2/(dydy),d2/(dydz);
%            d2/(dzdx),d2/(dzdy),d2/(dxdx)]; If spherical derivatives are
%            used, then the ordering is the same with (x,y,z) replaced by
%            (r,Az,El).
%
%This function is useful for the evaluation of scalar spherical harmonic
%series with vector coefficients, as can be used for modelling 3D far-field
%antenna response patterns as in [1].
%
%This function is implemented in the same manner as spherHarmonicEval,
%except minor changes to allow C and S to be matrices rather than vector
%have been made. Additionally, for numSet=1, the shape of the matrices
%outputted by this function differs from that outputted by
%spherHarmonicEval.
%
%It is recommended that the C++ version of this function be compiled and
%used in place of this as it is thousands of times faster when C and S are
%very large.
%
%REFERENCES:
%[1] J. Rahola, F. Belloni, and A. Richter, "Modelling of radiation
%    patterns using scalar spherical harmonics with vector coefficients,"
%    in Proceedings of the 3rd European Conference on Antennas and
%    Propagation, Berlin, Germany, 23-27 Mar. 2009, pp. 3361-3365.
%
%December 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<9||isempty(algorithm))
    algorithm=0;
end

if(nargin<8||isempty(scalFactor))
    scalFactor=10^(-280);
end

if(nargin<7||isempty(spherDerivs))
    spherDerivs=false;
end

if(nargin<6||isempty(systemType))
    systemType=0;
end

if(nargin<5||isempty(c))
    if(size(point,1)==2)
        c=1;
    else
        c=Constants.EGM2008GM;
    end
end

if(nargin<4||isempty(a))
    if(size(point,1)==2)
        a=1;
    else
        a=Constants.EGM2008SemiMajorAxis;
    end
end

if(systemType~=0&&systemType~=2)
    error('An unsupported systemType was specified.')
end

M=(1/2)*(sqrt(1+8*length(C))-1)-1;

if(M<3)
    error('The coefficients must be provided to at least degree 3. To use a lower degree, one can insert zero coefficients.');
end

numPoints=size(point,2);
%If we are evaluating terrain heights.
switch(size(point,1))
    case 2
        point=[ones(1,numPoints);point(1,:);point(2,:)];
    case 3
    otherwise
        error('Invalid point length');
end

%Using a CountingClusterSet simplifies the indexation of the coefficients.
C=CountingClusterSet(C);
S=CountingClusterSet(S);
numSets=C.numSets();

%Preallocate space used by the modified forward row algorithm when
%evaluating over multiple values with the same range and latitude but
%different longitudes.
if(algorithm==0||algorithm==1)
    A=zeros(numSets,M+1);
    B=zeros(numSets,M+1);
    if(nargout>1)
        Ar=zeros(numSets,M+1);
        Br=zeros(numSets,M+1);
        ATheta=zeros(numSets,M+1);
        BTheta=zeros(numSets,M+1);

        if(nargout>2)
            AThetaTheta=zeros(numSets,M+1);
            BThetaTheta=zeros(numSets,M+1);

            Arr=zeros(numSets,M+1);
            Brr=zeros(numSets,M+1);

            AThetar=zeros(numSets,M+1);
            BThetar=zeros(numSets,M+1);
        end
    end
end

V=zeros(numSets,numPoints);
gradV=zeros(3,numSets,numPoints);
HessianV=zeros(3,3,numSets,numPoints);

rPrev=Inf;
thetaPrev=Inf;
for curPoint=1:numPoints
    pointCur=point(:,curPoint);

    if(systemType==2)
        pointCur(3)=pi/2-pointCur(3);
    end

    r=pointCur(1);
    lambda=pointCur(2);
    thetaCur=pointCur(3);

    rChanged=r~=rPrev;
    thetaChanged=thetaCur~=thetaPrev;
    rPrev=r;
    thetaPrev=thetaCur;

    if(rChanged)
        crScal=(c/r)/scalFactor;

        %This stores all of the powers of a/r needed for the sum,
        %regardless of which algorithm is used.
        nCoeff=zeros(M+1,1);
        nCoeff(1)=1;
        for n=1:M
            nCoeff(n+1)=nCoeff(n)*(a/r);
        end
    end

    switch(algorithm)
        case 0
        %At latitudes that are not near the poles, the Legendre method is
        %used. It cannot be used for the gradient or Hessian near the
        %poles, because of the singularity of the spherical coordinate
        %system.
            useLegendre=abs(thetaCur)<88*(pi/180)||nargout<2;
        case 1
            useLegendre=true;
        case 2
            useLegendre=false;
        otherwise
            error('Unknown Algorithm option specified.');
    end

    if(useLegendre)
        %Compute the sine and cosine terms.
        [SinVec,CosVec]=calcSinCosTerms(lambda,M);
        %The formulae for spherical harmonic synthesis with Legendre's
        %method uses clolatitude, pi/2-elevation
        theta=pi/2-thetaCur;
        u=sin(theta);
        if(thetaChanged)
            if(nargout==2)
                [PBarUVals,dPBarUValsdTheta]=NALegendreCosRat(theta,M,scalFactor);
            elseif(nargout>2)
                [PBarUVals,dPBarUValsdTheta,d2PBarUValsdTheta2]=NALegendreCosRat(theta,M,scalFactor);
            else
                PBarUVals=NALegendreCosRat(theta,M,scalFactor);
            end
        end

        %Evaluate Equation 7 from the Holmes and Featherstone paper.
        if(rChanged||thetaChanged)
            A(:)=0;
            B(:)=0;
            for m=0:M
                for n=m:M
                    CScal=nCoeff(n+1)*C(n+1,m+1).';
                    SScal=nCoeff(n+1)*S(n+1,m+1).';

                    %From Table 4
                    A(:,m+1)=A(:,m+1)+CScal*PBarUVals(n+1,m+1);
                    B(:,m+1)=B(:,m+1)+SScal*PBarUVals(n+1,m+1);
                end
            end

            %If additional terms should be computed so a gradient can
            %be determined.
            if(nargout>1)
                Ar(:)=0;
                Br(:)=0;
                ATheta(:)=0;
                BTheta(:)=0;
                for m=0:M
                    for n=m:M
                        CScal=nCoeff(n+1)*C(n+1,m+1).';
                        SScal=nCoeff(n+1)*S(n+1,m+1).';

                        %From Table 4
                        Ar(:,m+1)=Ar(:,m+1)+(n+1)*CScal*PBarUVals(n+1,m+1);
                        Br(:,m+1)=Br(:,m+1)+(n+1)*SScal*PBarUVals(n+1,m+1);

                        %From Table 4
                        ATheta(:,m+1)=ATheta(:,m+1)+CScal*dPBarUValsdTheta(n+1,m+1);
                        BTheta(:,m+1)=BTheta(:,m+1)+SScal*dPBarUValsdTheta(n+1,m+1);
                    end
                end

                %If additional terms should be computed so a Hessian can
                %be determined.
                if(nargout>2)
                    Arr(:)=0;
                    Brr(:)=0;
                    AThetar(:)=0;
                    BThetar(:)=0;
                    AThetaTheta(:)=0;
                    BThetaTheta(:)=0;
                    for m=0:M
                        for n=m:M
                            CScal=nCoeff(n+1)*C(n+1,m+1).';
                            SScal=nCoeff(n+1)*S(n+1,m+1).';

                            %From Table 5, with the correction from the
                            %erratum.
                            Arr(:,m+1)=Arr(:,m+1)+(n+1)*(n+2)*CScal*PBarUVals(n+1,m+1);
                            Brr(:,m+1)=Brr(:,m+1)+(n+1)*(n+2)*SScal*PBarUVals(n+1,m+1);

                            %From Table 5
                            AThetar(:,m+1)=AThetar(:,m+1)+(n+1)*CScal*dPBarUValsdTheta(n+1,m+1);
                            BThetar(:,m+1)=BThetar(:,m+1)+(n+1)*SScal*dPBarUValsdTheta(n+1,m+1);

                            %From Table 5
                            AThetaTheta(:,m+1)=AThetaTheta(:,m+1)+CScal*d2PBarUValsdTheta2(n+1,m+1);
                            BThetaTheta(:,m+1)=BThetaTheta(:,m+1)+SScal*d2PBarUValsdTheta2(n+1,m+1);
                        end
                    end
                end
            end
        end

        %Use Horner's method to compute V all values.
        V(:,curPoint)=0;
        for m=M:-1:0
            V(:,curPoint)=V(:,curPoint)*u+(A(:,m+1)*CosVec(m+1)+B(:,m+1)*SinVec(m+1));
        end

        V(:,curPoint)=crScal*V(:,curPoint);

        if(nargout>1)
            %Use Horner's method to compute all dV values.
            dVdr=zeros(numSets,1);
            dVdLambda=zeros(numSets,1);
            dVdTheta=zeros(numSets,1);

            %The following first-order derivative formulae are from
            %Table 1 (expressed using Horner's method).
            for m=M:-1:0
                dVdr=dVdr*u-(Ar(:,m+1)*CosVec(m+1)+Br(:,m+1)*SinVec(m+1));
                dVdLambda=dVdLambda*u-m*(A(:,m+1)*SinVec(m+1)-B(:,m+1)*CosVec(m+1));
                dVdTheta=dVdTheta*u+(ATheta(:,m+1)*CosVec(m+1)+BTheta(:,m+1)*SinVec(m+1));
            end

            dVdr=crScal*dVdr.'/r;
            dVdLambda=crScal*dVdLambda.';
            %The minus sign adjusts for the coordinate system change.
            dVdTheta=-crScal*dVdTheta.';

            if(spherDerivs)
                %The sign change on the theta terms deals with the
                %different input coordinate system used.
                gradV(:,:,curPoint)=[dVdr;dVdLambda;dVdTheta];
            else%Convert the derivatives to Cartesian coordinates.
                J=calcSpherConvJacob(pointCur);

                for curSet=1:numSets
                    gradV(:,curSet,curPoint)=J'*[dVdr(curSet);dVdLambda(curSet);dVdTheta(curSet)];
                end
            end
        end

        if(nargout>2)
            %Use Horner's method to compute d2V all values.
            d2VdLambdadLambda=zeros(numSets,1);
            d2VdLambdadTheta=zeros(numSets,1);
            d2VdrdLambda=zeros(numSets,1);
            d2VdThetadTheta=zeros(numSets,1);
            d2VdrdTheta=zeros(numSets,1);
            d2Vdrdr=zeros(numSets,1);

            %The following second-order derivative formulae are from
            %Table 2 (expressed using Horner's method).
            for m=M:-1:0
                d2VdLambdadLambda=d2VdLambdadLambda*u-m^2*(A(:,m+1)*CosVec(m+1)+B(:,m+1)*SinVec(m+1));
                d2VdLambdadTheta=d2VdLambdadTheta*u+m*(ATheta(:,m+1)*SinVec(m+1)-BTheta(:,m+1)*CosVec(m+1));
                d2VdrdLambda=d2VdrdLambda*u+m*(Ar(:,m+1)*SinVec(m+1)-Br(:,m+1)*CosVec(m+1));
                d2VdThetadTheta=d2VdThetadTheta*u+(AThetaTheta(:,m+1)*CosVec(m+1)+BThetaTheta(:,m+1)*SinVec(m+1));
                d2VdrdTheta=d2VdrdTheta*u-(AThetar(:,m+1)*CosVec(m+1)+BThetar(:,m+1)*SinVec(m+1));
                d2Vdrdr=d2Vdrdr*u+(Arr(:,m+1)*CosVec(m+1)+Brr(:,m+1)*SinVec(m+1));
            end

            %The minus signs in the following equations adjust for the
            %spherical coordinate system difference.
            d2Vdrdr=crScal*d2Vdrdr.'/r^2;
            d2VdLambdadLambda=crScal*d2VdLambdadLambda.';
            d2VdThetadTheta=crScal*d2VdThetadTheta.';
            d2VdrdLambda=crScal*d2VdrdLambda.'/r;
            d2VdrdTheta=-crScal*d2VdrdTheta.'/r;
            d2VdLambdadTheta=crScal*d2VdLambdadTheta.';

            if(spherDerivs)
                HessianV(1,1,:,curPoint)=d2Vdrdr;
                HessianV(2,2,:,curPoint)=d2VdLambdadLambda;
                HessianV(3,3,:,curPoint)=d2VdThetadTheta;
                HessianV(1,2,:,curPoint)=d2VdrdLambda;
                HessianV(2,1,:,curPoint)=HessianV(1,2,:,curPoint);
                HessianV(1,3,:,curPoint)=d2VdrdTheta;
                HessianV(3,1,:,curPoint)=HessianV(1,3,:,curPoint);
                HessianV(2,3,:,curPoint)=d2VdLambdadTheta;
                HessianV(3,2,:,curPoint)=HessianV(2,3,:,curPoint);
            else%Convert the Hessian to Cartesian coordinates.
                drdx=J(1,1);
                drdy=J(1,2);
                drdz=J(1,3);
                dLambdadx=J(2,1);
                dLambdady=J(2,2);
                dLambdadz=J(2,3);
                dPhidx=J(3,1);
                dPhidy=J(3,2);
                dPhidz=J(3,3);

                H=calcSpherConvHessian(pointCur);

                drdxdx=H(1,1,1);
                drdydy=H(2,2,1);
                drdzdz=H(3,3,1);
                drdxdy=H(1,2,1);
                drdxdz=H(1,3,1);
                drdydz=H(2,3,1);

                dLambdadxdx=H(1,1,2);
                dLambdadydy=H(2,2,2);
                dLambdadzdz=H(3,3,2);
                dLambdadxdy=H(1,2,2);
                dLambdadxdz=H(1,3,2);
                dLambdadydz=H(2,3,2);

                dPhidxdx=H(1,1,3);
                dPhidydy=H(2,2,3);
                dPhidzdz=H(3,3,3);
                dPhidxdy=H(1,2,3);
                dPhidxdz=H(1,3,3);
                dPhidydz=H(2,3,3);

                HessianV(1,1,:,curPoint)=d2VdLambdadLambda*dLambdadx^2+2*d2VdLambdadTheta*dLambdadx*dPhidx+d2VdThetadTheta*dPhidx^2+2*dPhidx*drdx*d2VdrdTheta+2*dLambdadx*drdx*d2VdrdLambda+dVdLambda*dLambdadxdx+dVdTheta*dPhidxdx+dVdr*drdxdx+drdx^2*d2Vdrdr;
                HessianV(2,2,:,curPoint)=d2VdThetadTheta*dPhidy^2+2*dLambdady*dPhidy*d2VdLambdadTheta+dVdLambda*dLambdadydy+dVdTheta*dPhidydy+dLambdady^2*d2VdLambdadLambda+drdydy*dVdr+2*dPhidy*drdy*d2VdrdTheta+2*dLambdady*drdy*d2VdrdLambda+drdy^2*d2Vdrdr;
                HessianV(3,3,:,curPoint)=dVdTheta*dPhidzdz+dPhidz^2*d2VdThetadTheta+dLambdadzdz*dVdLambda+2*dLambdadz*dPhidz*d2VdLambdadTheta+(dLambdadz)^2*d2VdLambdadLambda+drdzdz*dVdr+2*dPhidz*drdz*d2VdrdTheta+2*dLambdadz*drdz*d2VdrdLambda+drdz^2*d2Vdrdr;
                HessianV(1,2,:,curPoint)=dPhidy*d2VdLambdadTheta*dLambdadx+dLambdady*d2VdLambdadLambda*dLambdadx+d2VdThetadTheta*dPhidy*dPhidx+dLambdady*d2VdLambdadTheta*dPhidx+drdy*dPhidx*d2VdrdTheta+dPhidy*drdx*d2VdrdTheta+dVdLambda*dLambdadxdy+dVdTheta*dPhidxdy+dVdr*drdxdy+drdy*dLambdadx*d2VdrdLambda+dLambdady*drdx*d2VdrdLambda+drdy*drdx*d2Vdrdr;
                HessianV(2,1,:,curPoint)=HessianV(1,2,:,curPoint);
                HessianV(1,3,:,curPoint)=dPhidz*d2VdLambdadTheta*dLambdadx+dLambdadz*d2VdLambdadLambda*dLambdadx+dPhidz*d2VdThetadTheta*dPhidx+dLambdadz*d2VdLambdadTheta*dPhidx+dVdLambda*dLambdadxdz+dVdTheta*dPhidxdz+dVdr*drdxdz+drdz*dPhidx*d2VdrdTheta+dPhidz*drdx*d2VdrdTheta+drdz*dLambdadx*d2VdrdLambda+dLambdadz*drdx*d2VdrdLambda+drdz*drdx*d2Vdrdr;
                HessianV(3,1,:,curPoint)=HessianV(1,3,:,curPoint);
                HessianV(2,3,:,curPoint)=dPhidz*d2VdThetadTheta*dPhidy+dVdLambda*dLambdadydz+dVdTheta*dPhidydz+dPhidz*dLambdady*d2VdLambdadTheta+dLambdadz*dPhidy*d2VdLambdadTheta+dLambdadz*dLambdady*d2VdLambdadLambda+drdydz*dVdr+drdz*dPhidy*d2VdrdTheta+dPhidz*drdy*d2VdrdTheta+drdz*dLambdady*d2VdrdLambda+dLambdadz*drdy*d2VdrdLambda+drdz*drdy*d2Vdrdr;
                HessianV(3,2,:,curPoint)=HessianV(2,3,:,curPoint);
            end
        end
    else
        %At latitudes that are near the poles, the non-singular algorithm
        %of Pines using the fully normalized Helmholtz equations from
        %Fantino and Casotto is used. The algorithm has been slightly
        %modified so that the c/r term is out front and the fully
        %normalized Helmholtz polynomials can be scaled. Also, lumped
        %coefficients are not used. The Pines algorithm is generally slower
        %than the algorithm of Holmes and Featherstone and it suffers a
        %loss of precision near the equator. Thus, the Pines algorithm is
        %only used near the poles where the other algorithm has issues with
        %a singularity.

        CartPoint=spher2Cart(pointCur);

        x=CartPoint(1);
        y=CartPoint(2);
        z=CartPoint(3);

        %Get the direction cosines used by Pines' algorithm.
        s=x/r;
        t=y/r;
        u=z/r;

        %Compute the fully normalized Helmholtz polynomials.
        if(thetaChanged)
            if(nargout>1)
                [HBar,dHBardu,d2HBardu2]=normHelmholtz(u,M,scalFactor);
            else
                HBar=normHelmholtz(u,M,scalFactor);
            end
        end

        %Recursively compute the rm and im terms for the sums.
        rm=zeros(M+1,1);
        im=zeros(M+1,1);
        rm(0+1)=1;
        im(0+1)=0;
        for m=1:M
            %These are equation 49 in the Fantino and Casotto paper.
            rm(m+1)=s*rm(m-1+1)-t*im(m-1+1);
            im(m+1)=s*im(m-1+1)+t*rm(m-1+1);
        end

        %Perform the sum for the potential from Equation 44 in the Fantino
        %and Casotto paper.
        V(:,curPoint)=0;
        for n=0:M
            innerTerm=0;
            for m=0:n
                innerTerm=innerTerm+(C(n+1,m+1)*rm(m+1)+S(n+1,m+1)*im(m+1))*HBar(n+1,m+1);
            end
            V(:,curPoint)=V(:,curPoint)+nCoeff(n+1)*innerTerm.';
        end

        V(:,curPoint)=crScal*V(:,curPoint);

        %If only the gradient is desired as the next output
        if(nargout==2)
            %If the gradient and not the Hessian is desired.
            a1=zeros(1,numSets);
            a2=zeros(1,numSets);
            a3=zeros(1,numSets);
            a4=zeros(1,numSets);

            for m=0:M
                A1=zeros(1,numSets);
                A2=zeros(1,numSets);
                A3=zeros(1,numSets);

                B1=zeros(1,numSets);
                B2=zeros(1,numSets);
                B3=zeros(1,numSets);

                %Compute the lumped coefficients for Pine's method from
                %Table 13 for the current m.
                for n=m:M
                    HVal=HBar(n+1,m+1);
                    dHVal=dHBardu(n+1,m+1);

                    %The expressions for Lmn, is from Table 14
                    Lmn=(n+m+1)*HVal+u*dHVal;

                    rhoC=nCoeff(n+1)*C(n+1,m+1);
                    rhoS=nCoeff(n+1)*S(n+1,m+1);

                    A1=A1+rhoC*HVal;
                    A2=A2+rhoC*dHVal;
                    A3=A3+rhoC*Lmn;

                    B1=B1+rhoS*HVal;
                    B2=B2+rhoS*dHVal;
                    B3=B3+rhoS*Lmn;
                end
                if(m>=1)
                    a1=a1+m*(A1*rm(m-1+1)+B1*im(m-1+1));
                    a2=a2+m*(B1*rm(m-1+1)-A1*im(m-1+1));
                end
                a3=a3+(A2*rm(m+1)+B2*im(m+1));
                a4=a4-(A3*rm(m+1)+B3*im(m+1));
            end

            a1=a1/r;
            a2=a2/r;
            a3=a3/r;
            a4=a4/r;

            dVdx=crScal*(a1+s*a4);
            dVdy=crScal*(a2+t*a4);
            dVdz=crScal*(a3+u*a4);

            if(spherDerivs)
                %Convert the derivatives to spherical coordinates.
                J=calcSpherInvJacob(pointCur)';

                gradV(:,:,curPoint)=J*[dVdx;dVdy;dVdz];
            else%If a gradient in Cartesian coordinates is desired.
                gradV(1,:,curPoint)=dVdx;
                gradV(2,:,curPoint)=dVdy;
                gradV(3,:,curPoint)=dVdz;
            end
        else
            %If the gradient and the Hessian are desired.
            a1=zeros(1,numSets);
            a2=zeros(1,numSets);
            a3=zeros(1,numSets);
            a4=zeros(1,numSets);
            a11=zeros(1,numSets);
            a12=zeros(1,numSets);
            a13=zeros(1,numSets);
            a14=zeros(1,numSets);
            a23=zeros(1,numSets);
            a24=zeros(1,numSets);
            a33=zeros(1,numSets);
            a34=zeros(1,numSets);
            a44=zeros(1,numSets);

            for m=0:M
                A1=zeros(1,numSets);
                A2=zeros(1,numSets);
                A3=zeros(1,numSets);
                A4=zeros(1,numSets);
                A5=zeros(1,numSets);
                A6=zeros(1,numSets);

                B1=zeros(1,numSets);
                B2=zeros(1,numSets);
                B3=zeros(1,numSets);
                B4=zeros(1,numSets);
                B5=zeros(1,numSets);
                B6=zeros(1,numSets);

                %Compute the lumped coefficients for Pine's method from
                %Table 13 for the current m.
                for n=m:M
                    HVal=HBar(n+1,m+1);
                    dHVal=dHBardu(n+1,m+1);
                    d2HVal=d2HBardu2(n+1,m+1);

                    %The expressions for Lmn, dLmn, and Omn are from
                    %Table 14
                    Lmn=(n+m+1)*HVal+u*dHVal;
                    dLmn=(n+m+2)*dHVal+u*d2HVal;
                    Omn=(n+m+1)*(n+m+2)*HVal+2*u*(n+m+2)*dHVal+u^2*d2HVal;

                    rhoC=nCoeff(n+1)*C(n+1,m+1);
                    rhoS=nCoeff(n+1)*S(n+1,m+1);

                    A1=A1+rhoC*HVal;
                    A2=A2+rhoC*dHVal;
                    A3=A3+rhoC*Lmn;
                    A4=A4+rhoC*d2HVal;
                    A5=A5+rhoC*dLmn;
                    A6=A6+rhoC*Omn;

                    B1=B1+rhoS*HVal;
                    B2=B2+rhoS*dHVal;
                    B3=B3+rhoS*Lmn;
                    B4=B4+rhoS*d2HVal;
                    B5=B5+rhoS*dLmn;
                    B6=B6+rhoS*Omn;
                end
                if(m>=1)
                    a1=a1+m*(A1*rm(m-1+1)+B1*im(m-1+1));
                    a2=a2+m*(B1*rm(m-1+1)-A1*im(m-1+1));
                end
                a3=a3+(A2*rm(m+1)+B2*im(m+1));
                a4=a4-(A3*rm(m+1)+B3*im(m+1));

                if(m>=2)
                    a11=a11+m*(m-1)*(A1*rm(m-2+1)+B1*im(m-2+1));
                    a12=a12+m*(m-1)*(B1*rm(m-2+1)-A1*im(m-2+1));
                end
                if(m>=1)
                    a13=a13+m*(A2*rm(m-1+1)+B2*im(m-1+1));
                    a14=a14-m*(A3*rm(m-1+1)+B3*im(m-1+1));
                    a23=a23+m*(B2*rm(m-1+1)-A2*im(m-1+1));
                    a24=a24-m*(B3*rm(m-1+1)-A3*im(m-1+1));
                end
                a33=a33+(A4*rm(m+1)+B4*im(m+1));
                a34=a34-(A5*rm(m+1)+B5*im(m+1));
                a44=a44+(A6*rm(m+1)+B6*im(m+1));
            end

            a1=a1/r;
            a2=a2/r;
            a3=a3/r;
            a4=a4/r;
            a11=a11/r^2;
            a12=a12/r^2;
            a13=a13/r^2;
            a14=a14/r^2;
            a23=a23/r^2;
            a24=a24/r^2;
            a33=a33/r^2;
            a34=a34/r^2;
            a44=a44/r^2;
            a22=-a11;

            crScal=(c/r)/scalFactor;

            dVdx=crScal*(a1+s*a4);
            dVdy=crScal*(a2+t*a4);
            dVdz=crScal*(a3+u*a4);

            if(spherDerivs)
                %Convert the derivatives to spherical coordinates.
                J=calcSpherInvJacob(pointCur)';

                gradV(:,:,curPoint)=J*[dVdx;dVdy;dVdz];
            else%If a gradient in Cartesian coordinates is desired.
                gradV(1,:,curPoint)=dVdx;
                gradV(2,:,curPoint)=dVdy;
                gradV(3,:,curPoint)=dVdz;
            end

            d2Vdxdx=crScal*(a11+2*s*a14+a4/r+s^2*a44-s^2*a4/r);
            d2Vdydy=crScal*(a22+2*t*a24+a4/r+t^2*a44-t^2*a4/r);
            d2Vdzdz=crScal*(a33+2*u*a34+a4/r+u^2*a44-u^2*a4/r);

            d2Vdxdy=crScal*(a12+s*t*a44+s*a24+t*a14-s*t*a4/r);
            d2Vdxdz=crScal*(a13+s*u*a44+s*a34+u*a14-s*u*a4/r);
            d2Vdydz=crScal*(a23+t*u*a44+t*a34+u*a24-t*u*a4/r);

            if(spherDerivs)
                dxdr=J(1,1);
                dxdAz=J(2,1);
                dxdEl=J(3,1);

                dydr=J(1,2);
                dydAz=J(2,2);
                dydEl=J(3,2);

                dzdr=J(1,3);
                dzdAz=J(2,3);
                dzdEl=J(3,3);

                H=calcSpherInvHessian(pointCur);

                d2xdrdr=H(1,1,1);
                d2xdAzdAz=H(2,2,1);
                d2xdEldEl=H(3,3,1);
                d2xdrdAz=H(1,2,1);
                d2xdrdEl=H(1,3,1);
                d2xdAzdEl=H(2,3,1);

                d2ydrdr=H(1,1,2);
                d2ydAzdAz=H(2,2,2);
                d2ydEldEl=H(3,3,2);
                d2ydrdAz=H(1,2,2);
                d2ydrdEl=H(1,3,2);
                d2ydAzdEl=H(2,3,2);

                d2zdrdr=H(1,1,3);
                d2zdAzdAz=H(2,2,3);
                d2zdEldEl=H(3,3,3);
                d2zdrdAz=H(1,2,3);
                d2zdrdEl=H(1,3,3);
                d2zdAzdEl=H(2,3,3);

                %d2Vdrdr
                HessianV(1,1,:,curPoint)=d2Vdydy*dydr^2+2*d2Vdydz*dydr*dzdr+d2Vdzdz*dzdr^2+2*dxdr*dzdr*d2Vdxdz+2*dxdr*dydr*d2Vdxdy+dxdr^2*d2Vdxdx+dVdx*d2xdrdr+dVdy*d2ydrdr+dVdz*d2zdrdr;
                %d2VdAzdAz
                HessianV(2,2,:,curPoint)=d2Vdzdz*dzdAz^2+2*dydAz*dzdAz*d2Vdydz+dydAz^2*d2Vdydy+dVdy*d2ydAzdAz+dVdz*d2zdAzdAz+d2xdAzdAz*dVdx+2*dxdAz*dzdAz*d2Vdxdz+2*dxdAz*dydAz*d2Vdxdy+dxdAz^2*d2Vdxdx;
                %d2VdEldEl
                HessianV(3,3,:,curPoint)=dzdEl^2*d2Vdzdz+dVdz*d2zdEldEl+d2ydEldEl*dVdy+2*dydEl*dzdEl*d2Vdydz+dydEl^2*d2Vdydy+d2xdEldEl*dVdx+2*dxdEl*dzdEl*d2Vdxdz+2*dxdEl*dydEl*d2Vdxdy+dxdEl^2*d2Vdxdx;
                %d2VdrdAz
                HessianV(1,2,:,curPoint)=dzdAz*d2Vdydz*dydr+dydAz*d2Vdydy*dydr+d2Vdzdz*dzdAz*dzdr+dydAz*d2Vdydz*dzdr+dzdAz*dxdr*d2Vdxdz+dxdAz*dzdr*d2Vdxdz+dydAz*dxdr*d2Vdxdy+dxdAz*dydr*d2Vdxdy+dVdx*d2xdrdAz+dVdy*d2ydrdAz+dVdz*d2zdrdAz+dxdAz*dxdr*d2Vdxdx;
                HessianV(2,1,:,curPoint)=HessianV(1,2,:,curPoint);
                %d2VdrdEl
                HessianV(1,3,:,curPoint)=dzdEl*d2Vdydz*dydr+dydEl*d2Vdydy*dydr+dzdEl*d2Vdzdz*dzdr+dydEl*d2Vdydz*dzdr+dzdEl*dxdr*d2Vdxdz+dxdEl*dzdr*d2Vdxdz+dVdx*d2xdrdEl+dVdy*d2ydrdEl+dVdz*d2zdrdEl+dydEl*dxdr*d2Vdxdy+dxdEl*dydr*d2Vdxdy+dxdEl*dxdr*d2Vdxdx;
                HessianV(3,1,:,curPoint)=HessianV(1,3,:,curPoint);
                %d2VdAzdEl
                HessianV(2,3,:,curPoint)=dzdEl*d2Vdzdz*dzdAz+dzdEl*dydAz*d2Vdydz+dydEl*dzdAz*d2Vdydz+dVdy*d2ydAzdEl+dVdz*d2zdAzdEl+dydEl*dydAz*d2Vdydy+d2xdAzdEl*dVdx+dzdEl*dxdAz*d2Vdxdz+dxdEl*dzdAz*d2Vdxdz+dydEl*dxdAz*d2Vdxdy+dxdEl*dydAz*d2Vdxdy+dxdEl*dxdAz*d2Vdxdx;
                HessianV(3,2,:,curPoint)=HessianV(2,3,:,curPoint);
            else
                HessianV(1,1,:,curPoint)=d2Vdxdx;
                HessianV(2,2,:,curPoint)=d2Vdydy;
                HessianV(3,3,:,curPoint)=d2Vdzdz;

                HessianV(1,2,:,curPoint)=d2Vdxdy;
                HessianV(2,1,:,curPoint)=HessianV(1,2,:,curPoint);
                HessianV(1,3,:,curPoint)=d2Vdxdz;
                HessianV(3,1,:,curPoint)=HessianV(1,3,:,curPoint);
                HessianV(2,3,:,curPoint)=d2Vdydz;
                HessianV(3,2,:,curPoint)=HessianV(2,3,:,curPoint);
            end
        end
    end
end

if(spherDerivs&&systemType==2)
    %Flip signs of the elevation terms reflecting the
    %difference in definition between systems 0 and 2.
    gradV(3,:)=-gradV(3,:);
    HessianV(1,3,:)=-HessianV(1,3,:);
    HessianV(3,1,:)=-HessianV(3,1,:);
    HessianV(2,3,:)=-HessianV(2,3,:);
    HessianV(3,2,:)=-HessianV(3,2,:);
end
end

function [SinVec,CosVec]=calcSinCosTerms(lambda,M)
    %Compute sin(m*lambda) and cos(m*lambda) for m=0 to m=M.
    SinVec=zeros(M+1,1);
    CosVec=zeros(M+1,1);
    %Explicitly set the first two terms.
    SinVec(0+1)=0;
    CosVec(0+1)=1;
    SinVec(1+1)=sin(lambda);
    CosVec(1+1)=cos(lambda);
    %Use a double angle identity to get the second order term.
    SinVec(2+1)=2*SinVec(1+1)*CosVec(1+1);
    CosVec(2+1)=1-2*SinVec(1+1)^2;
    %Use a two-part recursion for the rest of the terms.
    for m=3:M
        SinVec(m+1)=2*CosVec(1+1)*SinVec(m-1+1)-SinVec(m-2+1);
        CosVec(m+1)=2*CosVec(1+1)*CosVec(m-1+1)-CosVec(m-2+1);
    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 msm_to_mm_test06 ( )

%*****************************************************************************80
%
%% MSM_TO_MM_TEST06 tests MSM_TO_MM_ARRAY_INTEGER_SKEW_SYMMETRIC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MSM_TO_MM_TEST06\n' );
  fprintf ( 1, '  Convert an MSM to MM array integer skew-symmetric format.\n' );

  output_filename = 'msm_to_mm_test06.mm';

  a = i4mat_indicator ( 4, 4 );
  a = a - a';
%
%  Have MSM_TO_MM write the matrix to a file.
%
  msm_to_mm ( output_filename, a, 'array', 'integer', 'skew-symmetric' );

  return
end


function [pop] = mutation_pop(pop)
%   Applies mutation to whole population.
global pmut_real ;
global eta_m ;
global min_realvar ;
global max_realvar ;

[popsize,~] = size(pop);
nreal = length(min_realvar);

for i = 1:popsize
    pop(i,1:nreal) = real_mutate(pop(i,1:nreal), pmut_real, eta_m, ...
                                    min_realvar, max_realvar);
end

end


function [r, c] = kurtosis_ijkl_to_rc(i, j, k, l)
%KURTOSIS_IJKL_TO_RC converts indices in a kurtosis tensor from 3x3x3x3
%format to 6x6 format

% Author: Darryl McClymont <darryl.mcclymont@gmail.com>
% Copyright © 2014-2016 University of Oxford
% Version: 0.1.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

switch i
    case 1
        r1 = [1 2 3];
    case 2
        r1 = [2 4 5];
    case 3
        r1 = [3 5 6];
end
switch j
    case 1
        r2 = [1 2 3];
    case 2
        r2 = [2 4 5];
    case 3
        r2 = [3 5 6];
end

if i == j
    switch i
        case 1
            r =1;
        case 2
            r = 4;
        case 3
            r = 6;
    end
else
    r = intersect(r1, r2);
end

switch k
    case 1
        c1 = [1 2 3];
    case 2
        c1 = [2 4 5];
    case 3
        c1 = [3 5 6];
end
switch l
    case 1
        c2 = [1 2 3];
    case 2
        c2 = [2 4 5];
    case 3
        c2 = [3 5 6];
end

if k == l
    switch k
        case 1
            c =1;
        case 2
            c = 4;
        case 3
            c = 6;
    end
else
    c = intersect(c1, c2);
end


% IM = pgmRead( FILENAME )
%
% Load a pgm image into a MatLab matrix.
%   This format is accessible from the XV image browsing utility.
%   Only works for 8bit gray images (raw or ascii)

% Hany Farid, Spring '96.  Modified by Eero Simoncelli, 6/96.

function im = pgmRead( fname );

[fid,msg] = fopen( fname, 'r' );

if (fid == -1)
  error(msg);
end

%%% First line contains ID string:
%%% "P1" = ascii bitmap, "P2" = ascii greymap,
%%% "P3" = ascii pixmap, "P4" = raw bitmap,
%%% "P5" = raw greymap, "P6" = raw pixmap
TheLine = fgetl(fid);
format  = TheLine;

if ~((format(1:2) == 'P2') | (format(1:2) == 'P5'))
  error('PGM file must be of type P2 or P5');
end

%%% Any number of comment lines
TheLine  = fgetl(fid);
while TheLine(1) == '#'
	TheLine = fgetl(fid);
end

%%% dimensions
sz = sscanf(TheLine,'%d',2);
xdim = sz(1);
ydim = sz(2);
sz = xdim * ydim;

%%% Maximum pixel value
TheLine  = fgetl(fid);
maxval = sscanf(TheLine, '%d',1);

%%im  = zeros(dim,1);
if (format(2) == '2')
  [im,count]  = fscanf(fid,'%d',sz);
else
  [im,count]  = fread(fid,sz,'uchar');
end

fclose(fid);

if (count == sz)
  im = reshape( im, xdim, ydim )';
else
  fprintf(1,'Warning: File ended early!');
  im = reshape( [im ; zeros(sz-count,1)], xdim, ydim)';
end


function [hpb] = MW2hpb(MW)
% Convert power from megawatts to boiler horsepower.
% Chad A. Greene 2012
hpb = MW*101.941995005;


% LINSOLVE_LDLCHOL - Solves a matrix-vector equation when the matrix is a
%                    sparse symmetric positive definite matrix.
%
% Solves K*Y = X. Usage:
%
%   Y = LINSOLVE_LDLCHOL(LD,X)
%
% where LD = LDLCHOL(K).
%
% The function is just a simple wrapper in accordance with the naming
% conventions.
%
% See also LDLCHOL, LDLSOLVE.

% Last modified 2011-01-31
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function x = linsolve_ldlchol(LD,x,q)

%fprintf('Sparsity in linsolve_ldlchol: %f\n', sparsity(LD));

if nargin < 3
  x = ldlsolve(LD,x);
else
  x(q(:),:) = ldlsolve(LD,x(q(:),:));
end


function [kJ] = Btu2kJ(Btu)
% Convert energy or work from British thermal units to kilojoules.
% Chad A. Greene 2012
kJ = Btu*1.0550559 ;


% erpregout() - regress out the ERP from the data
%
% Usage:
%   newdata = erpregout(data);
%   [newdata erp factors] = erpregout(data, tlim, reglim);
%
% Inputs:
%   data    - [float] 2-D data (times x trials) or 3-D data
%             (channels x times x trials).
%
% Optional inputs:
%   tlim    - [min max] time limits in ms.
%   reglim  - [min max] regression time window in ms (by default
%             the whole time period is used
% Outputs:
%   newdata - data with ERP regressed out
%   erp     - data ERP
%   factors - factors used for regressing out the ERP (size is the same
%             as the number of trials or (channels x trials)
%
% Note: it is better to regress out the ERP about 4 times (launch the
%       function 4 times in a row) to really be able to regress out the
%       ERP and have a residual ERP close to 0.
%
% Author: Arnaud Delorme, Salk, SCCN, UCSD, CA, April 29, 2004

%123456789012345678901234567890123456789012345678901234567890123456789012

% Copyright (C) 2004 Arnaud Delorme
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function [data, erp, factors] = erpregout(data, tlim, reglim);

    if nargin < 1
        help erpregout;
        return;
    end;
    if nargin < 2
        tlim = [0 1];
    end;
    if nargin < 3
        reglim = tlim;
    end;
    if ndims(data) == 2
        data = reshape(data, 1, size(data,1), size(data,2));
        redim = 1;
    else
        redim = 0;
    end;

    % find closest points
    % -------------------
    timevect = linspace(tlim(1), tlim(2), size(data,2));
    [tmp begpoint] = min( abs(timevect-reglim(1)) );
    [tmp endpoint] = min( abs(timevect-reglim(2)) );
    erp = mean(data, 3);

    % regressing out erp in channels and trials
    % -----------------------------------------
    for chan = 1:size(data,1)
        fprintf('Channel %d (trials out of %d):', chan, size(data,3));
        for trial = 1:size(data,3)
            if ~mod(trial, 10) , fprintf('%d ', trial); end;
            if ~mod(trial, 200), fprintf('\n', trial); end;
            [factors(chan, trial) tmpf exitflag] = fminbnd('erpregoutfunc', 0, 10, [], ...
                                   data(chan, begpoint:endpoint, trial), erp(chan, begpoint:endpoint));
            data(chan,:,trial) = data(chan,:,trial) - factors(chan, trial)*erp(chan, :);
        end;
        fprintf('\n');
    end;

    if redim
        data = squeeze(data);
    end;


function SDP = relax_category_registration(problem,varargin)
%% Apply a sparse third-order relaxation to category registration
%% Depending on multivariate polynomial package in SPOT
%% Bounded translation is modelled as an inequality constraint, which leads
%% to an extra PSD block in the semidefinite relaxation
%% Heng Yang
%% July 05, 2021

params = inputParser;
params.CaseSensitive = false;

params.addParameter('checkMonomials',true, @(x) islogical(x));
params.addParameter('lambda',0.1, @(x) isscalar(x));

params.parse(varargin{:});

checkMonomials = params.Results.checkMonomials;
lambda         = params.Results.lambda;

fprintf('\n===================================================================')
fprintf('\nApplying SDP relaxation to category registration problem')
fprintf('\n===================================================================\n')
t0              = tic;

N               = problem.N;
K               = problem.K;
scene           = problem.scene;
shapes          = problem.shapes;
noiseBoundSq    = problem.noiseBoundSq;
tBound          = problem.translationBound;
tBoundSq        = tBound^2; % t'*t <= tBoundSq
cBoundSq        = problem.cBound^2; % should just be 1
barc2           = 1.0;

%% define POP variables
nrPrimalVars    = 9+3+K+N; % rotation: 9, translation: 3, binary: N, shape: K
p               = msspoly('p',nrPrimalVars);
r               = p(1:9);
R               = reshape(r,3,3);
col1 = R(:,1); col2 = R(:,2); col3 = R(:,3);
t               = p(10:12);
c               = p(12+1:12+K);
theta           = p(12+K+1:nrPrimalVars);

%% define cost function
shape           = combine_shapes(shapes,c);
residuals       = {};
for i = 1:N
    distance            = scene(:,i) - R * shape(:,i) - t;
    residuals{end+1}    = (distance' * distance) / noiseBoundSq;
end
f_cost = 0;
for i = 1:N
    f_cost = f_cost + (1+theta(i))/2 * residuals{i} + (1-theta(i))/2 * barc2;
end
% add regularization on c
f_cost = f_cost + lambda * (c'*c);

%% define constraints
h_r  = [1.0-col1'*col1;...
        1.0-col2'*col2;...
        1.0-col3'*col3;... % column unit length
        col1'*col2;...
        col2'*col3;...
        col3'*col1;... % colums orthogonal
        cross(col1,col2) - col3;...
        cross(col2,col3) - col1;...
        cross(col3,col1) - col2]; % columns righthandedness
% h_c  = [sum(c) - 1.0];

h_theta = [];
for i = 1:N
    h_theta =[h_theta; 1-theta(i)^2];
end

g_t = tBoundSq - t'*t; % Translation bounded
g_c = [cBoundSq - c'*c;c]; % nonnegative and bounded shape parameters

%% Formulate the sparse third-order relaxation
cr          = mykron(c,r);
x           = [r;t];
basis_p     = [1;x;c;cr;theta;mykron(theta,x);mykron(theta,cr)];
n           = length(basis_p);
basis_r     = get_multiplier_basis(p,basis_p,h_r(1));
basis_theta = get_multiplier_basis(p,basis_p,h_theta(1));
% basis_c     = get_multiplier_basis(p,basis_p,h_c(1));
basis_g_t   = [1;theta];
basis_g_t_f = mykron(basis_g_t,basis_g_t);
basis_g_c   = [1;r];
basis_g_c_f = mykron(basis_g_c,basis_g_c);
n1          = length(basis_g_t);
n2          = length(basis_g_c);
n1delta     = triangle_number(n1);
n2delta     = triangle_number(n2);

fprintf('Computing localizing and moment polynomials ...')
time_start  = tic;
pop = [mykron(basis_r,h_r);...
       mykron(basis_theta,h_theta);...
       mykron(basis_p,basis_p);...
       f_cost;...
       mykron(g_t,basis_g_t_f);...
       mykron(g_c,basis_g_c_f)];
[~,degmat,coef_all] = decomp(pop);
coef_all            = coef_all';
time_prep   = toc(time_start);
fprintf(' Done in %g seconds.\n',time_prep);

if checkMonomials
    fprintf('Checking consistency of monomials ...')
    time_check0   = tic;
    monomials_mom = mono(mykron(basis_p,basis_p));
    fprintf(' %d ... %d ...',size(degmat,1),length(monomials_mom));
    assert(size(degmat,1) == length(monomials_mom),'monomials not consistent');
    time_check    = toc(time_check0);
    fprintf('Done in %g seconds.\n',time_check);
end

dim_loc_eq      = length(basis_r)*length(h_r)+length(basis_theta)*length(h_theta);
dim_loc_ineq    = length(g_t)*n1delta+length(g_c)*n2delta;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% generate standard SDP data from degmat and coefficients
ndelta      = triangle_number(n);
nterms      = size(degmat,1);
m_mom       = ndelta - nterms;
m_loc       = dim_loc_eq;
m_loc_ineq  = dim_loc_ineq;
m           = m_mom + m_loc + m_loc_ineq + 1;

fprintf('SDP: n = %d, n1 = %d, m = %d, m_mom = %d, m_loc = %d, m_loc_ineq = %d, ndelta = %d.\n',...
        n,n1,m,m_mom,m_loc,m_loc_ineq,ndelta);

coef_mom    = coef_all(:,dim_loc_eq+1:dim_loc_eq+n^2);
coef_mom    = coef_mom';

B           = {};
B_normalize = {};
A           = {};

fprintf('Building B and A... Progress ')
for i = 1:nterms
    if rem(i,10000) == 1
        fprintf('%d/%d ',i,nterms);
    end

    [row,~,~]   = find(coef_mom(:,i));
    SDP_coli    = floor((row-1)./n) + 1;
    SDP_rowi    = mod(row-1,n) + 1;
    nnz         = length(SDP_rowi);

    Bi          = sparse(SDP_rowi,SDP_coli,ones(nnz,1),n,n);
    B{end+1}    = Bi;
    B_normalize{end+1} = Bi/nnz;

    mask_triu   = (SDP_rowi >= SDP_coli);
    si          = SDP_rowi(mask_triu);
    sj          = SDP_coli(mask_triu);

    nnz_triu    = length(si);

    if nnz_triu > 1
        [~,base_idx]        = max(sj);
        si_base             = si(base_idx);
        sj_base             = sj(base_idx);

        si_nonbase          = si;
        si_nonbase(base_idx)= [];
        sj_nonbase          = sj;
        sj_nonbase(base_idx)= [];

        is_base_diag        = (si_base == sj_base);

        if is_base_diag
            A_si            = [si_base];
            A_sj            = [sj_base];
            A_v             = [1];
        else
            A_si            = [si_base,sj_base];
            A_sj            = [sj_base,si_base];
            A_v             = [0.5,0.5];
        end

        for nonbase_idx = 1:length(si_nonbase)
            is_nonbase_diag = (si_nonbase(nonbase_idx) == sj_nonbase(nonbase_idx));
            if is_nonbase_diag
                A_sii       = [A_si,si_nonbase(nonbase_idx)];
                A_sjj       = [A_sj,sj_nonbase(nonbase_idx)];
                A_vv        = [A_v,-1];
            else
                A_sii       = [A_si,si_nonbase(nonbase_idx),sj_nonbase(nonbase_idx)];
                A_sjj       = [A_sj,sj_nonbase(nonbase_idx),si_nonbase(nonbase_idx)];
                A_vv        = [A_v,-0.5,-0.5];
            end
            A_temp          = sparse(A_sii,A_sjj,A_vv,n,n);

            A{end+1}        = A_temp;
        end
    end
end
fprintf('Done.\n')
assert(length(A) == m_mom,'length(A)+length(B) == ndelta!');

%% Now build A's associated with localizing constraints
if dim_loc_eq == 0
    % Do nothing
    A_local = {};
else
    coef_loc    = coef_all(:,1:dim_loc_eq);
    A_local     = {};
    fprintf('Building localizing constraints A_local... Progress ')
    for i = 1:dim_loc_eq
        if rem(i,10000) == 1
            fprintf('%d/%d ',i,m_loc);
        end

        [rowi,~,vi] = find(coef_loc(:,i));

        Ai      = sparse(n,n);
        for j   = 1:length(rowi)
            Ai  = Ai + vi(j) * B_normalize{rowi(j)};
        end
        A_local{end+1} = Ai;
    end
end
fprintf('Done.\n')

%% Leading A
A0 = sparse([1],[1],[1],n,n);

%% Combine all A for the main block
A = [{A0},A_local,A];

%% Now build the cost matrix
coef_cost   = coef_all(:,dim_loc_eq+n^2+1);
[row,~,v]   = find(coef_cost);
C           = sparse(n,n);
fprintf('Building cost matrix C... Progress ')
for i = 1:length(row)
    if rem(i,1000) == 1
        fprintf('%d/%d ',i,length(row));
    end
    C       = C + v(i) * B_normalize{row(i)};
end
fprintf('Done.\n')

%% Now build the sub PSD constraint g_t
coef_ineq   = coef_all(:,dim_loc_eq+n^2+1+1:dim_loc_eq+n^2+1+n1^2); % nterms by n1^2
A1          = {};
for ii = 1:n1^2
    row     = mod(ii-1,n1) + 1;
    col     = floor((ii-1)./n1) + 1;
    if row < col
        % Do nothing for upper triangular parts
    else
        if row == col
            A1tmp       = sparse([row],[col],[-1],n1,n1);
        else
            A1tmp       = sparse([row,col],[col,row],[-0.5,-0.5],n1,n1);
        end
        [termIds,~,v]   = find(coef_ineq(:,ii));
        Atmp            = sparse(n,n);
        for iii = 1:length(termIds)
            Atmp        = Atmp + v(iii) * B_normalize{termIds(iii)};
        end
        A{end+1}        = Atmp;
        A1{end+1}       = A1tmp;
    end
end

%% Now build the sub PSD constraint g_c (multiple of them)
Ac = {};
for k = 1:length(g_c)
    idx         = dim_loc_eq+n^2+1+n1^2+blkIndices(k,n2^2);
    coef_ineq   = coef_all(:,idx); % nterms by n2^2
    A2          = {};
    for ii = 1:n2^2
        row     = mod(ii-1,n2) + 1;
        col     = floor((ii-1)./n2) + 1;
        if row < col
            % Do nothing for upper triangular parts
        else
            if row == col
                A2tmp       = sparse([row],[col],[-1],n2,n2);
            else
                A2tmp       = sparse([row,col],[col,row],[-0.5,-0.5],n2,n2);
            end
            [termIds,~,v]   = find(coef_ineq(:,ii));
            Atmp            = sparse(n,n);
            for iii = 1:length(termIds)
                Atmp        = Atmp + v(iii) * B_normalize{termIds(iii)};
            end
            A{end+1}        = Atmp;
            A2{end+1}       = A2tmp;
        end
    end
    Ac{end+1} = A2;
end

assert(length(A) == m,'Total number of equality constraints wrong.')
count = length(A1);
for k = 1:length(g_c)
    count = count + length(Ac{k});
end
assert(count == m_loc_ineq,'Equality constraints from second PSD blk wrong')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
blk{1,1}        = 's';
blk{1,2}        = n;
blk{2,1}        = 's';
blk{2,2}        = n1;
for k = 1:length(g_c)
    blk{2+k,1}  = 's';
    blk{2+k,2}  = n2;
end
b               = sparse([1],[1],[1],m,1);
%% svec in sdpt3 format and output standard data
At0             = sparsesvec(blk(1,:),A);
At1             = [sparse(n1delta,m-m_loc_ineq),... % moment and localizing
                   sparsesvec(blk(2,:),A1),... % sub PSD t
                   sparse(n1delta,length(g_c)*n2delta)]; % sub PSD c
At              = {At0;At1};
for k = 1:length(g_c)
    Atc         = [sparse(n2delta,m-m_loc_ineq),... % moment and localizing
                   sparse(n2delta,n1delta),... % sub PSD t
                   sparse(n2delta,(k-1)*n2delta),... % sub PSD c
                   sparsesvec(blk(2+k,:),Ac{k}),... % sub PSD k-th c
                   sparse(n2delta,(length(g_c)-k)*n2delta)]; % sub PSD k-th c
    At          = [At;{Atc}];
end
C       = {C;sparse(n1,n1)};
for k = 1:length(g_c)
    C   = [C;{sparse(n2,n2)}];
end

SDP.blk = blk;
SDP.At  = At;
SDP.m   = m;
SDP.C   = C;
SDP.b   = b;
SDP.lam = lambda;
SDP.M   = 4+tBoundSq+cBoundSq+3*cBoundSq+N+(3+tBoundSq)*N+(3*cBoundSq)*N;

tf    = toc(t0);
fprintf('\nDone in %g seconds.\n',tf);
fprintf('===================================================================\n')
end


function g = grad(sF, v) % gradient

if nargin == 2 % direct evaluation
  v = v(:);
  bario = sF.tri.calcBario(v);
  g = vector3d(zeros(3, length(v)));
  for i = 1:length(v)
    I = find(bario(i, :));
    if length(I) == 3
      f = sF.values(I);
      v = sF.vertices(I);
      g(i) = vector3d(v.xyz \ (f(:)-(f(1)+f(2)+f(3))/3*ones(3, 1)));
    end
  end
else % return S2VectorField
  mp = sF.tri.midPoints;
  g = S2VectorFieldTri(mp, sF.grad(mp));
end

end


function h=m_windbarb(long,lat,u,v,varargin)
%  M_WINDBARB Project wind barbs onto map axes
%
%  M_WINDBARB(LONG,LAT,U,V) projects two dimensional wind barbs onto the
%  current map axes. The vector components (U,V) are assumed to be in units
%  of knots and are specified at the points (LON,LAT). It handles winds up
%  to 130 knots. Winds exceeding 130 knots will appear as 130 knots.
%
%  If (u,v) are both positive, barbs appear to the SW of the vector head.
%  (in quiver, the arrowhead would be in the NE)
%
%  M_WINDBARB(...,S) uses the input S to scale the vectors after
%  they have been automatically scaled to fit within the grid. If omitted,
%  S = 0.9 is assumed.
%
%  M_WINDBARB(...,'PropertyName',PropertyValue,...) allows you to specify
%  additional LINE properties.
%
%  An additional parameter/value pair allows for u/v vectors in units
%  other than knots:
%          'units' : 'knots' | 'm/s' | 'kmh' | 'mph'
%

% Original code:
%  MFILE:   m_windbarb.m
%  MATLAB:  9.0.0 (R2016a)
%  VERSION: 1.0 (19 March 2017)
%  AUTHOR:  Erye
%  CONTACT: tfoterye@gmail.com
%
%  Oct/2017 - code "improved" for m_map style

global MAP_PROJECTION MAP_VAR_LIST

% Have to have initialized a map first

if isempty(MAP_PROJECTION)
  disp('No Map Projection initialized - call M_PROJ first!');
  return;
end

scale=0.9;
if nargin>4 && ~ischar(varargin{1})
    scale=varargin{1};
    varargin(1)=[];
end

if scale==0
    error(['map:' mfilename ':invalidScale'], ...
            'Invalid scale factor - must be greater than zero.')
end
%      1 knot = 0.5144 m/s = 1.852 kmh = 1.151 mph

scf=1;  % assume knots
k=1;
while k<=length(varargin)
    switch lower(varargin{k}(1:3))
        case 'uni'
            switch lower(varargin{k+1})
                case 'knots'
                    scf=1;
                case {'m/s','meters/sec','meters/second'}
                    scf=0.5144;
                case 'kmh'
                    scf=1.852;
                case 'mph'
                    scf=1.151;
                otherwise
                    error(['map:' mfilename ':invalidUnits'], ...
                        'Units specified not recognized');
            end
            varargin([k k+1])=[];
        otherwise
            k=k+2;
    end
end

[X,Y]=m_ll2xy(long,lat,'clip','point');

[XN ,YN ]=m_ll2xy([long(:) long(:)]',[lat(:) lat(:)+.001]','clip','off');
[XE ,YE ]=m_ll2xy([long(:) long(:)+(.001)./cos(lat(:)*pi/180)]',[lat(:) lat(:)]','clip','off');
mU=u.*reshape(diff(XE),size(lat))*1000 + v.*reshape(diff(XN),size(lat))*1000;
mV=u.*reshape(diff(YE),size(lat))*1000 + v.*reshape(diff(YN),size(lat))*1000;

umag = sqrt(u.^2+v.^2)/scf ; %wind speed (should be in knots as input)
theta = atan2(mV,mU);

%create 18 logical matrices for 18 possible barbs. Non-zero when the barb
%is called for at that gridpoint.
g{1} =  umag > 7.5  & umag <= 47.5;
g{2} =  umag > 17.5 & umag <= 47.5;
g{3} =  umag > 27.5;
g{4} = (umag > 37.5 & umag <= 47.5) | (umag > 57.5 & umag <= 97.5);
g{5} =  umag > 67.5;
g{6} = (umag > 77.5 & umag <  97.5) | umag > 107.5;
g{7} =  umag > 87.5 & umag < 97.5   | umag > 117.5;
g{8} =  umag > 127.5;
g{9} = (umag > 2.5  & umag <= 7.5 ) | (umag > 12.5 & umag <= 17.5);
g{10} = umag > 22.5 & umag <= 27.5;
g{11} =(umag > 32.5 & umag <= 37.5) | (umag > 52.5 & umag <= 57.5);
g{12} =(umag > 42.5 & umag <= 47.5) | (umag > 62.5 & umag <= 67.5);
g{13} =(umag > 72.5 & umag <= 77.5) | (umag > 102.5 & umag <= 107.5);
g{14} =(umag > 82.5 & umag <= 87.5) | (umag > 112.5 & umag <= 117.5);
g{15} =(umag > 92.5 & umag <= 97.5) | (umag > 122.5 & umag <= 127.5);
g{16} = umag > 47.5;
g{17} = umag > 97.5;
g{18} = umag>0;    % All have background line

%position of each barb relative to grid point: [x0 y0; x1 y1]
c{1} = [-1    0; -1.125 .325];  % Full barb 1
c{2} = [-.875 0; -1     .325];  % Full barb 2
c{3} = [-.75  0; -.875  .325];  % Full barb 3
c{4} = [-.625 0; -.75   .325];  % Full barb 4
c{5} = [-.5   0; -.625  .325];  % Full barb 5
c{6} = [-.375 0; -.5    .325];  % Full barb 6
c{7} = [-.25  0; -.375  .325];  % Full barb 7
c{8} = [-.125 0; -.25   .325];  % Full barb 8
c{9} = [-.875 0; -.9375 .1625]; % Half barb 2
c{10} =[-.75  0; -.8125 .1625]; % Half barb 3
c{11} =[-.625 0; -.6875 .1625]; % Half barb 4
c{12} =[-.5   0; -.5625 .1625]; % Half barb 5
c{13} =[-.375 0; -.4375 .1625]; % Half barb 6
c{14} =[-.25  0; -.3125 .1625]; % Half barb 7
c{15} =[-.125 0; -.1875 .1625]; % Half barb 8
c{16} =[-1    0; -.875  .325];  % Slant other way for triangle 1
c{17} =[-.75  0; -.625  .325];  % Slant other way for taingle
c{18} =[0     0; -1      0  ];  % Base

%set scale based on average latitude spacing
[m,n]=size(X);
scale2 = scale*(max(max(X))-min(min(X)))/n;

%draw the barbs
Ax=[];Ay=[];
for nn = 1:18

   ivals=g{nn}(:);
   if sum(ivals)>0  % number of barbs to draw

      %rotation operations
      cthet=cos(theta(ivals));
      sthet=sin(theta(ivals));

      x1=c{nn}(1,1)*cthet-c{nn}(1,2)*sthet;
      y1=c{nn}(1,1)*sthet+c{nn}(1,2)*cthet;
      x2=c{nn}(2,1)*cthet-c{nn}(2,2)*sthet;
      y2=c{nn}(2,1)*sthet+c{nn}(2,2)*cthet;

      x1 = x1*scale2+X(ivals);
      x2 = x2*scale2+X(ivals);
      y1 = y1*scale2+Y(ivals);
      y2 = y2*scale2+Y(ivals);
      x = [x1 x2 NaN(size(x1))]';   % Speed up by vectorizing this call with
      y = [y1 y2 NaN(size(y1))]';   %    Nans between line segments.

      Ax=[Ax;x(:)];
      Ay=[Ay;y(:)];
   end
end
h=line(Ax,Ay,varargin{:});

set(h,'tag','m_windbarb');

if nargout==0
 clear h
end


function [ ap, rcond, z, info ] = dppco ( ap, n )

%*****************************************************************************80
%
%% DPPCO factors a real symmetric positive definite matrix in packed form.
%
%  Discussion:
%
%    DPPCO also estimates the condition of the matrix.
%
%    If RCOND is not needed, DPPFA is slightly faster.
%
%    To solve A*X = B, follow DPPCO by DPPSL.
%
%    To compute inverse(A)*C, follow DPPCO by DPPSL.
%
%    To compute determinant(A), follow DPPCO by DPPDI.
%
%    To compute inverse(A), follow DPPCO by DPPDI.
%
%  Packed storage:
%
%    The following program segment will pack the upper triangle of
%    a symmetric matrix.
%
%      k = 0
%      do j = 1, n
%        do i = 1, j
%          k = k + 1
%          ap(k) = a(i,j)
%        end
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real AP(N*(N+1)/2), the packed form of a symmetric matrix A.
%    The columns of the upper triangle are stored sequentially in a
%    one-dimensional array.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real AP(N*(N+1)/2), an upper riangular matrix R, stored
%    in packed form, so that A = R'*R.  If INFO /= 0, the factorization
%    is not complete.
%
%    Output, real RCOND, an estimate of the reciprocal condition
%    of A.  For the system A*X = B, relative perturbations in A and B of size
%    EPSILON may cause relative perturbations in X of size EPSILON/RCOND.
%    If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0D+00
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%    Output, real Z(N), a work vector whose contents are usually
%    unimportant.  If A is singular to working precision, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%    If INFO /= 0, Z is unchanged.
%
%    Output, integer INFO, error flag.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is
%    not positive definite.
%

%
%  Find the norm of A.
%
  j1 = 1;
  for j = 1 : n
    z(j) = dasum ( j, ap(j1:j1+j-1), 1 );
    ij = j1;
    j1 = j1 + j;
    for i = 1 : j-1
      z(i) = z(i) + abs ( ap(ij) );
      ij = ij + 1;
    end
  end

  anorm = max ( z(1:n) );
%
%  Factor.
%
  [ ap, info ] = dppfa ( ap, n );

  if ( info ~= 0 )
    return
  end
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A * Z = Y and A * Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where R'*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve R' * W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  kk = 0;

  for k = 1 : n

    kk = kk + k;

    if ( z(k) ~= 0.0 )
      ek = -abs ( ek ) * r8_sign ( z(k) );
    end

    if ( ap(kk) < abs ( ek - z(k) ) )
      s = ap(kk) / abs ( ek - z(k) );
      z(1:n) = s * z(1:n);
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = -ek - z(k);
    s = abs ( wk );
    sm = abs ( wkm );
    wk = wk / ap(kk);
    wkm = wkm / ap(kk);
    kj = kk + k;

    if ( k + 1 <= n )

      for j = k + 1 : n
        sm = sm + abs ( z(j) + wkm * ap(kj) );
        z(j) = z(j) + wk * ap(kj);
        s = s + abs ( z(j) );
        kj = kj + j;
      end

      if ( s < sm )

        t = wkm - wk;
        wk = wkm;
        kj = kk + k;

        for j = k+1 : n
          z(j) = z(j) + t * ap(kj);
          kj = kj + j;
        end

      end

    end

    z(k) = wk;

  end

  z(1:n) = z(1:n) / dasum ( n, z(1:n), 1 );
%
%  Solve R * Y = W.
%
  for k = n : -1 : 1

    if ( ap(kk) < abs ( z(k) ) );
      s = ap(kk) / abs ( z(k) );
      z(1:n) = s * z(1:n);
    end

    z(k) = z(k) / ap(kk);
    kk = kk - k;
    t = -z(k);
    z(1:k-1) = daxpy ( k-1, t, ap(kk+1:kk+k-1), 1, z(1:k-1), 1 );

  end

  z(1:n) = z(1:n) / dasum ( n, z(1:n), 1 );

  ynorm = 1.0;
%
%  Solve R' * V = Y.
%
  for k = 1 : n

    z(k) = z(k) - ddot ( k-1, ap(kk+1:kk+k-1), 1, z(1:k-1), 1 );
    kk = kk + k;

    if ( ap(kk) < abs ( z(k) ) )
      s = ap(kk) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    z(k) = z(k) / ap(kk);

  end

  s = 1.0 / dasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;
%
%  Solve R * Z = V.
%
  for k = n : -1 : 1

    if ( ap(kk) < abs ( z(k) ) )
      s = ap(kk) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    z(k) = z(k) / ap(kk);
    kk = kk - k;
    t = -z(k);
    z(1:k-1) = daxpy ( k-1, t, ap(kk+1:kk+k-1), 1, z(1:k-1), 1 );

  end
%
%  Make ZNORM = 1.0.
%
  s = 1.0 / dasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


function pass = test_biharm(pref)
% Test BIHARM command.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e6*pref.cheb3Prefs.chebfun3eps;

% Function to be used:
ff = @(x,y,z) x.^2.*y.^2 + x.^2.*z.^2 + y.^2.*z.^2;

% Bihamrmonic operator applied to ff:
f = chebfun3(ff);
fB = biharm(f);

% Exact solution:
gg = @(x,y,z) 24;
g = chebfun3(gg);

% Compare:
pass = norm(fB - g) < tol;

end


function [um] = ft2um(ft)
% Convert length from feet to micrometers.
% Chad Greene 2012
um = ft*304800;


function [in3] = m32in3(m3)
% Convert volume from cubic meters to cubic inches.
% Chad Greene 2012
in3 = m3*61023.744095;


function c = mid(a)
%MID          Implements  mid(a)  for intervals (rounded)
%
%   c = mid(a)
%
% mid(a) and rad(a) computed such that
%    alpha  in  < mid(a) , rad(a) >  for all alpha in a
%
%For intervals at least 3 bits wide, the midpoint is always an inner point.
%

% written  10/16/98     S.M. Rump
% modified 06/22/99     S.M. Rump  for sparse matrices
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 09/10/07     S.M. Rump  performance, huge arrays
% modified 10/18/08     S.M. Rump  again huge arrays
% modified 07/23/09     S.M. Rump  changed formula: midpoint now in rnd to nearest
%                                    to make sure mid([1-eps,1+2eps])=1
%                                    (thanks to Gerhard Heindl for pointing to this)
% modified 10/06/09     S.M. Rump  check for rndold
%

  if a.complex                            % complex or thin interval
    c = a.mid;
  else
    e = 1e-30;
    if 1+e==1-e                         % fast check for rounding to nearest
      rndold = 0;
    else
      rndold = getround;
      setround(0)
    end
    % use a.inf + (0.5*a.sup-0.5*a.inf) for correct result in case a.sup-a.inf overflows
    [m,n] = size(a.inf);
    if m*n<2^31                           % input not huge
      c = a.inf + (0.5*a.sup-0.5*a.inf);  % make sure result correct in underflow range
      indexinf = isinf(a.inf);
      indexsup = isinf(a.sup);
      anyindexinf = any(indexinf(:));
      anyindexsup = any(indexsup(:));
      if anyindexinf                      % make sure mid([-inf,x]) is x
        c(indexinf) = a.sup(indexinf);
      end
      if anyindexsup                      % make sure mid([x,inf]) is x
          c(indexsup) = a.inf(indexsup);
      end
      if anyindexinf | anyindexsup        % some components are inf
        c(indexinf & indexsup) = 0;       % make sure mid([-inf,inf]) is 0
      end
    else                                  % take care of huge matrices
      % check some components are inf
      % careful with intervals [0,2] or [-2,0] or [-2,2]
      [Iinf,Jinf,Sinf] = find(a.inf);
      [Isup,Jsup,Ssup] = find(a.sup);
      if ( ~isempty(Iinf) ) | ( ~isempty(Isup) )
        ainfsup = sparse([Iinf;Isup],[Jinf;Jsup],[complex(Sinf,0);complex(0,Ssup)],m,n);
        [I,J,S] = find(ainfsup);
        Sold = S;
        S = real(S) + (0.5*imag(S)-0.5*real(S));
        indexinf = isinf(real(Sold));
        if any(indexinf(:))               % make sure mid([-inf,x]) is x
          S(indexinf) = imag(Sold(indexinf));
        end
        indexsup = isinf(imag(Sold));
        if any(indexsup(:))               % make sure mid([x,inf]) is x
          S(indexsup) = real(Sold(indexsup));
        end
        S(indexinf & indexsup) = 0;       % make sure mid([-inf,inf]) is 0
        c = sparse(I,J,S,m,n);
      else
        c = a.inf + 0.5*(a.sup-a.inf);    % make sure result correct in underflow range
      end
    end
    if rndold
      setround(rndold)                    % reset rounding
    end
  end