function DeltaR=solidTideShift(rStation,rSun,rMoon,Jul1,Jul2,addPermTide)
%%SOLIDTIDESHIFT Compute the vector offset of a point on the surface of the
%             Earth due to solid Earth tidal effects. These effects can be
%             over 31cm. If a Julian date is given, third-order solid Earth
%             tidal effects will be taken into account. Note that site
%             displacement due to ocean loading, which is not included in
%             the solid Earth tide offset, can also be quite significant,
%             with the IERS Conventions 2010 listing it up to 10cm.
%
%INPUTS: rStation A vector from the geocenter to the (non-offset) location
%                 of a station on the surface of the Earth in WGS-84 Earth-
%                 centered Earth-fixed (ECEF) coordinates in meters. This
%                 value is converted into spherical coordinates by this
%                 function, and only the angles in spherical coordinates,
%                 not the radius, are important for this algorithm.
%                 However, since locations on the ground are generally
%                 given in WGS-84 ellipsoidal coordinates, the ellipsoidal
%                 height of the ground plays a role in the conversion to
%                 spherical coordinates. However, variations in the
%                 ellipsoidal height provided on the order of 2km result in
%                 differences in the output DeltaR on the order of microns,
%                 so in practice, one could just set rStation to the
%                 Cartesian position corresponding to a particular
%                 ellipsoidal latitude and longitude with zero ellipsoidal
%                 height. Ideally, however, rStation is the Cartesian
%                 location of the land in a mean-tide model (if
%                 addPermTide=false), or in a tide-free model if
%                 addPermTide=true.
%            rSun A vector from the geocenter to the sun in ITRS
%                 coordinates in meters. This can be obtained using the
%                 readJPLEphem and GCRS2ITRS functions.
%           rMoon A vector from the geocenter to the moon in ITRS
%                 coordinates in meters. This can be obtained using the
%                 readJPLEphem and GCRS2ITRS functions.
%      Jul1, Jul2 Two parts of a Julian date given in terrestrial time
%                 (TT). The units of the date are days. The full date is
%                 the sum of both terms. The date is broken into two
%                 parts to provide more bits of precision. It does not
%                 matter how the date is split. These parameters must be
%                 given for the third-order tidal components to be taken
%                 into account.
%     addPermTide The deltaR values are for a conventional tide-free model.
%                 If addPermTide is true, then a constant (depending on
%                 latitude) permanent tide offset will be added to put the
%                 point into a mean-tide model. If this parameter is
%                 omitted, the default value is "false".
%
%OUTPUTS: DeltaR The offset of a point on the surface of the Earth due to
%                solid Earth tides. rStation+DeltaR is the location of the
%                station (point on the ground) at the terrestial time given
%                by Jul1 and Jul2 taking into account solid Earth tides.
%                Note that the addPermTide term must be consistent with the
%                coordinate system of rStation. If rStation already
%                includes the permanent tides (is in a mean-tide model),
%                then addPermTide should be false. Otherwise, addPermTide
%                should be true.
%
%The Sun and Moon cause the crust of the Earth to warp and points on the
%Earth in WGS-84 coordinates to move over time. The formulae for computing
%tidal shits of the crust are given in Section 7.1 of [1].
%
%Section 7.1.1 discusses solid Earth tides. While the second-order
%corrections are well documneted, the third order corrections can not be
%implemented directly from the standard. However, the standard specifies
%FORTRAN routines, whose algorithms do not resemble the work given in the
%standard. These Fortran routines, have been converted to Matlab and are
%called for the third-order corrections if Julian dates are given. The
%converted routines are separate functions at the bottom of this file.
%
%Note that third-order model has an implied set of ephemerides built into
%it that might not be perfectly consistent with whatever model was used to
%obtain rSun and rMoon.
%
%One time component is supposed to be in Julian centuries since J2000.0. As
%J2000.0 is defined in TDB and the time is given in TT, the TDB2TT function
%is used. The extra parameters needed for the TDB2TT function are not
%requested as inputs to this function, since it is assumed that the
%difference between the tides is too small to matter for the model.
%
%To test this function, one can use the same values that are given in the
%IERS's implementation, DEHANTTIDEINEL.F Those are
% rStation = [4075578.385;931852.890;4801570.154];
% rSun=[137859926952.015;54228127881.4350;23509422341.6960];
% rMoon=[-179996231.920342;-312468450.131567;-169288918.592160];
% addPermTide=false;
% [Jul1,Jul2]=Cal2TT(2009,4,13,0,0,0);
% DeltaR=solidTideShift(rStation,rSun,rMoon,addPermTide,Jul1,Jul2);
%
%The value of DeltaR truncated to 9 places should be about
%DeltaR = 0.0770042035
%         0.0630405632
%         0.0551656815
%
%REFERENCES:
%[1] G. Petit and B. Luzum, IERS Conventions (2010), International Earth
%    Rotation and Reference Systems Service Std. 36, 2010.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The convention for indexing arrays is that 1 refers to a value with
%respect to lunar parameters and 2 refers to a value with respect to solar
%parameters.

if(nargin<6||isempty(addPermTide))
    addPermTide=false;
end

%%%%%%General constants that are used%%%%%%
Re=Constants.EarthEqRadius;%meters
GMGMe(1)=Constants.MoonEarthMassRatio;
GMGMe(2)=Constants.SunEarthMassRatio;

%%%%%%Convert the input parameters into different coordinate systems%%%%%%

%Obtain the spherical coordinates of the station.
pointSpherStation=Cart2Sphere(rStation);
lambda=pointSpherStation(2);%Geocentric longitude
phi=pointSpherStation(3);%Geocentric latitude.
r=pointSpherStation(1);
rHat=rStation/r;%A unit vector in the direction of the station.

%Get East-North-Up Axes using a local SPHERICAL Earth model. A flattening
%factor of zero makes the Earth flat.
uENU=getENUAxes([phi;lambda;0],false,r,0);
uE=uENU(:,1);%Spherical East unit vector
uN=uENU(:,2);%Spherical North unit vector.
uU=uENU(:,3);%Spherical radial *up) unit vector

%Extract the x,y, and z coordinates of the moon.
Xj(1)=rMoon(1);
Yj(1)=rMoon(2);
Zj(1)=rMoon(3);
%Obtain the spherical coordinates of the moon.
pointSpherMoon=Cart2Sphere(rMoon);
Lambdaj(1)=pointSpherMoon(2);
Phij(1)=pointSpherMoon(3);
Rj(1)=pointSpherMoon(1);
RjHat(:,1)=rMoon/Rj(1);%A unit vector in the direction of the moon.

%Extract the x,y, and z coordinates of the sun.
Xj(2)=rSun(1);
Yj(2)=rSun(2);
Zj(2)=rSun(3);
%Obtain the spherical coordinates of the sun.
pointSpherSun=Cart2Sphere(rSun);
Lambdaj(2)=pointSpherSun(2);
Phij(2)=pointSpherSun(3);
Rj(2)=pointSpherSun(1);
RjHat(:,2)=rSun/Rj(2);%A unit vector in the direction of the sun.

%%%%%%Determine the solid Earth tidal displacement.%%%%%%
%This is a three step process. The first two steps are described in the
%table on page 103 of the 2010 IERS conventions.

%%%Step 1: time-domain contributions
%%First, the in-phase contribution.

%Determine the second degree nomial Love (h) and Shida (l) numbers using
%the latitude correction in the second paragraph on page 105.
h0=0.6078;
h2=-0.0006;
l0=0.0847;
l2=0.0002;

h2=h0+h2*(3*sin(phi)^2-1)/2;
l2=l0+l2*(3*sin(phi)^2-1)/2;

%Equation 7.5 for the degree 2 tidal offset
DeltaR=0;
for n=1:2
    RjHatDotrHat=dot(RjHat(:,n),rHat);
    DeltaR=DeltaR+GMGMe(n)*(Re^4/Rj(n)^3)*(h2*rHat*(3*RjHatDotrHat^2-1)/2+3*l2*RjHatDotrHat*(RjHat(:,n)-RjHatDotrHat*rHat));
end

%Equation 7.6 for the degree 3 tidal offset
h3=0.292;
l3=0.015;
for n=1:2
    RjHatDotrHat=dot(RjHat(:,n),rHat);
    DeltaR=DeltaR+GMGMe(n)*(Re^5/Rj(n)^4)*(h3*rHat*((5/2)*RjHatDotrHat^3-(3/2)*RjHatDotrHat)+l3*((15/2)*RjHatDotrHat^2-3/2)*(RjHat(:,n)-RjHatDotrHat*rHat));
end

%Equation 7.8 for the diurnal latitude dependence of the Love numbers.
l1Diurn=0.0012;
deltaT=0;
for n=1:2
    P12Cos=3*Xj(n)*Zj(n)/Rj(n)^2;%Equation 7.7b
    P12Sin=3*Yj(n)*Zj(n)/Rj(n)^2;%Equation 7.7b

    %This is a term in the sum of Equation 7.8. However, it does not look
    %like Equation 7.8, since angle difference formulae had to be used to
    %get terms suitable for the values in Equation 7.7b to be used.
    %Specifically,
    %sin(lambda-Lambdaj(n))=sin(lambda)*cos(Lambdaj(n))-cos(lambda)*sin(Lambdaj(n));
    %cos(lambda-Lambdaj(n))=cos(lambda)*cos(Lambdaj(n))+sin(lambda)*sin(Lambdaj(n));
    deltaT=deltaT+GMGMe(n)*(Re^4/Rj(n)^3)*(uN*sin(phi)*(cos(lambda)*P12Cos+sin(lambda)*P12Sin)-uE*cos(2*phi)*(sin(lambda)*P12Cos-cos(lambda)*P12Sin));
end
deltaT=-l1Diurn*sin(phi)*deltaT;

DeltaR=DeltaR+deltaT;

%Equation 7.9 for the semi-diurnal latitude dependence of the Love numbers.
l1SemiDiurn=0.0024;
deltaT=0;
for n=1:2
    P22Cos=(3/Rj(n)^2)*(Xj(n)^2-Yj(n)^2);%Equation 7.7c
    P22Sin=(6/Rj(n)^2)*Xj(n)*Yj(n);%Equation 7.7c

    %This is a term in the sum of Equation 7.9. However, it does not look
    %like Equation 7.9, since angle difference formulae had to be used to
    %get terms suitable for the values in Equation 7.7c to be used.
    %Specifically,
    %sin(2*(lambda-Lambdaj(n)))=sin(2*lambda)*cos(2*Lambdaj(n))-cos(2*lambda)*sin(2*Lambdaj(n));
    %cos(2*(lambda-Lambdaj(n)))=cos(2*lambda)*cos(2*Lambdaj(n))+sin(2*lambda)*sin(2*Lambdaj(n));
    deltaT=deltaT+GMGMe(n)*(Re^4/Rj(n)^3)*(uN*(cos(2*lambda)*P22Cos+sin(2*lambda)*P22Sin)+uE*sin(phi)*(sin(2*lambda)*P22Cos-cos(2*lambda)*P22Sin));
end
deltaT=-(1/2)*l1SemiDiurn*sin(phi)*cos(phi)*deltaT;

DeltaR=DeltaR+deltaT;

%%Next, the out-of-phase contributions for the second degree terms

%First, the diurnal tides in Equations 7.10a and 7.10b
hIDiurn=-0.0025;
lIDirun=-0.0007;

deltaR=0;
deltaT=0;
for n=1:2
    deltaR=deltaR+GMGMe(n)*(Re^4/Rj(n)^3)*sin(2*Phij(n))*sin(2*phi)*sin(lambda-Lambdaj(n));
    deltaT=deltaT+GMGMe(n)*(Re^4/Rj(n)^3)*sin(2*Phij(n))*(cos(2*phi)*sin(lambda-Lambdaj(n))*uN+sin(phi)*cos(lambda-Lambdaj(n))*uE);
end
deltaR=-(3/4)*hIDiurn*deltaR;
deltaT=-(3/2)*lIDirun*deltaT;

DeltaR=DeltaR+deltaR*uU+deltaT;

%Next, the semidiurnal tides in Equations 7.11a and 7.11b.
hISemiDiurn=-0.0022;
lISemiDiurn=-0.0007;

deltaR=0;
deltaT=0;
for n=1:2
    deltaR=deltaR+GMGMe(n)*(Re^4/Rj(n)^3)*cos(Phij(n))^2*cos(phi)^2*sin(2*(lambda-Lambdaj(n)));
    deltaT=deltaT+GMGMe(n)*(Re^4/Rj(n)^3)*cos(Phij(n))^2*(sin(2*phi)*sin(2*(lambda-Lambdaj(n)))*uN-2*cos(phi)*cos(2*(lambda-Lambdaj(n)))*uE);
end
deltaR=-(3/4)*hISemiDiurn*deltaR;
deltaT=(3/4)*lISemiDiurn*deltaT;

DeltaR=DeltaR+deltaR*uU+deltaT;

%%%Step 2: frequency-domain contributions

%Only apply the corrections if a Julian date has been provided.
if(nargin>4)
    %%First, the contributions for the diurnal band
      %Convert Terrestrial Time to Julian centuries since J2000.0. A Julian
      %century is defined to have 36525 days in it. The offset of 2451545.0
      %days is the Julian date at J2000.0 in TDB. In TT, a difference of a
      %few milliseconds exists.
      [TDB1,TDB2]=TT2TDB(Jul1,Jul2);
      T=((TDB1-2451545.0)+TDB2)/36525;

      %Convert the Julian date to UTC and determine the fractional number
      %of hours passed in the day assuming precisely 24 hours in a say.
      [Jul1,Jul2]=TT2UTC(Jul1,Jul2);
      [~,~,~,dayFrac]=UTC2Cal(Jul1,Jul2,true);
      FHR=24*dayFrac;

    DeltaR=DeltaR+diurBandCorr(rStation,FHR,T);

    %%Next, the contributions from the long-period band
    DeltaR=DeltaR+longBandCorr(rStation,T);
end

if(addPermTide~=false)
    P2=(3*sin(phi)^2-1)/2;
    %Equation 7.14a in the IERS conventions
    deltaR=(-0.1206+0.0001*P2)*P2;
    %Equation 7.14b in the IERS conventions
    deltaT=(-0.0252-0.0001*P2)*sin(2*phi);
    DeltaR=DeltaR+deltaR*uU+deltaT*uN;
end
end

function XCORSTA=diurBandCorr(XSTA,FHR,T)
%%DIURBANDCORR  This subroutine is a Matlab translation of the subroutine
%               STEP2DIU that is provided by the IERS at
%               ftp://tai.bipm.org/iers/convupdt/chapter7/
%               The algorithm is not fully documented by the IERS 2010
%               conventions. For example, most of the constants are not
%               listed anywhere in the documentation.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%
%SUBROUTINE STEP2DIU (XSTA,FHR,T,XCORSTA)
%
%  - - - - - - - - - - -
%   S T E P 2 D I U
%  - - - - - - - - - - -
%
%  This routine is part of the International Earth Rotation and
%  Reference Systems Service (IERS) Conventions software collection.
%
%  This subroutine gives the in-phase and out-of-phase corrections
%  induced by mantle anelasticity in the diurnal band.
%
%  In general, Class 1, 2, and 3 models represent physical effects that
%  act on geodetic parameters while canonical models provide lower-level
%  representations or basic computations that are used by Class 1, 2, or
%  3 models.
%
%  Status: Class 1
%
%     Class 1 models are those recommended to be used a priori in the
%     reduction of raw space geodetic data in order to determine
%     geodetic parameter estimates.
%     Class 2 models are those that eliminate an observational
%     singularity and are purely conventional in nature.
%     Class 3 models are those that are not required as either Class
%     1 or 2.
%     Canonical models are accepted as is and cannot be classified as a
%     Class 1, 2, or 3 model.
%
%  Given:
%     XSTA          d(3)   Geocentric position of the IGS station (Note 1)
%     FHR           d      Fractional hours in the day (Note 2)
%     T             d      Centuries since J2000
%
%  Returned:
%     XCORSTA       d(3)   In phase and out of phase station corrections
%                          for diurnal band (Note 4)
%
%  Notes:
%
%  1) The IGS station is in ITRF co-rotating frame.  All coordinates are
%     expressed in meters.
%
%  2) The fractional hours in the day is computed as the hour + minutes/60.0
%     + sec/3600.0.  The unit is expressed in Universal Time (UT).
%
%  4) All coordinates are expressed in meters.
%
%  Test case:
%     given input: XSTA(1) = 4075578.385D0 meters
%                  XSTA(2) =  931852.890D0 meters
%                  XSTA(3) = 4801570.154D0 meters
%                  FHR     = 0.00D0 hours
%                  T       = 0.1059411362080767D0 Julian centuries
%
%     expected output:  XCORSTA(1) = 0.4193085327321284701D-02 meters
%                       XCORSTA(2) = 0.1456681241014607395D-02 meters
%                       XCORSTA(3) = 0.5123366597450316508D-02 meters
%
%  References:
%
%     Mathews, P. M., Dehant, V., and Gipson, J. M., 1997, ''Tidal station
%     displacements," J. Geophys. Res., 102(B9), pp. 20,469-20,477
%
%     Petit, G. and Luzum, B. (eds.), IERS Conventions (2010),
%     IERS Technical Note No. 36, BKG (2010)
%
%  Revisions:
%  1996 March    23 V. Dehant      Original code
%  2009 July     31 B.E. Stetzler  Initial standardization of code
%  2009 August   06 B.E. Stetzler  Provided a test case
%  2009 August   06 B.E. Stetzler  Capitalized all variables for
%                                  Fortran 77 compatibility
%  2010 October  20 B.E. Stetzler  Input T corrected to be number of
%                                  centuries since J2000
%-----------------------------------------------------------------------

      D2PI = 6.283185307179586476925287;

      DATDI=[-3,    0,  2,   0,  0, -0.01,   0,      0,      0;
             -3,    2,  0,   0,  0, -0.01,   0,      0,      0;
             -2,    0,  1,  -1,  0, -0.02,   0,      0,      0;
             -2,    0,  1,   0,  0, -0.08,   0,     -0.01,   0.01;
             -2,    2, -1,   0,  0, -0.02,   0,      0,      0;
             -1,    0,  0,  -1,  0, -0.10,   0,      0,      0;
             -1,    0,  0,   0,  0, -0.51,   0,     -0.02,   0.03;
             -1,    2,  0,   0,  0,  0.01,   0,      0,      0;
              0,   -2,  1,   0,  0,  0.01,   0,      0,      0;
              0,    0, -1,   0,  0,  0.02,   0,      0,      0;
              0,    0,  1,   0,  0,  0.06,   0,      0,      0;
              0,    0,  1,   1,  0,  0.01,   0,      0,      0;
              0,    2, -1,   0,  0,  0.01,   0,      0,      0;
              1,   -3,  0,   0,  1, -0.06,   0,      0,      0;
              1,   -2,  0,  -1,  0,  0.01,   0,      0,      0;
              1,   -2,  0,   0,  0, -1.23,  -0.07,   0.06,   0.01;
              1,   -1,  0,   0, -1,  0.02,   0,      0,      0;
              1,   -1,  0,   0,  1,  0.04,   0,      0,      0;
              1,    0,  0,  -1,  0, -0.22,   0.01,   0.01,   0;
              1,    0,  0,   0,  0, 12.00,  -0.80,  -0.67,  -0.03;
              1,    0,  0,   1,  0,  1.73,  -0.12,  -0.10,   0;
              1,    0,  0,   2,  0, -0.04,   0,      0,      0;
              1,    1,  0,   0, -1, -0.50,  -0.01,   0.03,   0;
              1,    1,  0,   0,  1,  0.01,   0,      0,      0;
              0,    1,  0,   1, -1, -0.01,   0,      0,      0;
              1,    2, -2,   0,  0, -0.01,   0,      0,      0;
              1,    2,  0,   0,  0, -0.11,   0.01,   0.01,   0;
              2,   -2,  1,   0,  0, -0.01,   0,      0,      0;
              2,    0, -1,   0,  0, -0.02,   0,      0,      0;
              3,    0,  0,   0,  0,  0,      0,      0,      0;
              3,    0,  0,   1,  0,  0,      0,      0,      0];

    DEG2RAD = D2PI/360;

%  Compute the phase angles in degrees.
    S = 218.31664563+(481267.88194+(-0.0014663889+(0.00000185139)*T)*T)*T;

    TAU = FHR*15+280.4606184+(36000.7700536+(0.00038793+(-0.0000000258)*T)*T)*T+(-S);

    PR = (1.396971278+(0.000308889+(0.000000021+(0.000000007)*T)*T)*T)*T;

    S = S + PR;

    H = 280.46645+(36000.7697489+(0.00030322222+(0.000000020+(-0.00000000654)*T)*T)*T)*T;

    P = 83.35324312+(4069.01363525+(-0.01032172222+(-0.0000124991+(0.00000005263)*T)*T)*T)*T;

    ZNS = 234.95544499+(1934.13626197+(-0.00207561111+(-0.00000213944+(0.00000001650)*T)*T)*T)*T;

    PS = 282.93734098+(1.71945766667+(0.00045688889+(-0.00000001778+(-0.00000000334)*T)*T)*T)*T;

% Reduce angles to between the range 0 and 360.
    S =  mod(S,360);
    TAU = mod(TAU,360);
    H =  mod(H,360);
    P =  mod(P,360);
    ZNS = mod(ZNS,360);
    PS = mod(PS,360);

    RSTA = norm(XSTA);
    SINPHI = XSTA(3)/RSTA;
    COSPHI = norm(XSTA(1:2))/RSTA;

    COSLA = XSTA(1)/COSPHI/RSTA;
    SINLA = XSTA(2)/COSPHI/RSTA;
    ZLA = atan2(XSTA(2),XSTA(1));

% Initialize.
    XCORSTA=zeros(3,1);

    for J=1:31
        % Convert from degrees to radians.
        THETAF=(TAU+DATDI(J,1)*S+DATDI(J,2)*H+DATDI(J,3)*P+DATDI(J,4)*ZNS+DATDI(J,5)*PS)*DEG2RAD;

        DR=DATDI(J,6)*2*SINPHI*COSPHI*sin(THETAF+ZLA)+DATDI(J,7)*2*SINPHI*COSPHI*cos(THETAF+ZLA);

        DN=DATDI(J,8)*(COSPHI^2-SINPHI^2)*sin(THETAF+ZLA)+DATDI(J,9)*(COSPHI^2-SINPHI^2)*cos(THETAF+ZLA);
        %      DE=DATDI(8,J)*SINPHI*COS(THETAF+ZLA)+
        %     Modified 20 June 2007

        DE=DATDI(J,8)*SINPHI*cos(THETAF+ZLA)-DATDI(J,9)*SINPHI*sin(THETAF+ZLA);

        XCORSTA(1)=XCORSTA(1)+DR*COSLA*COSPHI-DE*SINLA-DN*SINPHI*COSLA;
        XCORSTA(2)=XCORSTA(2)+DR*SINLA*COSPHI+DE*COSLA-DN*SINPHI*SINLA;
        XCORSTA(3)=XCORSTA(3)+DR*SINPHI+DN*COSPHI;
    end

    XCORSTA=XCORSTA/1000;

%  Finished.

%+----------------------------------------------------------------------
%
%  Copyright (C) 2008
%  IERS Conventions Center
%
%  ==================================
%  IERS Conventions Software License
%  ==================================
%
%  NOTICE TO USER:
%
%  BY USING THIS SOFTWARE YOU ACCEPT THE FOLLOWING TERMS AND CONDITIONS
%  WHICH APPLY TO ITS USE.
%
%  1. The Software is provided by the IERS Conventions Center ("the
%     Center").
%
%  2. Permission is granted to anyone to use the Software for any
%     purpose, including commercial applications, free of charge,
%     subject to the conditions and restrictions listed below.
%
%  3. You (the user) may adapt the Software and its algorithms for your
%     own purposes and you may distribute the resulting "derived work"
%     to others, provided that the derived work complies with the
%     following requirements:
%
%     a) Your work shall be clearly identified so that it cannot be
%        mistaken for IERS Conventions software and that it has been
%        neither distributed by nor endorsed by the Center.
%
%     b) Your work (including source code) must contain descriptions of
%        how the derived work is based upon and/or differs from the
%        original Software.
%
%     c) The name(s) of all modified routine(s) that you distribute
%        shall be changed.
%
%     d) The origin of the IERS Conventions components of your derived
%        work must not be misrepresented; you must not claim that you
%        wrote the original Software.
%
%     e) The source code must be included for all routine(s) that you
%        distribute.  This notice must be reproduced intact in any
%        source distribution.
%
%  4. In any published work produced by the user and which includes
%     results achieved by using the Software, you shall acknowledge
%     that the Software was used in obtaining those results.
%
%  5. The Software is provided to the user "as is" and the Center makes
%     no warranty as to its use or performance.   The Center does not
%     and cannot warrant the performance or results which the user may
%     obtain by using the Software.  The Center makes no warranties,
%     express or implied, as to non-infringement of third party rights,
%     merchantability, or fitness for any particular purpose.  In no
%     event will the Center be liable to the user for any consequential,
%     incidental, or special damages, including any lost profits or lost
%     savings, even if a Center representative has been advised of such
%     damages, or for any claim by any third party.
%
%  Correspondence concerning IERS Conventions software should be
%  addressed as follows:
%
%                     Gerard Petit
%     Internet email: gpetit[at]bipm.org
%     Postal address: IERS Conventions Center
%                     Time, frequency and gravimetry section, BIPM
%                     Pavillon de Breteuil
%                     92312 Sevres  FRANCE
%
%     or
%
%                     Brian Luzum
%     Internet email: brian.luzum[at]usno.navy.mil
%     Postal address: IERS Conventions Center
%                     Earth Orientation Department
%                     3450 Massachusetts Ave, NW
%                     Washington, DC 20392
%
%
%-----------------------------------------------------------------------
end

function XCORSTA=longBandCorr(XSTA,T)
%%DIURBANDCORR  This subroutine is a Matlab translation of the subroutine
%               STEP2LON that is provided by the IERS at
%               ftp://tai.bipm.org/iers/convupdt/chapter7/
%               The algorithm is not fully documented by the IERS 2010
%               conventions. For example, most of the constants are not
%               listed anywhere in the documentation.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%
%SUBROUTINE STEP2LON (XSTA,T,XCORSTA)
%
%  - - - - - - - - - - -
%   S T E P 2 L O N
%  - - - - - - - - - - -
%
%  This routine is part of the International Earth Rotation and
%  Reference Systems Service (IERS) Conventions software collection.
%
%  This subroutine gives the in-phase and out-of-phase corrections
%  induced by mantle anelasticity in the long period band.
%
%  In general, Class 1, 2, and 3 models represent physical effects that
%  act on geodetic parameters while canonical models provide lower-level
%  representations or basic computations that are used by Class 1, 2, or
%  3 models.
%
%  Status: Class 1
%
%     Class 1 models are those recommended to be used a priori in the
%     reduction of raw space geodetic data in order to determine
%     geodetic parameter estimates.
%     Class 2 models are those that eliminate an observational
%     singularity and are purely conventional in nature.
%     Class 3 models are those that are not required as either Class
%     1 or 2.
%     Canonical models are accepted as is and cannot be classified as a
%     Class 1, 2, or 3 model.
%
%  Given:
%     XSTA          d(3)   Geocentric position of the IGS station (Note 1)
%     T             d      Centuries since J2000
%
%  Returned:
%     XCORSTA       d(3)   In phase and out of phase station corrections
%                          for diurnal band (Note 2)
%
%  Notes:
%
%  1) The IGS station is in ITRF co-rotating frame.  All coordinates are
%     expressed in meters.
%
%  2) All coordinates are expressed in meters.
%
%  Test case:
%     given input: XSTA(1) = 4075578.385D0 meters
%                  XSTA(2) =  931852.890D0 meters
%                  XSTA(3) = 4801570.154D0 meters
%                  T       = 0.1059411362080767D0 Julian centuries
%
%     expected output:  XCORSTA(1) = -0.9780962849562107762D-04 meters
%                       XCORSTA(2) = -0.2236349699932734273D-04 meters
%                       XCORSTA(3) =  0.3561945821351565926D-03 meters
%
%  References:
%
%     Mathews, P. M., Dehant, V., and Gipson, J. M., 1997, ''Tidal station
%     displacements," J. Geophys. Res., 102(B9), pp. 20,469-20,477
%
%     Petit, G. and Luzum, B. (eds.), IERS Conventions (2010),
%     IERS Technical Note No. 36, BKG (2010)
%
%  Revisions:
%  1996 March    23 V. Dehant      Original code
%  2009 August   07 B.E. Stetzler  Initial standardization of code
%                                  and found unnecessary variables tau
%                                  and fhr
%  2009 August   07 B.E. Stetzler  Provided a test case
%  2009 August   07 B.E. Stetzler  Capitalized all variables for
%                                  Fortran 77 compatibility
%  2010 October  20 B.E. Stetzler  Input T corrected to be number of
%                                  centuries since J2000
%-----------------------------------------------------------------------

    D2PI = 6.283185307179586476925287;

    DATDI=[0, 0, 0, 1, 0,   0.47, 0.23, 0.16, 0.07;
         0, 2, 0, 0, 0,  -0.20,-0.12,-0.11,-0.05;
         1, 0,-1, 0, 0,  -0.11,-0.08,-0.09,-0.04;
         2, 0, 0, 0, 0,  -0.13,-0.11,-0.15,-0.07;
         2, 0, 0, 1, 0,  -0.05,-0.05,-0.06,-0.03];

    DEG2RAD = D2PI/360;

%  Compute the phase angles in degrees.
    S = 218.31664563+(481267.88194+(-0.0014663889+(0.00000185139)*T)*T)*T;

    PR = (1.396971278+(0.000308889+(0.000000021+(0.000000007)*T)*T)*T)*T;

    S = S + PR;

    H = 280.46645+(36000.7697489+(0.00030322222+(0.000000020+(-0.00000000654)*T)*T)*T)*T;

    P = 83.35324312+(4069.01363525+(-0.01032172222+(-0.0000124991+(0.00000005263)*T)*T)*T)*T;

    ZNS = 234.95544499+(1934.13626197+(-0.00207561111+(-0.00000213944+(0.00000001650)*T)*T)*T)*T;

    PS = 282.93734098+(1.71945766667+(0.00045688889+(-0.00000001778+(-0.00000000334)*T)*T)*T)*T;

    RSTA=norm(XSTA);
    SINPHI=XSTA(3)/RSTA;
    COSPHI=norm(XSTA(1:2))/RSTA;

    COSLA=XSTA(1)/COSPHI/RSTA;
    SINLA=XSTA(2)/COSPHI/RSTA;

% Reduce angles to between the range 0 and 360.
    S =  mod(S,360);
    %      TAU = DMOD(TAU,360D0)
    H =  mod(H,360);
    P =  mod(P,360);
    ZNS = mod(ZNS,360);
    PS = mod(PS,360);

    DR_TOT = 0;
    DN_TOT = 0;

    XCORSTA=zeros(3,1);

    for J=1:5
        THETAF=(DATDI(J,1)*S+DATDI(J,2)*H+DATDI(J,3)*P+DATDI(J,4)*ZNS+DATDI(J,5)*PS)*DEG2RAD;

        DR=DATDI(J,6)*(3D0*SINPHI^2-1)/2*cos(THETAF)+DATDI(J,8)*(3D0*SINPHI^2-1)/2*sin(THETAF);

        DN=DATDI(J,7)*(COSPHI*SINPHI*2)*cos(THETAF)+DATDI(J,9)*(COSPHI*SINPHI*2)*sin(THETAF);

        DE = 0;
        DR_TOT = DR_TOT+DR;
        DN_TOT = DN_TOT+DN;

        XCORSTA(1)=XCORSTA(1)+DR*COSLA*COSPHI-DE*SINLA-DN*SINPHI*COSLA;
        XCORSTA(2)=XCORSTA(2)+DR*SINLA*COSPHI+DE*COSLA-DN*SINPHI*SINLA;
        XCORSTA(3)=XCORSTA(3)+DR*SINPHI+DN*COSPHI;
    end

    XCORSTA=XCORSTA/1000;

%  Finished.

%+----------------------------------------------------------------------
%
%  Copyright (C) 2008
%  IERS Conventions Center
%
%  ==================================
%  IERS Conventions Software License
%  ==================================
%
%  NOTICE TO USER:
%
%  BY USING THIS SOFTWARE YOU ACCEPT THE FOLLOWING TERMS AND CONDITIONS
%  WHICH APPLY TO ITS USE.
%
%  1. The Software is provided by the IERS Conventions Center ("the
%     Center").
%
%  2. Permission is granted to anyone to use the Software for any
%     purpose, including commercial applications, free of charge,
%     subject to the conditions and restrictions listed below.
%
%  3. You (the user) may adapt the Software and its algorithms for your
%     own purposes and you may distribute the resulting "derived work"
%     to others, provided that the derived work complies with the
%     following requirements:
%
%     a) Your work shall be clearly identified so that it cannot be
%        mistaken for IERS Conventions software and that it has been
%        neither distributed by nor endorsed by the Center.
%
%     b) Your work (including source code) must contain descriptions of
%        how the derived work is based upon and/or differs from the
%        original Software.
%
%     c) The name(s) of all modified routine(s) that you distribute
%        shall be changed.
%
%     d) The origin of the IERS Conventions components of your derived
%        work must not be misrepresented; you must not claim that you
%        wrote the original Software.
%
%     e) The source code must be included for all routine(s) that you
%        distribute.  This notice must be reproduced intact in any
%        source distribution.
%
%  4. In any published work produced by the user and which includes
%     results achieved by using the Software, you shall acknowledge
%     that the Software was used in obtaining those results.
%
%  5. The Software is provided to the user "as is" and the Center makes
%     no warranty as to its use or performance.   The Center does not
%     and cannot warrant the performance or results which the user may
%     obtain by using the Software.  The Center makes no warranties,
%     express or implied, as to non-infringement of third party rights,
%     merchantability, or fitness for any particular purpose.  In no
%     event will the Center be liable to the user for any consequential,
%     incidental, or special damages, including any lost profits or lost
%     savings, even if a Center representative has been advised of such
%     damages, or for any claim by any third party.
%
%  Correspondence concerning IERS Conventions software should be
%  addressed as follows:
%
%                     Gerard Petit
%     Internet email: gpetit[at]bipm.org
%     Postal address: IERS Conventions Center
%                     Time, frequency and gravimetry section, BIPM
%                     Pavillon de Breteuil
%                     92312 Sevres  FRANCE
%
%     or
%
%                     Brian Luzum
%     Internet email: brian.luzum[at]usno.navy.mil
%     Postal address: IERS Conventions Center
%                     Earth Orientation Department
%                     3450 Massachusetts Ave, NW
%                     Washington, DC 20392
%
%
%-----------------------------------------------------------------------
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.


% edgeNL: Edge detection function based on nonlinear derivatives
%   (elementary demo):
%   This function differientiates the image to estimate the gradient.
%   The principle is based on polarized derivatives to automatically select
%   the best edge localization.
%   The main benefits are
%     - univocal edge localization for synthetic and real images
%     - noise reduction with no regularization: the noise level is weaker than
%       the noise level in the original image
%     - better direction estimation of the gradient
%     - can product a confident edge reference map for synthetic images
%     - extremely efficient on salt noise OR pepper noise (this last case needs
%       a change in the nonlinear derivatives)
%     - still noise reduction with regularized schemes (Canny, Demigny, ...,
%       can also be adapted to the asymetrical filters (Prewitt, Sobel, ...)
%   Drawback
%     - no detection of vertical and horizontal "white" thin (1 pixel) lines
%   Rk: this demo only performs edge detection and does not include edge
%       extraction (local maxima) and other steps to obtain a binary
%       edge map.
%   Written by O. Laligant - 2009, University of Burgundy
%   Ref: A Nonlinear Derivative Scheme Applied to Edge Detection,
%       Olivier Laligant, Frederic Truchetet, IEEE Transactions on Pattern
%       Analysis and Machine Intelligence - PAMI , vol. 32, no. 2,
%       pp. 242-257, 2010
function edgeNL()

%  ------------- Edge detection on a simple synthetic image  -------------
Im = example();
figure(1), imagesc(Im);
title ('Original image');

% edge detection (nonlinear gradient estimate)
% better localization and direction estimation than the linear scheme
gI = algoNL(Im);

% For a synthetic object, the edges are localized inside the object shape
figure(2), imagesc(gI);
title('Edge detection');

% ------------- Additive gaussian white noise ------------------------

Imn = imnoise(Im, 'gaussian', 0, 0.01);
figure(3), imagesc(Imn);
title ('Noisy image');

gIn = algoNL(Imn);

figure(4), imagesc(gIn);
title('Edge detection on the noisy image');

end
% ----------------------------------------------------------------------------

%
%  --------------------------- functions  -------------------------------------
%

% ------------- computation of the nonlinear derivatives  -------------
% polarized derivatives
% lead to an univocal localization of edges
function [gm, gh, gv] = algoNL(I);
% for classical regularization schemes, I can be replaced by a regularized
%   version
% for asymetrical schemes (Prewitt, Sobel, etc), I must be replaced by two
%   regularized versions (the regularization is different on columns and rows)
dph = thresh0(conv2(I, [0 1 -1], 'same'));
dnh = -thresh0(-conv2(I, [1 -1 0], 'same'));
gh = dph+dnh;
dpv = thresh0(conv2(I, [0; 1; -1], 'same'));
dnv = -thresh0(-conv2(I, [1;  -1; 0], 'same'));
gv = dpv + dnv;
gm = sqrt(gh.*gh + gv.*gv);

end

% ------------- threshold -------------
function st = thresh0(s)
	st = s.*(sign(s)+1)/2;
end%function

% ------------- test image -------------
% an example of image to illustrate the localization of the method
function Im = example()
Im =[
     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
     0     0   255   255   255     0     0     0     0     0     0     0   255     0     0
     0     0   255   255   255     0     0     0     0     0     0   255   255     0     0
     0     0   255   255   255     0     0     0     0     0   255   255   255     0     0
     0     0     0     0     0     0     0     0     0   255   255   255   255     0     0
     0     0     0     0     0     0     0     0   255   255   255   255   255     0     0
     0     0     0     0     0     0     0   255   255   255   255     0     0     0     0
     0     0     0     0     0     0   255   255   255   255     0     0     0     0     0
     0     0     0     0     0   255   255   255   255   255     0     0     0     0     0
     0     0     0     0   255   255   255     0   255   255     0     0     0     0     0
     0     0     0   255   255   255     0     0     0   255   255     0     0     0     0
     0     0   255   255   255   255   255     0   255   255   255   255   255     0     0
     0     0     0   255   255   255   255   255   255   255   255   255     0     0     0
     0     0     0     0   255   255   255   255   255   255   255   255   255     0     0
     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0
];
end


function W=twoMatDiag(C1,C2,algorithm)
%%TWOMATDIAG Given two real or complex Hermitian matrices, the first of
%            which must be positive definite, find a matrix W that
%            diagonalizes both of them. Specifically, W*C1*W'=I and
%            W*C2*W'=a diagonal matrix.
%
%INPUTS: C1 An NXN real or complex positive definite Hermitian matrix.
%        C2 An NXN real or complex Hermitian matrix. This matrix does not
%           have to be positive definite.
% algorithm An optional parameter specifying the algorithm to use. Possible
%           values are:
%           0 (The default if omitted or an empty matrix is passed) Use the
%             singular value decomposition (SVD)-based algorithm given in
%             [1].
%           1 Use the eigenvalue-based decomposition given in [2]. There is
%             generally not a need to use the algorithmic variant. It is
%             not implemented in the C++/mex version of this function.
%
%OUTPUTS: W An NXN matrix such that W*C1*W'=identity matrix and W*C2*W'=a
%           diagonal matrix.
%
%Joint diagonalization of a pair of matrices arises in the fusion problem
%discussed in [1] and [2], among other applications. If both matrices are not
%positive definite, then the jointMatDiagFrob function can produce the
%desired W diagonalization matrix.
%
%EXAMPLE 1:
%Here, we diagonalize two real, positive definite matrices:
% C1=[87, 25, 18, 31;
%     25, 63, 20, 17;
%     18, 20, 65, 29;
%     31, 17, 29, 65];
% C2=[57,  7, 12, 17;
%      7, 47, 17, 22;
%     12, 17, 37, 27;
%     17, 22, 27, 27];
% %C1 is positive definite. C2 is positive semi-definite.
% W=twoMatDiag(C1,C2);
% offDiagErr1=W*C1*W'-diag(diag(W*C1*W'))
% offDiagErr2=W*C2*W'-diag(diag(W*C2*W'))
%One will see that the off-diagonal errors are on the order of 1e-16, which
%is around what one would expect with finite precision limitiations.
%
%EXAMPLE 2:
%This is the diagonalization of two complex Hermitian matrices, the first
%of which is positive definite and the second of which has some negative
%eigenvalues.
%C1 is positive definite.
% C1=[  9+  0*1i,   -65+  0*1i,  -11-153*1i,  -91-173*1i;
%     -65+  0*1i,    83+  0*1i,   54- 38*1i,   31+ 28*1i;
%     -11+153*1i,    54+ 38*1i,  130+  0*1i,   16- 47*1i;
%     -91+173*1i,    31- 28*1i,   16+ 47*1i,   22+  0*1i]+215*eye(4);
% %C2 has both positive and negative eigenvalues.
% C2=[-16+  0*1i,  -32- 56*1i,  -12-128*1i,   16+114*1i;
%     -32+ 56*1i,   79+  0*1i,  -87- 67*1i,  -48+ 51*1i;
%     -12+128*1i,  -87+ 67*1i,   76+  0*1i,   -7- 96*1i;
%      16-114*1i,  -48- 51*1i,   -7+ 96*1i, -147+  0*1i];
% W=twoMatDiag(C1,C2);
% offDiagErr=W*C1*W'-diag(diag(W*C1*W'))
% offDiagErr=W*C2*W'-diag(diag(W*C2*W'))
%One will see that the errors are on the order of 1e-14 or less, which is
%around what one would expect with finite precision errors.
%
%REFERENCES:
%[1] J. Nygårds, V. Deleskog, and G. Hendeby, "Safe fusion compared to
%    established distributed fusion methods," in IEEE International
%    Conference on Multisensor Fusion and Integration for Intelligent
%    Systems, Baden-Baden, Germany, 19-21 Sep. 2016, pp. 265-271.
%[2] M. Reinhardt, B. Noack, and U. D. Hanebeck, "Closed-form optimization
%    of covariance intersection for low-dimensional matrices," in
%    Proceedings of the 15th International Conference on Information
%    Fusion, Singapore, 9-12 Jun. 2012, pp. 1891-1896.
%
%February 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

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

switch(algorithm)
    case 0
        %Use the SVD-based algorithm.
        %Note that C1=U1*D1*U1' for a valid covariance matrix.
        [U1,D1,~]=svd(C1);%Equation 6
        d1=diag(D1);

        D1Root=diag(1./sqrt(d1));

        temp=U1*D1Root;
        [U2,~,~]=svd(temp'*C2*temp);%Equation 7 in [1].

        W=U2'*D1Root*U1';%Equation 8a in [1].
        %Note that W*C1*W'=eye(xDim,xDim);
    case 1
        %Use the eigenvalue-based algorithm.
        [V1,E1]=eig(C1);
        T1=inv(V1*diag(sqrt(diag(E1))));
        C2p=T1*C2*T1';
        [V2p,~]=eig(C2p);

        %The transformation matrix in Equation 2 of [2].
        W=V2p'*diag(1./sqrt(diag(E1)))*V1';
    otherwise
        error('Unknown algorithm specified.')
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.


%% SOBOL_TEST04 tests SOBOL.
%
global SOBOL_lastq;
global SOBOL_seed;

fprintf ( 1, '\n' );
fprintf ( 1, 'SOBOL_TEST04\n' );
fprintf ( 1, '  SOBOL returns the next element\n' );
fprintf ( 1, '  of a Sobol sequence.\n' );
fprintf ( 1, '\n' );
fprintf ( 1, '  In this test, we call Sobol repeatedly.\n' );

dim_max = 4;

for ( dim_num = 2 : dim_max )

  SOBOL_seed = 0;
  seed = 0;
  qs = prime_ge ( dim_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Using dimension DIM_NUM =   %d\n', dim_num );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Seed  Seed   Sobol\n' );
  fprintf ( 1, '  In    Out\n' );
  fprintf ( 1, '\n' );
  for ( i = 0 : 110 )
    [ r, seed_out ] = sobol ( dim_num, seed );
    if ( i <= 11 || 95 <= i )
      fprintf ( 1, '%6d %6d  ', seed, seed_out );
      for ( j = 1 : dim_num )
        fprintf ( 1, '%10f  ', r(j) );
      end
      fprintf ( 1, '\n' );
    elseif ( i == 12 )
      fprintf ( 1, '......................\n' );
    end
    seed = seed_out;
  end

end


function [ out ] = proj_box( x,l,u )
%PROJ_BOX computes the orthogonal projection onto the box {x:l<=x<=u}
%
%  Usage:
%  out = PROJ_BOX(x,l,u)
%  ===========================================
%  Input:
%  x - point to be projected (vector/matrix)
%  l - lower bound (vector/matrix/scalar)
%  u - upper bound (vector/matrix/scalar)
%  ===========================================
%  Assumptions:
%  l<=u
%  ===========================================
%  Output:
%  out - projection vector

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

%reading the user x and
if (nargin < 3)
    error ('usage: proj_box( x,l,u )') ;
end

if any(any((l > u)))
    error('Set is infeasible') ;
end

out= min(max(l,x),u) ;
end


function vol=surfvolume(node,face,option)
%
% vol=surfvolume(node,face,option)
%
% calculate the enclosed volume for a closed surface
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%    node:  node coordinates
%    face:  surface triangle list
%
% output:
%    vol:   total volume of the enclosed space
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

face=face(:,1:3);

ed=surfedge(face);
if(~isempty(ed))
   error('open surface is detected, you have to close it first, consider meshcheckrepair() with meshfix option');
end

[no,el]=fillsurf(node,face);

vol=elemvolume(no,el);
vol=sum(vol);


function [ n_data, x, fx ] = i1ml1_values ( n_data )

%*****************************************************************************80
%
%% I1ML1_VALUES returns some values of the I1ML1 function.
%
%  Discussion:
%
%    The function is defined by:
%
%      I1ML1(x) = I1(x) - L1(x)
%
%    I1(x) is the modified Bessel function of the first kind of order 1,
%    L1(x) is the modified Struve function of order 1.
%
%    The data was reported by McLeod.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Allan McLeod,
%    Algorithm 757, MISCFUN: A software package to compute uncommon
%      special functions,
%    ACM Transactions on Mathematical Software,
%    Volume 22, Number 3, September 1996, pages 288-301.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 20;

  fx_vec = [ ...
     0.97575346155386267134E-03, ...
     0.77609293280609272733E-02, ...
     0.59302966404545373770E-01, ...
     0.20395212276737365307E+00, ...
     0.33839472293667639038E+00, ...
     0.48787706726961324579E+00, ...
     0.59018734196576517506E+00, ...
     0.62604539530312149476E+00, ...
     0.63209315274909764698E+00, ...
     0.63410179313235359215E+00, ...
     0.63417966797578128188E+00, ...
     0.63439268632392089434E+00, ...
     0.63501579073257770690E+00, ...
     0.63559616677359459337E+00, ...
     0.63591001826697110312E+00, ...
     0.63622113181751073643E+00, ...
     0.63636481702133606597E+00, ...
     0.63650653499619902120E+00, ...
     0.63655609126300261851E+00, ...
     0.63657902087183929223E+00 ];

  x_vec = [ ...
       0.0019531250E+00, ...
       0.0156250000E+00, ...
       0.1250000000E+00, ...
       0.5000000000E+00, ...
       1.0000000000E+00, ...
       2.0000000000E+00, ...
       4.0000000000E+00, ...
       8.0000000000E+00, ...
      12.0000000000E+00, ...
      16.0000000000E+00, ...
      16.2500000000E+00, ...
      17.0000000000E+00, ...
      20.0000000000E+00, ...
      25.0000000000E+00, ...
      30.0000000000E+00, ...
      40.0000000000E+00, ...
      50.0000000000E+00, ...
      75.0000000000E+00, ...
     100.0000000000E+00, ...
     125.0000000000E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function inter = learn_struct_dbn_reveal(seqs, ns, max_fan_in, penalty)
% LEARN_STRUCT_DBN_REVEAL Learn inter-slice adjacency matrix given fully observable discrete time series
% inter = learn_struct_dbn_reveal(seqs, node_sizes, max_fan_in, penalty)
%
% seqs{l}{i,t} = value of node i in slice t of time-series l.
%   If you have a single time series in an N*T array D, use
%      seqs = { num2cell(D) }.
%   If you have L time series, each of length T, in an N*T*L array D, use
%      seqs= cell(1,L); for l=1:L, seqs{l} = num2cell(D(:,:,l)); end
%   or, in vectorized form,
%      seqs = squeeze(num2cell(num2cell(D),[1 2]));
% Currently the data is assumed to be discrete (1,2,...)
%
% node_sizes(i) is the number of possible values for node i
% max_fan_in is the largest number of parents we allow per node (default: N)
% penalty is weight given to the complexity penalty (default: 0.5)
%  A penalty of 0.5 gives the BIC score.
%  A penalty of 0 gives the ML score.
%  Maximizing likelihood is equivalent to maximizing mutual information between parents and child.
%
% inter(i,j) = 1 iff node in slice t connects to node j in slice t+1
%
% The parent set for each node in slice 2 is computed by evaluating all subsets of nodes in slice 1,
% and picking the largest scoring one. This takes O(n^k) time per node, where n is the num. nodes
% per slice, and k <= n is the max fan in.
% Since all the nodes are observed, we do not need to use an inference engine.
% And since we are only learning the inter-slice matrix, we do not need to check for cycles.
%
% This algorithm is described in
% - "REVEAL: A general reverse engineering algorithm for inference of genetic network
%      architectures", Liang et al. PSB 1998
% - "Extended dependency analysis of large systems",
%       Roger Conant, Intl. J. General Systems, 1988, vol 14, pp 97-141
% - "Learning the structure of DBNs", Friedman, Murphy and Russell, UAI 1998.

n = length(ns);

if nargin < 3, max_fan_in = n; end
if nargin < 4, penalty = 0.5; end

inter = zeros(n,n);

if ~iscell(seqs)
  data{1} = seqs;
end

nseq = length(seqs);
nslices = 0;
data = cell(1, nseq);
for l=1:nseq
  nslices = nslices + size(seqs{l}, 2);
  data{l} = cell2num(seqs{l})'; % each row is a case
end
ndata = nslices - nseq; % subtract off the initial slice of each sequence

% We concatenate the sequences as in the following example.
% Let there be 2 sequences of lengths 4 and 5, with n nodes per slice,
% and let i be the target node.
% Then we construct following matrix D
%
% s{1}{1,1} ... s{1}{1,3}     s{2}{1,1} ... s{2}{1,4}
% ....
% s{1}{n,1} ... s{1}{n,3}     s{2}{n,1} ... s{2}{n,4}
% s{1}{i,2} ... s{1}{i,4}     s{2}{i,2} ... s{2}{i,5}
%
% D(1:n, i) is the i'th input and D(n+1, i) is the i'th output.
%
% We concatenate each sequence separately to avoid treating the transition
% from the end of one sequence to the beginning of another as a "normal" transition.

for i=1:n
  D = [];
  for l=1:nseq
    T = size(seqs{l}, 2);
    A = cell2num(seqs{l}(:, 1:T-1));
    B = cell2num(seqs{l}(i, 2:T));
    C = [A;B];
    D = [D C];
  end
  SS = subsets(1:n, max_fan_in, 1); % skip the empty set
  nSS = length(SS);
  bic_score = zeros(1, nSS);
  ll_score = zeros(1, nSS);
  target = n+1;
  ns2 = [ns ns(i)];
  for h=1:nSS
    ps = SS{h};
    dom = [ps target];
    counts = compute_counts(D(dom, :), ns2(dom));
    CPT = mk_stochastic(counts);
    [bic_score(h), ll_score(h)] = bic_score_family(counts, CPT, ndata);
  end
  if penalty == 0
    h = argmax(ll_score);
  else
    h = argmax(bic_score);
  end
  ps = SS{h};
  inter(ps, i) = 1;
end


function [ frames, indexes ] = vec2frames( vec, Nw, Ns, direction, window, padding )
% VEC2FRAMES Splits signal into overlapped frames using indexing.
%
%   B=vec2frames(A,M,N) creates a matrix B whose columns consist of
%   segments of length M, taken at every N samples along input vector A.
%
%   [B,R]=vec2frames(A,M,N,D,W,P) creates a matrix B whose columns
%   or rows, as specified by D, consist of segments of length M, taken
%   at every N samples along the input vector A and windowed using the
%   analysis window specified by W. The division of A into frames is
%   achieved using indexes returned in R as follows: B=A(R);
%
%   Summary
%
%           A is an input vector
%
%           M is a frame length (in samples)
%
%           N is a frame shift (in samples)
%
%           D specifies if the frames in B are rows or columns,
%             i.e., D = 'rows' or 'cols', respectively
%
%           W is an optional analysis window function to be applied to
%             each frame, given as a function handle, e.g., W = @hanning
%             or as a vector of window samples, e.g., W = hanning( M )
%
%           P specifies if last frame should be padded to full length,
%             or simply discarded, i.e., P = true or false, respectively
%
%           B is the output matrix of frames
%
%           R is a matrix of indexes used for framing, such that division
%             of A into frames is achieved as follows: B=A(R);
%
%   Examples
%
%           % divide the input vector into seven-sample-long frames with a shift
%           % of three samples and return frames as columns of the output matrix
%           % (note that the last sample of the input vector is discarded)
%           vec2frames( [1:20], 7, 3 )
%
%           % divide the input vector into seven-sample-long frames with a shift
%           % of three samples and return frames as rows of the output matrix
%           % (note that the last sample of the input vector is discarded)
%           vec2frames( [1:20], 7, 3, 'rows' )
%
%           % divide the input vector into seven-sample-long frames with a shift
%           % of three samples, pad the last frame with zeros so that no samples
%           % are discarded and return frames as rows of the output matrix
%           vec2frames( [1:20], 7, 3, 'rows', [], true )
%
%           % divide the input vector into seven-sample-long frames with a shift
%           % of three samples, pad the last frame with white Gaussian noise
%           % of variance (1E-5)^2 so that no samples are discarded and
%           % return frames as rows of the output matrix
%           vec2frames( [1:20], 7, 3, 'rows', false, { 'noise', 1E-5 } )
%
%           % divide the input vector into seven-sample-long frames with a shift
%           % of three samples, pad the last frame with zeros so that no samples
%           % are discarded, apply the Hanning analysis window to each frame and
%           % return frames as columns of the output matrix
%           vec2frames( [1:20], 7, 3, 'cols', @hanning, 0 )
%
%   See also FRAMES2VEC, DEMO

%   Author: Kamil Wojcicki, UTD, July 2011

    % usage information
    usage = 'usage: [ frames, indexes ] = vec2frames( vector, frame_length, frame_shift, direction, window, padding );';

    % default settings
    switch( nargin )
    case { 0, 1, 2 }, error( usage );
    case 3, padding=false; window=false; direction='cols';
    case 4, padding=false; window=false;
    case 5, padding=false;
    end

    % input validation
    if( isempty(vec) || isempty(Nw) || isempty(Ns) ), error( usage ); end;
    if( min(size(vec))~=1 ), error( usage ); end;
    if( Nw==0 || Ns==0 ), error( usage ); end;

    vec = vec(:);                       % ensure column vector

    L = length( vec );                  % length of the input vector
    M = floor((L-Nw)/Ns+1);             % number of frames

    % perform signal padding to enable exact division of signal samples into frames
    % (note that if padding is disabled, some samples may be discarded)
    if( ~isempty(padding) )

        % figure out if the input vector can be divided into frames exactly
        E = (L-((M-1)*Ns+Nw));

        % see if padding is actually needed
        if( E>0 )

            % how much padding will be needed to complete the last frame?
            P = Nw-E;

            % pad with zeros
            if( islogical(padding) && padding )
                vec = [ vec; zeros(P,1) ];

            % pad with a specific numeric constant
            elseif( isnumeric(padding) && length(padding)==1 )
                vec = [ vec; padding*ones(P,1) ];

            % pad with a low variance white Gaussian noise
            elseif( isstr(padding) && strcmp(padding,'noise') )
                vec = [ vec; 1E-6*randn(P,1) ];

            % pad with a specific variance white Gaussian noise
            elseif( iscell(padding) && strcmp(padding{1},'noise') )
                if( length(padding)>1 ), scale = padding{2};
                else, scale = 1E-6; end;
                vec = [ vec; scale*randn(P,1) ];

            % if not padding required, decrement frame count
            % (not a very elegant solution)
            else
                M = M-1;

            end

            % increment the frame count
            M = M+1;
        end
    end

    % compute index matrix
    switch( direction )

    case 'rows'                                                 % for frames as rows
        indf = Ns*[ 0:(M-1) ].';                                % indexes for frames
        inds = [ 1:Nw ];                                        % indexes for samples
        indexes = indf(:,ones(1,Nw)) + inds(ones(M,1),:);       % combined framing indexes

    case 'cols'                                                 % for frames as columns
        indf = Ns*[ 0:(M-1) ];                                  % indexes for frames
        inds = [ 1:Nw ].';                                      % indexes for samples
        indexes = indf(ones(Nw,1),:) + inds(:,ones(1,M));       % combined framing indexes

    otherwise
        error( sprintf('Direction: %s not supported!\n', direction) );

    end

    % divide the input signal into frames using indexing
    frames = vec( indexes );

    % return if custom analysis windowing was not requested
    if( isempty(window) || ( islogical(window) && ~window ) ), return; end;

    % if analysis window function handle was specified, generate window samples
    if( isa(window,'function_handle') )
        window = window( Nw );
    end

    % make sure analysis window is numeric and of correct length, otherwise return
    if( isnumeric(window) && length(window)==Nw )

        % apply analysis windowing beyond the implicit rectangular window function
        switch( direction )
        case 'rows', frames = frames * diag( window );
        case 'cols', frames = diag( window ) * frames;
        end

    end

% EOF


function [label, model, L] = mixGaussVb(X, m, prior)
% Variational Bayesian inference for Gaussian mixture.
% Input:
%   X: d x n data matrix
%   m: k (1 x 1) or label (1 x n, 1<=label(i)<=k) or model structure
% Output:
%   label: 1 x n cluster label
%   model: trained model structure
%   L: variational lower bound
% Reference: Pattern Recognition and Machine Learning by Christopher M. Bishop (P.474)
% Written by Mo Chen (sth4nth@gmail.com).
fprintf('Variational Bayesian Gaussian mixture: running ... \n');
[d,n] = size(X);
if nargin < 3
    prior.alpha = 1;
    prior.kappa = 1;
    prior.m = mean(X,2);
    prior.v = d+1;
    prior.M = eye(d);   % M = inv(W)
end
prior.logW = -2*sum(log(diag(chol(prior.M))));

tol = 1e-8;
maxiter = 2000;
L = -inf(1,maxiter);
model = init(X,m,prior);
for iter = 2:maxiter
    model = expect(X,model);
    model = maximize(X,model,prior);
    L(iter) = bound(X,model,prior);
    if abs(L(iter)-L(iter-1)) < tol*abs(L(iter)); break; end
end
L = L(2:iter);
label = zeros(1,n);
[~,label(:)] = max(model.R,[],2);
[~,~,label(:)] = unique(label);

function model = init(X, m, prior)
n = size(X,2);
if isstruct(m)  % init with a model
    model = m;
elseif numel(m) == 1  % random init k
    k = m;
    label = ceil(k*rand(1,n));
    model.R = full(sparse(1:n,label,1,n,k,n));
elseif all(size(m)==[1,n])  % init with labels
    label = m;
    k = max(label);
    model.R = full(sparse(1:n,label,1,n,k,n));
else
    error('ERROR: init is not valid.');
end
model = maximize(X,model,prior);

% Done
function model = maximize(X, model, prior)
alpha0 = prior.alpha;
kappa0 = prior.kappa;
m0 = prior.m;
v0 = prior.v;
M0 = prior.M;
R = model.R;

nk = sum(R,1); % 10.51
alpha = alpha0+nk; % 10.58
kappa = kappa0+nk; % 10.60
v = v0+nk; % 10.63
m = bsxfun(@plus,kappa0*m0,X*R);
m = bsxfun(@times,m,1./kappa); % 10.61

[d,k] = size(m);
U = zeros(d,d,k);
logW = zeros(1,k);
r = sqrt(R');
for i = 1:k
    Xm = bsxfun(@minus,X,m(:,i));
    Xm = bsxfun(@times,Xm,r(i,:));
    m0m = m0-m(:,i);
    M = M0+Xm*Xm'+kappa0*(m0m*m0m');     % equivalent to 10.62
    U(:,:,i) = chol(M);
    logW(i) = -2*sum(log(diag(U(:,:,i))));
end

model.alpha = alpha;
model.kappa = kappa;
model.m = m;
model.v = v;
model.U = U;
model.logW = logW;

% Done
function model = expect(X, model)
alpha = model.alpha; % Dirichlet
kappa = model.kappa;   % Gaussian
m = model.m;         % Gasusian
v = model.v;         % Whishart
U = model.U;         % Whishart
logW = model.logW;
n = size(X,2);
[d,k] = size(m);

EQ = zeros(n,k);
for i = 1:k
    Q = (U(:,:,i)'\bsxfun(@minus,X,m(:,i)));
    EQ(:,i) = d/kappa(i)+v(i)*dot(Q,Q,1);    % 10.64
end
ElogLambda = sum(psi(0,0.5*bsxfun(@minus,v+1,(1:d)')),1)+d*log(2)+logW; % 10.65
Elogpi = psi(0,alpha)-psi(0,sum(alpha)); % 10.66
logRho = -0.5*bsxfun(@minus,EQ,ElogLambda-d*log(2*pi)); % 10.46
logRho = bsxfun(@plus,logRho,Elogpi);   % 10.46
logR = bsxfun(@minus,logRho,logsumexp(logRho,2)); % 10.49
R = exp(logR);

model.logR = logR;
model.R = R;

% Done
function L = bound(X, model, prior)
alpha0 = prior.alpha;
kappa0 = prior.kappa;
v0 = prior.v;
logW0 = prior.logW;
alpha = model.alpha;
kappa = model.kappa;
v = model.v;
logW = model.logW;
R = model.R;
logR = model.logR;
[d,n] = size(X);
k = size(R,2);

Epz = 0;
Eqz = dot(R(:),logR(:));
logCalpha0 = gammaln(k*alpha0)-k*gammaln(alpha0);
Eppi = logCalpha0;
logCalpha = gammaln(sum(alpha))-sum(gammaln(alpha));
Eqpi = logCalpha;
Epmu = 0.5*d*k*log(kappa0);
Eqmu = 0.5*d*sum(log(kappa));
logB0 = -0.5*v0*(logW0+d*log(2))-logMvGamma(0.5*v0,d);
EpLambda = k*logB0;
logB =  -0.5*v.*(logW+d*log(2))-logMvGamma(0.5*v,d);
EqLambda = sum(logB);
EpX = -0.5*d*n*log(2*pi);
L = Epz-Eqz+Eppi-Eqpi+Epmu-Eqmu+EpLambda-EqLambda+EpX;


function [Q, R] = stiefelgeneralized_retraction_MGS(B, X, U, t)
% Retraction for generalized Stiefel based on Modified Gram-Schmidt.
% When used just as a retraction, only the output Q is relevant.
% NB, Dec. 16, 2018.
    if ~exist('t', 'var') || isempty(t)
        A = X + U;   % t = 1 by default
    else
        A = X + t*U;
    end
    [n, p] = size(X);
    Q = zeros(n, p);
    R = zeros(p, p);
    for j = 1 : p
        v = A(:, j);
        R(j, j) = sqrt(v'*B*v);
        Q(:, j) = v / R(j, j);
        R(j, (j+1):p) = Q(:, j)' * B * A(:, (j+1):p);
        A(:, (j+1):p) = A(:, (j+1):p) - Q(:, j) * R(j, (j+1):p);
    end
end


function [G] = uGal2G(uGal)
% Convert acceleration from microgals to average acceleration due to
% Earth's gravity.
% Chad A. Greene 2012
G = uGal/(9.80665e+8);
end


function [u,Du,eqn,info] = PoissonP3(node,elem,pde,bdFlag,option)
%% POISSONP2 Poisson equation: P3 quadratic element.
%
% u = PoissonP3(node,elem,pde,bdFlag,option) produces the cubic
%   finite element approximation of the Poisson equation
%
%       -div(d*grad(u))=f  in \Omega, with
%       Dirichlet boundary condition u=g_D on \Gamma_D,
%       Neumann boundary condition   d*grad(u)*n=g_N on \Gamma_N,
%       Robin boundary condition     g_R*u + d*grad(u)*n=g_N on \Gamma _R
%
% [u,Du,eqn,info] = PoissonP3(node,elem,pde,bdFlag,option)
%
% The usage is the same as Poisson. For quadratic elements, middle points
% of each edge are degree of freedom. See dofP2doc for detail.
%
% Example
%   femrateP2
%
% See also Poisson, Poisson3, Poisson3P2,PoissonP2
% Created by Jie Zhou
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end

tic;
%% Construct Data Structure
[elem2dof,elem2edge,edge,bdDof]  = dofP3(elem);
N = size(node,1);  NT = size(elem,1); NE = size(edge,1);
Ndof = N + 2*NE + NT;

%% Compute geometric quantities and gradient of local basis
[Dlambda,area] = gradbasis(node,elem);

%% Assemble stiffness matrix
% Since Dphi_i*Dphi_j is four degree, so four order numerical quadrature rule is used here
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'quadorder')
    option.quadorder = 5;   % default order 2(p-1)+1
end
[lambda, w] = quadpts(option.quadorder);
nQuad = size(lambda,1);
ii = zeros(55*NT,1); jj = zeros(55*NT,1); sA = zeros(55*NT,nQuad);
% generate sparse pattern
index = 0;
for i = 1:10
    for j = i:10
        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        index = index + NT;
    end
end
% compute non-zeros
for p = 1:nQuad
    % Dphi at quadrature points
    Dphip(:,:,1) = (27/2*lambda(p,1)*lambda(p,1)-9*lambda(p,1)+1).*Dlambda(:,:,1);
    Dphip(:,:,2) = (27/2*lambda(p,2)*lambda(p,2)-9*lambda(p,2)+1).*Dlambda(:,:,2);
    Dphip(:,:,3) = (27/2*lambda(p,3)*lambda(p,3)-9*lambda(p,3)+1).*Dlambda(:,:,3);
    Dphip(:,:,4) = 9/2*((3*lambda(p,2)*lambda(p,2)-lambda(p,2)).*Dlambda(:,:,3)+...
                   lambda(p,3)*(6*lambda(p,2)-1).*Dlambda(:,:,2));
    Dphip(:,:,5) = 9/2*((3*lambda(p,3)*lambda(p,3)-lambda(p,3)).*Dlambda(:,:,2)+...
                   lambda(p,2)*(6*lambda(p,3)-1).*Dlambda(:,:,3));
    Dphip(:,:,6) = 9/2*((3*lambda(p,3)*lambda(p,3)-lambda(p,3)).*Dlambda(:,:,1)+...
                   lambda(p,1)*(6*lambda(p,3)-1).*Dlambda(:,:,3));
    Dphip(:,:,7) = 9/2*((3*lambda(p,1)*lambda(p,1)-lambda(p,1)).*Dlambda(:,:,3)+...
                   lambda(p,3)*(6*lambda(p,1)-1).*Dlambda(:,:,1));
    Dphip(:,:,8) = 9/2*((3*lambda(p,1)*lambda(p,1)-lambda(p,1)).*Dlambda(:,:,2)+...
                   lambda(p,2)*(6*lambda(p,1)-1).*Dlambda(:,:,1));
    Dphip(:,:,9)  = 9/2*((3*lambda(p,2)*lambda(p,2)-lambda(p,2)).*Dlambda(:,:,1)+...
                   lambda(p,1)*(6*lambda(p,2)-1).*Dlambda(:,:,2));
    Dphip(:,:,10) = 27*(lambda(p,1)*lambda(p,2)*Dlambda(:,:,3)+lambda(p,1)*lambda(p,3)*Dlambda(:,:,2)+...
                   lambda(p,3)*lambda(p,2)*Dlambda(:,:,1));
    index = 0;
    for i = 1:10
        for j = i:10
            Aij = 0;
            if isempty(pde.d) || isnumeric(pde.d)
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2);
            else
                pxy = lambda(p,1)*node(elem(:,1),:) ...
                    + lambda(p,2)*node(elem(:,2),:) ...
                    + lambda(p,3)*node(elem(:,3),:);
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*pde.d(pxy);
            end
             if ~isempty(pde.d) && isnumeric(pde.d) % d is piecewise constant
                 Aij = pde.d.*Aij;
             end
            Aij = Aij.*area;
            sA(index+1:index+NT,p) = Aij;
            index = index + NT;
        end
    end
end
sA = sum(sA,2);
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
A = A + AU + AU';
clear Aij ii jj sA

%% Assemble right hand side by high order quadrature rule
b = zeros(Ndof,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 6;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,w] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
    phi(:,1) = 0.5*(3*lambda(:,1)-1).*(3*lambda(:,1)-2).*lambda(:,1);
    phi(:,2) = 0.5*(3*lambda(:,2)-1).*(3*lambda(:,2)-2).*lambda(:,2);
    phi(:,3) = 0.5*(3*lambda(:,3)-1).*(3*lambda(:,3)-2).*lambda(:,3);
    phi(:,4) = 9/2*lambda(:,3).*lambda(:,2).*(3*lambda(:,2)-1);
    phi(:,5) = 9/2*lambda(:,3).*lambda(:,2).*(3*lambda(:,3)-1);
    phi(:,6) = 9/2*lambda(:,1).*lambda(:,3).*(3*lambda(:,3)-1);
    phi(:,7) = 9/2*lambda(:,1).*lambda(:,3).*(3*lambda(:,1)-1);
    phi(:,8) = 9/2*lambda(:,1).*lambda(:,2).*(3*lambda(:,1)-1);
    phi(:,9) = 9/2*lambda(:,1).*lambda(:,2).*(3*lambda(:,2)-1);
    phi(:,10) = 27*lambda(:,1).*lambda(:,2).*lambda(:,3);

    bt = zeros(NT,10);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        if isfield(pde,'f') && isnumeric(pde.f)
            fp = pde.f;        % piecewise constant
        else
            fp = pde.f(pxy);   % function handle
        end
        for j = 1:10
            bt(:,j) = bt(:,j) + w(p)*phi(p,j)*fp;
        end
    end
    bt = bt.*repmat(area,1,10);
    b = accumarray(elem2dof(:),bt(:),[Ndof 1]);
end

%% Boundary Conditions
if nargin<=3, bdFlag = []; end
[AD,b,u,freeDof,isPureNeumann] = getbdP3(b);

%% Record assembeling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(freeDof), return; end
% Set up solver
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % Multigrid-type  solver for large size systems
        option.solver = 'mg';
    end
end

solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeDof) = AD(freeDof,freeDof)\b(freeDof);
        residual = norm(b - AD*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
        option.x0 = u;
        option.solver = 'CG';
        option.tol = Ndof^(-2);
        [u,info] = mg(AD,b,elem,option,edge);
    case 'amg'
        option.solver = 'CG';
        option.tol = Ndof^(-2);
        [u(freeDof),info] = amg(AD(freeDof,freeDof),b(freeDof),option);
end
clear phi lambda
% post-process for pure Neumann problem
if isPureNeumann
    intguh = double(sparse(NT,1));
    [lambda,w] = quadpts(3);  %This is P3 element.
    nQuad = size(lambda,1);
    phi(:,1) = 0.5*(3*lambda(:,1)-1).*(3*lambda(:,1)-2).*lambda(:,1);
    phi(:,2) = 0.5*(3*lambda(:,2)-1).*(3*lambda(:,2)-2).*lambda(:,2);
    phi(:,3) = 0.5*(3*lambda(:,3)-1).*(3*lambda(:,3)-2).*lambda(:,3);
    phi(:,4) = 9/2*lambda(:,3).*lambda(:,2).*(3*lambda(:,2)-1);
    phi(:,5) = 9/2*lambda(:,3).*lambda(:,2).*(3*lambda(:,3)-1);
    phi(:,6) = 9/2*lambda(:,1).*lambda(:,3).*(3*lambda(:,3)-1);
    phi(:,7) = 9/2*lambda(:,1).*lambda(:,3).*(3*lambda(:,1)-1);
    phi(:,8) = 9/2*lambda(:,1).*lambda(:,2).*(3*lambda(:,1)-1);
    phi(:,9) = 9/2*lambda(:,1).*lambda(:,2).*(3*lambda(:,2)-1);
    phi(:,10) = 27*lambda(:,1).*lambda(:,2).*lambda(:,3);
     for     p = 1:nQuad
     intguh(:) = intguh(:) + w(p)*(phi(p,1)*u(elem2dof(:,1))+phi(p,2)*u(elem2dof(:,2))+phi(p,3)*u(elem2dof(:,3))...
                           + phi(p,4)*u(elem2dof(:,4))+phi(p,5)*u(elem2dof(:,5))+phi(p,6)*u(elem2dof(:,6))...
                           + phi(p,7)*u(elem2dof(:,7))+phi(p,8)*u(elem2dof(:,8))+phi(p,9)*u(elem2dof(:,9))...
                           + phi(p,10)*u(elem2dof(:,10)));
     end                 %compute the intgrable of uh.
     intguh = intguh.*area;
         uc = sum(intguh)/sum(area);
          u = u - uc;    % normalization for pure Neumann problem
end

% Output information
eqn = struct('A',AD,'b',b,'edge',edge,'freeDof',freeDof);
info.assembleTime = assembleTime;

%% Compute Du
Du = [];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdP3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,b,u,freeDof,isPureNeumann] = getbdP3(b)
    %% Boundary conditions for Poisson equation: P3 quadratic FEM.
    %
    % The set up of boundary condition consists of two parts:
    %
    % 1) Modify the matrix for Dirichlet boundary nodes, which are not degree
    % of freedom. Values at these nodes are evaluatation of pde.g_D. The
    % original stiffness matrix A is turn into the matrix AD by enforcing
    % AD(fixedDof,fixedDof)=I, AD(fixedDof,freeDof)=0, AD(freeDof,fixedDof)=0.
    %
    % 2) Modify the right hand side b. The Neumann boundary integral is added
    % to b. For Dirichlet boundary ndoes, b(fixedDof) is the evaluation of
    % pde.g_D.
    %
    % Special attentation should be given for the pure Neumann boundary
    % condition. To enforce the compatible condition, the vector b should have
    % mean value zero. To avoid a singular matrix, the 1st node is chosen as
    % fixedDof.
    %
    % The order of assigning Neumann and Dirichlet boundary condition is
    % important to get the right setting at the intersection nodes of Dirichlet
    % and Neumann boundary edges.

    u = zeros(Ndof,1);

    %% Set up boundary and basic parameter
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Modify the matrix for Dirichlet and Robin condition
    % Robin boundary condition
    Robin = [];
    idxR = (bdFlag(:) == 3);      %index of Robin edges in bdFlag
    if any(idxR)
        isRobin = false(NE,1);
        isRobin(elem2edge(idxR)) = true;
        Robin = edge(isRobin,:);  % Robin edges
    end
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        if ~isfield(option,'gRquadorder')
            option.gRquadorder = 8;   % we should use six order  rule.
        end
        [lambdagR,weightgR] = quadpts1(option.gRquadorder);
        nQuadgR = size(lambdagR,1);
        % cubic bases (1--3--4--2)
        bdphi = zeros(nQuadgR,4);
        bdphi(:,1) = 0.5*(3*lambdagR(:,1)-1).*(3*lambdagR(:,1)-2).*lambdagR(:,1);
        bdphi(:,2) = 0.5*(3*lambdagR(:,2)-1).*(3*lambdagR(:,2)-2).*lambdagR(:,2);
        bdphi(:,3) = 9/2*lambdagR(:,1).*lambdagR(:,2).*(3*lambdagR(:,1)-1);
        bdphi(:,4) = 9/2*lambdagR(:,1).*lambdagR(:,2).*(3*lambdagR(:,2)-1);
        % length of edge
        el = sqrt(sum((node(Robin(:,1),:) - node(Robin(:,2),:)).^2,2));
        NR = size(Robin,1);
        ss = zeros(NR,4,4);
        for pp = 1:nQuadgR
            ppxy = lambdagR(pp,1)*node(Robin(:,1),:) ...
                 + lambdagR(pp,2)*node(Robin(:,2),:);
            gRp = pde.g_R(ppxy);
            for iR = 1:4
                for jR = iR:4   % only compute half of the off-diagonal part
                    ss(:,iR,jR) = ss(:,iR,jR) + ...
                    weightgR(pp)*gRp*bdphi(pp,iR).*bdphi(pp,jR);
                end
            end
        end
        ss(:) = ss(:).*repmat(el,16,1);
        Robin(:,3) = 2*find(isRobin)+N-1;  % the third one maps to corresponding dof
        Robin(:,4) = 2*find(isRobin)+N;    % the fourth one maps to corresponding dof
        index = 0;
        for iR = 1:4
            for jR = 1:4
                iiR(index+1:index+NR) = double(Robin(:,iR));
                jjR(index+1:index+NR) = double(Robin(:,jR));
                if jR>=iR
                    ssR(index+1:index+NR) = ss(:,iR,jR);
                else
                    ssR(index+1:index+NR) = ss(:,jR,iR);
                end
                index = index + NR;
            end
        end
        A = A + sparse(iiR,jjR,ssR,Ndof,Ndof);
    end

    % Find Dirichlet boundary dof: fixedDof
    fixedDof = []; freeDof = [];
    isFixedDof = false(Ndof,1);
    if ~isempty(bdFlag)
        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        isFixedDof(edge(isDirichlet,:)) = true;
        isFixedDof(N + 2*find(isDirichlet')-1) = true;
        isFixedDof(N + 2*find(isDirichlet')) = true;
        fixedDof = find(isFixedDof);
        freeDof = find(~isFixedDof);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        fixedDof = bdDof;
        isFixedDof(fixedDof) = true;
        freeDof = find(~isFixedDof);
    end
    isPureNeumann = false;
    if isempty(fixedDof) && isempty(Robin)  % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        isPureNeumann = true;
        fixedDof = 1;
        freeDof = 2:Ndof;    % eliminate the kernel by enforcing u(1) = 0;
    end
    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % |AD(fixedDof,fixedDof)=I, AD(fixedDof,freeDof)=0,
    % AD(freeDof,fixedDof)=0|.
    if ~isempty(fixedDof)
        bdidx = zeros(Ndof,1);
        bdidx(fixedDof) = 1;
        Tbd = sparse(1:Ndof,1:Ndof,bdidx,Ndof,Ndof);
        T = sparse(1:Ndof,1:Ndof,1-bdidx,Ndof,Ndof);
        AD = T*A*T + Tbd;
    else
        AD = A;
    end

    %% Part 2: Find boundary edges and modify the load b
    % Find boundary edges: Neumann and Robin
    Neumann = [];
    if ~isempty(bdFlag)
        idxN = (bdFlag(:) == 2);      % all Neumann edges in bdFlag
        Neumannidx = elem2edge(idxN | idxR); % index of Neumann and Robin edges
        % since boundary integral is also needed for Robin edges
        Neumann   = edge(Neumannidx,:);
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        Neumannidx = find(bdDof>N);
        Neumann = edge(Neumannidx,:);
    end

    % Neumann boundary condition
    if ~isempty(pde.g_N) && ~isempty(Neumann) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 6;
        end
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        nQuadgN = size(lambdagN,1);
        % quadratic bases (1---3---4--2)
        bdphi = zeros(nQuadgN,4);
        bdphi(:,1) = 0.5*(3*lambdagN(:,1)-1).*(3*lambdagN(:,1)-2).*lambdagN(:,1);
        bdphi(:,2) = 0.5*(3*lambdagN(:,2)-1).*(3*lambdagN(:,2)-2).*lambdagN(:,2);
        bdphi(:,3) = 9/2*lambdagN(:,1).*lambdagN(:,2).*(3*lambdagN(:,1)-1);
        bdphi(:,4) = 9/2*lambdagN(:,1).*lambdagN(:,2).*(3*lambdagN(:,2)-1);
        % length of edge
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        ge = zeros(size(Neumann,1),4);
        for pp = 1:nQuadgN
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNp = pde.g_N(ppxy);
            ge(:,1) = ge(:,1) + weightgN(pp)*gNp*bdphi(pp,1);
            ge(:,2) = ge(:,2) + weightgN(pp)*gNp*bdphi(pp,2);
            ge(:,3) = ge(:,3) + weightgN(pp)*gNp*bdphi(pp,3);
            ge(:,4) = ge(:,4) + weightgN(pp)*gNp*bdphi(pp,4);
        end
        ge = ge.*repmat(el,1,4);
        b(1:N) = b(1:N) + accumarray(Neumann(:), [ge(:,1); ge(:,2)],[N,1]);
        b(N+2*Neumannidx-1) = b(N+2*Neumannidx-1) + ge(:,3);
        b(N+2*Neumannidx)   = b(N+2*Neumannidx) + ge(:,4);
    end

    % Dirichlet boundary conditions
    if ~isPureNeumann && ~isempty(fixedDof) && ...
       ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && (pde.g_D == 0))
        % interpolation
        idx = (fixedDof > N);  % index of edge nodes
        u(fixedDof(~idx)) = pde.g_D(node(fixedDof(~idx),:)); % bd value at vertex dofs
        % for P3,  we should divide the points of edge into two parts.
        bdEdgeIdx = fixedDof(idx) - N;
        %  First parts, the points  * is in  1---*------2
        bdEdgeMid = node(edge(isDirichlet,1),:)+(node(edge(isDirichlet,2),:) ...
                  - node(edge(isDirichlet,1),:))/3;
        u(N + bdEdgeIdx(1:2:end)) = pde.g_D(bdEdgeMid);
      %  Second parts, the points * is in  1------*---2
        bdEdgeMid = node(edge(isDirichlet,1),:)+2*(node(edge(isDirichlet,2),:)...
                  - node(edge(isDirichlet,1),:))/3;
        u(N + bdEdgeIdx(2:2:end)) = pde.g_D(bdEdgeMid);
        % modify the right hand side
        b = b - A*u;
    end
    if ~isPureNeumann % non-empty Dirichlet boundary condition
        b(fixedDof) = u(fixedDof);
    end

    % Pure Neumann boundary condition
    if isPureNeumann
        b = b - mean(b);   % compatilbe condition: sum(b) = 0
        b(1) = 0;
    end
    end % end of getbdP3
end % end of function PoissonP3


function S = tsmooth(I,lambda,sigma,sharpness,maxIter)
%tsmooth - Structure Extraction from Texture via Relative Total Variation
%   S = tsmooth(I, lambda, sigma, maxIter) extracts structure S from
%   structure+texture input I, with smoothness weight lambda, scale
%   parameter sigma and iteration number maxIter.
%
%   Paras:
%   @I         : Input UINT8 image, both grayscale and color images are acceptable.
%   @lambda    : Parameter controlling the degree of smooth.
%                Range (0, 0.05], 0.01 by default.
%   @sigma     : Parameter specifying the maximum size of texture elements.
%                Range (0, 6], 3 by defalut.
%   @sharpness : Parameter controlling the sharpness of the final results,
%                which corresponds to \epsilon_s in the paper [1]. The smaller the value, the sharper the result.
%                Range (1e-3, 0.03], 0.02 by defalut.
%   @maxIter   : Number of itearations, 4 by default.
%
%   Example
%   ==========
%   I  = imread('Bishapur_zan.jpg');
%   S  = tsmooth(I); % Default Parameters (lambda = 0.01, sigma = 3, sharpness = 0.02, maxIter = 4)
%   figure, imshow(I), figure, imshow(S);
%
%   ==========
%   The Code is created based on the method described in the following paper
%   [1] "Structure Extraction from Texture via Relative Total Variation", Li Xu, Qiong Yan, Yang Xia, Jiaya Jia, ACM Transactions on Graphics,
%   (SIGGRAPH Asia 2012), 2012.
%   The code and the algorithm are for non-comercial use only.
%
%   Author: Li Xu (xuli@cse.cuhk.edu.hk)
%   Date  : 08/25/2012
%   Version : 1.0
%   Copyright 2012, The Chinese University of Hong Kong.
%

    if (~exist('lambda','var'))
       lambda=0.01;
    end
    if (~exist('sigma','var'))
       sigma=3.0;
    end
    if (~exist('sharpness','var'))
        sharpness = 0.02;
    end
    if (~exist('maxIter','var'))
       maxIter=4;
    end
    I = im2double(I);
    x = I;
    sigma_iter = sigma;
    lambda = lambda/2.0;
    dec=2.0;
    for iter = 1:maxIter
        [wx, wy] = computeTextureWeights(x, sigma_iter, sharpness);
        x = solveLinearEquation(I, wx, wy, lambda);
        sigma_iter = sigma_iter/dec;
        if sigma_iter < 0.5
            sigma_iter = 0.5;
        end
    end
    S = x;
end

function [retx, rety] = computeTextureWeights(fin, sigma,sharpness)

   fx = diff(fin,1,2);
   fx = padarray(fx, [0 1 0], 'post');
   fy = diff(fin,1,1);
   fy = padarray(fy, [1 0 0], 'post');

   vareps_s = sharpness;
   vareps = 0.001;

   wto = max(sum(sqrt(fx.^2+fy.^2),3)/size(fin,3),vareps_s).^(-1);
   fbin = lpfilter(fin, sigma);
   gfx = diff(fbin,1,2);
   gfx = padarray(gfx, [0 1], 'post');
   gfy = diff(fbin,1,1);
   gfy = padarray(gfy, [1 0], 'post');
   wtbx = max(sum(abs(gfx),3)/size(fin,3),vareps).^(-1);
   wtby = max(sum(abs(gfy),3)/size(fin,3),vareps).^(-1);
   retx = wtbx.*wto;
   rety = wtby.*wto;

   retx(:,end) = 0;
   rety(end,:) = 0;

end

function ret = conv2_sep(im, sigma)
  ksize = bitor(round(5*sigma),1);
  g = fspecial('gaussian', [1,ksize], sigma);
  ret = conv2(im,g,'same');
  ret = conv2(ret,g','same');
end

function FBImg = lpfilter(FImg, sigma)
    FBImg = FImg;
    for ic = 1:size(FBImg,3)
        FBImg(:,:,ic) = conv2_sep(FImg(:,:,ic), sigma);
    end
end

function OUT = solveLinearEquation(IN, wx, wy, lambda)
%
% The code for constructing inhomogenious Laplacian is adapted from
% the implementaion of the wlsFilter.
%
% For color images, we enforce wx and wy be same for three channels
% and thus the pre-conditionar only need to be computed once.
%
    [r,c,ch] = size(IN);
    k = r*c;
    dx = -lambda*wx(:);
    dy = -lambda*wy(:);
    B(:,1) = dx;
    B(:,2) = dy;
    d = [-r,-1];
    A = spdiags(B,d,k,k);
    e = dx;
    w = padarray(dx, r, 'pre'); w = w(1:end-r);
    s = dy;
    n = padarray(dy, 1, 'pre'); n = n(1:end-1);
    D = 1-(e+w+s+n);
    A = A + A' + spdiags(D, 0, k, k);
    if exist('ichol','builtin')
        L = ichol(A,struct('michol','on'));
        OUT = IN;
        for ii=1:ch
            tin = IN(:,:,ii);
            [tout, flag] = pcg(A, tin(:),0.1,100, L, L');
            OUT(:,:,ii) = reshape(tout, r, c);
        end
    else
        OUT = IN;
        for ii=1:ch
            tin = IN(:,:,ii);
            tout = A\tin(:);
            OUT(:,:,ii) = reshape(tout, r, c);
        end
    end

end


function cs_star = surfaceConcentration(cs_barrato,jflux,Q,T,param)
% surfaceConcentration evaluates the concentration of Li-ions at the electrode surfaces.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

% Diffusion coefficients for the solid phase
[Dps_eff, Dns_eff] = param.SolidDiffusionCoefficientsFunction(T,param);

% Check what kind of solid diffusion model has been chosen.
if(param.SolidPhaseDiffusion==1) % Two parameters model
    % Evaluates the average surface concentration in both the electrodes.
    % Cathode
    cs_star_p = cs_barrato(1:param.Np)-(param.Rp_p./(Dps_eff.*5)).*jflux(1:param.Np);
    % Anode
    cs_star_n = cs_barrato(param.Np+1:end)-(param.Rp_n./(Dns_eff.*5)).*jflux(param.Np+1:end);
elseif(param.SolidPhaseDiffusion==2) % Three parameters model
    % Cathode
    cs_star_p = cs_barrato(1:param.Np)+(param.Rp_p./(Dps_eff.*35)).*(-jflux(1:param.Np)+8*Dps_eff.*Q(1:param.Np));
    % Anode
    cs_star_n = cs_barrato(param.Np+1:end)+(param.Rp_n./(Dns_eff.*35)).*(-jflux(param.Np+1:end)+8*Dns_eff.*Q(param.Np+1:end));
elseif(param.SolidPhaseDiffusion==3) % Full model
    p_indices = param.Nr_p:param.Nr_p:param.Nr_p*param.Np;
    n_indices = param.Nr_n:param.Nr_n:param.Nr_n*param.Nn;
    % If the full model has been used, just take the concentration data at r=Rp
    cs_star_p = cs_barrato(p_indices);
    cs_star_n = cs_barrato(n_indices+p_indices(end));
end
% Return the residuals
cs_star = [cs_star_p;cs_star_n];

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Reduce an image applying Gaussian Pyramid.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function IResult = GPReduce(I,displayflag)

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

Wt = [0.0500    0.2500    0.4000    0.2500    0.0500];

dim = size(I);
newdim = ceil(dim*0.5);
IResult = zeros(newdim,class(I)); % Initialize the array in the beginning ..
I = single(I);
m = [-2:2];n=m;

switch length(dim)
	case 1
		%% Pad the boundaries.
		I = [ I(1) ; I(1) ;  I ; I(dim(1));  I(dim(1)) ];  % Add two rows towards the beginning and the end.
		for i = 0 : newdim(1) -1
			A = I(2*i+m+3).*Wt;;
			IResult(i + 1)= sum(A(:));
		end
	case 2
		%% Pad the boundaries.
		I = [ I(1,:) ; I(1,:) ;  I ; I(dim(1),:);  I(dim(1),:) ];  % Add two rows towards the beginning and the end.
		I = [ I(:,1)  I(:,1)     I   I(:,dim(2))  I(:,dim(2)) ];  % Add two columns towards the beginning and the end.

		Wt2 = Wt'*Wt;

		for i = 0 : newdim(1) -1
			for j = 0 : newdim(2) -1
				A = I(2*i+m+3,2*j+m+3).*Wt2;
				IResult(i + 1, j + 1) = sum(A(:));
			end
		end

	case 3
		Wt3 = ones(5,5,5);
		for i = 1:5
			Wt3(i,:,:) = Wt3(i,:,:) * Wt(i);
			Wt3(:,i,:) = Wt3(:,i,:) * Wt(i);
			Wt3(:,:,i) = Wt3(:,:,i) * Wt(i);
		end

		%% Pad the boundaries.
		I2 = zeros(dim+4,class(I));
		I2(3:2+dim(1),3:2+dim(2),3:2+dim(3)) = I;
		I2(1,:,:)=I2(3,:,:);I2(1,:,:)=I2(3,:,:);I2(end,:,:)=I2(end-2,:,:);I2(end-1,:,:)=I2(end-2,:,:);
		I2(:,1,:)=I2(:,3,:);I2(:,2,:)=I2(:,3,:);I2(:,end,:)=I2(:,end-2,:);I2(:,end-1,:)=I2(:,end-2,:);
		I2(:,:,1)=I2(:,:,3);I2(:,:,2)=I2(:,:,3);I2(:,:,end)=I2(:,:,end-2);I2(:,:,end-1)=I2(:,:,end-2);
		I=I2; clear I2;

		if( displayflag==1)
			H = waitbar(0,'Progress');
			set(H,'Name','GPReduce ...');
		end
		for k = 0 : newdim(3) - 1
			if( displayflag==1)
				waitbar(k/newdim(3),H,sprintf('(%d%%) %d out of %d',round(k/newdim(3)*100),k,newdim(3)));
			else
				fprintf('.');
			end
			for j = 0 : newdim(2) -1
				for i = 0 : newdim(1) -1
					A = I(2*i+m+3,2*j+m+3,2*k+m+3).*Wt3;
					IResult(i+1,j+1,k+1) = sum(A(:));
				end
			end
		end
		if( displayflag==1)
			close(H);
		else
			fprintf('\n');
		end
end


function spline_test04 ( )

%*****************************************************************************80
%
%% TEST04 tests BASIS_MATRIX_BETA_UNI, BASIS_MATRIX_TMP.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author
%
%    John Burkardt
%
  n = 4;
  ndata = 4;
  nsample = 4;
  tdata = [ -1.0E+00, 0.0E+00, 1.0E+00, 2.0E+00 ]';
  ydata = [  4.0E+00, 7.0E+00, 12.0E+00, 19.0E+00 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04\n' );
  fprintf ( 1, '  BASIS_MATRIX_BETA_UNI sets up the basis matrix\n' );
  fprintf ( 1, '    for the uniform beta spline.\n' );
%
%  First test
%
  beta1 = 1.0E+00;
  beta2 = 0.0E+00;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  BETA1 = %14f\n', beta1 );
  fprintf ( 1, '  BETA2 = %14f\n', beta2 );

  mbasis = basis_matrix_beta_uni ( beta1, beta2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '    TDATA, YDATA\n' );
  fprintf ( 1, '\n' );
  for i = 1 : ndata
    fprintf ( 1, '%12f  %12f\n', tdata(i), ydata(i) );
  end

  left = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, '    T, Spline(T)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : ndata

    if ( i == 0 )
      tlo = tdata(1) - 0.5E+00 * ( tdata(2) - tdata(1) );
      thi = tdata(1);
    elseif ( i < ndata )
      tlo = tdata(i);
      thi = tdata(i+1);
    elseif ( ndata <= i )
      tlo = tdata(ndata);
      thi = tdata(ndata) + 0.5E+00 * ( tdata(ndata) - tdata(ndata-1) );
    end

    if ( i < ndata )
      jhi = nsample - 1;
    else
      jhi = nsample;
    end

    for j = 0 : jhi

      tval = ( ( nsample - j ) * tlo   ...
             + (           j ) * thi ) ...
             /   nsample;

      yval = basis_matrix_tmp ( left, n, mbasis, ndata, tdata, ydata, tval );

      if ( 0 < i & j == 0 )
        mark = '*';
      else
        mark = ' ';
      end

      fprintf ( 1, '%c  %12f  %12f\n', mark, tval, yval );

    end

  end
%
%  Second test
%
  beta1 = 1.0E+00;
  beta2 = 100.0E+00;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  BETA1 = %14f\n', beta1 );
  fprintf ( 1, '  BETA2 = %14f\n', beta2 );

  mbasis = basis_matrix_beta_uni ( beta1, beta2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '    TDATA, YDATA\n' );
  fprintf ( 1, '\n' );
  for i = 1 : ndata
    fprintf ( 1, '%12f  %12f\n', tdata(i), ydata(i) );
  end

  left = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, '    T, Spline(T)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : ndata

    if ( i == 0 )
      tlo = tdata(1) - 0.5E+00 * ( tdata(2) - tdata(1) );
      thi = tdata(1);
    elseif ( i < ndata )
      tlo = tdata(i);
      thi = tdata(i+1);
    elseif ( ndata <= i )
      tlo = tdata(ndata);
      thi = tdata(ndata) + 0.5E+00 * ( tdata(ndata) - tdata(ndata-1) );
    end

    if ( i < ndata )
      jhi = nsample - 1;
    else
      jhi = nsample;
    end

    for j = 0 : jhi

      tval = ( ( nsample - j ) * tlo   ...
             + (           j ) * thi ) ...
             /   nsample;

      yval = basis_matrix_tmp ( left, n, mbasis, ndata, tdata, ydata, tval );

      if ( 0 < i & j == 0 )
        mark = '*';
      else
        mark = ' ';
      end

      fprintf ( 1, '%c  %12f  %12f\n', mark, tval, yval );

    end

  end
%
%  Third test
%
  beta1 = 100.0E+00;
  beta2 = 0.0E+00;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  BETA1 = %14f\n', beta1 );
  fprintf ( 1, '  BETA2 = %14f\n', beta2 );

  mbasis = basis_matrix_beta_uni ( beta1, beta2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '    TDATA, YDATA\n' );
  fprintf ( 1, '\n' );
  for i = 1 : ndata
    fprintf ( 1, '%12f  %12f\n', tdata(i), ydata(i) );
  end

  left = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, '    T, Spline(T)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : ndata

    if ( i == 0 )
      tlo = tdata(1) - 0.5E+00 * ( tdata(2) - tdata(1) );
      thi = tdata(1);
    elseif ( i < ndata )
      tlo = tdata(i);
      thi = tdata(i+1);
    elseif ( ndata <= i )
      tlo = tdata(ndata);
      thi = tdata(ndata) + 0.5E+00 * ( tdata(ndata) - tdata(ndata-1) );
    end

    if ( i < ndata )
      jhi = nsample - 1;
    else
      jhi = nsample;
    end

    for j = 0 : jhi

      tval = ( ( nsample - j ) * tlo   ...
             + (           j ) * thi ) ...
             /   nsample;

      yval = basis_matrix_tmp ( left, n, mbasis, ndata, tdata, ydata, tval );

      if ( 0 < i & j == 0 )
        mark = '*';
      else
        mark = ' ';
      end

      fprintf ( 1, '%c  %12f  %12f\n', mark, tval, yval );

    end

  end

  return
end


function [I] = corrindex(XData, YData, YDataM, par_number)
%
% Function for calculation of the correlation index
%
% Inputs:   XData:  x data
%           YData:  y data
%           YDataM: model data
%           par_number: number of model parameters
% Output:   I: correlation index
%
% Authors:
% Ivo Petras (ivo.petras@tuke.sk)
% Dagmar Bednarova (dagmar.bednarova@tuke.sk)
%
% Date: 21/002/2007
%
kx=length(XData);
ky=length(YData);
kym=length(YDataM);
if kx ~= ky
    disp('Incompatible X and Y data.');
    close all;
end
n=kym;
sey=(sum((YData-YDataM).^2))/(n-par_number);
sy=var(YData);
I=(1-(sey./sy))^0.5;
%


function out=mp(x,y)
%MP multiple precision class constructor.
%   p = mp(x,y) creates a mp object from the matrices x and y,
%   where x contains the double to be converted into an mp object
%         y contains the precision
% Special calls to mp include:
%   mp('pi',precision) => returns pi to precision (precision is optional)

maxDoublePrec=16; %digits
if nargin == 0
 out.rval=[];
 out.ival=[];
 out.precision=[];
 out=class(out,'mp');
elseif isa(x,'mp')
 %out=x;
 if nargin==1
  out=x;
 else
  for ii=1:numel(x)
   out(ii)=mp(x(ii).rval,y(min(numel(y),ii)));
   if ~isreal(x(ii))
    out(ii)=out(ii)+mp(x(ii).ival,y(min(numel(y),ii)))*i;
   end
  end
 end % if nargin==2
else
 mp_defaults
 precision=default_precision;
 out_rval=cell(size(x));
 out_ival=cell(size(x));
 if nargin==2
  precision=double(y(1));
 end % if nargin==2
 if isa(x,'double')
  for ii=1:numel(x)
   [str,exponent]=mpfr_construct_dd(real(x(ii)),precision);
   % throw away anything past maxDoublePrec, set to 0's
   if length(str)>maxDoublePrec
    str(maxDoublePrec+1:end)='0';
   end
   out_rval{ii}=mpExpForm(mpAddDecimal(str),exponent);
   if ~isreal(x(ii))
    [str,exponent]=mpfr_construct_dd(imag(x(ii)),precision);
    % throw away anything past maxDoublePrec, set to 0's
    if length(str)>maxDoublePrec
     str(maxDoublePrec+1:end)='0';
    end
    out_ival{ii}=mpExpForm(mpAddDecimal(str),exponent);
   end
  end % for ii=1:size(x,
 elseif isa(x,'cell')
  for ii=1:numel(x)
   [str,exponent]=mpfr_construct_cd(x{ii},precision);
   out_rval{ii}=mpExpForm(mpAddDecimal(str),exponent);
  end % for ii=1:size(x,
 elseif isa(x,'char')
  out_rval=cell(1);
  out_ival=cell(1);
  if any(strfind(lower(x),'pi'))
   out_rval=mpfr_pi(precision);
%%%   [str,exponent]=mpfr_pi(precision);
%%%   out_rval=mpExpForm(mpAddDecimal(str),exponent);
  else
   [str,exponent]=mpfr_construct_cd(x,precision);
   out_rval{1,1}=mpExpForm(mpAddDecimal(str),exponent);
  end
 end
 out=class(struct('rval',out_rval,...
                  'ival',out_ival,...
                  'precision',precision),'mp');
end

%%%out_rval
%%%out_rexp
%%%out_ival
%%%out_iexp

%'rrrrrrr',kb
% x=magic(3),s1='023e1',s2='-.002e-1'
% mp(x)


function [connmat] = triangle2connectivity(tri, pos)

% TRIANGLE2CONNECTIVITY computes a connectivity-matrix from a triangulation.
%
% Use as
%  [connmat] = triangle2connectivity(tri)
% or
%  [connmat] = triangle2connectivity(tri, pos)
%
% The input tri is an Mx3 matrix describing a triangulated surface,
% containing indices to connecting vertices. The output connmat is a sparse
% logical NxN matrix, with ones, where vertices are connected, and zeros
% otherwise.
%
% If you specify the vertex positions in the second input argument as Nx3
% matrix, the output will be a sparse matrix with the lengths of the
% edges between the connected vertices.
%
% See also CHANNELCONNECTIVIY

% Copyright (C) 2015, Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% ensure that the vertices are indexed starting from 1
if min(tri(:))==0,
  tri = tri + 1;
end

% ensure that the vertices are indexed according to 1:number of unique vertices
tri = tri_reindex(tri);

% create the unique edges from the triangulation
edges  = [tri(:,[1 2]); tri(:,[1 3]); tri(:,[2 3])];
edges  = double(unique(sort([edges; edges(:,[2 1])],2), 'rows'));

% fill the connectivity matrix
n        = size(edges,1);
if nargin<2
  % create sparse binary matrix
  connmat = sparse([edges(:,1);edges(:,2)],[edges(:,2);edges(:,1)],true(2*n,1));
else
  % create sparse matrix with edge lengths
  dpos    = sqrt(sum( (pos(edges(:,1),:) - pos(edges(:,2),:)).^2, 2));
  connmat = sparse([edges(:,1);edges(:,2)],[edges(:,2);edges(:,1)],[dpos(:);dpos(:)]);
end

function [newtri] = tri_reindex(tri)

% this subfunction reindexes tri such that they run from 1:number of unique vertices
newtri       = tri;
[srt, indx]  = sort(tri(:));
tmp          = cumsum(double(diff([0;srt])>0));
newtri(indx) = tmp;