function bratu ( )

%*****************************************************************************80
%
%% BRATU uses BVP4C to solve the BRATU problem.
%
%  Discussion:
%
%    The Bratu equation includes a parameter lambda.  Depending on the
%    value of lambda, there may be 2, 1, or no solutions to the BVP.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2013
%
%  Author:
%
%    Original MATLAB version by Shampine, Kierzenka, Reichelt.
%    This version by John Burkardt.
%
%  Reference:
%
%    Lawrence Shampine, Jacek Kierzenka, Mark Reichelt,
%    Solving boundary value problems for ordinary differential equations
%    in MATLAB with bvp4c.
%
  global lambda

  lambda_test = [ 0.45, 1.00, 3.50 ];

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BRATU:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Use BVP4C to solve the following boundary value problem:\n' );
  fprintf ( 1, '  y" + lambda * exp ( y ) = 0\n' );
  fprintf ( 1, '  y(0) = 0, y(1) = 0\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  When lambda = 1, there are two solutions.\n' );
  fprintf ( 1, '  Try\n' );
  fprintf ( 1, '    y(x) = 0.1, y''(x) = 0.\n' );
  fprintf ( 1, '  and\n' );
  fprintf ( 1, '    y(x) = 3.0, y''(x) = 0.0\n' );

  figure_num = 0;

  for test = 1 : 3
    lambda = lambda_test(test);
    figure_num = bratu_solver ( lambda, figure_num );
  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BRATU:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function figure_num = bratu_solver ( lambda, figure_num )

%*****************************************************************************80
%
%% BRATU_SOLVER calls BVP4C to solve the Bratu problem for a particular lambda.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real LAMBDA, the value of the parameter lambda.
%
%    Input/output, integer FIGURE_NUM, the number of figures being displayed.
%

%
%  Compute SOL1 with the first initial guess.
%
  x_init = linspace ( 0.0, 1.0, 5 );
  y_init = [ 0.1, 0.0 ];
  solinit = bvpinit ( x_init, y_init );
  sol1 = bvp4c ( @bratu_ode, @bratu_bc, solinit );
%
%  Compute SOL2 with the second initial guess.
%
  x_init = linspace ( 0.0, 1.0, 5 );
  y_init = [ 3.0, 0.0 ];
  solinit = bvpinit ( x_init, y_init );
  sol2 = bvp4c ( @bratu_ode, @bratu_bc, solinit );

  x = linspace ( 0.0, 1.0, 101 );
  y1 = deval ( sol1, x );
  y2 = deval ( sol2, x );
%
%  Display a plot of the two solutions.
%
  figure_num = figure_num + 1;
  figure ( figure_num )
  plot ( x, y1(1,:), 'r-', ...
         x, y2(1,:), 'g-', 'Linewidth', 2 );
  xlabel ( '<--- X --->' )
  ylabel ( '<--- Y --->' )
  title ( sprintf ( 'Bratu''s equation for \\lambda = %g\n', lambda ) );
  grid on
  filename = sprintf ( 'bratu_%f.png', lambda );
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saving plot file as "%s"\n', filename );

  return
end
function dydx = bratu_ode ( x, y )

%*****************************************************************************80
%
%% BRATU_ODE evaluates the right hand side of the ODE.
%
%  Discussion:
%
%    We assume that the differential equation has been rewritten as a
%    system of first order equations of the form
%
%      dydx = f(x,y)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the point at which the ODE is to be evaluated.
%
%    Input, real Y(M), the value of the solution at X.
%
%    Output, real DYDX(M), the value of the right hand side given X and Y.
%
  global lambda

  dydx(1) = y(2);
  dydx(2) = - lambda * exp ( y(1) );

  return
end
function bc = bratu_bc ( ya, yb )

%*****************************************************************************80
%
%% SAMPLE1_BC evaluates the boundary conditions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real YA(M), YB(M), the solution value at the left and right endpoints.
%
%    Output, real BC(2), the value of the boundary conditions.
%
  bc(1) = ya(1);
  bc(2) = yb(1);

  return
end
function timestamp ( )

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

  return
end


function demoBallisticScenario()
%%DEMOBALLISTICSCENARIO Demonstrate how the Joint Probabilistic Data
%             Association Filter (JPDAF) and the Global Nearest Neighbor
%             (GNN)-JPDAF can be used when tracking  two ballistic targets
%             with the possibility of missed detections and false alarms.
%             Track initiation and termination are not considered. This
%             function demonstrates how nonlinear filtering algorithms can
%             be used with the JPDAF/ GNN-JPDAF. Clistering is not
%             performed.
%
%A simple scenario of a ballistic target going from the Sea of Japan to
%an area in the middle of the Pacific (ignoring atmospheric drag, using a
%simple J2 model for the Earth's gravitational field is considered
%(ignoring light pressure and the gravitational attraction of other
%celestial bodies). Measurements are obtained by a radar in the Aleutian
%islands using a spherical measurement model. Refraction and other
%atmospheric effects are not simulated; Doppler is not used. Tracking and
%measurements are given in an Earth-centered Earth-fixed (ECEF) coordinate,
%system, requiring corrections for the Coriolis effect. Measurement-warping
%effects due to the Earth's rotation are not considered.
%
%The true trajectories for simulation are created using the
%createBallisticTrajectories subroutine in this file. Measurements are
%assumed to be available when the target is above a certain elevation in
%the receiver's coordinate system. The elevation in the receiver's
%coordinate system is measured with respect to the local tangent plane to
%the WGS84 reference ellipsoid. The trajectories go from the Sea of Japan
%to the middle of the Pacific. The function orbVelDet2PtMinEng is used to
%get an approximate initial velocity for a minimum energy trajectory
%between the points. However, the function is only an approximation as it
%is meant for use in an Earth-centered-inertial coordinate system (thus
%trajectories are minimum energy ignoring the energy imparted by the
%rotation of the Earth. Additionally, the endpoints are specified in an
%ECEF coordinate system, so the initial conditions outputted by the
%function are only approximate (a ballistic projectile would miss its
%target due to the Earth's rotation). Additionally, atmospheric drag is
%not taken into account and the function assumes a Keplerian dynamic model.
%Thus, the results might be useful as initial estimates for a
%higher-fidelity trajectory estimation routine, but would be unsuitable for
%use on their own. For the purpose of this simulation, the trajectory is
%sufficiently realistic.
%
%To make two targets sufficiently close that they will contest
%measurements (making a JPDAF relevant), the second target's trajectory was
%set to be that of the first target, but with a 50m offset in range. The
%resulting trajectory is no longer truly ballistic, but is close enough for
%the simulation.
%
%The tracks are started using the spher2CartOrbitCubature function (which
%is for a Keplerian model, not a J2 model). The correct measurements for
%two scans are used to start the tracks. After that, data association
%uncertainty can exist. This method of starting the tracks demonstrates how
%cubature integration can be used with an arbitrary dynamic model and
%measurement function to start a track. Note that additional steps would
%need to be taken if one were tracking satellites and multiple orbits
%between signtings were allowed.
%
%The JPDAF and GNN-JPDAF algorithms require a likelihood matrix to
%make their (soft) associations of measurements to targets. The function
%makeStandardCartOnlyLRMatHyps creates the likelihood matrix based on the
%exponent of the dimensionless score function of [1]. This requires that
%the clutter density in measurement space and the detection probability be
%known (or estimated). A nominal detection probability of 80% is used.
%
%The makeStandardCartOnlyLRMatHyps function also updates the tracks for
%each of the measurements. The sqrtKalmanUpdate function is used with
%Cartesian-converted measurements. The measurement conversion is performed
%using the spher2CartCubature function via fifth-order cubature
%integration.
%
%The target state is predicted using the momentMatchPred function, which
%can handle the nonlinear dynamic model. The momentMatchPred function
%utilizes non-stochastic Runge-Kutta methods to propagate the mean and
%square root covariance matrix.
%
%The trajectories are plotted along with the measurements. Also plotted is
%the offset from the range estimates of the target and the location of the
%first target. One could plot the mean squared error of the trajectories as
%well, but this is less informative regarding how well targets are
%separated as the cross-range errors for a monostatic tracking scenario
%such as this tend to be very high.
%
%REFERENCES:
%[1] Y. Bar-Shalom, S. S. Blackman, and R. J. Fitzgerald, "Dimensionless
%    score function for multiple hypothesis tracking," IEEE Transactions on
%    Aerospace and Electronic Systems, vol. 43, no. 1, pp. 392-400, Jan.
%    2007.
%
%June 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

disp('An example of simple nonlinear filtering and data association for a ballistic model')

%%%SETUP THE MODEL
disp('Setting parameters and creating the simulated trajectories.')
PD=0.8;%Detection probability --same for all targets.
%The number of observations in the simulation.
numObs=100;

%Assumed clutter parameter in terms of false alarms per meter-radian^2
%(spherical coordinates). False alarms are generated in the measurement
%domain of the radar. The tracker can still work if the value for lambda
%does not perfectly match the value simulated/ the real value. However, one
%should note that lambda in the tracker cannot be arbitrarily large lest
%the tracker eventually ignore all measurements (false alarms become always
%more likely than true tracks).
lambda=1e-2;
numMethods=2;%2 filters, a JPDAF and an GNN-JPDAF.

%The AlgSel parameters are inputs to the singleScanUpdate function.
%Method 1 uses the JPDAF
algSel1(1)=1;
algSel2(1)=0;
%Method 2 uses the GNN-JPDAF
algSel1(2)=0;
algSel2(2)=0;

%The location of the simulated radar in the Aleutian islands.
posRx=[51.86310; -177.98669]*(pi/180);
rRx=ellips2Cart([posRx;0]);

%The continuous-time dynamic model.
rDot=@(rState,t)aJ2Gravity(rState);

%Get the local coordinate axes of the receiver.
uENU=getENUAxes(posRx);

%Rotation matrix from global Cartesian coordinates to ENU
RGlob2ENU=findTransParam(eye(3,3),uENU);

%The nonlinear measurement function
h=@(x)Cart2Sphere(x(1:3,:),0,true,rRx,rRx,RGlob2ENU);

%Next, create the ballistic trajectories
[tObs,xObs,zObsTrue]=createBallisticTrajectories(rDot,h,numObs);
numTargets=size(zObsTrue,2);
PD=repmat(PD,[numTargets,1]);
zDim=size(zObsTrue,1);

%The time between samples is constant in this simulation, though missed
%detections are possible.
deltaT=tObs(2)-tObs(1);

sigmaR=10;
sigmaAzel=0.1*(pi/180);
%The square root measurement covariance matrix, local spherical
%coordinates. Here, it is assumed diagonal.
SR=diag([sigmaR;sigmaAzel;sigmaAzel]);

%Region in range around the targets in which to generate false alarms.
clutR=50e3;%50km
%Angular region in which to generate random false alarms around the
%targets.
clutAngs=2*(pi/180);
%The volume used for generating false alarms based times the given false
%alarm density is the expected number of false alarms per scan.
lambdaV=lambda*(2*clutR)*(2*clutAngs)^2;

%For measurement conversion
[xi3,w3]=fifthOrderCubPoints(3);

%For state propagation
[xi6,w6]=fifthOrderCubPoints(6);

%Initializations
q0=processNoiseSuggest('PolyKal-ROT',1e-2,deltaT);

a=rDot;
D=@(x,t)DPoly(1,q0,1,3);

disp('Initializing the tracks.')
%Generate the first two observations explicitly to start the tracks using an
%information filter. All methods get the same initialization.
xStates=zeros(6,numTargets,numObs,numMethods);
SStates=zeros(6,6,numTargets,numObs,numMethods);
zScans=cell(numObs,1);

zScans{1}=[];
zScans{2}=[];
zScansSpher{1}=[];
zScansSpher{2}=[];
for curTrack=1:numTargets
    z1=zObsTrue(:,curTrack,1)+SR*randn(3,1);
    [zCart1,RCart1]=spher2CartCubature(z1,SR,0,true,rRx,rRx,RGlob2ENU,xi3,w3);
    z2=zObsTrue(:,curTrack,2)+SR*randn(3,1);
    [zCart2,RCart2]=spher2CartCubature(z2,SR,0,true,rRx,rRx,RGlob2ENU,xi3,w3);

    zScansSpher{1}=z1;
    zScansSpher{2}=z2;

    [xInit,PInit]=spher2CartOrbitCubature([z1,z2],SR,0,true,deltaT,rRx,rRx,RGlob2ENU,true,xi6,w6);

    SInit=chol(PInit,'lower');

    for curMethod=1:numMethods
        xStates(:,curTrack,2,curMethod)=xInit;
        SStates(:,:,curTrack,2,curMethod)=SInit;
    end
    zScans{1}=[zScans{1},zCart1];
    zScans{2}=[zScans{2},zCart2];
end

disp('Running the tracker simulation (this will take a little while).')
%Run the tracker starting with the third measurement.
for curObs=3:numObs
    %Determine the number of false alarms to generate.
    numFalse=PoissonD.rand(1,lambdaV);

    %Determine which, if any, targets should be detected.
    isDet=rand(numTargets,1)<PD;

    %Allocate space for the detections.
    numMeas=numFalse+sum(isDet);
    zCur=zeros(3,numMeas);
    curDet=1;

    %Generate the detection from the targets, if any.
    for curTar=1:numTargets
        if(isDet(curTar)||curObs<5)
            zCur(:,curDet)=zObsTrue(:,curTar,curObs)+SR*randn(3,1);
            curDet=curDet+1;
        end
    end

    %Generate the false alarm detections, if any. They are centered around
    %the first target.
    rClutBounds=zObsTrue(1,1,curObs)+[-clutR;clutR];
    azBounds=zObsTrue(2,1,curObs)+[-clutAngs;clutAngs];
    elBounds=zObsTrue(3,1,curObs)+[-clutAngs;clutAngs];
    for curFalse=1:numFalse
        r=UniformD.rand(1,rClutBounds);
        az=UniformD.rand(1,azBounds);
        el=UniformD.rand(1,elBounds);

        zCur(:,curDet)=[r;az;el]+SR*randn(3,1);
        curDet=curDet+1;
    end

    %Convert all of the measurements to Cartesian coordinates using
    %cubature integration.
    zCart=zeros(zDim,numMeas);
    SRCart=zeros(zDim,zDim,numMeas);
    measJacobDet=zeros(numMeas,1);
    for curMeas=1:numMeas
        [zCart(:,curMeas),RCart]=spher2CartCubature(zCur(:,curMeas),SR,0,true,rRx,rRx,RGlob2ENU,xi3,w3);
        SRCart(:,:,curMeas)=chol(RCart,'lower');

        %The measurement Jacobian determinants are used for transforming
        %the clutter density.
        measJacobDet(curMeas)=det(calcSpherConvJacob(zCur(:,curMeas),0,true,rRx,rRx,RGlob2ENU));
    end
    zScans{curObs}=zCart;
    zScansSpher{curObs}=zCur;

    tPrev=tObs(curObs-1);
    tCur=tObs(curObs);

    %Update all of the tracks for the methods.
    for curMethod=1:numMethods
        %Predict the tracker to the current time for each target.
        x=xStates(:,:,curObs-1,curMethod);
        S=SStates(:,:,:,curObs-1,curMethod);
        for curTar=1:numTargets
            [x(:,curTar),S(:,:,curTar)]=momentMatchPred(x(:,curTar),S(:,:,curTar),a,D,3,tPrev,tCur,xi6,w6);
        end

        %Brute-force hypothesis formation and likelihood matrix
        %computation. A is the likelihood matrix.
        [A,xHyp,PHyp]=makeStandardCartOnlyLRMatHyps(x,S,zCart,SRCart,[],PD,lambda,[],[],measJacobDet);

        %Perform the single scan assignment
        [xPost,PPost]=singleScanUpdate(xHyp,PHyp,A,algSel1(curMethod),algSel2(curMethod));

        xStates(:,:,curObs,curMethod)=xPost;
        for curTar=1:numTargets
            SStates(:,:,curTar,curObs,curMethod)=chol(PPost(:,:,curTar),'lower');
        end
    end
end

disp('Plotting the measurements and tracks over the Earth.')
%Indicates how the lines for the method are drawn.
methodStyle{1}='r';
methodStyle{2}='b';

%%%Plot the estimated trajectories on the true trajectory
figure(1)
clf
hold on
clf
plotMapOnEllipsoid();
hold on;

%Plot all of the measurements for all of the scans
for curObs=1:numObs
    zCur=zScans{curObs};
    scatter3(zCur(1,:),zCur(2,:),zCur(3,:),'.b')
end

%Plot the tracks
plot3(xObs(1,:),xObs(2,:),xObs(3,:),'-g','linewidth',4)
for curMethod=1:numMethods
    %Track 1 is drawn in red; track 2 is drawn in blue.
    for curTrack=1:numTargets
        if(curTrack==1)
            lineOpts=['-',methodStyle{curMethod}];
        else
            lineOpts=['--',methodStyle{curMethod}];
        end

        x=xStates(:,curTrack,:,curMethod);
        plot3(x(1,2:end),x(2,2:end),x(3,2:end),lineOpts,'LineWidth',2)
    end
end

disp('Plotting the offset in range of the track estimates from the first target.')
disp('The lines for track 1 are solid; those for track 2 are dashed.')
disp('JPDAF estimates are red; GNN estimates are blue.')
disp('The observations are shown in black. The estimates are relatively close to the observations.')
%%%Compute and then plot the offset in range from the primary target.
rErrTargets=zeros(numObs,numTargets,numMethods);
for curObs=2:numObs
    for curMethod=1:2
        for curTrack=1:numTargets
            xState=xStates(:,curTrack,curObs,curMethod);
            SState=SStates(:,:,curTrack,curObs,curMethod);
            r=unbiasedR(xState,SState,RGlob2ENU,rRx,xi6,w6);

            rErrTargets(curObs,curTrack,curMethod)=r-zObsTrue(1,1,curObs);
        end
    end
end

figure(3)
clf
hold on
for curMethod=1:numMethods
    %Track 1 is drawn in red; track 2 is drawn in blue.
    for curTrack=1:numTargets
        if(curTrack==1)
            lineOpts=['-',methodStyle{curMethod}];
        else
            lineOpts=['--',methodStyle{curMethod}];
        end

        r=rErrTargets(:,curTrack,curMethod);
        plot(tObs(2:end),r(2:end),lineOpts,'LineWidth',2)
    end
end

%Plot the range of all of the measurements for all of the scans too
for curObs=2:numObs
    zCur=zScansSpher{curObs};
    numObs=size(zCur,2);
    if(numObs>0)
        scatter(repmat(tObs(curObs),[numObs,1]),bsxfun(@minus,zCur(1,:),zObsTrue(1,1,curObs)),'.k');
    end
end

h1=xlabel('Time (seconds)');
h2=ylabel('Offset in Range (meters)');

set(gca,'FontSize',18,'FontWeight','bold','FontName','Times')
set(h1,'FontSize',20,'FontWeight','bold','FontName','Times')
set(h2,'FontSize',20,'FontWeight','bold','FontName','Times')
axis([tObs(2), tObs(end), -300, 300])

end

function [tObs,xObs,zObsTrue]=createBallisticTrajectories(rDot,h,numObs)
    posStart=([39.61693; 134.54975])*pi/180;%The Sea of Japan
    %The middle of the Pacific.
    posEnd=([40.56751; -159.53953])*pi/180;
    rStart=ellips2Cart([posStart;0]);
    rEnd=ellips2Cart([posEnd;0]);

    minEL=10*(pi/180);%Minimum elevation for tracking.

    %Get starting velocity
    [vStart,tMinEllip]=orbVelDet2PtMinEng(rStart,rEnd);
    tSpan=[0; tMinEllip*1.25];

    %The function RKAdaptiveOverRange could be used to integrate over the
    %trajectory to a higher polynomial accuracy, but it is simpler to use
    %ode45, because the interpolation routine deval for the solution is
    %easy to use to get observations at desired times.
    options=odeset('RelTol',1e-8,'AbsTol',1e-10,'Refine',20);
    [tFull,xFull]=ode45(@(t,rState)rDot(rState,t),tSpan,[rStart;vStart],options);
    xFull=xFull';

    %Noise-free observations in the local coordinate system of the receiver.
    zTrue=h(xFull(1:3,:));
    elIdx=find(zTrue(3,:)>minEL);

    %Allocate space depending on the chosen scenario.
    zObsTrue=zeros(3,2,numObs);%The main target and one other return.

    %Get the primary trajectory at desired times
    tObs=linspace(tFull(elIdx(1)),tFull(elIdx(end)),numObs);
    options=odeset('RelTol',1e-8,'AbsTol',1e-10);
    sol=ode45(@(t,rState) rDot(rState,t),tSpan,[rStart;vStart],options);
    xObs=deval(sol,tObs);
    zObsTrue(:,1,:)=h(xObs(1:3,:));

    %The second target is offset by a constant amount in range.
    zObsTrue(2:end,2,:)=zObsTrue(2:end,1,:);
    zObsTrue(1,2,:)=zObsTrue(1,1,:)+50;
end

function r=unbiasedR(xState,SState,RGlob2ENU,rRx,xi,w)
    %Get an unbiased conversion of a Cartesian state into a range to a
    %sensor using cubature conversion (ignoring all propagation effects).

    %Transform the cubature points.
    xi=bsxfun(@plus,SState*xi,xState);

    %Subtract the receiver location and rotate into ENU coordinates.
    xiLocal=RGlob2ENU*bsxfun(@minus,xi(1:3,:),rRx);

    %Compute ranges
    rCub=sqrt(sum(xiLocal(1:3,:).^2,1));
    r=sum(bsxfun(@times,rCub(:),w(:)));
end

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


function edge = clipLine3d(line, box)
%CLIPLINE3D Clip a line with a box and return an edge.
%
%   EDGE = clipLine3d(LINE, BOX);
%   Clips the line LINE with the bounds given in BOX, and returns the
%   corresponding edge.
%
%   If the line lies totally outside of the box, returns a 1-by-6 row array
%   containing only NaN's.
%
%   If LINE is a N-by-6 array, with one line by row, returns the clipped
%   edge coresponding to each line in a N-by-6 array.
%
%   See also:
%   lines3d, edges3d, createLine3d
%

% ---------
% author : David Legland
% e-mail: david.legland@inra.fr
% INRA - TPV URPOI - BIA IMASTE
% created the 30/10/2008 from drawLine3d

%   HISTORY
%   30/10/2008 replace intersectPlaneLine by intersectLinePlane
%   25/11/2008 improve test for bounds, and use more explicit code
%   22/06/2009 fig bug, add support for several lines
%   16/11/2010 use middle point for checking edge bounds

% get box limits
xmin = box(1); xmax = box(2);
ymin = box(3); ymax = box(4);
zmin = box(5); zmax = box(6);

% extreme corners of the box
p000 = [xmin ymin zmin];
p111 = [xmax ymax zmax];

% main vectors
ex   = [1 0 0];
ey   = [0 1 0];
ez   = [0 0 1];

% box faces parallel to Oxy
planeZ0 = [p000 ex ey];
planeZ1 = [p111 ex ey];

% box faces parallel to Oxz
planeY0 = [p000 ex ez];
planeY1 = [p111 ex ez];

% box faces parallel to Oyz
planeX0 = [p000 ey ez];
planeX1 = [p111 ey ez];

% number of lines
nLines = size(line, 1);

% allocate memory for result
edge = zeros(nLines, 6);

% iterate over lines to clip
for i = 1:nLines

    % compute intersection point with each plane
    ipZ0 = intersectLinePlane(line(i,:), planeZ0);
    ipZ1 = intersectLinePlane(line(i,:), planeZ1);
    ipY0 = intersectLinePlane(line(i,:), planeY0);
    ipY1 = intersectLinePlane(line(i,:), planeY1);
    ipX1 = intersectLinePlane(line(i,:), planeX1);
    ipX0 = intersectLinePlane(line(i,:), planeX0);

    % concatenate resulting points
    points  = [ipX0;ipX1;ipY0;ipY1;ipZ0;ipZ1];

    % compute position of each point on the line
    pos     = linePosition3d(points, line(i,:));

    % keep only defined points
    ind     = find(~isnan(pos));
    pos     = pos(ind);
    points  = points(ind,:);

    % sort points with respect to their position
    [pos, ind] = sort(pos); %#ok<ASGLU>
    points  = points(ind, :);

    % keep median points wrt to position. These points define the limit of
    % the clipped edge.
    nv      = length(ind)/2;

    % create resulting edge.
    edge(i,:)   = [points(nv, :) points(nv+1, :)];
end

% check that middle point of the edge is contained in the box
midX = mean(edge(:, [1 4]), 2);
xOk  = xmin <= midX & midX <= xmax;
midY = mean(edge(:, [2 5]), 2);
yOk  = ymin <= midY & midY <= ymax;
midZ = mean(edge(:, [3 6]), 2);
zOk  = zmin <= midZ & midZ <= zmax;

% if one of the bounding condition is not met, set edge to NaN
edge (~(xOk & yOk & zOk), :) = NaN;


function M = fixedrankfactory_2factors_subspace_projection(m, n, k)
% Manifold of m-by-n matrices of rank k with two factor quotient geometry.
%
% function M = fixedrankfactory_2factors_subspace_projection(m, n, k)
%
% A point X on the manifold is represented as a structure with two
% fields: L and R. The matrix L (mxk) is orthonormal,
% while the matrix R (nxk) is a full column-rank
% matrix such that X = L*R'.
%
% Tangent vectors are represented as a structure with two fields: L, R.
%
% Note: L is orthonormal, i.e., columns are orthogonal to each other.
% Such a geometry might be of interest where the left factor has a
% subspace interpretation. A motivation is in Sections 3.3 and 6.4 of the
% paper below.
%
% Please cite the Manopt paper as well as the research paper:
%     @Article{mishra2014fixedrank,
%       Title   = {Fixed-rank matrix factorizations and {Riemannian} low-rank optimization},
%       Author  = {Mishra, B. and Meyer, G. and Bonnabel, S. and Sepulchre, R.},
%       Journal = {Computational Statistics},
%       Year    = {2014},
%       Number  = {3-4},
%       Pages   = {591--621},
%       Volume  = {29},
%       Doi     = {10.1007/s00180-013-0464-z}
%     }
%
% See also: fixedrankfactory_2factors fixedrankembeddedfactory fixedrankfactory_2factors_preconditioned

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

    M.name = @() sprintf('LR'' quotient manifold of %dx%d matrices of rank %d', m, n, k);

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

    % Some precomputations at the point X to be used in the inner product (and
    % pretty much everywhere else).
    function X = prepare(X)
        if ~all(isfield(X,{'RtR'}) == 1)
            X.RtR = X.R'*X.R;
        end
    end

    % The choice of the metric is motivated by symmetry and scale
    % invariance in the total space.
    M.inner = @iproduct;
    function ip = iproduct(X, eta, zeta)
        X = prepare(X);

        ip = eta.L(:).'*zeta.L(:)  + trace(X.RtR\(eta.R'*zeta.R));
    end

    M.norm = @(X, eta) sqrt(M.inner(X, eta, eta));

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

    M.typicaldist = @() 10*k;

    skew = @(X) .5*(X-X');
    symm = @(X) .5*(X+X');
    stiefel_proj = @(L, H) H - L*symm(L'*H);

    M.egrad2rgrad = @egrad2rgrad;
    function rgrad = egrad2rgrad(X, egrad)
        X = prepare(X);

        rgrad.L = stiefel_proj(X.L, egrad.L);
        rgrad.R = egrad.R*X.RtR;
    end

    M.ehess2rhess = @ehess2rhess;
    function Hess = ehess2rhess(X, egrad, ehess, eta)
        X = prepare(X);

        % Riemannian gradient.
        rgrad = egrad2rgrad(X, egrad);

        % Directional derivative of the Riemannian gradient.
        Hess.L = ehess.L - eta.L*symm(X.L'*egrad.L);
        Hess.L = stiefel_proj(X.L, Hess.L);

        Hess.R = ehess.R*X.RtR + 2*egrad.R*symm(eta.R'*X.R);

        % Correction factor for the non-constant metric on the factor R.
        Hess.R = Hess.R - rgrad.R*(X.RtR\(symm(X.R'*eta.R))) - eta.R*(X.RtR\(symm(X.R'*rgrad.R))) + X.R*(X.RtR\(symm(eta.R'*rgrad.R)));

        % Projection onto the horizontal space.
        Hess = M.proj(X, Hess);
    end

    M.proj = @projection;
    function etaproj = projection(X, eta)
        X = prepare(X);

        eta.L = stiefel_proj(X.L, eta.L); % On the tangent space.
        SS = X.RtR;
        AS1 = 2*X.RtR*skew(X.L'*eta.L)*X.RtR;
        AS2 = 2*skew(X.RtR*(X.R'*eta.R));
        AS  = skew(AS1 + AS2);

        Omega = nested_sylvester(SS,AS);
        etaproj.L = eta.L - X.L*Omega;
        etaproj.R = eta.R - X.R*Omega;
    end

    M.tangent = M.proj;
    M.tangent2ambient = @(X, eta) eta;

    M.retr = @retraction;
    function Y = retraction(X, eta, t)
        if nargin < 3
            t = 1.0;
        end
        Y.L = uf(X.L + t*eta.L);
        Y.R = X.R + t*eta.R;

        % These are reused in the computation of the gradient and Hessian.
        Y = prepare(Y);
    end

    M.exp = @exponential;
    function R = exponential(X, eta, t)
        if nargin < 3
            t = 1.0;
        end

        R = retraction(X, eta, t);
        warning('manopt:fixedrankfactory_2factors_subspace_projection:exp', ...
            ['Exponential for fixed rank ' ...
            'manifold not implemented yet. Lsed retraction instead.']);
    end

    M.hash = @(X) ['z' hashmd5([X.L(:) ; X.R(:)])];

    M.rand = @random;
    % Factors L lives on Stiefel manifold, hence we will reuse
    % its random generator.
    stiefelm = stiefelfactory(m, k);
    function X = random()
        X.L = stiefelm.rand();
        X.R = randn(n, k);
    end

    M.randvec = @randomvec;
    function eta = randomvec(X)
        eta.L = randn(m, k);
        eta.R = randn(n, k);
        eta = projection(X, eta);
        nrm = M.norm(X, eta);
        eta.L = eta.L / nrm;
        eta.R = eta.R / nrm;
    end

    M.lincomb = @lincomb;

    M.zerovec = @(X) struct('L', zeros(m, k),...
        'R', zeros(n, k));

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

    % vec and mat are not isometries, because of the scaled inner metric.
    M.vec = @(X, U) [U.L(:) ; U.R(:)];
    M.mat = @(X, u) struct('L', reshape(u(1:(m*k)), m, k), ...
        'R', reshape(u((m*k+1):end), n, k));
    M.vecmatareisometries = @() false;

end

% Linear combination of tangent vectors.
function d = lincomb(x, a1, d1, a2, d2) %#ok<INLSL>

    if nargin == 3
        d.L = a1*d1.L;
        d.R = a1*d1.R;
    elseif nargin == 5
        d.L = a1*d1.L + a2*d2.L;
        d.R = a1*d1.R + a2*d2.R;
    else
        error('Bad use of fixedrankfactory_2factors_subspace_projection.lincomb.');
    end

end

function A = uf(A)
    [L, unused, R] = svd(A, 0); %#ok
    A = L*R';
end

function omega = nested_sylvester(sym_mat, asym_mat)
    % omega=nested_sylvester(sym_mat,asym_mat)
    % This function solves the system of nested Sylvester equations:
    %
    %     X*sym_mat + sym_mat*X = asym_mat
    %     Omega*sym_mat+sym_mat*Omega = X
    % Mishra, Meyer, Bonnabel and Sepulchre, 'Fixed-rank matrix factorizations and Riemannian low-rank optimization'

    % Uses built-in lyap function, but does not exploit the fact that it's
    % twice the same sym_mat matrix that comes into play.

    X = lyap(sym_mat, -asym_mat);
    omega = lyap(sym_mat, -X);

end


%% Copyright (C) 2014-2016, 2018-2019, 2022 Colin B. Macdonald
%% Copyright (C) 2022 Chris Gorman
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod  @@sym heaviside (@var{x})
%% @defmethodx @@sym heaviside (@var{x}, @var{zero_value})
%% Symbolic Heaviside step function.
%%
%% Example:
%% @example
%% @group
%% syms x
%% @c doctest: +SKIP_UNLESS(pycall_sympy__ ('return Version(spver) > Version("1.4")'))
%% y = heaviside (x)
%%   @result{} y = (sym) θ(x)
%% @end group
%% @end example
%%
%% There are various conventions for 'heaviside(sym(0))'; this function
%% returns the midpoint by default.
%% @example
%% @group
%% @c doctest: +SKIP_UNLESS(pycall_sympy__ ('return Version(spver) > Version("1.8")'))
%% heaviside(sym(0))
%%   @result{} (sym) 1/2
%% @end group
%% @end example
%%
%% The optional second argument overrides the default.  For example
%% to make the function right-continuous,
%% @example
%% @group
%% heaviside(0, sym(1))
%%   @result{} (sym) 1
%% @end group
%% @end example
%% or left-continuous.
%% @example
%% @group
%% heaviside(0, sym(0))
%%   @result{} (sym) 0
%% @end group
%% @end example
%%
%% If passed a matrix, the heaviside function is computed for each
%% element.
%% @example
%% @group
%% @c doctest: +SKIP_UNLESS(pycall_sympy__ ('return Version(spver) > Version("1.8")'))
%% heaviside([sym(-1) sym(0) sym(1) sym(x)])
%%   @result{} (sym) [0  1/2  1  θ(x)]  (1×4 matrix)
%% @end group
%% @end example
%% @seealso{heaviside, @@sym/dirac}
%% @end defmethod

function y = heaviside(x, h0)
  if (nargin == 1)
    y = elementwise_op ('Heaviside', x);
  elseif (nargin == 2)
    y = elementwise_op ('Heaviside', sym(x), sym(h0));
  else
    print_usage ();
  end
end

%!error heaviside (sym(1), 2, 3)

%!assert (isequal (heaviside (sym(1)), sym(1)))
%!assert (isequal (heaviside (-sym(1)), sym(0)))

%!assert (double (heaviside (1)), heaviside (1))

%!test
%! D = [1 -1; -10 20];
%! A = sym(D);
%! assert (double (heaviside (A)), heaviside (D))

%!test
%! H0 = sym([1 -2 0; 3 0 pi]);
%! A = heaviside (sym(0), H0);
%! assert (isequal (A, H0))

%!test
%! A = heaviside ([-1 0 1], sym(1)/2);
%! assert (isequal (A, [0 sym(1)/2 1]))

%!test
%! A = heaviside ([-1 0 1], sym(1)/2);
%! assert (isequal (A, [0 sym(1)/2 1]))

%!assert (isequaln (heaviside (sym(nan)), sym(nan)))

%!test
%! assert (isequaln (heaviside (sym(nan), sym(nan)), sym(nan)))
%! assert (isequaln (heaviside (0, sym(nan)), sym(nan)))
%! assert (isequaln (heaviside (2, sym(nan)), sym(1)))
%! assert (isequaln (heaviside (-2, sym(nan)), sym(0)))

%!test
%! % round trip
%! syms x
%! A = heaviside (1);
%! f = heaviside (x);
%! h = function_handle (f);
%! B = h (1);
%! assert (A, B, -eps)

%!test
%! % round trip
%! syms x h0
%! f = heaviside (x, h0);
%! h = function_handle (f, 'vars', {x h0});
%! A = heaviside (1, 1/2);
%! B = h (1, 1/2);
%! assert (A, B, -eps)
%! A = heaviside (0, 1/2);
%! B = h (0, 1/2);
%! assert (A, B, -eps)


%RT2TR Convert rotation and translation to homogeneous transform
%
% TR = RT2TR(R, t) is a homogeneous transformation matrix (MxM) formed from an
% orthonormal rotation matrix R (NxN) and a translation vector t (Nx1) where
% M=N+1.
%
% For a sequence R (NxNxK) and t (NxK) results in a transform sequence (MxMxK).
%
% Notes::
% - Works for R in SO(2) or SO(3)
%  - If R is 2x2 and t is 2x1, then TR is 3x3
%  - If R is 3x3 and t is 3x1, then TR is 4x4
% - The validity of R is not checked
%
% See also T2R, R2T, TR2RT.

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

function T = rt2tr(R, t)
    if numcols(R) ~= numrows(R)
        error('R must be square');
    end
    if numrows(R) ~= numrows(t)
        error('R and t must have the same number of rows');
    end

    if size(R,3) ~= numcols(t)
        error('For sequence size(R,3) must equal size(t,2)');
    end

    if size(R,3) > 1
        Z = zeros(numcols(R),1);
        B = [Z' 1];
        T = zeros(4,4,size(R,3));
        for i=1:size(R,3)
            T(:,:,i) = [R(:,:,i) t(:,i); B];
        end
    else
        T = [R t; zeros(1,numcols(R)) 1];
    end


function [z,output] = minf_lbfgs(f,g,z0,options)
%MINF_LBFGS Minimize a function by L-BFGS with line search.
%   [z,output] = minf_lbfgs(f,g,z0) starts at z0 and attempts to find a
%   local minimizer of the real-valued function f(z). The input variables z
%   may be a scalar, vector, matrix, tensor or even a (nested) cell array
%   of tensors and its contents may be real or complex.
%
%   If f(x) is a function of real variables x, the function g(x) should
%   compute the partial derivatives of f with respect to the real variables
%   x, i.e. g(xk) := df(xk)/dx. If f(z) is a function of complex variables
%   z, the function g(z) should compute two times the partial derivative
%   of f with respect to conj(z) (treating z as constant), i.e. g(zk) :=
%   2*df(zk)/d(conj(z)) = 2*conj(df(zk)/dz). If g is the empty matrix [],
%   the real gradient or scaled conjugate cogradient is approximated with
%   finite differences. The output of the function g(z) may have the same
%   structure as z (although this is not necessary). The structure output
%   returns additional information:
%
%      output.alpha      - The line search step length in every iteration.
%      output.fevals     - The total number of function/gradient calls.
%      output.fval       - The value of the objective function f in every
%                          iteration.
%      output.info       - The circumstances under which the procedure
%                          terminated:
%                             1: Objective function tolerance reached.
%                             2: Step size tolerance reached.
%                             3: Maximum number of iterations reached.
%      output.infols     - The circumstances under which the line search
%                          terminated in every iteration.
%      output.iterations - The number of iterations.
%      output.relfval    - The difference in objective function value
%                          between every two successive iterates, relative
%                          to its initial value.
%      output.relstep    - The step size relative to the norm of the
%                          current iterate in every iteration.
%
%   minf_lbfgs(f,g,z0,options) may be used to set the following options:
%
%      options.Display = 10      - Displays output information each
%                                  options.Display iterations. Set to 0 to
%                                  disable.
%      options.LineSearch        - The line search used to minimize the
%      = @ls_mt                    objective function in the quasi-Newton
%                                  descent direction.
%      options.LineSearchOptions - The options structure passed to the line
%                                  search routine.
%      options.M                 - The number of L-BFGS updates to store.
%      = min(30,length(z0))
%      options.MaxIter = 500     - The maximum number of iterations.
%      options.TolFun = 1e-6     - The tolerance for output.relfval.
%      options.TolX = 1e-8       - The tolerance for output.relstep.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Unconstrained
%       optimization of real functions in complex variables", SIAM J. Opt.,
%       Vol. 22, No. 3, 2012, pp. 879-898.

% Evaluate the objective function and gradient.
dim = structure(z0);
fval = f(z0);
if ~isa(g,'function_handle') && isempty(g)
    grad = serialize(deriv(f,z0,fval));
else
    grad = serialize(g(z0));
end
z = z0;

% Check the options structure.
isfunc = @(f)isa(f,'function_handle');
if nargin < 4, options = struct; end
if ~isfield(options,'Display'), options.Display = 10; end
if ~isfield(options,'LineSearch') || ~isfunc(options.LineSearch)
    options.LineSearch = @ls_mt;
end
if ~isfield(options,'LineSearchOptions')
    options.LineSearchOptions = struct;
end
if ~isfield(options.LineSearchOptions,'alpha')
    options.LineSearchOptions.alpha = 1;
end
if ~isfield(options.LineSearchOptions,'c2')
    options.LineSearchOptions.c2 = 0.9;
end
if ~isfield(options,'M'), options.M = min(30,length(grad)); end
if ~isfield(options,'MaxIter'), options.MaxIter = 500; end
if ~isfield(options,'TolFun'), options.TolFun = 1e-6; end
if ~isfield(options,'TolX'), options.TolX = 1e-8; end

% Initialize the algorithm.
S = zeros(numel(grad),options.M);
Y = zeros(numel(grad),options.M);
a = zeros(1,options.M);
r = zeros(1,options.M);
m = 0;
midx = 0;

% L-BFGS with line search.
output.alpha = [];
output.fevals = 1;
output.fval = fval;
output.info = false;
output.infols = [];
output.iterations = 0;
output.relfval = [];
output.relstep = [];
while ~output.info

    % Compute the quasi-Newton step pqn = -H*grad.
    pqn = -grad;
    for i = 1:m
        a(i) = r(midx(i))*real(S(:,midx(i))'*pqn);
        pqn = pqn-a(i)*Y(:,midx(i));
    end
    if m > 0
        y1 = Y(:,midx(1));
        y1y1 = y1'*y1;
        gamma = y1y1*r(midx(1));
        pqn = 1/gamma*pqn;
    end
    for i = m:-1:1
        b = r(midx(i))*real(Y(:,midx(i))'*pqn);
        pqn = pqn+(a(i)-b)*S(:,midx(i));
    end

    % Minimize f along z+alpha*pqn.
    state = output; state.grad = grad;
    [alpha,outputls] = options.LineSearch( ...
        f,g,z,deserialize(pqn,dim),state,options.LineSearchOptions);
    output.alpha(:,end+1) = alpha;
    if length(alpha) < 2, alpha(2) = 1; end

    % Update iterate.
    z1 = serialize(z);
    z = alpha(2)*(z1+alpha(1)*pqn);
    if alpha(2) == 1
        s = alpha(1)*pqn;
    else
        s = z-z1;
    end
    z = deserialize(z,dim);

    % Update gradient and Hessian approximation.
    grad1 = grad;
    if isfield(outputls,'grad')
        grad = outputls.grad;
    else
        if ~isa(g,'function_handle') && isempty(g)
            grad = serialize(deriv(f,z,output.fval(end)));
        else
            grad = serialize(g(z));
        end
    end
    y = grad-grad1;
    sy = real(y'*s);
    if sy > 0
        m = min(m+1,options.M);
        midx = [midx(1)+1:-1:1,m:-1:midx(1)];
        S(:,midx(1)) = s;
        Y(:,midx(1)) = y;
        r(:,midx(1)) = 1/sy;
    end

    % Update the output structure.
    if isfield(outputls,'fevals')
        output.fevals = output.fevals+outputls.fevals;
    end
    if isfield(outputls,'fval')
        output.fval(end+1) = outputls.fval;
    else
        output.fval(end+1) = f(z);
    end
    if isfield(outputls,'info')
        output.infols(end+1) = outputls.info;
    end
    output.iterations = output.iterations+1;
    output.relfval(end+1) = ...
        abs(diff(output.fval(end:-1:end-1)))/abs(output.fval(1));
    output.relstep(end+1) = norm(s)/norm(z1);
    if isnan(output.relstep(end)), output.relstep(end) = 0; end
    if output.relfval(end) <= options.TolFun, output.info = 1; end
    if output.relstep(end) <= options.TolX, output.info = 2; end
    if output.iterations >= options.MaxIter, output.info = 3; end

    % Display progress.
    if options.Display > 0 && (output.iterations == 1 || output.info || ...
       mod(output.iterations,options.Display) == 0)
        if output.iterations == 1
            bold = '%s';
            [~,~,~,~,v] = regexp(version('-release'),'([0-9]+)([ab])');
            if usejava('Desktop') && str2double(v{1}{1}) > 2011 || ...
               (str2double(v{1}{1}) == 2011 && strcmpi(v{1}{2},'b'))
                bold = '%s';
            end
        end
        if output.iterations == 1 || ...
           mod(output.iterations,15*options.Display) == 0
            fprintf('\n%7s%s','',sprintf(bold,'fval'));
            fprintf('%13s%s','',sprintf(bold,'relfval'));
            fprintf('%10s%s','',sprintf(bold,'relstep'));
            fprintf('%10s%s','',sprintf(bold,'alpha'));
            fprintf('\n%30s = %4.e %6s = %4.e\n\n', ...
                    'TolFun',options.TolFun,'TolX',options.TolX);
        end
        if output.iterations == 1
            fprintf('%4i: % 14.8e |\n',0,output.fval(1));
        end
        stralpha = [repmat('%10.4e ',1,size(output.alpha,1)) '\n'];
        fprintf(['%4i: % 14.8e | %14.8e | %14.8e | ' stralpha], ...
                output.iterations,output.fval(end), ...
                output.relfval(end),output.relstep(end), ...
                abs(output.alpha(:,end)));
    end

end

% Display termination message.
if options.Display > 0
    ahref = '\n%s\n\n';
    x = round(linspace(0,output.iterations,min(500,output.iterations)));
    if length(bold) > 2
        ahref = sprintf(['\n%%s\n\n'],mat2str(x'), ...
            mat2str([output.fval(x+1)' [nan output.relfval(x(2:end))]' ...
                    [nan output.relstep(x(2:end))]'],3));
    end
    switch output.info
        case 1, fprintf(ahref,'Objective function tolerance reached.');
        case 2, fprintf(ahref,'Step size tolerance reached.');
        case 3, fprintf(ahref,'Maximum number of iterations reached.');
    end
end

end

function [z,offset] = deserialize(z,dim,offset)
    if iscell(dim)
        v = z;
        z = cell(size(dim));
        if nargin < 3, offset = 0; end
        for i = 1:numel(z)
            if iscell(dim{i})
                [z{i},offset] = deserialize(v,dim{i},offset);
            else
                n = prod(dim{i}(:));
                z{i} = reshape(v(offset+(1:n)),dim{i});
                offset = offset+n;
            end
        end
    elseif ~isempty(dim)
        z = reshape(z,dim);
    end
end

function z = serialize(z)
    if iscell(z)
        for i = find(cellfun(@iscell,z(:).'))
            z{i} = serialize(z{i});
        end
        s = cellfun(@numel,z(:)); o = [0; cumsum(s)];
        c = z; z = zeros(o(end),1);
        for i = 1:length(s), z(o(i)+(1:s(i))) = c{i}(:); end
    else
        z = z(:);
    end
end

function dim = structure(z)
    if iscell(z)
        dim = cellfun(@size,z,'UniformOutput',false);
        for i = find(cellfun(@iscell,z(:).'))
            dim{i} = structure(z{i});
        end
    else
        dim = size(z);
        if numel(z) == dim(1), dim = []; end
    end
end


function [ x, more ] = sgmga_vcn ( n, w, x, q_min, q_max, more )

%*****************************************************************************80
%
%% SGMGA_VCN returns the next constrained vector.
%
%  Discussion:
%
%    This function is intended to replace the "naive" version, now called
%    SGMGA_VCN_NAIVE, which is too slow for high dimensional problems.
%
%    For nonnegative vectors X of dimension N, and nonnegative
%    weights W, we define:
%
%      Q = sum ( 1 <= I <= N ) W(I) * X(I)
%
%    and seek X satisfying the constraint:
%
%      Q_MIN < Q <= Q_MAX
%
%    This routine returns, one at a time exactly those X which satisfy
%    the constraint.  No attempt is made to return the X values in
%    any particular order as far as Q goes.
%
%  Example:
%
%        W               4.0 3.0 5.0
%      MIN     16.0       0   0   0
%      ---     ----      -----------
%        1     20.0       5   0   0
%        2     19.0       4   1   0
%        3     18.0       3   2   0
%        4     17.0       2   3   0
%        5     20.0       2   4   0
%        6     19.0       1   5   0
%        7     18.0       0   6   0
%        8     17.0       3   0   1
%        9     20.0       3   1   1
%       10     19.0       2   2   1
%       11     18.0       1   3   1
%       12     17.0       0   4   1
%       13     20.0       0   5   1
%       14     18.0       2   0   2
%       15     17.0       1   1   2
%       16     20.0       1   2   2
%       17     19.0       0   3   2
%       18     19.0       1   0   3
%       19     18.0       0   1   3
%       20     20.0       0   0   4
%      ---     ----      ----------
%      MAX     20.0       6   7   5
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 May 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    An Anisotropic Sparse Grid Stochastic Collocation Method for Partial
%    Differential Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2411-2442.
%
%  Parameters:
%
%    Input, integer N, the dimension of the vector.
%
%    Input, real W(N), the weights, which should be nonnegative.
%    At least one weight must be positive.
%
%    Input, integer X(DIM_NUM).  On first call, with
%    MORE = FALSE, the input value of X is not important.  On subsequent calls,
%    the input value of X should be the output value from the previous call.
%
%    Input, real Q_MIN, Q_MAX, the lower and upper
%    limits on the sum.
%
%    Input, logical MORE.  On input, if the user has set MORE
%    FALSE, the user is requesting the initiation of a new sequence
%    of values.  If MORE is TRUE, then the user is requesting "more"
%    values in the current sequence.
%
%    Output, integer X(DIM_NUM).
%    On output, (with MORE = TRUE), the value of X will be the "next"
%    vector in the reverse lexicographical list of vectors that satisfy
%    the condition.  However, on output with MORE = FALSE, the vector
%    X is meaningless, because there are no more vectors in the list.
%
%    Output, logical MORE.  If MORE is TRUE on output,
%    then another value was found and returned in X, but if MORE is
%    FALSE, then there are no more values in the sequence, and X is
%    NOT the next value.
%
  persistent dir;
  persistent n2;
  persistent nstart;
  persistent xmax;
  persistent xmin;
%
%  Initialization for first call.
%
%  Allocate XMAX to remember the currently maximum possible value for each X.
%
%  Locate NSTART, the index of the first nonzero weight.
%  The algorithm is easier to program if the last index we look at
%  has a nonzero weight, so that it can always make up the remainder.
%
  if ( ~more )

    xmax = zeros ( n, 1 );
    nstart = - 1;

    for i = 1 : n
      if ( 0.0 < w(i) )
        nstart = i;
        break
      end
    end
%
%  Theoretically, we could even handle the case where all weights are zero.
%  That case is ruled out elsewhere in this software, so I will not try
%  to deal with it here for now.
%
    if ( nstart == - 1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SGMGA_VCN - Fatal error!\n' );
      fprintf ( 1, '  No weight is positive.\n' );
      error ( 'SGMGA_VCN - Fatal error!' );
    end
%
%  Initialize X to zero, even the indices we ignore.
%
    x = zeros ( n, 1 );
%
%  N2 points to our current index of interest.
%
    n2 = n + 1;
    dir = - 1;

    more = 1;

  end
%
%  Look for the next solution vector X.
%
  while ( 1 )
%
%  If no more, the search is terminated.
%
    if ( ~more )

      break
%
%  DIR = -1, decrement N2, and, if possible, set X(N2) to XMIN.
%  DIR =  0, hold N2 at current value, and see if we can increment X(N2).
%
    elseif ( dir == - 1 || dir == 0 )

      if ( dir == - 1 )
        n2 = n2 - 1;
      end

      if ( w(n2) == 0.0 )

        xmin = 0;
        xmax(n2) = 0;

      elseif ( nstart < n2 )

        xmin = 0;
        xmax(n2) = floor ( ( q_max -  w(n2+1:n,1)' * x(n2+1:n,1) ) / w(n2) );

      elseif ( n2 == nstart && dir == - 1 )

        xmin = ceil ( ( q_min - w(n2+1:n,1)' * x(n2+1:n,1) ) / w(n2) );
        xmin = max ( xmin, 0 );
        if ( w(1:n2-1,1)' * x(1:n2-1,1) ...
           + w(n2) * xmin ...
           + w(n2+1:n,1)' * x(n2+1:n,1)  ...
          <= q_min )
          xmin = xmin + 1;
        end

        x(n2) = xmin;

        xmax(n2) = floor ( ( q_max -  w(n2+1:n,1)' * x(n2+1:n,1) ) / w(n2) );

      end

      if ( xmax(n2) < xmin )

        dir = + 1;

      else

        if ( n2 == nstart )

          if ( dir == - 1 )
            dir = 0;
            break
          elseif ( dir == 0 )
            x(n2) = x(n2) + 1;
            if ( x(n2) <= xmax(n2) )
              break
            else
              dir = + 1;
            end

          end

        else

          x(n2) = xmin;

        end

      end
%
%  DIR = + 1:
%  Try moving backwards to find an index N2 whose X we can increment.
%
    elseif ( dir == + 1 )

      while ( 1 )

        if ( n2 == n )
          dir = 0;
          more = 0;
          xmax = [];
          break;
        end

        n2 = n2 + 1;

        if ( 0.0 < w(n2) )

          if ( x(n2) < xmax(n2) )
            x(n2) = x(n2) + 1;
            dir = - 1;
            break
          end

        end
      end
    end
  end

  return
end


% contourlet2tree.m
% written by: Duncan Po
% Date: August 24, 2002
% convert the contourlet coefficients from subband structure to tree structure
% Usage: [tree, scaling] = contourlet2tree(coef, dir)
% Inputs:   coef        - contourlet coefficients
%           dir         - the directional coefficients to be processed
% Output:   tree        - the resulting tree structure
%           scaling     - the scaling function of the coefficients

function [tree, scaling] = contourlet2tree(coef, dir)

M = length(coef{2});
nlevel = length(coef)-1;

for l = 2:nlevel+1
    levndir(l-1) = log2(length(coef{l}));
end;

scaling = coef{1};

for r = 1:(length(levndir)-1)
    if levndir(r+1)==levndir(r)
        split(r) = 0;
    elseif levndir(r+1)-levndir(r)==1
        split(r) = 1;
    else
        return;
    end;
end;

for l=2:nlevel+1
    if dir> M/2
      for i=1+2^(levndir(l-1)-levndir(1))*(dir-1):2^(levndir(l-1)-levndir(1))*dir
          coef{l}{i} = coef{l}{i}.';
      end;
    end;
end;

for l=2:nlevel+1
    if (l>2) & (levndir(l-1)~=levndir(1))
        j=1;
        if split(l-2)==1
            for i=1+2^(levndir(l-1)-levndir(1))*(dir-1):2:2^(levndir(l-1)-levndir(1))*dir
                coef{l}{j} = type3transform(coef{l}{i},coef{l}{i+1});
                j = j + 1;
            end;
        else
            for i=1+2^(levndir(l-1)-levndir(1))*(dir-1):2:2^(levndir(l-1)-levndir(1))*dir
                coef{l}{j} = type4transform(coef{l}{i},coef{l}{i+1});
                j = j + 1;
            end;
        end;

        i=length(coef{l})/length(coef{2})/2;
        while i>1
            j=1;
            for k=1:2:i
                coef{l}{j} = type4transform(coef{l}{k}, coef{l}{k+1});
                j = j+1;
            end;
            i = i/2;
        end;
        interimstructure{l-1} = coef{l}{1};
    else;
        interimstructure{l-1} = coef{l}{dir};
    end;
end;

tree{1} = interimstructure{1}(:);
for l = 2:nlevel
    tree{l} = [];
    for col = 1:2:size(interimstructure{l}, 2)
        temp = interimstructure{l}(:,col:col+1);
        temp = temp.';
        tree{l} = [tree{l}; temp(:)];
    end;
end;


%TRANSL Create or unpack an SE3 translational transform
%
% Create a translational transformation matrix::
%
% T = TRANSL(X, Y, Z) is an SE(3) homogeneous transform (4x4) representing
% a pure translation of X, Y and Z.
%
% T = TRANSL(P) is an SE(3) homogeneous transform (4x4) representing a
% translation of P=[X,Y,Z]. If P (Mx3) it represents a sequence and T
% (4x4xM) is a sequence of homogeneous transforms such that T(:,:,i)
% corresponds to the i'th row of P.
%
% Unpack the translational part of a transformation matrix::
%
% P = TRANSL(T) is the translational part of a homogeneous transform T as a
% 3-element column vector.  If T (4x4xM) is a homogeneous transform
% sequence the rows of P (Mx3) are the translational component of the
% corresponding transform in the sequence.
%
% [X,Y,Z] = TRANSL(T) is the translational part of a homogeneous transform
% T as three components.  If T (4x4xM) is a homogeneous transform sequence
% then X,Y,Z (1xM) are the translational components of the corresponding
% transform in the sequence.
%
% Notes::
% - Somewhat unusually this function performs a function and its inverse.  An
%   historical anomaly.
%
% See also CTRAJ.

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

function [t1,t2,t3] = transl(x, y, z)
    if nargin == 1
        if ishomog(x)
            if ndims(x) == 3
                % transl(T)  -> P, trajectory case
                if nargout == 1
                    t1 = squeeze(x(1:3,4,:))';
                elseif nargout == 3
                    t1 = squeeze(x(1,4,:))';
                    t2 = squeeze(x(2,4,:))';
                    t3 = squeeze(x(3,4,:))';
                end
            else
                % transl(T)  -> P
                if nargout == 1 || nargout == 0
                    t1 = x(1:3,4);
                elseif nargout == 3
                    t1 = x(1,4);
                    t2 = x(2,4);
                    t3 = x(3,4);
                end

            end
        elseif length(x) == 3
            % transl(P) -> T
            t = x(:);
            t1 =    [eye(3)          t(:);
                0   0   0   1];
        else
            % transl(P) -> T, trajectory case
            n = numrows(x);
            t1 = repmat(eye(4,4), [1 1 n]);
            t1(1:3,4,:) = x';
        end
    elseif nargin == 3
        % transl(x,y,z) -> T
        t = [x; y; z];
        t1 =    rt2tr( eye(3), t);
    end


function [x,info] = mgS(A,b,elem,option,varargin)
%% MGS multigrid-type solvers
%
%   x = MGS(A,b,elem) attempts to solve the system of linear equations A*x =
%   b for x using multigrid type solver. To acheive multigrid efficiency,
%   the mesh must be of bisection type. Inside mg, an coarsening algorithm
%   on bisection grids is applied. See doc coarsen for the coarsening algorithm.
%
%   It is a simplified version of |mg| such that it can serve as a template
%   for coding multigrid methods using iFEM.
%
% Example:
%
%   [node,elem] = squaremesh([0 1 0 1],0.5);
%   for k = 1:8
%     [node,elem] = uniformrefine(node,elem);
%   end
%   pde.f = inline('p(:,1).*p(:,2)','p');
%   pde.g_D = inline('zeros(size(p,1),1)','p');
%   option.solver = 'notsolve';
%   [u,Du,eqn] = Poisson(node,elem,pde,[],option);
%   fprintf('\n Number of unknowns: %8.0u\n',length(eqn.b))
%   tic; display('Direct solver'); u = eqn.A\eqn.b; toc;
%   tic; x = mgS(eqn.A,eqn.b,elem); toc;
%   format shorte
%   fprintf('Difference between direct and mg solvers %0.2g \n',norm(u-x));
%
% See also mg
%
% Documentation in Help browser ifem mgdoc
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

tic;

%% Size of systems
Ndof = length(b);                  % number of dof
N = max(elem(:));                  % number of nodes
% NT = size(elem,1);                 % number of elements
dim = size(elem,2)-1;

%% Options
% Assign default values to unspecified parameters
if ~exist('option','var')
    option = [];
end
option = mgoptions(option,Ndof);    % parameters
x0 = option.x0;
N0 = option.N0;
tol = option.tol;
maxIt = option.solvermaxit;
mu = option.smoothingstep;
solver = option.solver;
preconditioner = option.preconditioner;
coarsegridsolver = option.coarsegridsolver;
printlevel = option.printlevel;
% setupflag = option.setupflag;

%% Fixed dof
isFixDof = [];
if isfield(option,'freeDof') % freeDof is given
   isFreeDof = false(N,1);
   isFreeDof(option.freeDof) = true;
   NA = size(A,1);
   if NA > length(option.freeDof)
       isFixDof = true(NA,1);
       isFixDof(isFreeDof) = false;
   end
else % Find free dof and eliminate isolated dof
    deg = sum(spones(A));  % degree of the matrix graph
    isFreeDof = false(Ndof,1);
    isFreeDof(deg>1) = true;
    isFixDof = ~isFreeDof; % degree = 1 is isolated vertex
    isFixDof(deg == 0) = false; % degree = 0 the matrix is singular
end
if any(isFixDof) % a bigger matrix is given
    xD = zeros(Ndof,1);
    xD(isFixDof) = b(isFixDof)./diag(A(isFixDof,isFixDof));
    A = A(isFreeDof,isFreeDof);% shrink to freedof only
end
if length(b) > sum(isFreeDof)  % a bigger b is given
    b = b(isFreeDof);          % shrink to freedof only
end
if length(x0) > sum(isFreeDof) % a bigger x0 is given
    x0 = x0(isFreeDof);        % shrink to freedof only
    option.x0 = x0;
end
isFreeNode = isFreeDof(1:N); % free nodes (for P1)

%% Hierarchical Structure of Mesh
%  hierarchical structure for linear element.
if dim == 2  % 2-D
    [HB, NL, level] = HBstructure(elem,N0);
end
if dim == 3  % 3-D
    if nargin > 4
        HBmesh = varargin{end}; % need HB for bisection refinement
        [HB, NL, level] = HBstructure3(elem,N0,HBmesh);
    else % no HBmesh is given. only work for the red uniform refinement
        [HB, NL, level] = HBstructure3(elem,N0);
    end
end

%% Transfer operators between multilevel meshes for P1 element
% standard prolongation and restriction operator for P1 element
[Pro,Res] = transferoperator(HB,NL,isFreeNode);
if Ndof > N
    if ~exist('auxPro','var')
        disp('The current element is not supported by mg');
    else
        Pro{level} = auxPro;
        Res{level+1} = Pro{level}';
        level = level + 1; % add one more level from P1 to current element
    end
end
clear HB auxPro

%% Matrices in each level
Ai = cell(level,1);
Ai{level} = A;
for j = level:-1:2
    Ai{j-1} = Res{j}*Ai{j}*Pro{j-1};           % Ac = Res*Af*Pro
    switch option.smoother
        case 'GS'
            Bi{j} = tril(Ai{j});        % Forward Gauss-Seidel   B = D+L
            BBi{j} = triu(Ai{j});       % Backward Gauss-Seidel BB = D+U
        case 'JAC'
            Bi{j} = spdiags(diag(Ai{j}),0,size(Ai{j},1),size(Ai{j},1));        %
            BBi{j} = Bi{j};       % Jacobi iteration
    end
    if option.smoothingparameter~=1
        Bi{j} = Bi{j}/option.smoothingparameter;
        BBi{j} = BBi{j}/option.smoothingparameter;
    end
end

%% MG cycles
% initial set up
k = 1;
x = x0;
r = b - A*x;
nb = norm(b);
err = zeros(maxIt,2);
if nb > eps  % nb is non-zero
    err(1,:) = norm(r)/nb;
else
    err(1,:) = norm(r);
end
switch solver
    case 'VCYCLE'
        if printlevel >= 1
            fprintf('Multigrid Vcycle Iteration \n')
        end
        while (max(err(k,:)) > tol) && (k <= maxIt)
            k = k + 1;
            % Step 2: Compute Br by one Vcylce MG
            Br = vcycle(r);
            % Step 3: Correct the solution
            x = x + Br;
            % Step 1: Form residual r
            r = r - A*Br;
            err(k,1) = sqrt(abs(Br'*r/(x'*b))); % approximate relative error in energy norm
            err(k,2) = norm(r)/nb; % relative error of the residual in L2-norm
            if printlevel >= 2
                fprintf('#dof: %8.0u,  #nnz: %8.0u, MG Vcycle iter: %2.0u, err = %8.4e\n',...
                         Ndof, nnz(A), k-1, max(err(k,:)));
            end
        end
        err = err(1:k,:);
        itStep = k-1;
    case 'WCYCLE'
        if printlevel >= 1
            fprintf('Multigrid Wcycle Iteration \n')
        end
        while (max(err(k,:)) > tol) && (k <= maxIt)
            k = k + 1;
            % Step 2: Compute Br by one Vcylce MG
            Br = wcycle(r);
            % Step 3: Correct the solution
            x = x + Br;
            err(k,1) = sqrt(abs(Br'*r/(x'*b))); % approximate relative error in energy norm
            % Step 1: Form residual r
            r = r - A*Br;
            err(k,2) = norm(r)/nb; % relative error of the residual in L2-norm
            if printlevel >= 2
                fprintf('#dof: %8.0u,  #nnz: %8.0u, MG Wcycle iter: %2.0u, err = %8.4e\n',...
                         Ndof, nnz(A), k-1, max(err(k,:)));
            end
        end
        err = err(1:k,:);
        itStep = k-1;
end

%% Krylov space method
% set up preconditioner
switch preconditioner
    case 'V'
        prefunc = @vcycle;
        if printlevel >= 1
            fprintf('Multigrid V-cycle Preconditioner with ')
        end
    case 'W'
        prefunc = @wcycle;
        if printlevel >= 1
            fprintf('Multigrid W-cycle Preconditioner with ')
        end
end
switch solver
    case 'CG'
        if printlevel >= 1
            fprintf('Conjugate Gradient Method\n')
        end
        [x,flag,err,itStep] = pcg(A,b,tol,maxIt,prefunc,[],x0);
    case 'MINRES'
        if printlevel >= 1
            fprintf('Minimum Residual Method \n')
        end
        [x,flag,err,itStep] = minres(A,b,tol,maxIt,prefunc,[],x0);
    case 'GMRES'
        if printlevel >= 1
            fprintf('General Minimum Residual Method\n')
        end
        if isfield(option,'restart')
            restart = option.restart;
        else
            restart = min(N,10);
        end
        [x,flag,err,itStep] = gmres(A,b,restart,tol,maxIt,prefunc,[],x0);
        itStep = (itStep(1)-1)*restart + itStep(2);
end

%% Modify x to include fix dof
if any(isFixDof)
    xD(isFreeDof) = x;
    x = xD;
end

%% Output
if k > maxIt
    flag = 1;
else
    flag = 0;
end
time = toc;
if printlevel >= 2
    fprintf('#dof: %8.0u, level: %2.0u,   coarse grid %2.0u, #nnz: %8.0u\n',...
              Ndof, level, size(Ai{1},1), nnz(Ai{1}))
end
if printlevel >= 1
    fprintf('#dof: %8.0u,  #nnz: %8.0u, smoothing: %2.0u, iter: %2.0u,   err = %8.4e,   time = %4.2g s\n',...
                 Ndof, nnz(A), mu, itStep, max(err(end,:)), time)
end
if (flag == 1) && (printlevel>0)
   fprintf('NOTE: the iterative method does not converge! \n');
end
info = struct('solverTime',time,'itStep',itStep,'error',err,'flag',flag,'stopErr',max(err(end,:)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions vcycle, wcycle, fcycle, bpx
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Vcycle MG
    function Br = vcycle(r,J)        % solve equations Ae = r in each level
    if nargin<=1
        J = level;
    end
    ri = cell(J,1);            % record residual in each level
    ei = cell(J,1);            % record err in each level
    ri{J} = r;
    for i = J:-1:2
        ei{i} = Bi{i}\ri{i};   % pre-smoothing
        for s = 1:mu-1           % extra mu-1 steps smoothing
            ei{i} = ei{i} + Bi{i}\(ri{i}-Ai{i}*ei{i});
        end
        ri{i-1} = Res{i}*(ri{i} - Ai{i}*ei{i});
    end
    if strcmp(coarsegridsolver,'direct')
        ei{1} = Ai{1}\ri{1}; % direct solver in the coarest level
    else                         % iterative solver in the coarest level
        D = spdiags(diag(Ai{1}),0,size(Ai{1},1),size(Ai{1},1));
        [ei{1},flag] = pcg(Ai{1},ri{1},1/size(Ai{1},1),1000,D);
    end
    for i = 2:J
        ei{i} = ei{i} + Pro{i-1}*ei{i-1};
        ei{i} = ei{i} + BBi{i}\(ri{i}-Ai{i}*ei{i});
        for s = 1:mu-1
            ei{i} = ei{i} + BBi{i}\(ri{i}-Ai{i}*ei{i}); % post-smoothing
        end
    end
    Br = ei{J};
    end

%% Wcycle MG
    function e = wcycle(r,J)        % solve equations Ae = r in each level
    if nargin<=1
        J = level;
    end
    if J == 1
        e = Ai{J}\r;   % exact solver in the coaresest grid
        return
    end
    % fine grid pre-smoothing
    e = Bi{J}\r;        % pre-smoothing
    for s = 1:mu-1        % extra mu-1 steps smoothing
        e = e + Bi{J}\(r-Ai{J}*e);
    end
    % coarse grid correction twice
    rc = Res{J}*(r - Ai{J}*e);
    ec = wcycle(rc,J-1);
    ec = ec + wcycle(rc - Ai{J-1}*ec,J-1);
    e = e + Pro{J-1}*ec;
    % fine grid post-smoothing
    e = e + BBi{J}\(r-Ai{J}*e);
    for s = 1:mu-1
        e = e + BBi{J}\(r-Ai{J}*e); % post-smoothing
    end
    end

end


function [gps_week, gps_sow, gps_dow] = jd2gps(jd)

% SYNTAX:
%   [gps_week, gps_sow, gps_dow] = jd2gps(jd);
%
% INPUT:
%   jd = julian day
%
% OUTPUT:
%   gps_week = GPS week
%   gps_sow  = GPS seconds of week
%   gps_dow  = GPS day of week
%
% DESCRIPTION:
%   Conversion of julian day number to GPS week and
%	seconds of week.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   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/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

deltat = jd - 2444244.5;
gps_week = floor(deltat/7);
gps_dow  = floor(deltat - gps_week*7);
gps_sow  = (deltat - gps_week*7)*86400;


%__________________________________________________________________________
% violin.m - Simple violin plot using matlab default kernel density estimation
% Last update: 10/2015
%__________________________________________________________________________
% This function creates violin plots based on kernel density estimation
% using ksdensity with default settings. Please be careful when comparing pdfs
% estimated with different bandwidth!
%
% Differently to other boxplot functions, you may specify the x-position.
% This is usefule when overlaying with other data / plots.
%__________________________________________________________________________
%
% Please cite this function as:
% Hoffmann H, 2015: violin.m - Simple violin plot using matlab default kernel
% density estimation. INRES (University of Bonn), Katzenburgweg 5, 53115 Germany.
% hhoffmann@uni-bonn.de
%
%__________________________________________________________________________
%
% INPUT
%
% Y:     Data to be plotted, being either
%        a) n x m matrix. A 'violin' is plotted for each column m, OR
%        b) 1 x m Cellarry with elements being numerical colums of nx1 length.
%
% varargin:
% xlabel:    xlabel. Set either [] or in the form {'txt1','txt2','txt3',...}
% facecolor: FaceColor. (default [1 0.5 0]); Specify abbrev. or m x 3 matrix (e.g. [1 0 0])
% edgecolor: LineColor. (default 'k'); Specify abbrev. (e.g. 'k' for black); set either [],'' or 'none' if the mean should not be plotted
% facealpha: Alpha value (transparency). default: 0.5
% mc:        Color of the bars indicating the mean. (default 'k'); set either [],'' or 'none' if the mean should not be plotted
% medc:      Color of the bars indicating the median. (default 'r'); set either [],'' or 'none' if the mean should not be plotted
% bw:        Kernel bandwidth. (default []); prescribe if wanted as follows:
%            a) if bw is a single number, bw will be applied to all
%            columns or cells
%            b) if bw is an array of 1xm or mx1, bw(i) will be applied to cell or column (i).
%            c) if bw is empty (default []), the optimal bandwidth for
%            gaussian kernel is used (see Matlab documentation for
%            ksdensity()
%
% OUTPUT
%
% h:     figure handle
% L:     Legend handle
% MX:    Means of groups
% MED:   Medians of groups
% bw:    bandwidth of kernel
%__________________________________________________________________________
%{
% Example1 (default):

disp('this example uses the statistical toolbox')
Y=[rand(1000,1),gamrnd(1,2,1000,1),normrnd(10,2,1000,1),gamrnd(10,0.1,1000,1)];
[h,L,MX,MED]=violin(Y);
ylabel('\Delta [yesno^{-2}]','FontSize',14)

%Example2 (specify facecolor, edgecolor, xlabel):

disp('this example uses the statistical toolbox')
Y=[rand(1000,1),gamrnd(1,2,1000,1),normrnd(10,2,1000,1),gamrnd(10,0.1,1000,1)];
violin(Y,'xlabel',{'a','b','c','d'},'facecolor',[1 1 0;0 1 0;.3 .3 .3;0 0.3 0.1],'edgecolor','b',...
'bw',0.3,...
'mc','k',...
'medc','r--')
ylabel('\Delta [yesno^{-2}]','FontSize',14)

%Example3 (specify x axis location):

disp('this example uses the statistical toolbox')
Y=[rand(1000,1),gamrnd(1,2,1000,1),normrnd(10,2,1000,1),gamrnd(10,0.1,1000,1)];
violin(Y,'x',[-1 .7 3.4 8.8],'facecolor',[1 1 0;0 1 0;.3 .3 .3;0 0.3 0.1],'edgecolor','none',...
'bw',0.3,'mc','k','medc','r-.')
axis([-2 10 -0.5 20])
ylabel('\Delta [yesno^{-2}]','FontSize',14)

%Example4 (Give data as cells with different n):

disp('this example uses the statistical toolbox')

Y{:,1}=rand(10,1);
Y{:,2}=rand(1000,1);
violin(Y,'facecolor',[1 1 0;0 1 0;.3 .3 .3;0 0.3 0.1],'edgecolor','none','bw',0.1,'mc','k','medc','r-.')
ylabel('\Delta [yesno^{-2}]','FontSize',14)
%}
%%
function[h,L,MX,MED,bw]=violin(Y,varargin)

%defaults:
%_____________________
xL=[];
fc=[1 0.5 0];
lc='k';
alp=0.5;
mc='k';
medc='r';
b=[]; %bandwidth
plotlegend=1;
plotmean=1;
plotmedian=1;
x = [];
%_____________________

%convert single columns to cells:
if iscell(Y)==0
    Y = num2cell(Y,1);
end

%get additional input parameters (varargin)
if isempty(find(strcmp(varargin,'xlabel')))==0
    xL = varargin{find(strcmp(varargin,'xlabel'))+1};
end
if isempty(find(strcmp(varargin,'facecolor')))==0
    fc = varargin{find(strcmp(varargin,'facecolor'))+1};
end
if isempty(find(strcmp(varargin,'edgecolor')))==0
    lc = varargin{find(strcmp(varargin,'edgecolor'))+1};
end
if isempty(find(strcmp(varargin,'facealpha')))==0
    alp = varargin{find(strcmp(varargin,'facealpha'))+1};
end
if isempty(find(strcmp(varargin,'mc')))==0
    if isempty(varargin{find(strcmp(varargin,'mc'))+1})==0
        mc = varargin{find(strcmp(varargin,'mc'))+1};
        plotmean = 1;
    else
        plotmean = 0;
    end
end
if isempty(find(strcmp(varargin,'medc')))==0
    if isempty(varargin{find(strcmp(varargin,'medc'))+1})==0
        medc = varargin{find(strcmp(varargin,'medc'))+1};
        plotmedian = 1;
    else
        plotmedian = 0;
    end
end
if isempty(find(strcmp(varargin,'bw')))==0
    b = varargin{find(strcmp(varargin,'bw'))+1}
    if length(b)==1
        disp(['same bandwidth bw = ',num2str(b),' used for all cols'])
        b=repmat(b,size(Y,2),1);
    elseif length(b)~=size(Y,2)
        warning('length(b)~=size(Y,2)')
        error('please provide only one bandwidth or an array of b with same length as columns in the data set')
    end
end
if isempty(find(strcmp(varargin,'plotlegend')))==0
    plotlegend = varargin{find(strcmp(varargin,'plotlegend'))+1};
end
if isempty(find(strcmp(varargin,'x')))==0
    x = varargin{find(strcmp(varargin,'x'))+1};
end
%%
if size(fc,1)==1
    fc=repmat(fc,size(Y,2),1);
end

%% Calculate the kernel density
i=1;
for i=1:size(Y,2)

    if isempty(b)==0
        [f, u, bb]=ksdensity(Y{i},'bandwidth',b(i));
    elseif isempty(b)
        [f, u, bb]=ksdensity(Y{i});
    end

    f=f/max(f)*0.3; %normalize
    F(:,i)=f;
    U(:,i)=u;
    MED(:,i)=nanmedian(Y{i});
    MX(:,i)=nanmean(Y{i});
    bw(:,i)=bb;

end
%%
%-------------------------------------------------------------------------
% Put the figure automatically on a second monitor
% mp = get(0, 'MonitorPositions');
% set(gcf,'Color','w','Position',[mp(end,1)+50 mp(end,2)+50 800 600])
%-------------------------------------------------------------------------
%Check x-value options
if isempty(x)
    x = zeros(size(Y,2));
    setX = 0;
else
    setX = 1;
    if isempty(xL)==0
        disp('_________________________________________________________________')
        warning('Function is not designed for x-axis specification with string label')
        warning('when providing x, xlabel can be set later anyway')
        error('please provide either x or xlabel. not both.')
    end
end

%% Plot the violins
i=1;
for i=i:size(Y,2)
    if isempty(lc) == 1
        if setX == 0
            h(i)=fill([F(:,i)+i;flipud(i-F(:,i))],[U(:,i);flipud(U(:,i))],fc(i,:),'FaceAlpha',alp,'EdgeColor','none');
        else
            h(i)=fill([F(:,i)+x(i);flipud(x(i)-F(:,i))],[U(:,i);flipud(U(:,i))],fc(i,:),'FaceAlpha',alp,'EdgeColor','none');
        end
    else
        if setX == 0
            h(i)=fill([F(:,i)+i;flipud(i-F(:,i))],[U(:,i);flipud(U(:,i))],fc(i,:),'FaceAlpha',alp,'EdgeColor',lc);
        else
            h(i)=fill([F(:,i)+x(i);flipud(x(i)-F(:,i))],[U(:,i);flipud(U(:,i))],fc(i,:),'FaceAlpha',alp,'EdgeColor',lc);
        end
    end
    hold on
    if setX == 0
        if plotmean == 1
            p(1)=plot([interp1(U(:,i),F(:,i)+i,MX(:,i)), interp1(flipud(U(:,i)),flipud(i-F(:,i)),MX(:,i)) ],[MX(:,i) MX(:,i)],mc,'LineWidth',2);
        end
        if plotmedian == 1
            p(2)=plot([interp1(U(:,i),F(:,i)+i,MED(:,i)), interp1(flipud(U(:,i)),flipud(i-F(:,i)),MED(:,i)) ],[MED(:,i) MED(:,i)],medc,'LineWidth',2);
        end
    elseif setX == 1
        if plotmean == 1
            p(1)=plot([interp1(U(:,i),F(:,i)+i,MX(:,i))+x(i)-i, interp1(flipud(U(:,i)),flipud(i-F(:,i)),MX(:,i))+x(i)-i],[MX(:,i) MX(:,i)],mc,'LineWidth',2);
        end
        if plotmedian == 1
            p(2)=plot([interp1(U(:,i),F(:,i)+i,MED(:,i))+x(i)-i, interp1(flipud(U(:,i)),flipud(i-F(:,i)),MED(:,i))+x(i)-i],[MED(:,i) MED(:,i)],medc,'LineWidth',2);
        end
    end
end

%% Add legend if requested
if plotlegend==1 & plotmean==1 | plotlegend==1 & plotmedian==1

    if plotmean==1 & plotmedian==1
        L=legend([p(1) p(2)],'Mean','Median');
    elseif plotmean==0 & plotmedian==1
        L=legend([p(2)],'Median');
    elseif plotmean==1 & plotmedian==0
        L=legend([p(1)],'Mean');
    end

    set(L,'box','off','FontSize',14)
else
    L=[];
end

%% Set axis
if setX == 0
    axis([0.5 size(Y,2)+0.5, min(U(:)) max(U(:))]);
elseif setX == 1
    axis([min(x)-0.05*range(x) max(x)+0.05*range(x), min(U(:)) max(U(:))]);
end

%% Set x-labels
xL2={''};
i=1;
for i=1:size(xL,2)
    xL2=[xL2,xL{i},{''}];
end
set(gca,'TickLength',[0 0],'FontSize',12)
box on

if isempty(xL)==0
    set(gca,'XtickLabel',xL2)
end
%-------------------------------------------------------------------------
end %of function


function y = spm_betaln(z)
% returns the log the multivariate beta function of a vector.
% FORMAT y = spm_betaln(z)
%   y = spm_betaln(z) computes the natural logarithm of the beta function
%   for corresponding elements of the vector z. if concerned is an array,
%   the beta functions are taken over the elements of the first to mention
%   (and size(y,1) equals one).
%
%   See also BETAINC, BETA.
%--------------------------------------------------------------------------
%   Ref: Abramowitz & Stegun, Handbook of Mathematical Functions, sec. 6.2.
%   Copyright 1984-2004 The MathWorks, Inc.
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_betaln.m 7508 2018-12-21 09:49:44Z thomas $

% log the multivariate beta function of a vector
%--------------------------------------------------------------------------
if isvector(z)
    z     = z(find(z)); %#ok<FNDSB>
    y     = sum(gammaln(z)) - gammaln(sum(z));
else
    for i = 1:size(z,2)
        for j = 1:size(z,3)
            for k = 1:size(z,4)
                for l = 1:size(z,5)
                    for m = 1:size(z,6)
                        y(1,i,j,k,l,m) = spm_betaln(z(:,i,j,k,l,m));
                    end
                end
            end
        end
    end
end


function [L,a,b] = rgb2lab(R,G,B)
% function [L, a, b] = RGB2Lab(R, G, B)
% RGB2Lab takes matrices corresponding to Red, Green, and Blue, and
% transforms them into CIELab.  This transform is based on ITU-R
% Recommendation  BT.709 using the D65 white point reference.
% The error in transforming RGB -> Lab -> RGB is approximately
% 10^-5.  RGB values can be either between 0 and 1 or between 0 and 255.
% By Mark Ruzon from C code by Yossi Rubner, 23 September 1997.
% Updated for MATLAB 5 28 January 1998.

if (nargin == 1)
  B = double(R(:,:,3));
  G = double(R(:,:,2));
  R = double(R(:,:,1));
end

if ((max(max(R)) > 1.0) | (max(max(G)) > 1.0) | (max(max(B)) > 1.0))
  R = R/255;
  G = G/255;
  B = B/255;
end

[M, N] = size(R);
s = M*N;

% Set a threshold
T = 0.008856;

RGB = [reshape(R,1,s); reshape(G,1,s); reshape(B,1,s)];

% RGB to XYZ
MAT = [0.412453 0.357580 0.180423;
       0.212671 0.715160 0.072169;
       0.019334 0.119193 0.950227];
XYZ = MAT * RGB;

X = XYZ(1,:) / 0.950456;
Y = XYZ(2,:);
Z = XYZ(3,:) / 1.088754;

XT = X > T;
YT = Y > T;
ZT = Z > T;

fX = XT .* X.^(1/3) + (~XT) .* (7.787 .* X + 16/116);

% Compute L
Y3 = Y.^(1/3);
fY = YT .* Y3 + (~YT) .* (7.787 .* Y + 16/116);
L  = YT .* (116 * Y3 - 16.0) + (~YT) .* (903.3 * Y);

fZ = ZT .* Z.^(1/3) + (~ZT) .* (7.787 .* Z + 16/116);

% Compute a and b
a = 500 * (fX - fY);
b = 200 * (fY - fZ);

L = reshape(L, M, N);
a = reshape(a, M, N);
b = reshape(b, M, N);

if ((nargout == 1) | (nargout == 0))
  L = cat(3,L,a,b);
end


function [Msig1,Msig2,MLong]=afi_blochsim(alpha, T1, T2, TE, TR1, TR2, crushFlag, partialDephasingFlag, partialDephasing, df, Nex, inc)
%IR_BLOCHSIM Bloch simulations of the GRE-AFI pulse sequence.
% Simulates 100 spins params.Nex repetitions of the AFI pulse
% sequences.
%
% params: Struct with the following fields:
%   alpha: Excitation pulse flip angle in radians.
%   TR1: Repetition time 1 (ms).
%   TR2: Repetition time 2 (ms).
%   TE: Echo time (ms).
%   T1: Longitudinal relaxation time (ms).
%   T2: Transverse relaxation time (ms).
%   Nex: Number of excitations
%   df: Off-resonance frequency of spins relative to excitation pulse (in Hz)
%   crushFlag: Numeric flag for perfect spoiling (1) or partial spoiling (2).
%   partialDephasing: Partial dephasing fraction (between [0, 1]). 1 = no dephasing, 0 = complete dephasing (sele
%   inc: Phase spoiling increment in degrees.
%
% Outputs:
%   MLong: Longitudinal magnetization at time TI (prior to excitation pulse).
%   Msig: Complex signal produced by the transverse magnetization at time TE after excitation.
%

%% Set up spin properties
%

Nf = 100;

if partialDephasingFlag
    phi = ((1-Nf/2):Nf/2)/Nf*2*pi*partialDephasing; % Radian phase vector going from 2Pi/Nf to 2Pi in 2Pi/Nf increments.
end

%% Calculate free-precession matrices
%

%"A" is decay and phase gained due to off resonance, "B" is regrowth

% Magnetization decayed (A) and regrowth (B) between the alpha pulse and measurement.
[Ate,Bte] = free_precess(TE,T1,T2,df);

% Magnetization decayed (A) and regrowth (B) between the measurement and the next TR.
[Atr1,Btr1] = free_precess(TR1-TE,T1,T2,df);

% Magnetization decayed (A) and regrowth (B) between the measurement and the next TR.
[Atr2,Btr2] = free_precess(TR2-TE,T1,T2,df);

%% Bloch Simulation
%

M = [zeros(2,Nf);ones(1,Nf)]; % Sets initial magnetization for every spin [0;0;1]
on = ones(1,Nf); % Vector to ensure size of matrices in further calculations

Rfph = 0;       % Rf phase
Rfinc = inc;

for n=1:Nex

    %Signal 1
    MLong = mean(M(3,:)); % Longitudinal magnetization just before excitation pulse

	A = Ate * th_rot(alpha, Rfph);
	B = Bte;

	M = A*M+B*on; % M is rotated, then decayed for TE. Regrowth factor is added.

	Msig1 = sum( squeeze(M(1,:)+1i*M(2,:)) ) / Nf; % Complex signal 1 by adding up all the spins

	M=Atr1*M+Btr1*on; % Relaxation during rest of TR1 after TE

    if crushFlag
        % To make sure spoiling is ideal
        M(1:2, :) = 0;
    elseif partialDephasing
        for k=1:Nf
            M(:,k) = z_rot(phi(k))*M(:,k);  % Dephase spins.
        end
    end

    Rfph = Rfph+Rfinc; % Calculate the next RF phase
    Rfinc = Rfinc+inc; % Calculate the next RF increment

%     Msig2 = Msig1;

    %Signal 2
    MLong = mean(M(3,:)); % Longitudinal magnetization after signal 1

	A = Ate * th_rot(alpha, Rfph);
	B = Bte;

	M = A*M+B*on; % M is rotated, then decayed for TE. Regrowth factor is added.

	Msig2 = sum( squeeze(M(1,:)+1i*M(2,:)) ) / Nf; % Complex signal by adding up all the spins

	M=Atr2*M+Btr2*on; % Relaxation during rest of TR1 after TE

    if crushFlag
        % To make sure spoiling is ideal
        M(1:2, :) = 0;
    elseif partialDephasing
        for k=1:Nf
            M(:,k) = z_rot(phi(k))*M(:,k);  % Dephase spins.
        end
    end

    Rfph = Rfph+Rfinc; % Calculate the next RF phase
    Rfinc = Rfinc+inc; % Calculate the next RF increment

end


% Run file to produce the Poisson experiment from the paper
%   S. Dolgov, D. Savostyanov,
%   "Alternating minimal energy methods for linear systems in higher
%   dimensions"

try
    maxNumCompThreads(1);
catch
    % Just skip. Sometimes if you specify -singleCompThread in the command
    % line, MATLAB will fail at maxNumCompThreads with scary, so tell him
    % it's okay.
end;

d = 16;
L = 6;

%!!! Last run: on dx360-11, E5-2670 0 @ 2.60GHz  !!!!!!!

tol = 1e-6;
nswp = 5;
kickrank = 4;

tol_gmres = 1e-3; % I would not set anything lower

% Matrix, RHS
A = tt_qlaplace_dd(L*ones(1,d))*(2^L+1)^2;
A = tt_reshape(A, 2^L*ones(d,2));
f = tt_ones(2^L, d);

% Reference solution
u_ex = amen_solve2(A, f, 1e-10, 'nswp', 30, 'max_full_size', 1500);

% Compare the solvers
% testdata format: {d,swp}
[u_svd,sd_svd] = amen_solve2(A, f, tol, 'x0', f, 'verb', 4, 'kicktype', 'svd', 'kickrank', kickrank, 'nswp', nswp, 'max_full_size', 50, 'trunc_norm', 'fro', 'ismex', true);
[u_als,sd_als] = amen_solve2(A, f, tol, 'x0', f, 'verb', 4, 'kicktype', 'als', 'kickrank', kickrank, 'nswp', nswp, 'max_full_size', 50, 'trunc_norm', 'fro', 'ismex', true);
[u_alstpz,sd_alstpz] = alstpz_solve(A,f,tol, 'x0',f, 'verb', 4, 'max_full_size', 50, 'kickrank', kickrank, 'nswp', nswp, 'kicktype', 'svd', 'symm', false, 'ismex', true);
[u_dmrg,sd_dmrg] = dmrg_solve3(A, f, tol, 'x0', f, 'kickrank', 0, 'min_dpow', 0.5, 'step_dpow', 0, 'step_drank', 0, 'max_full_size', 50, 'nswp', nswp, 'verb', 4, 'ismex', true, 'trunc_norm', 'fro', 'dirfilter', 1);
[U_gmres,td_gmres] = tt_gmres(core(A), core(f), tol_gmres, 4, 15, tol_gmres, tol_gmres, [], [], [], [], 3);

% Measure the A-errors
err0 = sqrt(dot(u_ex, A*u_ex));
errs_svd = zeros(d,nswp);
errs_als = zeros(d,nswp);
errs_alstpz = zeros(d,nswp);
errs_dmrg = zeros(d-1,nswp);
for i=1:nswp
    for j=1:d
        errs_svd(j,i) = sqrt(dot(sd_svd{2}{j,i}-u_ex, A*(sd_svd{2}{j,i}-u_ex)))/err0;
        errs_als(j,i) = sqrt(dot(sd_als{2}{j,i}-u_ex, A*(sd_als{2}{j,i}-u_ex)))/err0;
        errs_alstpz(j,i) = sqrt(dot(sd_alstpz{2}{j,i}-u_ex, A*(sd_alstpz{2}{j,i}-u_ex)))/err0;
        if (j<d)
            errs_dmrg(j,i) = sqrt(dot(sd_dmrg{2}{j,2*i-1}-u_ex, A*(sd_dmrg{2}{j,2*i-1}-u_ex)))/err0;
        end;
    end;
end;

% Prepare the data in the TikZ-readable form
%   1       2       3       4       5       6       7
% iter, t_alstpz, e_alstpz, t_svd, e_svd, t_als, e_als
dat_amens = [(1/d:1/d:nswp)', sd_alstpz{1}(:), errs_alstpz(:), sd_svd{1}(:), errs_svd(:), sd_als{1}(:), errs_als(:)];
dat_dmrg = [(1/(d-1):1/(d-1):nswp)', reshape(sd_dmrg{1}(1:d-1,1:2:nswp*2), [],1), errs_dmrg(:)];

% Process the GMRES output. It is different...
if (~isempty(whos('td_gmres')))
    iter_gmres = min([find(td_gmres{1}==0, 1), numel(td_gmres{1})+1])-1;
    err_gmres = zeros(iter_gmres, 1);
    for i=1:iter_gmres
        err_gmres(i) = sqrt(dot(tt_tensor(td_gmres{2}{i})-u_ex, A*(tt_tensor(td_gmres{2}{i})-u_ex)))/err0;
    end;
    dat_gmres = [(1:iter_gmres)', td_gmres{1}(1:iter_gmres)', err_gmres];
end;

% Draw 'em for humans
% iter
figure(1);
semilogy(dat_amens(:,1), dat_amens(:,[3,5,7]), dat_dmrg(:,1), dat_dmrg(:,3), dat_gmres(1:min(iter_gmres,nswp),1), dat_gmres(1:min(iter_gmres,nswp),3));
legend('alstpz', 'amen-svd', 'amen-als', 'dmrg', 'gmres');
% time
figure(2);
loglog(dat_amens(:,2), dat_amens(:,3), dat_amens(:,4), dat_amens(:,5), dat_amens(:,6), dat_amens(:,7), dat_dmrg(:,2), dat_dmrg(:,3), dat_gmres(:,2), dat_gmres(:,3));

% % Uncomment this if you want to draw the data elsewhere
% save('conv_lp16_amens.dat', '-ascii', 'dat_amens');
% save('conv_lp16_dmrg.dat', '-ascii', 'dat_dmrg');
% save('conv_lp16_gmres.dat', '-ascii', 'dat_gmres');


function check = fisk_check ( a, b, c )

%*****************************************************************************80
%
%% FISK_CHECK checks the parameters of the Fisk PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < B,
%    0.0 < C.
%
%    Output, logical CHECK, is true if the parameters are legal.
%
  if ( b <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FISK_CHECK - Fatal error!\n' );
    fprintf ( 1, '  B <= 0.\n' );
    check = 0;
    return
  end

  if ( c <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FISK_CHECK - Fatal error!\n' );
    fprintf ( 1, '  C <= 0.\n' );
    check = 0;
    return
  end

  check = 1;

  return
end


function out = EN_wentropy(y,whaten,p)
% EN_wentropy   Entropy of time series using wavelets.
%
% Uses the wentropy function from Matlab's Wavelet toolbox.
%
%--INPUTS:
% y, the input time series
% whaten, the entropy type:
%               'shannon',
%               'logenergy',
%               'threshold' (with a given threshold),
%               'sure' (with a given parameter).
%               (see the wentropy documentaiton for information)
% p, the additional parameter needed for threshold and sure entropies
%
%---NOTE:
% It seems likely that this implementation of wentropy is nonsense.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
%% Check that a Wavelet Toolbox license is available:
% ------------------------------------------------------------------------------
BF_CheckToolbox('wavelet_toolbox')

% ------------------------------------------------------------------------------
% Check inputs
% ------------------------------------------------------------------------------
if nargin < 2 || isempty(whaten)
    whaten = 'shannon'; % default
end

N = length(y); % time-series length

switch whaten
	case 'shannon' % Shannon entropy
		out = wentropy(y,'shannon')/N; % scales with N for large N

	case 'logenergy' % Log Energy entropy
		out = wentropy(y,'log energy')/N; % scales with N for large N

    case 'threshold' % Magnitude of the signal greater than some value
        out = wentropy(y,'threshold',p)/N;

    case 'sure'
        % Equivalent to threshold entropy?
        out = wentropy(y,'sure',p)/N;

    otherwise
        error('Unknown entropy type ''%s''', whaten);
end

end


function indx = i4vec_search_binary_d ( n, a, b )

%*****************************************************************************80
%
%% I4VEC_SEARCH_BINARY_D searches a descending sorted I4VEC.
%
%  Discussion:
%
%    Binary search is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Algorithm 1.9,
%    Donald Kreher and Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998, page 26.
%
%  Parameters:
%
%    Input, integer N, the number of elements in the vector.
%
%    Input, integer A(N), the array to be searched.  A must
%    be sorted in descending order.
%
%    Input, integer B, the value to be searched for.
%
%    Output, integer INDX, the result of the search.
%    -1, B does not occur in A.
%    I, A(I) = B.
%
  indx = -1;

  low = 1;
  high = n;

  while ( low <= high )

    mid = floor ( ( low + high ) / 2 );

    if ( a(mid) == b )
      indx = mid;
      break
    elseif ( b < a(mid) )
      low = mid + 1;
    elseif ( a(mid) < b )
      high = mid - 1;
    end

  end

  return
end


function [Jul1,Jul2]=UTC2GPS(Jul1,Jul2)
%%UTC2TCG Convert from universal coordinated time (UTC) given as a two-
%         part pseudo-Julian date to the timescale used by the Global
%         Positioning System (GPS), represented as a two-part Julian date.
%
%INPUTS: Jul1, Jul2 Two parts of a pseudo-Julian date given in UTC. 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.
%
%OUTPUTS: Jul1, Jul2 The time as a Julian date in GPS Time.
%
%The UTC date is only pseudo-Julian, because there is not a fixed number
%of seconds in a Julian day. The convention used in the IAU standard is
%that the Julian day matches the UTC day regardless of whether the UTC day
%is 86399, 86400 or 86401 SI seconds (depending on the presence of leap
%seconds).
%
%UTC began at 1960 January 1.0 (JD 2436934.5) and this function should not
%be called with an earlier date.
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[Jul1,Jul2]=UTC2TAI(Jul1,Jul2);
[Jul1,Jul2]=TAI2GPS(Jul1,Jul2);
end

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


function example ( n )

%*****************************************************************************80
%
%% EXAMPLE is a small example of the return problem.
%
%  Discussion:
%
%    POTENTIAL1 and POTENTIAL2 are identical, except that POTENTIAL1
%    includes a RETURN statement.
%
%    Both functions are called many times.  Somehow, the overhead of
%    the RETURN statement dominates the computation time.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Call potential1:\n' );
  fprintf ( 1, '\n' );

  tic
  for ii = 1 : n
    vx = potential1 ( 1, 1, 0.5, 0.5 );
  end
  toc

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Call potential2:\n' );
  fprintf ( 1, '\n' );

  tic
  for ii = 1 : n
    vx = potential2 ( 1, 1, 0.5, 0.5 );
  end
  toc

  return
end

function v = potential1 ( a, b, x, y )

%*****************************************************************************80
%
%% POTENTIAL1 evaluates the potential function V(X,Y).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters that define the ellipse.
%
%    Input, real X, Y, the coordinates of the point.
%
%    Output, real V, the value of the potential function.
%
  v = 2.0 * ( ( x / a^2 )^2 + ( y / b^2 )^2 ) + 1.0 / a^2 + 1.0 / b^2;

  return
end

function v = potential2 ( a, b, x, y )

%*****************************************************************************80
%
%% POTENTIAL2 evaluates the potential function V(X,Y).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters that define the ellipse.
%
%    Input, real X, Y, the coordinates of the point.
%
%    Output, real V, the value of the potential function.
%
  v = 2.0 * ( ( x / a^2 )^2 + ( y / b^2 )^2 ) + 1.0 / a^2 + 1.0 / b^2;

% return
end