function value = r8_gamma ( x )

%*****************************************************************************80
%
%% R8_GAMMA evaluates Gamma(X) for a real argument.
%
%  Discussion:
%
%    This routine calculates the gamma function for a real argument X.
%
%    Computation is based on an algorithm outlined in reference 1.
%    The program uses rational functions that approximate the gamma
%    function to at least 20 significant decimal digits.  Coefficients
%    for the approximation over the interval (1,2) are unpublished.
%    Those for the approximation for 12 <= X are from reference 2.
%
%    MATLAB provides a GAMMA function, which is likely to be faster, more
%    accurate, and which vectorizes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by William Cody, Laura Stoltz.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    William Cody,
%    An Overview of Software Development for Special Functions,
%    in Numerical Analysis Dundee, 1975,
%    edited by GA Watson,
%    Lecture Notes in Mathematics 506,
%    Springer, 1976.
%
%    John Hart, Ward Cheney, Charles Lawson, Hans Maehly,
%    Charles Mesztenyi, John Rice, Henry Thatcher,
%    Christoph Witzgall,
%    Computer Approximations,
%    Wiley, 1968,
%    LC: QA297.C64.
%
%  Parameters:
%
%    Input, real X, the argument of the function.
%
%    Output, real VALUE, the value of the function.
%

%
%  Coefficients for minimax approximation over (12, INF).
%
  c = [ ...
   -1.910444077728E-03, ...
    8.4171387781295E-04, ...
   -5.952379913043012E-04, ...
    7.93650793500350248E-04, ...
   -2.777777777777681622553E-03, ...
    8.333333333333333331554247E-02, ...
    5.7083835261E-03 ];
%
%  Mathematical constants
%
  sqrtpi = 0.9189385332046727417803297;
%
%  Machine dependent parameters
%
  xbig = 171.624E+00;
  xminin = 2.23E-308;
  eps = 2.22E-16;
  xinf = 1.79E+308;
%
%  Numerator and denominator coefficients for rational minimax
%  approximation over (1,2).
%
  p = [ ...
   -1.71618513886549492533811E+00, ...
    2.47656508055759199108314E+01, ...
   -3.79804256470945635097577E+02, ...
    6.29331155312818442661052E+02, ...
    8.66966202790413211295064E+02, ...
   -3.14512729688483675254357E+04, ...
   -3.61444134186911729807069E+04, ...
    6.64561438202405440627855E+04 ];

  q = [ ...
   -3.08402300119738975254353E+01, ...
    3.15350626979604161529144E+02, ...
   -1.01515636749021914166146E+03, ...
   -3.10777167157231109440444E+03, ...
    2.25381184209801510330112E+04, ...
    4.75584627752788110767815E+03, ...
   -1.34659959864969306392456E+05, ...
   -1.15132259675553483497211E+05 ];

  parity = 0;
  fact = 1.0;
  n = 0;
  y = x;
%
%  Argument is negative.
%
  if ( y <= 0.0 )

    y = - x;
    y1 = floor ( y );
    res = y - y1;

    if ( res ~= 0.0 )

      if ( y1 ~= floor ( y1 * 0.5 ) * 2.0 )
        parity = 1;
      end

      fact = - pi / sin ( pi * res );
      y = y + 1.0;

    else

      res = xinf;
      value = res;
      return

    end

  end
%
%  Argument is positive.
%
  if ( y < eps )
%
%  Argument < EPS.
%
    if ( xminin <= y )
      res = 1.0 / y;
    else
      res = xinf;
      value = res;
      return
    end

  elseif ( y < 12.0 )

    y1 = y;
%
%  0.0 < argument < 1.0.
%
    if ( y < 1.0 )

      z = y;
      y = y + 1.0;
%
%  1.0 < argument < 12.0.
%  Reduce argument if necessary.
%
    else

      n = floor ( y ) - 1;
      y = y - n;
      z = y - 1.0;

    end
%
%  Evaluate approximation for 1.0 < argument < 2.0.
%
    xnum = 0.0;
    xden = 1.0;
    for i = 1 : 8
      xnum = ( xnum + p(i) ) * z;
      xden = xden * z + q(i);
    end

    res = xnum / xden + 1.0;
%
%  Adjust result for case  0.0 < argument < 1.0.
%
    if ( y1 < y )

      res = res / y1;
%
%  Adjust result for case 2.0 < argument < 12.0.
%
    elseif ( y < y1 )

      for i = 1 : n
        res = res * y;
        y = y + 1.0;
      end

    end

  else
%
%  Evaluate for 12.0 <= argument.
%
    if ( y <= xbig )

      ysq = y * y;
      sum = c(7);
      for i = 1 : 6
        sum = sum / ysq + c(i);
      end
      sum = sum / y - y + sqrtpi;
      sum = sum + ( y - 0.5 ) * log ( y );
      res = exp ( sum );

    else

      res = xinf;
      value = res;
      return

    end

  end
%
%  Final adjustments and return.
%
  if ( parity )
    res = - res;
  end

  if ( fact ~= 1.0 )
    res = fact / res;
  end

  value = res;

  return
end


% OP_SU_EV_TP: assemble the matrix A = [a(i,j)], a(i,j) = 1/2 (sigma (u_j), epsilon (v_i)), exploiting the tensor product structure.
%
%   mat = op_su_ev_tp (spu, spv, msh, lambda, mu);
%   [rows, cols, values] = op_su_ev_tp (spu, spv, msh, lambda, mu);
%
% INPUT:
%
%   spu:     object representing the space of trial functions (see sp_vector)
%   spv:     object representing the space of test functions (see sp_vector)
%   msh:     object that defines the domain partition and the quadrature rule (see msh_cartesian)
%   lambda, mu: function handles to compute the Lame' coefficients
%
% OUTPUT:
%
%   mat:    assembled matrix
%   rows:   row indices of the nonzero entries
%   cols:   column indices of the nonzero entries
%   values: values of the nonzero entries
%
% Copyright (C) 2011, 2017 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function varargout = op_su_ev_tp (space1, space2, msh, lambda, mu)

  for icomp = 1:space1.ncomp_param
    for idim = 1:msh.ndim
      size1 = size (space1.scalar_spaces{icomp}.sp_univ(idim).connectivity);
      size2 = size (space2.scalar_spaces{icomp}.sp_univ(idim).connectivity);
      if (size1(2) ~= size2(2) || size1(2) ~= msh.nel_dir(idim))
        error ('One of the discrete spaces is not associated to the mesh')
      end
    end
  end

  A = spalloc (space2.ndof, space1.ndof, 5*space1.ndof);

  for iel = 1:msh.nel_dir(1)
    msh_col = msh_evaluate_col (msh, iel);
    sp1_col = sp_evaluate_col (space1, msh_col, 'value', false, ...
                               'gradient', true, 'divergence', true);
    sp2_col = sp_evaluate_col (space2, msh_col, 'value', false, ...
                               'gradient', true, 'divergence', true);

    for idim = 1:msh.rdim
      x{idim} = reshape (msh_col.geo_map(idim,:,:), msh_col.nqn, msh_col.nel);
    end

    A = A + op_su_ev (sp1_col, sp2_col, msh_col, lambda (x{:}), mu (x{:}));
  end

  if (nargout == 1)
    varargout{1} = A;
  elseif (nargout == 3)
    [rows, cols, vals] = find (A);
    varargout{1} = rows;
    varargout{2} = cols;
    varargout{3} = vals;
  end

end


%--- help for arima/simulate ---
%
% SIMULATE Simulate ARIMA model responses and conditional variances
%
%  Syntax:
%
%    [Y,E,V] = simulate(Mdl,numObs)
%    [Y,E,V] = simulate(Mdl,numObs,param1,val1,param2,val2,...)
%
%  Description:
%
%    Simulate sample paths of responses, innovations, and conditional
%    variances of a univariate ARIMA process.
%
%  Input Arguments:
%
%    Mdl - ARIMA model specification object, as produced by the ARIMA
%      constructor or ARIMA/ESTIMATE method.
%
%    numObs - Positive integer indicating the number of observations (rows)
%      generated for each path of the outputs Y, E, and V.
%
%  Optional Input Parameter Name/Value Pairs:
%
%    'NumPaths'   Positive integer indicating the number of sample paths
%                 (columns) generated for all simulated outputs. The default
%                 is 1.
%
%    'Y0'         Presample response data, providing initial values for the
%                 model. Y0 is a column vector or a matrix. If Y0 is a
%                 column vector, then it is applied to each simulated path.
%                 If Y0 is a matrix, then it must have at least NumPaths
%                 columns. Y0 may have any number of rows, provided at least
%                 Mdl.P observations exist to initialize the model. If the
%                 number of rows exceeds Mdl.P, then only the most recent
%                 Mdl.P observations are used. If the number of columns
%                 exceeds NumPaths, then only the first NumPaths columns are
%                 used. If Y0 is unspecified, any necessary presample
%                 observations are set to the unconditional mean for stationary
%                 AR processes, and to zero if the process is non-stationary
%                 or contains a regression component. The last row contains
%                 the most recent observation.
%
%    'E0'         Mean-zero presample innovations, providing initial values
%                 for the model. E0 is a column vector or a matrix. If E0 is
%                 a column vector, then it is applied to each simulated path.
%                 If E0 is a matrix, then it must have at least NumPaths
%                 columns. E0 may have any number of rows, provided sufficient
%                 observations exist to initialize the ARIMA model as well
%                 as any conditional variance model (the number of observations
%                 required is at least Mdl.Q, but may be more if a conditional
%                 variance model is included). If the number of rows
%                 exceeds the number necessary, then only the most recent
%                 observations are used. If the number of columns exceeds
%                 NumPaths, then only the first NumPaths columns are used. If
%                 no presample data is specified, any necessary observations
%                 are set to zero. The last row contains the most recent
%                 observation.
%
%    'V0'         Positive presample conditional variances, providing initial
%                 values for any conditional variance model; if the variance
%                 of the model is constant, then V0 is unnecessary. V0 is a
%                 column vector or a matrix. If V0 is a column vector, then
%                 it is applied to each simulated path. If V0 is a matrix,
%                 then it must have at least NumPaths columns. V0 may have
%                 any number of rows, provided sufficient observations exist
%                 to initialize any conditional variance model. If the number
%                 of rows exceeds the number necessary, then only the most
%                 recent observations are used. If the number of columns
%                 exceeds NumPaths, then only the first NumPaths columns are
%                 used. If no presample variance data is specified, any
%                 necessary observations are set to the unconditional variance
%                 of the conditional variance process. The last row contains
%                 the most recent observation.
%
%    'X'          Matrix of predictor data used to include a regression
%                 component in the conditional mean. Each column of X is a
%                 separate time series, and the last row of each contains
%                 the most recent observation of each series. The number of
%                 observations in X must equal or exceed numObs. When the
%                 number of observations in X exceeds numObs, only the most
%                 recent observations are used. If missing, the conditional
%                 mean will have no regression component regardless of the
%                 presence of any regression coefficients found in the model.
%
%  Output Arguments:
%
%    Y - numObs-by-NumPaths matrix of simulated response data.
%
%    E - numObs-by-NumPaths matrix of simulated mean-zero innovations.
%
%    V - numObs-by-NumPaths matrix of conditional variances of the
%      innovations in E.
%
%  Notes:
%
%    o Missing data values, indicated by NaNs, are removed from X by listwise
%      deletion (i.e., any row in X with at least one NaN is removed), reducing
%      the effective sample size. The presample data Y0, E0, and V0 are merged
%      into a composite series, and any row of the combined series with at
%      least one NaN is also removed by listwise deletion. The presample data
%      is also synchronized such that the last (most recent) observation of
%      each series occurs at the same time.
%
%   o  Regression models included in the conditional mean are based on the
%      presence of the predictor matrix X. Although each column of the output
%      time series represents a different path of the corresponding univariate
%      stochastic process, the regression matrix X represents as a single
%      path of a (possibly) multivariate time series matrix in which each
%      column is a different time series. When the conditional mean has a
%      regression component, the entire predictor matrix X is applied to
%      every column of the output time series.
%
%  References:
%
%    [1] Box, G. E. P., G. M. Jenkins, and G. C. Reinsel. Time Series
%        Analysis: Forecasting and Control. 3rd edition. Upper Saddle River,
%        NJ: Prentice-Hall, 1994.
%
%    [2] Enders, W. Applied Econometric Time Series. Hoboken, NJ: John Wiley
%        & Sons, 1995.
%
%    [3] Hamilton, J. D. Time Series Analysis. Princeton, NJ: Princeton
%        University Press, 1994.
%
%  See also ARIMA, FORECAST, ESTIMATE, INFER, FILTER.
%
%    Reference page in Doc Center
%       doc arima/simulate
%
%    Other functions named simulate
%
%       conjugateblm/simulate    generic/simulate
%       customblm/simulate       gjr/simulate
%       diffuseblm/simulate      regARIMA/simulate
%       dsge/simulate            sde/simulate
%       dtmc/simulate            semiconjugateblm/simulate
%       egarch/simulate          ssm/simulate
%       empiricalblm/simulate    varm/simulate
%       garch/simulate           vecm/simulate
%


function p = nodes_brick27 ( )

%*****************************************************************************80
%
%% NODES_BRICK27 returns the natural coordinates of the BRICK27 element.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real P(3,27), the coordinates.
%
  p = [ ...
    -1.0, -1.0, -1.0; ...
    +1.0, -1.0, -1.0; ...
    +1.0, +1.0, -1.0; ...
    -1.0, +1.0, -1.0; ...
    -1.0, -1.0, +1.0; ...
    +1.0, -1.0, +1.0; ...
    +1.0, +1.0, +1.0; ...
    -1.0, +1.0, +1.0; ...
     0.0, -1.0, -1.0; ...
    +1.0,  0.0, -1.0; ...
     0.0, +1.0, -1.0; ...
    -1.0,  0.0, -1.0; ...
    -1.0, -1.0,  0.0; ...
    +1.0, -1.0,  0.0; ...
    +1.0, +1.0,  0.0; ...
    -1.0, +1.0,  0.0; ...
     0.0, -1.0, +1.0; ...
    +1.0,  0.0, +1.0; ...
     0.0, +1.0, +1.0; ...
    -1.0,  0.0, +1.0; ...
     0.0,  0.0, -1.0; ...
     0.0, -1.0,  0.0; ...
    +1.0,  0.0,  0.0; ...
     0.0, +1.0,  0.0; ...
    -1.0,  0.0,  0.0; ...
     0.0,  0.0, +1.0; ...
     0.0,  0.0,  0.0 ]';

  return
end


function [p1Clip, p2Clip]=clipLineSegment2Rect(rectMaxMin,p1,p2)
%%CLIPLINESEGMENT2RECT Given a 2D rectangle (axis-aligned) and given the
%           two endpoints of a line segment clip the line segment to the
%           rectangle. If the segment and the rectangle do not overlap,
%           then an empty matrix is returned.
%
%INPUTS: rectMaxMin A length-4 array specifying the maximum and minimum
%                   bounds in x and y. rectMaxMin=[xMin;xMax;yMin;yMax].
%                   The minimum values must be <=the maximum values.
%                p1 A length-2 array specifying the start of the line
%                   segment.
%                p2 A length-2 array specifying the end of the line
%                   segment.
%
%OUTPUTS: p1Clip, p2Clip These are the same p1 and p2 but have been clipped to the
%                start and end of the line segment. If nothing clips, the
%                empty matrices are returned.
%
%The algorithm of [1] is used, without speed improvements that are possible
%from direct substitution, which is mentioned in Section IX of [1]. The
%performance of the algorithm is discussed in [2].
%
%EXAMPLE:
% rect=[-1;1;-1;1];
% numLines=6;
% p1=zeros(2,numLines);
% %Lines entirely inside the box.
% p1(:,1)=[0;0]; p2(:,1)=[0.8;0.8];
% p1(:,2)=[0.6;0]; p2(:,2)=[-0.8;0.8];
% %Lines leaving via each edge of the box.
% p1(:,3)=[0.6;-0.1]; p2(:,3)=[1.6;-0.2];
% p1(:,4)=[-0.6;0.1]; p2(:,4)=[-1.6;0.2];
% p1(:,5)=[-0.1;0.5]; p2(:,5)=[-0.2;2.8];
% p1(:,6)=[0.1;-0.5]; p2(:,6)=[0.2;-1.8];
%
% figure(1)
% clf
% hold on
% %Draw the bounding box lines, extending them.
% extDist=3;
% plot([-extDist,extDist],[-1,-1],'-k','linewidth',2)
% plot([-extDist,extDist],[1,1],'-k','linewidth',2)
% plot([-1,-1],[-extDist,extDist],'-k','linewidth',2)
% plot([1,1],[-extDist,extDist],'-k','linewidth',2)
% for k=1:numLines
%     %Plot the line segments
%     plot([p1(1,k),p2(1,k)],[p1(2,k),p2(2,k)],'-b','linewidth',4)
%     %Clip the segments
%     [p1Clip,p2Clip]=clipLineSegment2Rect(rect,p1(:,k),p2(:,k));
%     %Plot the clipped segments.
%     if(~isempty(p1Clip))
%         plot([p1Clip(1),p2Clip(1)],[p1Clip(2),p2Clip(2)],'-r','linewidth',2)
%     end
% end
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.
%[2] F. Dévai, "Analysis of the Nichioll-Lee-Nicholl Algorithm," in
%    Proceedings of the International Conference on Computational Science
%    and its Applications, Signapore, pp. 726-736, 9-12 May, 2005.
%
%August 2020 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xMin=rectMaxMin(1);
xMax=rectMaxMin(2);
yMin=rectMaxMin(3);
yMax=rectMaxMin(4);

x1=p1(1);
y1=p1(2);
x2=p2(1);
y2=p2(2);

if(x1<xMin)
    [x1,y1,x2,y2,isVisible]=leftColumn(xMin,yMax,xMax,yMin,x1,y1,x2,y2);
elseif(x1>xMax)
    [x1,y1]=rotate180c(x1,y1);
    [x2,y2]=rotate180c(x2,y2);

    [x1,y1,x2,y2,isVisible]=leftColumn(-xMax,-yMin,-xMin,-yMax,x1,y1,x2,y2);

    [x1,y1]=rotate180c(x1,y1);
    [x2,y2]=rotate180c(x2,y2);
else
    [x1,y1,x2,y2,isVisible]=centerColumn(xMin,yMax,xMax,yMin,x1,y1,x2,y2);
end

if(isVisible==false)
    %If the line is not in the box at all.
    p1Clip=[];
    p2Clip=[];
else
    p1Clip=[x1;y1];
    p2Clip=[x2;y2];
end
end

function [x1,y1,x2,y2,isVisible]=leftColumn(xL,yT,xR,yB,x1,y1,x2,y2)
%%LEFTCOLUMN The case where (x1,y1) is to the left of the vertical line
%            x=xL.

if(x2<xL)
    isVisible=false;
elseif(y1>yT)
    [x1,y1,x2,y2,isVisible]=topLeftCorner(xL,yT,xR,yB,x1,y1,x2,y2);
elseif(y1<yB)
    y1=reflectxAxis(y1);
    y2=reflectxAxis(y2);
    [x1,y1,x2,y2,isVisible]=topLeftCorner(xL,-yB,xR,-yT,x1,y1,x2,y2);

    y1=reflectxAxis(y1);
    y2=reflectxAxis(y2);
else
    [x1,y1,x2,y2,isVisible]=leftEdge(xL,yT,xR,yB,x1,y1,x2,y2);
end
end

function [x1,y1,x2,y2,isVisible]=topLeftCorner(xL,yT,xR,yB,x1,y1,x2,y2)
%TOPLEFTCORNER The case where (x1,y1) is in the top-left corner.

    if(y2>yT)
        isVisible=false;
    else
        relx2=x2-x1;
        rely2=y2-y1;

        topProd=(yT-y1)*relx2;
        leftProd=(xL-x1)*rely2;
        %If (x2,y2) is below the line connecting (x1,y1) and (xL,yT).
        if(topProd>leftProd)
            [x1,y1,x2,y2,isVisible]=leftBottomRegion(xL,xR,yB,x1,y1,x2,y2,relx2,rely2,leftProd);
        else
            [x1,y1]=reflectxMinusy(x1,y1);
            [x2,y2]=reflectxMinusy(x2,y2);

            [x1,y1,x2,y2,isVisible]=leftBottomRegion(-yT,-yB,-xR,x1,y1,x2,y2,-rely2,-relx2,topProd);

            [x1,y1]=reflectxMinusy(x1,y1);
            [x2,y2]=reflectxMinusy(x2,y2);
        end
    end
end

function [x1,y1,x2,y2,isVisible]=leftBottomRegion(xL,xR,yB,x1,y1,x2,y2,relx2,rely2,leftProd)
%%LEFTBOTTOMREGION The case where (x1,y1) is in the left bottom corner.

if(y2>=yB)
    if(x2>xR)
        %The right-edge intersection.
        y2=y1+(xR-x1)*rely2/relx2;
        x2=xR;
    end

    %The left-edge intersection.
    y1=y1+leftProd/relx2;
    x1=xL;
    isVisible=true;
else
    bottomProd=(yB-y1)*relx2;

    %If (x2,y2) is below the (x1,y1) to (xR,yB) line.
    if(bottomProd>leftProd)
        isVisible=false;
    else
        if(x2>xR)
            rightProd=(xR-x1)*rely2;

            %If (x2,y2) is below the (x1,y1) to (xR,yB) line .
            if(bottomProd>rightProd)
                %The bottom-edge intersection.
                x2=x1+bottomProd/rely2;
                y2=yB;
            else
                %The right-edge intersection.
                y2=y1+rightProd/relx2;
                x2=xR;
            end
        else
            %The bottom-edge intersection.
            x2=x1+bottomProd/rely2;
            y2=yB;
        end

        %The left-edge intersection.
        y1=y1+leftProd/relx2;
        x1=xL;
        isVisible=true;
    end
end
end

function [x1,y1,x2,y2,isVisible]=leftEdge(xL,yT,xR,yB,x1,y1,x2,y2)
%%LEFTEDGE (x1,y1) is in the left edge.

    if(x2<xL)
        isVisible=false;
    elseif(y2<yB)
        [x1,y1,x2,y2,isVisible]=p2Bottom(xL,xR,yB,x1,y1,x2,y2);
    elseif(y2>yT)
        y1=reflectxAxis(y1);
        y2=reflectxAxis(y2);

        [x1,y1,x2,y2,isVisible]=p2Bottom(xL,xR,-yT,x1,y1,x2,y2);

        y1=reflectxAxis(y1);
        y2=reflectxAxis(y2);
    else
        relx2=x2-x1;
        rely2=y2-y1;

        if(x2>xR)%The right-edge intersection.
            y2=y1+(xR-x1)*rely2/relx2;
            x2=xR;
        end

        %The left-edge intersection.
        y1=y1+(xL-x1)*rely2/relx2;
        x1=xL;
        isVisible=true;
    end
end

function [x1,y1,x2,y2,isVisible]=centerColumn(xL,yT,xR,yB,x1,y1,x2,y2)
%CENTERCOLUMN The case where (x1,y1) is between the left and right
%             boundaries.

    if(y1>yT)
        [x1,y1]=rotate270c(x1,y1);
        [x2,y2]=rotate270c(x2,y2);
        [x1,y1,x2,y2,isVisible]=leftEdge(-yT,xR,-yB,xL,x1,y1,x2,y2);
        [x1,y1]=rotate90c(x1,y1);
        [x2,y2]=rotate90c(x2,y2);
    elseif(y1<yB)
        [x1,y1]=rotate90c(x1,y1);
        [x2,y2]=rotate90c(x2,y2);
        [x1,y1,x2,y2,isVisible]=leftEdge(yB,-xL,yT,-xR,x1,y1,x2,y2);
        [x1,y1]=rotate270c(x1,y1);
        [x2,y2]=rotate270c(x2,y2);
    else
        [x1,y1,x2,y2,isVisible]=inside(xL,yT,xR,yB,x1,y1,x2,y2);
    end
end

function [x1,y1,x2,y2,isVisible]=inside(xL,yT,xR,yB,x1,y1,x2,y2)
%%INSIDE The case where (x1,y1) is inside the rectangle.

if(x2<xL)
    [x1,y1,x2,y2]=p2Left(xL,yT,yB,x1,y1,x2,y2);
elseif(x2>xR)
	[x1,y1]=rotate180c(x1,y1);
	[x2,y2]=rotate180c(x2,y2);
    [x1,y1,x2,y2]=p2Left(-xR,-yB,-yT,x1,y1,x2,y2);
    [x1,y1]=rotate180c(x1,y1);
	[x2,y2]=rotate180c(x2,y2);
elseif(y2>yT)
    x2=x1+(x2-x1)*(yT-y1)/(y2-y1);
    y2=yT;
elseif(y2<yB)
    x2=x1+(x2-x1)*(yB-y1)/(y2-y1);
    y2=yB;
end

isVisible=true;
end

function [x1,y1,x2,y2]=p2Left(xL,yT,yB,x1,y1,x2,y2)
%%P2LEFT

    if(y2>yT)
        [x1,y1,x2,y2]=p2LeftTop(xL,yT,x1,y1,x2,y2);
    elseif(y2<yB)
        [x1,y1]=rotate90c(x1,y1);
        [x2,y2]=rotate90c(x2,y2);
        [x1,y1,x2,y2]=p2LeftTop(yB,-xL,x1,y1,x2,y2);
        [x1,y1]=rotate270c(x1,y1);
        [x2,y2]=rotate270c(x2,y2);
    else
        y2=y1+(y2-y1)*(xL-x1)/(x2-x1);
        x2=xL;
    end
end

function [x1,y1,x2,y2]=p2LeftTop(xL,yT,x1,y1,x2,y2)
%%P2LEFTTOP

    relx2=x2-x1;
    rely2=y2-y1;
    leftProduct=rely2*(xL-x1);
    topProduct=relx2*(yT-y1);

    if(topProduct>leftProduct)
        x2=x1+topProduct/rely2;
        y2=yT;
    else
        y2=y1+leftProduct/relx2;
        x2=xL;
    end
end

function [x1,y1,x2,y2,isVisible]=p2Bottom(xL,xR,yB,x1,y1,x2,y2)
%%P2BOTTOM (x1,y1) is in the left edge; (x2,y2) is not beyond the left
%          edge,  and (x2,y2) is beyond the bottom edge.

    relx2=x2-x1;
    rely2=y2-y1;
    leftProd=(xL-x1)*rely2;
    bottomProd=(yB-y1)*relx2;
    if(bottomProd>leftProd)
        isVisible=false;
    else
       if(x2<=xR)
           x2=x1+bottomProd/rely2;
           y2=yB;
       else
           rightProd=(xR-x1)*rely2;
           if(bottomProd>rightProd)
               x2=x1+bottomProd/rely2;
               y2=yB;
           else
               y2=y1+rightProd/relx2;
               x2=xR;
           end
       end
       y1=y1+leftProd/relx2;
       x1=xL;
       isVisible=true;
    end
end

function [x,y]=reflectxMinusy(x,y)
%%REFLECTXMINUSY This function is from Section VI-2 of [1].
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.

t=x;
x=-y;
y=-t;

end

function y=reflectxAxis(y)
%%REFLECTAXIS This function is from Section VI-2 of [1].
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.

y=-y;

end

function [x,y]=rotate90c(x,y)
%%ROTATE90C This function is from Section VI-2 of [1].
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.

t=x;
x=y;
y=-t;

end

function [x,y]=rotate180c(x,y)
%%ROTATE180C This function is from Section VI-2 of [1].
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.

x=-x;
y=-y;
end

function [x,y]=rotate270c(x,y)
%%ROTATE270C This function is from Section VI-2 of [1].
%
%REFERENCES:
%[1] T. M. Nicholl, D. T. Lee, and R. A. Nicholl, "An Efficient  New
%    Algorithm for 2-D Line Clipping: its Development and Analysis",
%    ACM SIGGRAPH Computer Graphics, vol. 21, no. 4, pp. 253-262, Jul.
%    1987.

t=x;
x=-y;
y=t;

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 f=midi2frq(n,s)
%MIDI2FRQ	Convert musical note numbers to frequencies F=(N,S)
%		s is:	'e' equal tempered (default)
%			'p' pythagorean scale
%			'j' just intonation
%
% notes are numbered in semitones with middle C being 60
% On the equal tempered scale, note 69 (the A above middle C)
% has a frequency of 440 Hz.
%
% see FRQ2NOTE for the inverse transform

% Pythagorean
%     sharps 1 2187/2048 9/8 19683/16384 81/64 4/3 729/512  3/2 6561/4096 27/16 59049/32768 243/128 2
%     flats  1 256/243   9/8 32/27       81/64 4/3 1024/729 3/2 128/81    27/16 16/9        243/128 2
%
% Just Intonation
%     sharps 1 25/24 9/8 75/64 5/4 4/3 45/32  3/2 25/16 5/3 225/128 15/8 2
%     flats  1 16/15 9/8 6/5   5/4 4/3 108/75 3/2 8/5   5/3 18/10   15/8 2

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: midi2frq.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin > 1
  if s(1)=='p'
    r=[256/243 9/8 32/27 81/64 4/3 729/512 3/2 128/81 27/16 16/9 243/128];
  elseif s(1)=='j'
    r=[16/15 9/8 6/5 5/4 4/3 36/25 3/2 8/5 5/3 9/5 15/8];
  else
    r=0;
  end
  if r(1)
    c=[0 0 12*log(r)/log(2)-(1:11) 0];
    nm=mod(n,12);
    na=floor(nm);
    nb=nm-na;
    f=440*exp((n+c(na+2).*(1-nb)+c(na+3).*nb-69)*log(2)/12);
  else
    f=440*exp((n-69)*log(2)/12);
  end
else
  f=440*exp((n-69)*log(2)/12);
end


x=[1 2 3 4 5 6 7 8 9]
pause
find(x<6)
pause
y=[1 2 3 4; 5 6 7 8; 9 10 11 12; 13 14 15 16]
pause
find(y<6)'
pause
[l,c,v]=find(y<6);
pause
l'
pause
c'
pause
v'


%% Projection Based Shape Parameters
%
%%
% In this section we discuss shape parameters grains that depend on one
% dimensional projections, i.e.,
%
% || <grain2d.caliper.html |caliper|>  || caliper or Feret diameter in $\mu m$ || <grain2d.diameter.html |diameter|>  || diameter in $\mu m$ ||
%
% In order to demonstrate these parameters we first import a small sample
% data set.

% load sample EBSD data set
mtexdata forsterite silent

% reconstruct grains, discard boudnary grains and smooth them
[grains, ebsd.grainId] = calcGrains(ebsd('indexed'),'angle',5*degree);
ebsd(grains(grains.grainSize<5)) = [];
[grains, ebsd.grainId] = calcGrains(ebsd('indexed'),'angle',5*degree);
grains(grains.isBoundary) = [];

grains = smooth(grains('indexed'),10,'moveTriplePoints');

% plot all grains and highlight a specific one
plot(grains)

ind = 654;
hold on
plot(grains(ind).boundary,'lineWidth',5,'linecolor','blue')
hold off

%%
% The most well known projection based parameter is the
% <grain2d.diamter.html |diameter|> which refers to the longest distance
% between any two boundary points and is given in $\mu m$.

grains(ind).diameter

%%
% The diameter is a special case of the <grain2d.caliper.html |caliper|> or
% Feret diameter of a grain. By definition the caliper is the length of a
% grain when projected onto a line. We may trace the caliper with respect
% to projection direction

close all
omega = linspace(0,180);
plot(omega,grains(ind).caliper(omega*degree),'LineWidth',2)
ylabel('length in $\mu$m','Interpreter','latex')
xlabel('angle of the projection line in degree')
xlim([0,180])

%%
% We observe that that maximum caliper is about 7000 while the minimum
% caliper is about 2000. We may compute the exact direction and length of
% the maximum or minimum by passing the options |'longest'| or |'shortest'|
% to the function <grain2d.caliper.html |caliper|>. In this case the the
% output is of type @vector3d indicating the direction. The
% <vector3d.norm.html |norm|> of the vector coincides with the caliper for
% this projection direction. Hence, the |norm(grains.caliper('longest'))|
% coincides with the diameter.

plot(grains(ind),'micronbar','off')
legend('off')

norm(grains(ind).caliper('longest'))
norm(grains(ind).caliper('shortest'))

hold on
quiver(grains(ind),grains(ind).caliper('longest'),'noScaling')
quiver(grains(ind),grains(ind).caliper('shortest'),'noScaling')
hold off

%%
% The difference between the longest and the shortest caliper can be taken
% as a measure how round a grain is.

cMin = grains.caliper('shortest');
cMax = grains.caliper('longest');

plot(grains,(norm(cMax) - norm(cMin))./norm(cMax),'micronbar','off')
mtexColorbar('title','TODO')

%%
% This longest and shortest caliper are comparable to
% <grain2d.longAxis.html |grains.longAxis|> and <grain2d.shortAxis.html
% |grains.shortAxis|> computed from an ellipse fitted to the grain. In the
% case of rectangular particles, one might not primarily be interested in
% the longest caliper of a grain but rather in the direction normal to the
% shortest caliper. This is computed when specifying the option
% |'shortestPerp'|. If we imagine a very strong alignment of the long axes
% of orthorhombic particles, the maximum diameter may show a bimodal
% distribution (the two, roughly equally distributed diagonals of the
% particle).

% load some test grains
testgrains = mtexdata('testgrains');
testgrains = smooth(testgrains([6 8]),10);

% compute the longest caliper and the caliper perpendicular to the shortest
cMax = testgrains.caliper('longest');
cMinPerp = testgrains.caliper('shortestPerp');

% plot the grains and visualize the different long axes
plot(testgrains,'micronbar','off','lineWidth',2)
hold on
quiver(testgrains,cMax,'DisplayName','longest calliper','LineWidth',3)
quiver(testgrains,testgrains.longAxis,'DisplayName','long axis','LineWidth',3)
quiver(testgrains,cMinPerp,'DisplayName','perp to shortest','LineWidth',3)
hold off
legend('Location','east')

%% PAROR and SURFOR
%
% Another way of quantifying shape farbics is by making use of the
% cumulative projection function of the grains or the grain boundary
% segments. These methods are heavily inspired by
% <https://en.wikipedia.org/wiki/Flatland Edwin A. Abbotts 'Flatland - A
% romance of many dimensions' (1884)> and based on Panozzo, R., 1983,
% "Two-dimensional analysis of shape fabric using projections of digitized
% lines in a plane". Tectonophysics 95, 279-294. and Panozzo, R., 1984,
% "Two-dimensional strain from the orientation of lines in a plane." J.
% Struct. Geol. 6, 215-221. implemented in Mtex as <grain2d.paror.html
% |grains.paror|> and <grainBoundary.surfor.html |grainBoudnary.surfor|>
%
% As mentioned above the function <grain2d.caliper.html |caliper|> can be
% called with a list of angles and returns the projection length of all
% grains with respect to all angles.

% projection angle
omega = linspace(0,360*degree,361);
c = grains('Fo').caliper(omega);

subplot(1,2,1)
polarplot(omega,c,'LineWidth',2,'color',[0 0.25 0.5 0.25])
title('Forsterite')

% take the average
hold on
polarplot(omega,5*mean(c),'LineWidth',3,'color','k');
hold off

subplot(1,2,2)
c = grains('Enstatite').caliper(omega);

polarplot(omega,c,'LineWidth',2,'color',[0 0.25 0.5 0.25])
title('Enstatite')

% take the average
hold on
polarplot(omega,5*mean(c),'LineWidth',3,'color','k');
hold off

%%
% The above averaged caliper can be computed more directly by the function
% <grain2d.paror.html |grains.paror|> which returns the cumulative particle
% projection function normalized to 1. The projection angles can be
% regarded as the rotation angle of the particle (counterclockwise) while
% projecting from the y-axis onto the x-axis.

close all
cumplF = paror(grains('fo'),omega);
cumplE = paror(grains('en'),omega);

subplot(1,2,1)
polarplot(omega,cumplF,'LineWidth',3,'color','k')

subplot(1,2,2)
polarplot(omega,cumplE,'LineWidth',3,'color','k')

%%
% We can interpret the results in the following way. The minimum of the
% curve is a measure of the amplitude of the projection function and can be
% compared to an averaged axial ratio $b/a$ of the entire  fabric;
% isotropic fabrics would have a $b/a$ close to 1 while highly anisotropic
% fabrics can be identified by small $b/a$ values.

min(cumplF), min(cumplE)

%%
% The position of the maxima and minima of the projection function derived
% from <grain2d.paror.html |paror|> can be interpreted in the following
% way: the maximum position represents the preferred axis parallel to the
% longest projection and the normal the minimum position represents the
% preferred axis related to the normal to the shortest projection function.

% for the Forsterite
[~, id_max] = max(cumplF);
[~, id_min] = min(cumplF);

mod(omega(id_max)./degree,180)
mod(omega(id_min)./degree-90,180)

% for the Enstatite
[~, id_max] = max(cumplE);
[~, id_min] = min(cumplE);

mod(omega(id_max)./degree,180)
mod(omega(id_min)./degree-90,180)

%%
% The smaller the difference between these values, the closer the fabric is
% to an orthorhombic symmetry.
%
% Similarly to using the entire particle (the convex hull in case of the
% projection functions), we can use a distribution of lines
% <grainBoundary.surfor.html |grainBoudnary.surfor|>. This can be useful
% for the quantification of the grain boundary anisotropy or in general
% might be needed if we look at boundaries which do not form closed
% outlines, e.g. a list of subgrain or twin boundaries or the contact
% between certain phases.
%
% Let's compare the boundaries between the different unlike phases and
% between forsterite-forsterite in our sample:

close all
pairs = [1 1; nchoosek(1:3,2)];
phase = {'Fo' 'En' 'Di'};
for i=1:length(pairs)

  gB = grains.boundary(phase{pairs(i,:)});
  polarplot(omega, surfor(gB,omega), 'linewidth',2, ...
    'DisplayName',[phase{pairs(i,1)} '-' phase{pairs(i,2)}]);
  hold on

end
hold off
legend('Location','southoutside','Orientation','horizontal')

%%
% We can see that Forsterite-Forsterite boundaries form a fabric slightly
% more inclined with respect to the other phase boundariesand that the
% phase boundaries between the two pyroxenes (Enstatite and Diopside) show
% the lowest anisotropy.
%
%% Characteristic Shape
%
% The characteristic shape results from the cummulative sum of all grain
% boundary segements ordered by the angle of the segment direction. It can
% be regarded as to represent the average grain shape, however without the
% need to use closed areas such as it would be required when working with
% grains.
%
% Here we can compare the shape defined by Forterite-Forsterite,
% Enstatite-Enstatite and Forsterite-Enstatite boundaries

plotopts = {'normalize','linewidth',2, 'plain'};

shapeF = characteristicShape(grains.boundary('Fo','Fo'))
plot(shapeF,plotopts{:},'DisplayName','Fo-Fo')

hold on
shapeE = characteristicShape(grains.boundary('En','En'));
plot(shapeE,plotopts{:},'DisplayName','En-En')

hold on
shapeEF = characteristicShape(grains.boundary('En','Fo'));
plot(shapeEF,plotopts{:},'DisplayName','En-Fo')
hold off

legend('Location','southoutside','Orientation','horizontal')

%%
% The output of the command <grainBoundary.characteristicShape.html
% |characteristicShape|> is a <shape2d.shape2d.html |shape2d|> object which
% behaves very similar to a <grain2d.grain2d.html |grain2d|> object. Hence
% it is easy to derive things such as a long axis or e.g. the angle between
% the longest and the shortest caliper which can be regarded as a measure
% of asymmetry.

angle(shapeF.caliper('longest'),shapeF.caliper('shortest'))/degree
angle(shapeE.caliper('longest'),shapeE.caliper('shortest'))/degree
angle(shapeEF.caliper('longest'),shapeF.caliper('shortest'))/degree


function [alpha,xp] = StepSizeSW(f,x,d,alpha,params)
% Line search algorithm satisfying strong Wolfe conditions.
% Algorithms 3.5 on pages 60-61 in Nocedal and Wright.
% Requires x.p, x.f and x.g to be initialized.

alpha0 = params.stpmin;
c1 = params.ftol;
c2 = params.gtol;
% alpha is alpha_i
gxd = x.g'*d;
% alphap is alpha_{i-1}
alphap = alpha0;
fxp = x.f;
i=1;
while norm(alphap-alpha) > params.xtol
  xp.p = x.p + alpha*d;
  xp.f = feval(f,xp.p,1);
  if (xp.f > x.f + c1*alpha*gxd) | ((i > 1) & (xp.f >= fxp)),
    [alpha,xp] = zoom(f,x,d,alphap,alpha,fxp,c1,c2);
    return;
  end
  xp.g = feval(f,xp.p,2); gxpd = xp.g'*d;
  if abs(gxpd) <= -c2*gxd,
    return;
  end
  if gxpd >= 0,
    [alpha,xp] = zoom(f,x,d,alpha,alphap,xp.f,c1,c2);
    return;
  end
  alphap = alpha;
  fxp = xp.f;
  %  alpha = alpha + (params.stpmax-alpha)*rand(1);
  alpha = alpha + (params.stpmax-alpha)*0.5;
  i = i+1;
end
alpha,
error('No stepsize found');

function [alpha,xp] = zoom(f,x,d,alphal,alphah,fxl,c1,c2)
% function [alpha,xp] = zoom(f,x,d,alphal,alphah,fxl)
% Algorithm 3.6 on page 61 in Nocedal and Wright

gxd = x.g'*d;

while 1
   alpha = 1/2*(alphal+alphah);
   xp.p = x.p + alpha*d;
   xp.f = feval(f,xp.p,1);
   if ((xp.f > x.f + c1*alpha*gxd) | (xp.f >= fxl)),
      alphah = alpha;
   else
      xp.g = feval(f,xp.p,2); gxpd = xp.g'*d;
      if abs(gxpd) <= -c2*gxd,
        return;
      end
      if gxpd*(alphah-alphal) >= 0,
        alphah = alphal;
      end
      alphal = alpha;
      fxl = xp.f;
   end
end


function [x, infos] = admm_seq_conv_nmf(V, rank, t, in_options)
% ADMM based convolutive non-negative matrix factorization (ADMM-Conv-NMF).
%
% The problem of interest is defined as
%
%
% Given a non-negative matrix V, factorized non-negative matrices {W, H} are calculated.
%
%
% Inputs:
%       matrix      V
%       rank        rank
%
% Output:
%       w           solution of w
%       infos       information
%
% References:
%
%
% This file is part of NMFLibrary
%
% This file has been ported from
% convNMF_MM1.m and convNMF_MM2.m at https://github.com/lyn202206/ADMM-Convolutive-NMF
% by Yinan Li.
%
% Ported by H.Kasai on June 29, 2022
%
% Change log:
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.metric_type = 'beta-div';
    local_options.d_beta = 2;
    local_options.rho = 1;
    local_options.flag = 1;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    if ~isfield(options, 'x_init')
        W = zeros(m, rank, t);
        for i = 1 : t
            [init_factors, ~] = generate_init_factors(V, rank, init_options);
            W(:, :, i) = init_factors.W;
        end
        H = init_factors.H;
    else
        W = init_options.x_init.W;
        H = init_options.x_init.H;
    end

    % initialize
    epoch = 0;
    grad_calc_count = 0;

    options = check_divergence(options);
    sub_mode = sprintf('beta=%.1f', options.d_beta);
    if ~strcmp(options.metric_type, 'beta-div')
        sub_mode = options.metric_type;
    end
    method_name = sprintf('ADMM-Seq-Conv (%s)', sub_mode);

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % initialize for this algorithm
    X = zeros(m, n, t);
    for i=0:t-1
        tW = W(:, :, i+1);
        tH = shift_t(H, i);
        % initial X
        X(:, :, i+1) = tW * tH;
    end

    % some variable for test
    Xplus = X;
    U = zeros(m, n,t);

    alphaX = zeros(m, n,t);
    alphaH = zeros(rank,n);
    alphaW = zeros(m,rank,t);

    Wplus = W;
    Hplus = H;

    % store initial info
    clear infos;

    [Wcon, Hcon] = reconstruct_wh(W, H, t);
    [infos, f_val, optgap] = store_nmf_info(V, Wcon, Hcon, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('ADMM-Seq-Conv (%s): Epoch = 0000, cost = %.16e, optgap = %.4e\n', sub_mode, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % update H by accumulation
        P1 = zeros(rank,rank);
        P2 = zeros(rank,n);
        P3 = zeros(rank,n);
        for i = 0 : t-1
            tW = W(:, :, i+1);
            tX = shift_t(X(:, :, i+1), -i);
            talphaX = shift_t(alphaX(:, :, i+1), -i);
            P1 = P1 + tW'*tW;
            P2 = P2 + tW'*tX;
            P3 = P3 + tW'*talphaX;
        end
        H = (P1 + eye(rank)) \ (P2 + Hplus + 1/options.rho * (P3-alphaH));

        % update Hplus
        Hplus = max(H + 1/options.rho * alphaH, 0);

        % update alphaH
        alphaH = alphaH + options.rho * (H - Hplus);

        % update parameters sequentially
        for i = 0 : t-1
            % update Xplus in each time slice
            tW = Wplus(:, :, i+1);
            tH = shift_t(Hplus, i);
            % calculate Xplus
            Xplus(:, :, i+1) = tW * tH;

            % splite V in each time slice which result in U
            U(:, :, i+1) = (tW * tH + eps).*V./(sum(Xplus, 3) + t*eps);

            % update X in each time slice
            if options.d_beta ==2

                % update for Euclidean distance
                tW = W(:, :, i+1);
                tH = shift_t(H, i);
                tV = U(:, :, i+1);

                % update X
                X(:, :, i+1) = (options.rho*tW * tH + tV - alphaX(:, :, i+1)) / (1 + options.rho);

            elseif options.d_beta == 1

                % update for Kullback-Leibler divergence
                tW = W(:, :, i+1);
                tH = shift_t(H, i);
                tV = U(:, :, i+1);

                % update X
                b = options.rho * tW * tH - alphaX(:, :, i+1) - 1;
                X(:, :, i+1) = (b + sqrt(b.^2 + 4 * options.rho * tV)) / (2 * options.rho);

            elseif options.d_beta == 0

                % update for Itakura-Saito divergence
                tW = W(:, :, i+1);
                tH = shift_t(H, i);
                tV = U(:, :, i+1);

                % parameters for update
                A = alphaX(:, :, i+1) / options.rho - tW * tH;
                B = 1/(3 * options.rho) - A.^2/9;
                C = - A.^3/27 + A/(6 * options.rho) + tV / (2*options.rho);
                D = B.^3 + C.^2;

                % update X
                tX = X(:, :, i+1);
                tX(D>=0) = nthroot(C(D>=0)+sqrt(D(D>=0)),3) + ...
                nthroot(C(D>=0)-sqrt(D(D>=0)),3) - ...
                A(D>=0)/3;

                phi = acos(C(D<0) ./ ((-B(D<0)).^1.5));
                tX(D<0) = 2*sqrt(-B(D<0)).*cos(phi/3) - A(D<0)/3;
                X(:, :, i+1) = tX;
            else
                error('The options.d_beta you specified is not currently supported.')
            end

            % update alphaX
            alphaX(:, :, i+1) = alphaX(:, :, i+1) +  options.rho * (X(:, :, i+1) - tW * tH);

            % update W
            tH = shift_t(H, i);
            tX = X(:, :, i+1);
            P = tH * tH' + eye(rank);
            Q = tH * tX' + Wplus(:, :, i+1)' + 1/options.rho*(tH * alphaX(:, :, i+1)' - alphaW(:, :, i+1)');
            W(:, :, i+1) = (P \ Q)';

            % update Wplus
            Wplus(:, :, i+1) = max(W(:, :, i+1) + 1/options.rho * alphaW(:, :, i+1), 0);

            % udpate alphaW
            alphaW(:, :, i+1) = alphaW(:, :, i+1) + options.rho * (W(:, :, i+1) - Wplus(:, :, i+1));
        end

        %  if to update X before the update of U
        if options.flag == 1

            %  update Xplus in each time slice
            for i=0:t-1
                tW = Wplus(:, :, i+1);
                tH = shift_t(Hplus, i);
                % initial X
                Xplus(:, :, i+1) = tW * tH;
            end

            % splite V in each time slice which result in U
            for i=0:t-1
                tW = Wplus(:, :, i+1);
                tH = shift_t(Hplus, i);
                U(:, :, i+1) = (tW * tH + eps) .* V ./ (sum(Xplus, 3) + t*eps);
            end

            % update X in each time slice after the update of the whole W
            if options.d_beta == 2

                % update for Euclidean distance
                for i=0:t-1
                    tW = W(:, :, i+1);
                    tH = shift_t(H, i);
                    tV = U(:, :, i+1);
                    % update X
                    X(:, :, i+1) = (options.rho*tW * tH + tV-alphaX(:, :, i+1)) / (1 + options.rho);
                end

            elseif options.d_beta == 1

                % update for Kullback-Leibler divergence
                for i=0:t-1
                    tW = W(:, :, i+1);
                    tH = shift_t(H, i);
                    tV = U(:, :, i+1);
                    % update X
                    b = options.rho * tW * tH - alphaX(:, :, i+1) - 1;
                    X(:, :, i+1) = (b + sqrt(b.^2 + 4 * options.rho * tV)) / (2 * options.rho);
                end

            elseif options.d_beta == 0

                % update for Itakura-Saito divergence
                for i=0:t-1
                    tW = W(:, :, i+1);
                    tH = shift_t(H, i);
                    tV = U(:, :, i+1);

                    % parameters for update
                    A = alphaX(:, :, i+1) / options.rho - tW * tH;
                    B = 1 / (3 * options.rho) - A.^2/9;
                    C = - A.^3/27 + A / (6 * options.rho) + tV/(2 * options.rho);
                    D = B.^3 + C.^2;

                    % update X
                    tX = X(:, :, i+1);
                    tX(D>=0) = nthroot(C(D>=0)+sqrt(D(D>=0)),3) + ...
                    nthroot(C(D>=0)-sqrt(D(D>=0)),3) - ...
                    A(D>=0)/3;

                    phi = acos(C(D<0) ./ ((-B(D<0)).^1.5));
                    tX(D<0) = 2*sqrt(-B(D<0)).*cos(phi/3) - A(D<0)/3;
                    X(:, :, i+1) = tX;
                end
            else
                error('The options.d_beta you specified is not currently supported.')
            end

           % update for dual variables
            for i=0:t-1
                tW = W(:, :, i+1);
                tH = shift_t(H, i);
                % update alphaX
                alphaX(:, :, i+1) = alphaX(:, :, i+1) +  options.rho*(X(:, :, i+1)-tW * tH);
            end
        end

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        [Wcon, Hcon] = reconstruct_wh(W, H, t);
        infos = store_nmf_info(V, Wcon, Hcon, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    W = Wplus;
    H = Hplus;
    [W, H] = renormalize_convNMF(W, H);

    x.W = W;
    x.H = H;

end

function [W_concat, H_concat] = reconstruct_wh(W, H, t)

    W_concat = [];
    H_concat = [];
    for j = 1 : t
        W_concat = [W_concat W(:, :,j)];
        H_concat = [H_concat; shift_t(H, j-1)];
    end
end


function DSO3 = refine(DSO3)

% step 1: compute center of any two orientation connected by an edge

% all combinations of vertices of all tetrahegons
v1 = DSO3.tetra(:,[1 1 1 2 2 3]).';
v2 = DSO3.tetra(:,[2 3 4 3 4 4]).';

% unique edges
vv = sort([v1(:),v2(:)],2);
[v,~,iCenterEdges] = unique(vv,'rows');
iCenterEdges = length(DSO3) + reshape(iCenterEdges,6,[]).';

% step 2: set up corner tetrahegons
% bases
base = iCenterEdges(:,[1 2 3  1 4 5  2 4 6  3 5 6]);
base = reshape(base.',3,[]);
corner = reshape(DSO3.tetra.',1,[]);

% split center octaeder into four tetrahegons
octaeder = iCenterEdges(:,[1 3 4 5  3 4 5 6  1 2 3 4  2 3 4 6]);
octaeder = reshape(octaeder.',4,[]);

% set up tetraeder
newTetra = [[base;corner],octaeder].';

% new vertices as mean of all edges
centerEdges = mean2(DSO3.subSet(v(:,1)),DSO3.subSet(v(:,2)));

% set up refined grid
DSO3.a = [DSO3.a(:);centerEdges.a];
DSO3.b = [DSO3.b(:);centerEdges.b];
DSO3.c = [DSO3.c(:);centerEdges.c];
DSO3.d = [DSO3.d(:);centerEdges.d];
DSO3.i = zeros(size(DSO3.d));

% set up new tetrahegons
DSO3.tetra = sort(newTetra,2);

% compute neighbouring list
DSO3.tetraNeighbour = calcNeighbour(DSO3.tetra);

% the neighbours of the four corner tetrahegons
%1 2 3 4
%cornerNeighbour =
%DSO3.tetraNeighbour

% set up new lookup
%DSO3.lookup = DSO3.lookup*8;
%DSO3.lookup = calcLookUp(DSO3,5*degree);
DSO3.lookup = [];
res = 40*degree;
for i = 1:3
  res = res / 2;
  DSO3.lookup = calcLookUp(DSO3,res);
end

end

% -----------------------------------------------------------------
function v = mean2(v1,v2)
% compute the mean of two orientations

dv = inv(v1) .* v2;
dv.SS = specimenSymmetry;

[dv,omega] = dv.project2FundamentalRegion;

% half angle
dv.a = dv.a .* cos(omega./4) ./ cos(omega./2);
dv.b = dv.b .* sin(omega./4) ./ sin(omega./2);
dv.c = dv.c .* sin(omega./4) ./ sin(omega./2);
dv.d = dv.d .* sin(omega./4) ./ sin(omega./2);

% new vertices
v = v1 .* dv;

end


function b = dppsl ( ap, n, b )

%*****************************************************************************80
%
%% DPPSL solves a real symmetric positive definite system factored by DPPCO or DPPFA.
%
%  Discussion:
%
%    To compute inverse(A) * C where C is a matrix with P columns
%
%      call dppco ( ap, n, rcond, z, info )
%
%      if ( rcond is too small .or. info /= 0 ) then
%        exit
%      end if
%
%      do j = 1, p
%        call dppsl ( ap, n, c(1,j) )
%      end do
%
%    A division by zero will occur if the input factor contains
%    a zero on the diagonal.  Technically this indicates
%    singularity but it is usually caused by improper subroutine
%    arguments.  It will not occur if the subroutines are called
%    correctly and INFO == 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real AP(N*(N+1)/2), the output from DPPCO or DPPFA.
%
%    Input, integer N, the order of the matrix.
%
%    Input, real B(N), the right hand side.
%
%    Output, real B(N), the solution.
%
  kk = 0;

  for k = 1 : n
    t = ddot ( k-1, ap(kk+1:kk+k-1), 1, b(1:k-1), 1 );
    kk = kk + k;
    b(k) = ( b(k) - t ) / ap(kk);
  end

  for k = n : -1 : 1
    b(k) = b(k) / ap(kk);
    kk = kk - k;
    t = -b(k);
    b(1:k-1) = daxpy ( k-1, t, ap(kk+1:kk+k-1), 1, b(1:k-1), 1 );
  end

  return
end


function M = productmanifold(elements)
% Returns a structure describing a product manifold M = M1 x M2 x ... x Mn.
%
% function M = productmanifold(elements)
%
% Input: an elements structure such that each field contains a manifold
% structure.
%
% Output: a manifold structure M representing the manifold obtained by
% taking the Cartesian product of the manifolds described in the elements
% structure, with the metric obtainded by element-wise extension. Points
% and vectors are stored as structures with the same fieldnames as in
% elements.
%
% Example:
% M = productmanifold(struct('X', spherefactory(3), 'Y', spherefactory(4)))
% disp(M.name());
% x = M.rand()
%
% Points of M = S^2 x S^3 are represented as structures with two fields, X
% and Y. The values associated to X are points of S^2, and likewise points
% of S^3 for the field Y. Tangent vectors are also represented as
% structures with two corresponding fields X and Y.
%
% See also: powermanifold

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%
%   July  4, 2013 (NB):
%       Added support for vec, mat, tangent.
%       Added support for egrad2rgrad and ehess2rhess.
%       Modified hash function to make hash strings shorter.
%
%   Dec. 17, 2018 (NB):
%       Added check all_elements_provide() to many functions, so that if,
%       for example, one of the elements does not provide exp(), then the
%       product manifold also won't provide exp(). This makes it easier for
%       tools such as, for example, checkgradient, to determine whether exp
%       is available or not.
%
%   Feb. 10, 2020 (NB):
%       Added warnings about calling egrad2rgrad and ehess2rhess without
%       storedb and key, even if some base manifolds allow them.
%
%   Jan. 4, 2021 (NB):
%       Changes for compatibility with Octave 6.1.0: by introducing a
%       "helper" function, we separate out the pre-computations. This way,
%       all pre-computed quantities are passed as input to the helper
%       function. This makes them available to nested subfunctions.
%       The extra step is not necessary in Matlab.

    elems = fieldnames(elements);
    nelems = numel(elems);

    assert(nelems >= 1, ...
           'elements must be a structure with at least one field.');

    % Below are some precomputations for the mat/vec pair.
    %
    % Gather the length of the column vector representations of tangent
    % vectors for each of the manifolds. Raise a flag if any of the base
    % manifolds has no vec function available.
    vec_available = true;
    vec_lens = zeros(nelems, 1);
    for ii = 1 : nelems
        Mi = elements.(elems{ii});
        if isfield(Mi, 'vec')
            rand_x = Mi.rand();
            zero_u = Mi.zerovec(rand_x);
            vec_lens(ii) = length(Mi.vec(rand_x, zero_u));
        else
            vec_available = false;
            break;
        end
    end
    vec_pos = cumsum([1 ; vec_lens]);
    %
    vecmatareisometries = vec_available;
    for ii = 1 : nelems
        if ~isfield(elements.(elems{ii}), 'vecmatareisometries') || ...
           ~elements.(elems{ii}).vecmatareisometries()
            vecmatareisometries = false;
            break;
        end
    end
    %
    % Above are some precomputations for the mat/vec pair.

    % The helper function is the actual factory.
    M = productmanifoldhelper(elements, elems, nelems, vec_available, ...
                              vec_pos, vecmatareisometries);

end

function M = productmanifoldhelper(elements, elems, nelems, ...
                                   vec_available, vec_pos, ...
                                   vecmatareisometries)

    % Handy function to check if all elements provide the necessary methods
    function answer = all_elements_provide(method_name)
        answer = false;
        for i = 1 : nelems
            if ~isfield(elements.(elems{i}), method_name)
                return;
            end
        end
        answer = true;
    end

    M.name = @name;
    function str = name()
        str = 'Product manifold: ';
        str = [str sprintf('[%s: %s]', ...
                           elems{1}, elements.(elems{1}).name())];
        for i = 2 : nelems
            str = [str sprintf(' x [%s: %s]', ...
                   elems{i}, elements.(elems{i}).name())]; %#ok<AGROW>
        end
    end

    M.dim = @dim;
    function d = dim()
        d = 0;
        for i = 1 : nelems
            d = d + elements.(elems{i}).dim();
        end
    end

    M.inner = @inner;
    function val = inner(x, u, v)
        val = 0;
        for i = 1 : nelems
            val = val + elements.(elems{i}).inner(x.(elems{i}), ...
                                               u.(elems{i}), v.(elems{i}));
        end
    end

    M.norm = @(x, d) sqrt(M.inner(x, d, d));

    if all_elements_provide('dist')
        M.dist = @dist;
    end
    function d = dist(x, y)
        sqd = 0;
        for i = 1 : nelems
            sqd = sqd + elements.(elems{i}).dist(x.(elems{i}), ...
                                                 y.(elems{i}))^2;
        end
        d = sqrt(sqd);
    end

    if all_elements_provide('typicaldist')
        M.typicaldist = @typicaldist;
    end
    function d = typicaldist
        sqd = 0;
        for i = 1 : nelems
            sqd = sqd + elements.(elems{i}).typicaldist()^2;
        end
        d = sqrt(sqd);
    end

    M.proj = @proj;
    function v = proj(x, u)
        for i = 1 : nelems
            v.(elems{i}) = elements.(elems{i}).proj(x.(elems{i}), ...
                                                    u.(elems{i}));
        end
    end

    M.tangent = @tangent;
    function v = tangent(x, u)
        for i = 1 : nelems
            v.(elems{i}) = elements.(elems{i}).tangent(x.(elems{i}), ...
                                                       u.(elems{i}));
        end
    end

    % True by default, false if any false encountered
    M.tangent2ambient_is_identity = true;
    for k = 1 : nelems
        if isfield(elements.(elems{k}), 'tangent2ambient_is_identity')
            if ~elements.(elems{k}).tangent2ambient_is_identity
                M.tangent2ambient_is_identity = false;
                break;
            end
        end
    end

    M.tangent2ambient = @tangent2ambient;
    function v = tangent2ambient(x, u)
        for i = 1 : nelems
            if isfield(elements.(elems{i}), 'tangent2ambient')
                v.(elems{i}) = ...
                    elements.(elems{i}).tangent2ambient( ...
                                               x.(elems{i}), u.(elems{i}));
            else
                v.(elems{i}) = u.(elems{i});
            end
        end
    end

    M.egrad2rgrad = @egrad2rgrad;
    function g = egrad2rgrad(x, g)
        for i = 1 : nelems
            g.(elems{i}) = elements.(elems{i}).egrad2rgrad(...
                                               x.(elems{i}), g.(elems{i}));
        end
    end
    for ii = 1 : nelems
        if nargin(elements.(elems{ii}).egrad2rgrad) > 2
            warning('manopt:productmanifold:egrad2rgrad', ...
                   ['Product manifolds call M.egrad2rgrad with only two ', ...
                    'inputs:\nstoredb and key won''t be available.']);
            break;
        end
    end

    M.ehess2rhess = @ehess2rhess;
    function h = ehess2rhess(x, eg, eh, h)
        for i = 1 : nelems
            h.(elems{i}) = elements.(elems{i}).ehess2rhess(...
                 x.(elems{i}), eg.(elems{i}), eh.(elems{i}), h.(elems{i}));
        end
    end
    for ii = 1 : nelems
        if nargin(elements.(elems{ii}).ehess2rhess) > 4
            warning('manopt:productmanifold:ehess2rhess', ...
                   ['Product manifolds call M.ehess2rhess with only two ', ...
                    'inputs:\nstoredb and key won''t be available.']);
            break;
        end
    end

    if all_elements_provide('exp')
        M.exp = @exp;
    end
    function y = exp(x, u, t)
        if nargin < 3
            t = 1.0;
        end
        for i = 1 : nelems
            y.(elems{i}) = elements.(elems{i}).exp(x.(elems{i}), ...
                                                   u.(elems{i}), t);
        end
    end

    M.retr = @retr;
    function y = retr(x, u, t)
        if nargin < 3
            t = 1.0;
        end
        for i = 1 : nelems
            y.(elems{i}) = elements.(elems{i}).retr(x.(elems{i}), ...
                                                    u.(elems{i}), t);
        end
    end

    if all_elements_provide('log')
        M.log = @log;
    end
    function u = log(x1, x2)
        for i = 1 : nelems
            u.(elems{i}) = elements.(elems{i}).log(x1.(elems{i}), ...
                                                   x2.(elems{i}));
        end
    end

    M.hash = @hash;
    function str = hash(x)
        str = '';
        for i = 1 : nelems
            str = [str elements.(elems{i}).hash(x.(elems{i}))]; %#ok<AGROW>
        end
        str = ['z' hashmd5(str)];
    end

    M.lincomb = @lincomb;
    function v = lincomb(x, a1, u1, a2, u2)
        if nargin == 3
            for i = 1 : nelems
                v.(elems{i}) = elements.(elems{i}).lincomb(x.(elems{i}), ...
                                                        a1, u1.(elems{i}));
            end
        elseif nargin == 5
            for i = 1 : nelems
                v.(elems{i}) = elements.(elems{i}).lincomb(x.(elems{i}), ...
                                     a1, u1.(elems{i}), a2, u2.(elems{i}));
            end
        else
            error('Bad usage of productmanifold.lincomb');
        end
    end

    M.rand = @rand;
    function x = rand()
        for i = 1 : nelems
            x.(elems{i}) = elements.(elems{i}).rand();
        end
    end

    M.randvec = @randvec;
    function u = randvec(x)
        for i = 1 : nelems
            u.(elems{i}) = elements.(elems{i}).randvec(x.(elems{i}));
        end
        u = M.lincomb(x, 1/sqrt(nelems), u);
    end

    M.zerovec = @zerovec;
    function u = zerovec(x)
        for i = 1 : nelems
            u.(elems{i}) = elements.(elems{i}).zerovec(x.(elems{i}));
        end
    end

    if all_elements_provide('transp')
        M.transp = @transp;
    end
    function v = transp(x1, x2, u)
        for i = 1 : nelems
            v.(elems{i}) = elements.(elems{i}).transp(x1.(elems{i}), ...
                                              x2.(elems{i}), u.(elems{i}));
        end
    end

    if all_elements_provide('pairmean')
        M.pairmean = @pairmean;
    end
    function y = pairmean(x1, x2)
        for i = 1 : nelems
            y.(elems{i}) = elements.(elems{i}).pairmean(x1.(elems{i}), ...
                                                        x2.(elems{i}));
        end
    end

    if vec_available
        M.vec = @vec;
        M.mat = @mat;
    end

    function u_vec = vec(x, u_mat)
        u_vec = zeros(vec_pos(end)-1, 1);
        for i = 1 : nelems
            range = vec_pos(i) : (vec_pos(i+1)-1);
            u_vec(range) = elements.(elems{i}).vec(x.(elems{i}), ...
                                                   u_mat.(elems{i}));
        end
    end

    function u_mat = mat(x, u_vec)
        u_mat = struct();
        for i = 1 : nelems
            range = vec_pos(i) : (vec_pos(i+1)-1);
            u_mat.(elems{i}) = elements.(elems{i}).mat(x.(elems{i}), ...
                                                       u_vec(range));
        end
    end

    M.vecmatareisometries = @() vecmatareisometries;

    if all_elements_provide('lie_identity')
        M.lie_identity = @lie_identity;
    end

    function I = lie_identity()
        I = struct();
        for i = 1 : nelems
            Mi = elements.(elems{i});
            Ii = Mi.lie_identity();
            I.(elems{i}) = Ii;
        end
    end
end


% SP_EVALUATE_ELEMENT_LIST_PARAM: compute the basis functions, in the parametric domain, in a given list of elements.
%
%     sp = sp_evaluate_element_list_param (space, msh_elems, 'option1', value1, ...)
%
% INPUTS:
%
%    space:   object defining the space of discrete functions (see sp_vector)
%    msh_elems: msh structure containing the information of quadrature or
%               visualization points, for a given list of elements
%               (see msh_cartesian/msh_evaluate_element_list)
%   'option', value: additional optional parameters, currently available options are:
%
%              Name     |   Default value |  Meaning
%           ------------+-----------------+----------------------------------
%            value      |      true       |  compute shape_functions
%            gradient   |      false      |  compute shape_function_gradients
%            divergence |      false      |  compute shape_function_divs
%            curl       |      false      |  compute shape_function_curls
%            hessian    |      false      |  compute shape_function_hessians
%
% OUTPUT:
%
%    sp: struct representing the discrete function space, with the following fields:
%              (see the article for a detailed description)
%
%    FIELD_NAME      (SIZE)                                     DESCRIPTION
%    ncomp           (scalar)                                   number of components of the functions of the space
%    ndof            (scalar)                                   total number of degrees of freedom
%    ndof_dir        (ncomp_param x ndim matrix)                for each component, number of degrees of freedom along each direction
%    nsh_max         (scalar)                                   maximum number of shape functions per element
%    nsh             (1 x msh_col.nel vector)                   actual number of shape functions per each element
%    connectivity    (nsh_max x msh_col.nel vector)             indices of basis functions that do not vanish in each element
%    shape_functions (ncomp_param x msh_col.nqn x nsh_max x msh_col.nel)  basis functions evaluated at each quadrature node in each element
%    shape_function_gradients
%       (ncomp_param x ndim x msh_col.nqn x nsh_max x msh_col.nel) basis function gradients evaluated at each quadrature node in each element
%    shape_function_hessians
%       (ncomp_param x ndim x ndim x msh_col.nqn x nsh_max x msh_col.nel) basis function hessians evaluated at each quadrature node in each element
%    shape_function_divs (msh_col.nqn x nsh_max x msh_col.nel)     basis function divergence evaluated at each quadrature node in each element
%    shape_function_curls
%         2D:  (msh_col.nqn x nsh_max x msh_col.nel)               basis function curl evaluated at each quadrature node in each element
%         3D:  (3 x msh_col.nqn x nsh_max x msh_col.nel)
%
% Copyright (C) 2009, 2010, 2011 Carlo de Falco
% Copyright (C) 2011, 2015, 2019 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function sp = sp_evaluate_element_list_param (space, msh, varargin)

value = true;
gradient = false;
divergence = false;
curl = false;
hessian = false;
if (~isempty (varargin))
  if (~rem (length (varargin), 2) == 0)
    error ('sp_evaluate_element_list_param: options must be passed in the [option, value] format');
  end
  for ii=1:2:length(varargin)-1
    if (strcmpi (varargin {ii}, 'value'))
      value = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'gradient'))
      gradient = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'curl'))
      curl = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'divergence'))
      divergence = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'hessian'))
      hessian = varargin {ii+1};
    else
      warning ('Ignoring unknown option %s', varargin {ii});
    end
  end
end

first_der = gradient || divergence || curl;
for icomp = 1:space.ncomp_param
  sp_col_scalar(icomp) = sp_evaluate_element_list_param (space.scalar_spaces{icomp}, msh, 'value', value, 'gradient', first_der, 'hessian', hessian);
end

ndof_scalar = [sp_col_scalar.ndof];

ndof = sum (ndof_scalar);
nsh  = zeros (1, msh.nel);
connectivity = [];
aux = 0;
for icomp = 1:space.ncomp_param
  ndof_dir(icomp,:) = sp_col_scalar(icomp).ndof_dir;
  nsh = nsh + sp_col_scalar(icomp).nsh(:)';

  inds = find (sp_col_scalar(icomp).connectivity);
  sp_col_scalar(icomp).connectivity(inds) = sp_col_scalar(icomp).connectivity(inds) + aux;
  connectivity = [connectivity; sp_col_scalar(icomp).connectivity];
  aux = aux + ndof_scalar(icomp);
end

sp = struct('nsh_max', space.nsh_max, 'nsh', nsh, 'ndof', ndof,  ...
            'ndof_dir', ndof_dir, 'connectivity', connectivity, ...
            'ncomp', space.ncomp, 'ncomp_param', space.ncomp_param);

if (value)
  sp.shape_functions = zeros (sp.ncomp_param, msh.nqn, sp.nsh_max, msh.nel);
  for icomp = 1:space.ncomp_param
    indices = space.cumsum_nsh(icomp)+(1:sp_col_scalar(icomp).nsh_max);
    sp.shape_functions(icomp,:,indices,:) = sp_col_scalar(icomp).shape_functions;
  end
end

if (gradient || curl || divergence)
  shape_fun_grads = zeros (space.ncomp_param, msh.ndim, msh.nqn, sp.nsh_max, msh.nel);

  for icomp = 1:space.ncomp_param
    indices = space.cumsum_nsh(icomp)+(1:sp_col_scalar(icomp).nsh_max);
    shape_fun_grads(icomp,:,:,indices,:) = sp_col_scalar(icomp).shape_function_gradients;
  end

  if (gradient)
    sp.shape_function_gradients = shape_fun_grads;
  end

  if (divergence)
    sp.shape_function_divs = zeros (msh.nqn, sp.nsh_max, msh.nel);
    for icomp = 1:space.ncomp_param
      sp.shape_function_divs = sp.shape_function_divs + ...
        reshape (shape_fun_grads(icomp,icomp,:,:,:), msh.nqn, sp.nsh_max, msh.nel);
    end
  end

  if (curl)
    if (space.ncomp_param == 2)
      sp.shape_function_curls = reshape (shape_fun_grads(2,1,:,:,:) - ...
	 	       shape_fun_grads(1,2,:,:,:), msh.nqn, sp.nsh_max, msh.nel);
    elseif (space.ncomp_param == 3)
      shape_fun_curls = zeros (space.ncomp, msh.nqn, sp.nsh_max, msh.nel);
      for icomp = 1:space.ncomp
        ind1 = mod(icomp,3) + 1;
        ind2 = mod(ind1, 3) + 1;
        shape_fun_curls(icomp,:,:,:) = reshape (shape_fun_grads(ind2,ind1,:,:,:) - ...
                   shape_fun_grads(ind1,ind2,:,:,:), 1, msh.nqn, sp.nsh_max, msh.nel);
      end
      sp.shape_function_curls = shape_fun_curls;
    end
  end
end

if (hessian)
  sp.shape_function_hessians = zeros (space.ncomp, msh.ndim, msh.ndim, msh.nqn, sp.nsh_max, msh.nel);
  for icomp = 1:space.ncomp_param
    indices = space.cumsum_nsh(icomp)+(1:sp_col_scalar(icomp).nsh_max);
    sp.shape_function_hessians(icomp,:,:,:,indices,:) = sp_col_scalar(icomp).shape_function_hessians;
  end
end

end


function obox = orientedBox(points)
%ORIENTEDBOX Minimum-width oriented bounding box of a set of points
%
%   OBOX = orientedBox(PTS)
%   Computes the oriented bounding box of a set of points. Oriented box is
%   defined by a center, two dimensions (the length and the width), and the
%   orientation of the length axis. Orientation is counted in degrees,
%   counter-clockwise.
%
%   Example
%     % Draw oriented bounding box of an ellipse
%     elli = [30 40 40 20 30];
%     pts = ellipseToPolygon(elli, 120);
%     obox = orientedBox(pts);
%     figure; hold on;
%     drawEllipse(elli);
%     drawOrientedBox(obox, 'm');
%
%   See also
%   drawOrientedBox, orientedBoxToPolygon
%

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2012-03-29,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2012 INRA - Cepia Software Platform.

%% initialisations

% first, compute convex hull of the polygon
inds = convhull(points(:,1), points(:,2));
hull = points(inds, :);

% if first and last points are the same, remove the last one
if inds(1) == inds(end)
    hull = hull(1:end-1, :);
end

% compute convex hull centroid, that corresponds to approximate
% location of rectangle center
center = mean(hull, 1);
hull = bsxfun(@minus, hull, center);

% number of hull vertices
nV = size(hull, 1);

% default values
rotatedAngle = 0;
minWidth = inf;
minAngle = 0;

% avoid degenerated cases
if nV < 3
    return;
end

% indices of vertices in extreme y directions
[tmp, indA] = min(hull(:, 2)); %#ok<ASGLU>
[tmp, indB] = max(hull(:, 2)); %#ok<ASGLU>

caliperA = [ 1 0];    % Caliper A points along the positive x-axis
caliperB = [-1 0];    % Caliper B points along the negative x-axis

%% Find the direction with minimum width (rotating caliper algorithm)

while rotatedAngle < pi
    % compute the direction vectors corresponding to each edge
    indA2 = mod(indA, nV) + 1;
    vectorA = hull(indA2, :) - hull(indA, :);

    indB2 = mod(indB, nV) + 1;
    vectorB = hull(indB2, :) - hull(indB, :);

    % Determine the angle between each caliper and the next adjacent edge
    % in the polygon
    angleA = vectorAngle(caliperA, vectorA);
    angleB = vectorAngle(caliperB, vectorB);

    % Determine the smallest of these angles
    angleIncrement = min(angleA, angleB);

    % Rotate the calipers by the smallest angle
    caliperA = rotateVector(caliperA, angleIncrement);
    caliperB = rotateVector(caliperB, angleIncrement);

    rotatedAngle = rotatedAngle + angleIncrement;

    % compute current width, and update opposite vertex
    if angleA < angleB
        line = createLine(hull(indA, :), hull(indA2, :));
        width = distancePointLine(hull(indB, :), line);
        indA = mod(indA, nV) + 1;

    else
        line = createLine(hull(indB, :), hull(indB2, :));
        width = distancePointLine(hull(indA, :), line);
        indB = mod(indB, nV) + 1;

    end

    % update minimum width and corresponding angle if needed
    if width < minWidth
        minWidth = width;
        minAngle = rotatedAngle;
    end
end

%% Compute box dimensions

% orientation of the main axis
theta = rad2deg(minAngle);

% pre-compute trigonometric functions
cot = cos(minAngle);
sit = sin(minAngle);

% elongation in direction of rectangle length
x = hull(:,1);
y = hull(:,2);
x2  =   x * cot + y * sit;
y2  = - x * sit + y * cot;

% compute extension along main directions
xmin = min(x2);    xmax = max(x2);
ymin = min(y2);    ymax = max(y2);

% position of the center with respect to the centroid compute before
dl = (xmax + xmin)/2;
dw = (ymax + ymin)/2;

% change  coordinate from rectangle to user-space
dx  = dl * cot - dw * sit;
dy  = dl * sit + dw * cot;

% coordinates of oriented box center
center = center + [dx dy];

% size of the rectangle
rectLength  = xmax - xmin;
rectWidth   = ymax - ymin;

% concatenate rectangle data
obox = [center rectLength rectWidth theta];


function LnX = coefficient(x,y,L)
% Lagrange form for the polynomial interpolation
% this function produces elements of the form y_k*L_k(x)
n = length(x);
Num = '';
Den = '';
syms X

for i = 1:n
    if i ~= L
    TempNum = strcat('(X-x(',num2str(i),')',')','*');
    Num = strcat(Num,TempNum);
    TempDen = strcat('(x(',num2str(L),')','-','x(',num2str(i),')',')','*');
    Den = strcat(Den,TempDen);
    end
end

Num(end) = []; Den(end) = [];
LnX = (eval(strcat('(',Num,')','/','(',Den,')')))*y(L);


function [au] = km2au(km)
% Convert length from kilometers to astronomical units.
% Chad A. Greene 2012
au = km*6.684587122671e-9;


function [A,B]= useconvhulln(Z2)

% This file is part of SOSTOOLS - Sum of Squares Toolbox ver 3.00.
%
% Copyright (C)2002, 2004, 2013  A. Papachristodoulou (1), J. Anderson (1),
%                                G. Valmorbida (1), S. Prajna (2),
%                                P. Seiler (3), P. A. Parrilo (4)
% (1) Department of Engineering Science, University of Oxford, Oxford, U.K.
% (2) Control and Dynamical Systems - California Institute of Technology,
%     Pasadena, CA 91125, USA.
% (3) Aerospace and Engineering Mechanics Department, University of
%     Minnesota, Minneapolis, MN 55455-0153, USA.
% (4) Laboratory for Information and Decision Systems, M.I.T.,
%     Massachusetts, MA 02139-4307
%
% Send bug reports and feedback to: sostools@cds.caltech.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.

% AP Apr 03 02
% AP Feb 01 03

% First, find the convex hull of Z2

convh = convhulln(Z2);
facets = size(convh,1);
[nZ1,nZ2] = size(Z2);

% Form the hyperplanes
% A hyperplane of the form [x1 ... xn -ones][A';b'] = 0
% [A';b'] is in the nullspace of the matrix [x1,...,xn -ones].
% Ignore it if it has dimension greater than 1
% Normalise if possible.

for i = 1:facets
    vert = [Z2(convh(i,:),:) -ones(nZ2,1)];
    nullvert = null(vert,'r');
    if size(nullvert,2) == 1
        if nullvert(end) ~=0
            nullvert = nullvert./nullvert(end);
        end
        coeff(:,i) = nullvert(1:end-1);
        cold(i,1) = nullvert(end);
    end
end

coeff = coeff';

% Condition the matrix a bit

coeffcold = [coeff cold];
coeffcold = round(coeffcold*100000)/100000;

% Discard same hyperplanes (due to convhulln result)
[coeff2cold,Ix,Iy] = unique(coeffcold,'rows');

%Remove a possible zero row

yind = find(sum(coeff2cold.^2,2) == 0);
if ~isempty(yind)
    Ix = [Ix([1:yind-1],1);Ix(yind+1:end,1)];
end

coeff2 = coeff(Ix,:);
convhnew = convh(Ix,:);
cnew = cold(Ix);
facetsnew = size(convhnew,1);

% Make inequalities out of them by testing a point not on the hyperplane
% Notation: convex hull is now Ax-b<=0
for fac = 1:facetsnew
    for ind = 1:nZ1
        matr = find(convhnew(fac,:) - ind*ones(1,nZ2) == 0);
        tests(fac) = coeff2(fac,:)*Z2(ind,:)'-cnew(fac);
        if isempty(matr) & abs(tests(fac)) > 1e-8
            break
        end
    end
    if tests(fac)>0
        coeff2(fac,:)=-coeff2(fac,:);
        cnew(fac)=-cnew(fac);
    end
end

A = coeff2;
B = cnew;


function [M, RHS, Mx, My, Mz, RHSx, RHSy, RHSz] = ...
    convectionTvdTermCylindrical3D(u, phi, FL)
% This function uses the TVD scheme to discretize a 3D
% convection term in the form \grad (u \phi) where u is a face vactor
% It also returns the x and y parts of the matrix of coefficient.
%
% SYNOPSIS:
%
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nr = u.domain.dims(1);
Ntetta = u.domain.dims(2);
Nz = u.domain.dims(3);
G=reshape((1:(Nr+2)*(Ntetta+2)*(Nz+2)), Nr+2, Ntetta+2, Nz+2);
DRp = repmat(u.domain.cellsize.x(2:end-1), 1, Ntetta, Nz);
DTHETAp = repmat(u.domain.cellsize.y(2:end-1)', Nr, 1, Nz);
DZ = zeros(1,1,Nz+2);
DZ(1,1,:) = u.domain.cellsize.z;
DZp=repmat(DZ(1,1,2:end-1), Nr, Ntetta, 1);
rp = repmat(u.domain.cellcenters.x, 1, Ntetta, Nz);
rf = repmat(u.domain.facecenters.x, 1, Ntetta, Nz);
dx=repmat(0.5*(u.domain.cellsize.x(1:end-1)+u.domain.cellsize.x(2:end)), 1, Ntetta, Nz);
dy=repmat(0.5*(u.domain.cellsize.y(1:end-1)+u.domain.cellsize.y(2:end))', Nr, 1, Nz);
dz=zeros(1, 1, Nz+1);
dz(1,1,:)=0.5*(u.domain.cellsize.z(1:end-1)+u.domain.cellsize.z(2:end));
dz=repmat(dz, Nr, Ntetta, 1);
psiX_p = zeros(Nr+1,Ntetta,Nz);
psiX_m = zeros(Nr+1,Ntetta,Nz);
psiY_p = zeros(Nr,Ntetta+1,Nz);
psiY_m = zeros(Nr,Ntetta+1,Nz);
psiZ_p = zeros(Nr,Ntetta,Nz+1);
psiZ_m = zeros(Nr,Ntetta,Nz+1);

% define the vectors to stores the sparse matrix data
iix = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
jjx = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
sx = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
iiy = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
jjy = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
sy = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
iiz = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
jjz = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
sz = zeros(3*(Nr+2)*(Ntetta+2)*(Nz+2),1);
mnx = Nr*Ntetta*Nz;	mny = Nr*Ntetta*Nz;   mnz = Nr*Ntetta*Nz;

% extract the velocity data
% note: size(ux) = [1:m+1, 1:n] and size(uy) = [1:m, 1:n+1]
ux = u.xvalue;
uy = u.yvalue;
uz = u.zvalue;

% calculate the upstream to downstream gradient ratios for u>0 (+ ratio)
% x direction
dphiX_p = (phi.value(2:Nr+2, 2:Ntetta+1, 2:Nz+1)-phi.value(1:Nr+1, 2:Ntetta+1, 2:Nz+1))./dx;
rX_p = dphiX_p(1:end-1,:,:)./fsign(dphiX_p(2:end,:,:));
psiX_p(2:Nr+1,:,:) = 0.5*FL(rX_p).* ...
    (phi.value(3:Nr+2,2:Ntetta+1,2:Nz+1)-phi.value(2:Nr+1,2:Ntetta+1,2:Nz+1));
psiX_p(1,:,:) = 0; % left boundary
% y direction
dphiY_p = (phi.value(2:Nr+1, 2:Ntetta+2, 2:Nz+1)-phi.value(2:Nr+1, 1:Ntetta+1, 2:Nz+1))./dy;
rY_p = dphiY_p(:,1:end-1,:)./fsign(dphiY_p(:,2:end,:));
psiY_p(:,2:Ntetta+1,:) = 0.5*FL(rY_p).* ...
    (phi.value(2:Nr+1,3:Ntetta+2,2:Nz+1)-phi.value(2:Nr+1, 2:Ntetta+1,2:Nz+1));
psiY_p(:,1,:) = 0; % Bottom boundary
% z direction
dphiZ_p = (phi.value(2:Nr+1, 2:Ntetta+1, 2:Nz+2)-phi.value(2:Nr+1, 2:Ntetta+1, 1:Nz+1))./dz;
rZ_p = dphiZ_p(:,:,1:end-1)./fsign(dphiZ_p(:,:,2:end));
psiZ_p(:,:,2:Nz+1) = 0.5*FL(rZ_p).* ...
    (phi.value(2:Nr+1,2:Ntetta+1,3:Nz+2)-phi.value(2:Nr+1,2:Ntetta+1,2:Nz+1));
psiZ_p(:,:,1) = 0; % Back boundary

% calculate the upstream to downstream gradient ratios for u<0 (- ratio)
% x direction
rX_m = dphiX_p(2:end,:,:)./fsign(dphiX_p(1:end-1,:,:));
psiX_m(1:Nr,:,:) = 0.5*FL(rX_m).* ...
    (phi.value(1:Nr, 2:Ntetta+1, 2:Nz+1)-phi.value(2:Nr+1, 2:Ntetta+1, 2:Nz+1));
psiX_m(Nr+1,:,:) = 0; % right boundary
% y direction
rY_m = dphiY_p(:,2:end,:)./fsign(dphiY_p(:,1:end-1,:));
psiY_m(:,1:Ntetta,:) = 0.5*FL(rY_m).* ...
    (phi.value(2:Nr+1,1:Ntetta,2:Nz+1)-phi.value(2:Nr+1,2:Ntetta+1,2:Nz+1));
psiY_m(:,Ntetta+1,:) = 0; % top boundary
% z direction
rZ_m = dphiZ_p(:,:,2:end)./fsign(dphiZ_p(:,:,1:end-1));
psiZ_m(:,:,1:Nz) = 0.5*FL(rZ_m).* ...
    (phi.value(2:Nr+1,2:Ntetta+1,1:Nz)-phi.value(2:Nr+1,2:Ntetta+1,2:Nz+1));
psiZ_m(:,:,Nz+1) = 0; % front boundary
% reassign the east, west, north, and south velocity vectors for the
% code readability
ue = ux(2:Nr+1,:,:);		uw = ux(1:Nr,:,:);
vn = uy(:,2:Ntetta+1,:);     vs = uy(:,1:Ntetta,:);
wf = uz(:,:,2:Nz+1);     wb = uz(:,:,1:Nz);
re = rf(2:Nr+1,:,:);         rw = rf(1:Nr,:,:);

% find the velocity direction for the upwind scheme
ue_min = min(ue,0);	ue_max = max(ue,0);
uw_min = min(uw,0);	uw_max = max(uw,0);
vn_min = min(vn,0);	vn_max = max(vn,0);
vs_min = min(vs,0);	vs_max = max(vs,0);
wf_min = min(wf,0);	wf_max = max(wf,0);
wb_min = min(wb,0);	wb_max = max(wb,0);

% calculate the coefficients for the internal cells
AE = re.*ue_min./(DRp.*rp);
AW = -rw.*uw_max./(DRp.*rp);
AN = vn_min./(DTHETAp.*rp);
AS = -vs_max./(DTHETAp.*rp);
AF = wf_min./DZp;
AB = -wb_max./DZp;
APx = (re.*ue_max-rw.*uw_min)./(DRp.*rp);
APy = (vn_max-vs_min)./(DTHETAp.*rp);
APz = (wf_max-wb_min)./DZp;

% Also correct for the boundary cells (not the ghost cells)
% Left boundary:
APx(1,:,:) = APx(1,:,:)-rw(1,:,:).*uw_max(1,:,:)./(2*rp(1,:,:)*DRp(1));   AW(1,:,:) = AW(1,:,:)/2;
% Right boundary:
AE(end,:,:) = AE(end,:,:)/2;    APx(end,:,:) = APx(end,:,:)+re(end,:,:).*ue_min(end,:,:)./(2*DRp(end)*rp(end,:,:));
% Bottom boundary:
APy(:,1,:) = APy(:,1,:)-vs_max(:,1,:)./(2*DTHETAp(1)*rp(:,1,:));   AS(:,1,:) = AS(:,1,:)/2;
% Top boundary:
AN(:,end,:) = AN(:,end,:)/2;    APy(:,end,:) = APy(:,end,:)+vn_min(:,end,:)./(2*DTHETAp(end)*rp(:,end,:));
% Back boundary:
APz(:,:,1) = APz(:,:,1)-wb_max(:,:,1)/(2*DZp(1));   AB(:,:,1) = AB(:,:,1)/2;
% Front boundary:
AF(:,:,end) = AF(:,:,end)/2;    APz(:,:,end) = APz(:,:,end) + wf_min(:,:,end)/(2*DZp(end));

AE = reshape(AE,mnx,1);
AW = reshape(AW,mnx,1);
AN = reshape(AN,mny,1);
AS = reshape(AS,mny,1);
AF = reshape(AF,mnz,1);
AB = reshape(AB,mnz,1);
APx = reshape(APx,mnx,1);
APy = reshape(APy,mny,1);
APz = reshape(APz,mnz,1);

% build the sparse matrix based on the numbering system
rowx_index = reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mnx,1); % main diagonal x
iix(1:3*mnx) = repmat(rowx_index,3,1);
rowy_index = reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mny,1); % main diagonal y
iiy(1:3*mny) = repmat(rowy_index,3,1);
rowz_index = reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mnz,1); % main diagonal z
iiz(1:3*mnz) = repmat(rowz_index,3,1);
jjx(1:3*mnx) = [reshape(G(1:Nr,2:Ntetta+1,2:Nz+1),mnx,1); reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mnx,1); reshape(G(3:Nr+2,2:Ntetta+1,2:Nz+1),mnx,1)];
jjy(1:3*mny) = [reshape(G(2:Nr+1,1:Ntetta,2:Nz+1),mny,1); reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mny,1); reshape(G(2:Nr+1,3:Ntetta+2,2:Nz+1),mny,1)];
jjz(1:3*mnz) = [reshape(G(2:Nr+1,2:Ntetta+1,1:Nz),mnz,1); reshape(G(2:Nr+1,2:Ntetta+1,2:Nz+1),mnz,1); reshape(G(2:Nr+1,2:Ntetta+1,3:Nz+2),mnz,1)];
sx(1:3*mnx) = [AW; APx; AE];
sy(1:3*mny) = [AS; APy; AN];
sz(1:3*mnz) = [AB; APz; AF];

% calculate the TVD correction term
div_x = -(1./(DRp.*rp)).*(re.*(ue_max.*psiX_p(2:Nr+1,:,:)+ue_min.*psiX_m(2:Nr+1,:,:))- ...
              rw.*(uw_max.*psiX_p(1:Nr,:,:)+uw_min.*psiX_m(1:Nr,:,:)));
div_y = -(1./(DTHETAp.*rp)).*((vn_max.*psiY_p(:,2:Ntetta+1,:)+vn_min.*psiY_m(:,2:Ntetta+1,:))- ...
              (vs_max.*psiY_p(:,1:Ntetta,:)+vs_min.*psiY_m(:,1:Ntetta,:)));
div_z = -(1./DZp).*((wf_max.*psiZ_p(:,:,2:Nz+1)+wf_min.*psiZ_m(:,:,2:Nz+1))- ...
              (wb_max.*psiZ_p(:,:,1:Nz)+wb_min.*psiZ_m(:,:,1:Nz)));

% define the RHS Vector
RHS = zeros((Nr+2)*(Ntetta+2)*(Nz+2),1);
RHSx = zeros((Nr+2)*(Ntetta+2)*(Nz+2),1);
RHSy = zeros((Nr+2)*(Ntetta+2)*(Nz+2),1);
RHSz = zeros((Nr+2)*(Ntetta+2)*(Nz+2),1);

% assign the values of the RHS vector
row_index = rowx_index;
RHS(row_index) = reshape(div_x+div_y+div_z,Nr*Ntetta*Nz,1);
RHSx(rowx_index) = reshape(div_x,Nr*Ntetta*Nz,1);
RHSy(rowy_index) = reshape(div_y,Nr*Ntetta*Nz,1);
RHSz(rowz_index) = reshape(div_z,Nr*Ntetta*Nz,1);

% build the sparse matrix
kx = 3*mnx;
ky = 3*mny;
kz = 3*mnz;
Mx = sparse(iix(1:kx), jjx(1:kx), sx(1:kx), (Nr+2)*(Ntetta+2)*(Nz+2), (Nr+2)*(Ntetta+2)*(Nz+2));
My = sparse(iiy(1:ky), jjy(1:ky), sy(1:ky), (Nr+2)*(Ntetta+2)*(Nz+2), (Nr+2)*(Ntetta+2)*(Nz+2));
Mz = sparse(iiz(1:kz), jjz(1:kz), sz(1:kz), (Nr+2)*(Ntetta+2)*(Nz+2), (Nr+2)*(Ntetta+2)*(Nz+2));
M = Mx + My + Mz;

end

function phi_out = fsign(phi_in)
% This function checks the value of phi_in and assigns an eps value to the
% elements that are less than or equal to zero, while keeping the signs of
% the nonzero elements
    phi_out = (abs(phi_in)>=eps).*phi_in+eps*(phi_in==0)+eps*(abs(phi_in)<eps).*sign(phi_in);
end


function [f,g] = call_log_reg(v, y, X, lambda)
% Primal objective function (Omega = inv(Sigma))
% f = 0.5*(logdet(V) - logdet(Sigma) - tr(V*SigmaInv) - (m-mu)'*SigmaInv*(m-mu)
%     + L) - sum_d fb(mbar_d, vbar_d)
% where mbar = X*m, vbar = diag(X*V*X')
%
% Written by Emtiyaz,
% Modified by Wu Lin
  global m_cache;
  global V_cache;
  global iter_counter;
  global is_cache;
  global times_cache;
  iter_counter = iter_counter + 1;

  [D L] = size(X);

  Omega = diag(lambda);
  %Extract mean, Cholesky and bias
  m = v(1:L);
  idx = L + [1:L*(L+1)/2];
  U = triu(unpackcovariance(v(idx),L));

  % compute V
  V = U'*U;

  if is_cache==1
      m_cache(:,iter_counter) = m;
      V_cache(:,:,iter_counter) = V;
  end

  % compute kl and its gradient
  kl = 0.5*(2*sum(log(diag(U))) + sum(log(lambda(2:end))) - trace(V*Omega) -m'*(Omega*m) + L);

  % contribution from the bound
  mbar = X*m;
  vbar = sum(X.*(V*X')',2); % diag(X*V*X') efficient
  [fb, gmb, gvb] = E_log_p('bernoulli_logit', y, mbar, vbar, []);
  fb = -fb;
  gm_lvb = X'*(-gmb);
  gV_lvb = X'*bsxfun(@times, -gvb, X); %efficient X'*diag(gllp/2)*X;

  % final
  f = kl - sum(fb);
  gm = - Omega*m - gm_lvb;
  gU = diag(1./diag(U)) - triu(U*Omega) - triu(2*U*gV_lvb);

  g=[gm(:); gU(triu(ones(L))==1)];

  % return
  g=-g;
  f=-f;

  if iter_counter+1<=length(times_cache)
      times_cache(iter_counter+1) = toc;
  else
      times_cache(end) = toc;
  end


function [Rlat,Rlon,alt] = LatLongCalcSingle_ADI(msg, inputLat, inputLong)
% Calculate latitude, longitude and altitude from message bits
% Copyright 2010, The MathWorks, Inc.

persistent NL
persistent latzones
persistent Dlat0
persistent Dlat1
persistent latOffset0
persistent latOffset1

if isempty(NL)
    Dlat0 = 360/(4*15-0);
    latOffset0 = floor(inputLat/Dlat0);
    Dlat1 = 360/(4*15-1);
    latOffset1 = floor(inputLat/Dlat1);
    NL=2:59;
    latzones = [(180/pi)*acos(sqrt((1-cos(pi/2/15))./(1-cos(2*pi./NL)))) 0];
end

% Altitude calculation
q = msg(48);
if q == 0
    af = 100;
else
    af = 25;
end
altBits=[msg(41:47);msg(49:52)]';
alt = altBits*[1024;512;256;128;64;32;16;8;4;2;1]*af - 1000;

evenOdd1 = msg(54);
latBits = msg(55:71)';
longBits = msg(72:88)';
la1 = latBits*[65536;32768;16384;8192;4096;2048;1024;512;256;128;64;32;16;8;4;2;1];
lo1 = longBits*[65536;32768;16384;8192;4096;2048;1024;512;256;128;64;32;16;8;4;2;1];

% Technically you need both even and odd messages to calculate lat/long
% unambiguously. For this code, use a single message and then check to see
% if the lat/long values are reasonable. If not, change the lat/long base
% factors (LL.a1, LL.a2, etc.) and recompute.

% Latitude calculation
if evenOdd1 == 0
    Rlat = Dlat0*(latOffset0 + la1/131072);
else
    Rlat = Dlat1*(latOffset1 + la1/131072);
end

% Compare latitude to known location. If it's off by more than two degrees,
% use new base factors.
if Rlat > inputLat+2
    if strcmp(evenOdd1,'Even')
        Rlat = Dlat0*(latOffset0 - 1 + la1/131072);
    else
        Rlat = Dlat1*(latOffset1 - 1 + la1/131072);
    end
elseif Rlat < inputLat-2
    if strcmp(evenOdd1,'Even')
        Rlat = Dlat0*(latOffset0 + 1 + la1/131072);
    else
        Rlat = Dlat1*(latOffset1 + 1 + la1/131072);
    end
end

% Based on latitude, calculate longitude
NL0 = find(latzones<Rlat,1,'first');
ni0 = NL0;
ni1 = NL0 - 1;

Dlon0 = 360/ni0;
longOffset0 = floor(inputLong/Dlon0);
Dlon1 = 360/ni1;
longOffset1 = floor(inputLong/Dlon1);

if evenOdd1 == 0
    Rlon = Dlon0*(longOffset0 + lo1/131072);
else
    Rlon = Dlon1*(longOffset1 + lo1/131072);
end

% Compare longitude to known location. If it's off by more than two
% degrees, use new base factors.
if Rlon > inputLong+2
    if strcmp(evenOdd1,'Even')
        Rlon = Dlon0*(longOffset0 - 1 + lo1/131072);
    else
        Rlon = Dlon1*(longOffset1 - 1 + lo1/131072);
    end
elseif Rlon < inputLong-2
    if strcmp(evenOdd1,'Even')
        Rlon = Dlon0*(longOffset0 + 1 + lo1/131072);
    else
        Rlon = Dlon1*(longOffset1 + 1 + lo1/131072);
    end
end

% disp(sprintf('Plane is at altitude %d\nLatitude value: %d\nLongitude value: %d', alt, la1, lo1));

% GoogleMap(aircraftID, alt1, Rlat, Rlon)


function out = Dstar(R, n, e, fun)
%finds excess distortion for a given rate at a given blocklength for
%Gaussian source with unit variance
%R - rate
%n - block length (scalar)
%e - excess probability
%fun - which function to use for calculation

%
%   Created in 2012 by Victoria Kostina (vkostina@caltech.edu)
%

%starting points - 'persistent' to make optimization faster
persistent x0;
persistent y0;

try

    switch lower(fun)
        case 'shannon'
            %achievability via Shannon's upper bound:
            tol = 1e-4;
            qglb = [2^(-2*R); 1];
            qgub = [1; Inf];
            options = optimset('TolX',tol, 'MaxFunEvals', 500, 'Algorithm', 'active-set');

            if isempty(x0) && isempty (y0)
                x0 = [2^(-2*R) 1];
                y0 = [(2^(-2*R)+1)/2 1]; %q gamma
            end
            out = DShannon();
        case 'normal'
            %normal approximation
            out = DNormal();
        case 'spherecoveringc'
            %converse via sphere covering
            out = DSphereCovering();
        case 'spherecoveringa'
            %achievability via sphere covering
            if isempty(x0) && isempty (y0)
                x0 = [2^(-2*R) .5]; %distortion
             %   y0 = [0 1];        %auxiliary parameter q
            end
            out = DSphereCoveringA();
        otherwise
            disp('Unknown type.')
    end

catch ME
    fprintf('Error: %s\n', ME.message);
    out = NaN;
end

%--------------------------------------------------------------------------
    function out = DShannon()
        %achievability via Shannon's upper bound:
        out = fzero(@(x)Pexcess(x) - e,x0);
        x0 = out;
        function out = Pexcess(x)
            D = x;
            [y0, Pe] = fmincon(@optqg, y0,[],[],[],[],qglb, qgub, [], options);
            out = Pe;

            function out = optqg(y)
                q = y(1);
                g = log2(y(2));
                out = PeubShannon(R, n, D, q, g);
            end
        end

    end

%--------------------------------------------------------------------------
    function out = DNormal()
        out = 2^(-2*R)*(1 + sqrt(2/n)*Qinv(e));
    end

%--------------------------------------------------------------------------
    function out = DSphereCovering()
        %converse via sphere covering

        %find rstar:
        factor = 10;
        rstar = fzero(@(x)chi2cdf(x^2*n, n) - chi2cdf(factor*n, n) + e, 1);
        out = fminbnd(@(x) abs(R - rate(x)),2^(-2*R),1);

        function out = rate(x)
            out = 1/2*log2(rstar^2/x);
        end
    end

%--------------------------------------------------------------------------
    function out = DSphereCoveringA()
        %achievability via sphere covering
        M = 2.^(n*R);
        out = fzero(@(x)Pexcess(x) - e, x0);
    function out = Pexcess(x)
        d = x;
        s0 = 1; %variance of source
        r0 = sqrt(s0^2 - d); %distance from the center of the representation sphere

        out = quad(@density, r0 - sqrt(d), r0 + sqrt(d))...
            + 1 - chi2cdf((r0 + sqrt(d))^2*n,n)...
            + chi2cdf((r0 - sqrt(d))^2*n,n);
        out = real(out);

        function out = density(r)
          cosa = (r0^2 + r.^2 - d)./(2*r0.*r);
          sina = (1 - cosa.^2).^(1/2);
          out = ub(sina).*chi2pdf(n*r.^2, n).*n*2.*r;
        end

        function out = ub(sina)
            if (n < 50)
                A = 1/sqrt(pi).*gamma(n/2+1)./n./gamma((n-1)/2+1);
                out = (1 - A.*(sina).^(n-1)).^M;
            else
                out = exp(-M*sina.^(n-1)./sqrt(2*pi*n));
            end
        end
    end

    end

end