function [uv, out] = surface_param(x, param)
% surface_param  2D parametrization of a scattered set of 3D points or of
% the vertices of a triangular mesh, for open and closed surfaces.
%
%   surface_param takes a scattered data set X in 3D that is supposed to
%   represent a sampling of a surface, and finds a 2D parameterisation U, V
%   for them. The surface can be open or closed, in which case the
%   parametrization domain will be the R^2 plane or the unit sphere,
%   respectively.
%
%   surface_param provides several parametrization methods, some
%   implemented by us, some by other authors.
%
% [UV, OUT] = surface_param(X, PARAM)
%
%   X is a 3-column matrix. Each row has the coordinates of a point that
%   belongs to the surface we want to interpolate.
%
%   UV is a 2-column matrix with the parameterisation of X, (U, V)->X. In
%   planar parameterisations, UV has units of meters. In spherical
%   parameterisations, UV=[LAT, LON], in units of radians.
%
%   OUT is a struct with the extra output from some of the parametrization
%   methods.
%
%   PARAM is a string or struct that selects the parametrization method
%   and possibly arguments for the method.
%
%     Parametrization methods summary (PARAM.type):
%
%     XY plane:
%                 'xy':       Simple projection on XY.
%                 'pca':      Projection on main PCA plane.
%                 'isomap':   Isomap (with Dijkstra's shortest path).
%                 'cmdsmap':  MDS mapping, global neighbourhood, classical
%                             MDS.
%                 'lmdscale': MDS mapping, local neighbourhood, local MDS.
%
%     Unit sphere:
%                 'sphproj':  Simple projection on sphere around centroid.
%                 'cald':     CALD method by Shen & Makedon 2006.
%                 'smdscale': Spherical extension of lmdscale.
%
%=========================== OPEN SURFACES ================================
%
%   * 'xy': Projection on the xy-plane, i.e. xy-coordinates of the points
%   in X.
%
%--------------------------------------------------------------------------
%
%   * 'pca': Projection on the plane defined by the two eigenvectors with
%     the largest eigenvalues.
%
%--------------------------------------------------------------------------
%
%   * 'isomap': Isometric mapping (Tenenbaum et al. [1][2]). This is a
%     non-linear method that can be applied to complex open surfaces that
%     bend on themselves or are rolled-up. This option uses the
%     third-party function IsomapII. Because isomap uses Dijkstra's
%     shortest-path algorithm internally, it suffers strongly from
%     metrication errors in regular meshes. On the other hand, it can be
%     applied to any adjacency graph, not only meshes.
%
%     Small neighbourhoods capture higher curvature in the surface, but
%     too small neighbourhoods create several connected components and
%     will produce an error.
%
%     PARAM.d: [Opt] Distance matrix between points. The parameter can have
%       the following formats:
%       (1) a full N x N matrix with distances between all pairs of points.
%       In this case, you can define the local neighbourhoods using
%       PARAM.neigh and PARAM.size, so that some of the connections in the
%       full matrix will be deleted and replaced by Disjkstra's
%       shortest-path lengths.
%       (2) a sparse N x N matrix (missing entries are treated as Inf). In
%       this case, you usually want to make PARAM.size=Inf, because the
%       local neighbourhood is already given by the sparse matrix. The
%       missing connections will be replaced internally by Disjkstra's
%       shortest-path lengths.
%       (3) the name of a function (e.g. 'd_fun') that takes one argument,
%       i, and returns a row vector containing the distances from all N
%       points to point i.
%
%       If not provided, PARAM.d is the full Euclidean matrix of distances
%       between every pair of points.
%
%       As a particular case, note that in case of a triangular mesh, the
%       sparse local distance that corresponds to the mesh can be computed
%       using dmatrix_mesh().
%
%     PARAM.neigh: [Opt] Type of neighbourhood ('epsilon' or 'k').
%     PARAM.size:  [Opt] Size of neighbourhood.
%
%       Each point in X is directly connected only to neighbours within a
%       distance PARAM.size (for 'epsilon'), or the PARAM.size closest
%       neighbours (for 'k'), according to the distances in PARAM.d. This
%       allows to further restrict the neighbourhoods in PARAM.d.
%
%     PARAM.options: [Opt] Extra arguments passed to IsomapII. Most users
%       won't need to change this. For details, see help to IsomapII().
%
%--------------------------------------------------------------------------
%
%   * 'cmdsmap': Open surface classical MDS mapping improved with a Fast
%     Marching method (Zigelman et al. [3]). The fast marching algorithm
%     doesn't suffer from metrication errors on regular meshes as badly as
%     Dijkstra's. On the other hand, the current implementation is limited
%     to triangular meshes, while Dijkstra's will work with any adjacency
%     graph. This option uses classical MDS on a full distance matrix, so
%     it's not valid for local neighbourhoods or closed surfaces. For local
%     neighbourhoods, see 'lmdscale' below.
%
%     PARAM.d or PARAM.tri must be provided. If PARAM.d is provided,
%     PARAM.tri and PARAM.options are ignored. Otherwise, PARAM.d is
%     computed from PARAM.tri and PARAM.options.
%
%     PARAM.d: [Req/Opt] This is expected to be the full distance matrix
%       between mesh vertices that can be computed with
%         [~, param.d] = dmatrix_mesh(param.tri, x, 'fastmarching');
%       Note that for comparison purposes, Dijkstra's algorithm can be used
%       instead of Fast Marching
%         [~, param.d] = dmatrix_mesh(param.tri, x, 'dijkstra');
%       The latter is equivalent to the 'isomap' method above.
%
%     If PARAM.d is not provided:
%
%     PARAM.tri: [Req/Opt] 3-column matrix. Each row contains the 3 nodes
%       that form one triangular facet in the mesh.
%
%     PARAM.dmethod: [Opt] String with the method to compute distances
%       between non-neighbours: 'fastmarching' (default) or 'dijkstra'. The
%       latter is faster, but suffers much more from metrication errors.
%
%     PARAM.options: [Opt] Extra arguments passed to the fast marching
%       method. From the help to perform_fast_marching_mesh():
%       - options.W: non-uniform speed.
%       - options.heuristic: heuristic that tries to guess the distance
%         that remains from a given node to a given target. This is an
%         array of same size as W.
%       - parameters that create a local neighbourhood are ignored with a
%         warning.
%
%--------------------------------------------------------------------------
%
%   * 'lmdscale': Open surface local neighbourhood MDS mapping (Schwartz,
%     Wolfson and Shaw [4, 5]), improved with a Fast Marching method
%     (Zigelman et al. [3]). This is like 'cmdsmap' above, but with local
%     neighbourhoods. Local neighbourhoods were already proposed in the
%     first publications [4, 5] on what we are calling MDS mapping. We use
%     our own implementation of this idea, lmdscale().
%
%     PARAM.d or PARAM.tri must be provided. If PARAM.d is provided,
%     PARAM.tri and PARAM.options are ignored. Otherwise, PARAM.d is
%     computed from PARAM.tri and PARAM.options.
%
%     PARAM.d: [Req/Opt] Can be a sparse or full distance matrix. Distances
%       greater than 0 reflect that two vertices are within a common local
%       neighbourhood.
%         [~, param.d] = dmatrix_mesh(param.tri, x, 'fastmarching');
%
%     If PARAM.d is not provided:
%
%     PARAM.tri: [Req/Opt] 3-column matrix. Each row contains the 3 nodes
%       that form one triangular facet in the mesh.
%
%     PARAM.dmethod: [Opt] String with the method to compute distances
%       between non-neighbours: 'fastmarching' (default) or 'dijkstra'. The
%       latter is faster, but suffers much more from metrication errors,
%       and produces only full matrices in the current implementation, so
%       it's not valid for local neighbourhoods.
%
%     PARAM.options: [Opt] Extra arguments passed to the fast marching
%       method. From the help to perform_fast_marching_mesh():
%       - options.constraint_map: Exploration radius to reduce the set of
%         explored points. Only points with current distance smaller than L
%         will be expanded. Set some entries of L to -Inf to avoid any
%         exploration of these points.
%       - options.W: non-uniform speed.
%       - options.end_points: stop when these points are reached.
%       - options.nb_iter_max: stop when a given number of iterations is
%         reached.
%       - options.heuristic: heuristic that tries to guess the distance
%         that remains from a given node to a given target. This is an
%         array of same size as W.
%
%     PARAM.options2: [Opt] Extra arguments passed to the local
%       neighbourhood MDS algorithm. See help to lmdscale().
%       - opt.MaxIter: (default = 20) maximum number of iterations we allow
%         the optimisation algorithm. Each iteration consists of an entire
%         sweep of all points on the sphere.
%       - opt.MaxInc:  inc is the Euclidean distance that each output point
%         is moved in an iteration. The algorithm will stop if no point has
%         been moved more than MaxInc.
%
%     OUT.stopCondition: cell-array with the condition/s that made the
%       algorithm stop, in string form.
%
%     OUT.err:  struct with several error measures at each algorithm
%     iteration. See help to lmdscale() for details.
%
%
%========================== CLOSED SURFACES ===============================
%
%   * 'sphproj':   Direct projection on unit sphere centered on point set
%     centroid.
%
%     OUT.medrad:  Median value of the radii of all points in the point
%       set.
%
%--------------------------------------------------------------------------
%
%   * 'smdscale': Local neighbourhood Multidimensional scaling on a sphere.
%     Our extension to lmdscale() based on method by Agarwal et al. 2010
%     [6].
%
%     PARAM.d or PARAM.tri must be provided. If PARAM.d is provided,
%     PARAM.tri and PARAM.options are ignored. Otherwise, PARAM.d is
%     computed from PARAM.tri and PARAM.options.
%
%     PARAM.d: [Req/Opt] Can be a sparse or full distance matrix. Distances
%       greater than 0 reflect that two vertices are within a common local
%       neighbourhood.
%         [~, param.d] = dmatrix_mesh(param.tri, x, 'fastmarching');
%
%     If PARAM.d is not provided:
%
%     PARAM.tri: [Req/Opt] 3-column matrix. Each row contains the 3 nodes
%       that form one triangular facet in the mesh.
%
%     PARAM.dmethod: [Opt] String with the method to compute distances
%       between non-neighbours: 'fastmarching' (default) or 'dijkstra'. The
%       latter is faster, but suffers much more from metrication errors,
%       and produces only full matrices in the current implementation, so
%       it's not valid for local neighbourhoods.
%
%     PARAM.options: [Opt] Extra arguments passed to the fast marching
%       method. From the help to perform_fast_marching_mesh():
%       - options.constraint_map: Exploration radius to reduce the set of
%         explored points. Only points with current distance smaller than L
%         will be expanded. Set some entries of L to -Inf to avoid any
%         exploration of these points.
%       - options.W: non-uniform speed.
%       - options.end_points: stop when these points are reached.
%       - options.nb_iter_max: stop when a given number of iterations is
%         reached.
%       - options.heuristic: heuristic that tries to guess the distance
%         that remains from a given node to a given target. This is an
%         array of same size as W.
%
%     PARAM.init: [Opt] Parameterization initialisation:
%       - 'sphproj' (default): Direct projection on unit sphere centered on
%         point set centroid, as the option above.
%       - 'random': Random distribution of points on the sphere. This is
%         useful if 'sphproj' produces a fold-over of points that
%         corresponds to a local minimum.
%       - [lat lon]: A 2-column matrix with the initial latitude and
%         longitude coordinates of the parameterization.
%
%     PARAM.sphrad: [Opt] Parameterization sphere's radius. If not
%       provided, it is computed as the median of the distance of the
%       points in X to their centroid.
%
%     PARAM.options2: [Opt] Extra arguments passed to the spherical local
%       neighbourhood MDS algorithm. See help to smdscale().
%       - opt.MaxIter: (default = 20) maximum number of iterations we allow
%         the optimisation algorithm. Each iteration consists of an entire
%         sweep of all points on the sphere.
%       - opt.MaxAlpha:  inc is the angular distance in radians that each
%         output point is moved in an iteration. The algorithm will stop if
%         no point has been moved more than MaxAlpha.
%
%     OUT.stopCondition: cell-array with the condition/s that made the
%       algorithm stop, in string form.
%
%     OUT.err: Error measures vs. iterations:
%       - err.rawstress: Raw stress (sigma_r), as defined above.
%       - err.stress1:   Stress-1 (sigma_1).
%         sigma_1 = sqrt(sigma_r / sum_{i,j} (d_ij)^2)
%       - err.maxalpha:  Maximum angular displacement of any point in each
%         iteration.
%       - err.medalpha:  Median angular displacement of all points in each
%         iteration.
%
%--------------------------------------------------------------------------
%
%   * 'cald': Li Shen's Control Area and Length Distortions (CALD)
%     spherical parametrization [7, 8].
%
%     PARAM.tri: [Req] 3-column matrix. Each row contains the 3 nodes that
%       form one triangular facet in the mesh.
%
%     PARAM.options: [Opt] Extra arguments passed to CALD:
%       - options.MeshGridSize: The interpolation mesh used to smooth the
%         spherical parametrization has length 2*PARAM.MeshGridSize+1. By
%         default, MeshGridSize = 50.
%       - options.MaxSPHARMDegree: Degree of the spherical harmonics used
%         for interpolation to smooth the spherical parametrization. By
%         default, MaxSPHARMDegree = 6.
%       - options.Tolerance: Scalar. In the interpolation smoothing, grid
%         points with a height larger than PARAM.Tolerance*gmin, where gmin
%         is the lowest point in the grid, will be truncated. By default,
%         Tolerance = 2.
%       - options.Smoothing: Scalar. Before parametrization smoothing,
%         parametrization triangle areas relative to triangular mesh areas
%         are smoothed by elevating to (1/PARAM.Smoothing). By default,
%         Smoothing = 2.
%       - options.Iteration: Maximum number of smoothing iterations.
%         Smoothing will stop before reaching Iteration if the algorithm
%         converges to a solution. By default, Iteration = 100.
%       - options.LocalIteration: Number of smoothing iteration in the
%         local smoothing algorithm. By default, LocalIteration = 10.
%
%--------------------------------------------------------------------------
%
% [1] J.B. Tenenbaum, V. de Silva and J.C. Langford, "A Global Geometric
% Framework for Nonlinear Dimensionality Reduction", Science 290(5500):
% 2319-2323, 2000.
%
% [2] Isomap Homepage, http://isomap.stanford.edu/
%
% [3] G. Zigelman, R. Kimmel, and N. Kiryati, "Texture mapping using
% surface flattening via multidimensional scaling,” Visualization and
% Computer Graphics, IEEE Transactions on, vol. 8, no. 2, pp. 198–207,
% 2002.
%
% [4] E. Wolfson and E.L. Schwartz, "Computing minimal distances on
% polyhedral surfaces", IEEE Pattern Analysis and Machine Intelligence,
% 11(9):1001-1005, 1989.
%
% [5] E.L. Schwartz, A. Shaw, E. Wolfson, "A numerical solution to the
% generalized mapmaker’s problem: flattening nonconvex polyhedral", IEEE
% Pattern Analysis and Machine Intelligence, 11(9):1005-1008, 1989.
%
% [6] A. Agarwal et al., "Universal Multi-Dimensional Scaling", Procs. of
% the 16th ACM SIGKDD international conference on Knowledge discovery and
% data mining (KDD '10), p. 1149-1158.
%
% [7] Shen and Makedon, "Spherical mapping for processing of 3D closed
% surfaces", Image and Vision Computing, 24(7):743–761, 2006.
% http://www.sciencedirect.com/science/article/pii/S0262885606000485
%
% [8] CALD and SPHARM-MAT homepage,
% http://www.iupui.edu/~shenlab/software.html
%
% See also: surface_tridomain, surface_interp, scimat_surface2seg.

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

% check arguments
narginchk(2, 2);

if (size(x, 2) ~= 3)
    error('X must be a 3-column matrix')
end

if ischar(param)
    aux = param; param = [];
    param.type = aux;
end

out = [];

% defaults
if (isempty(param) || ~isfield(param, 'type') ...
        || isempty(param.type))
    error('No PARAM.type provided');
end

% obtain a 2D parameterisation for the input 3D points
switch param.type

%--------------------------------------------------------------------------
% Open surface
%--------------------------------------------------------------------------

    case 'xy'

        % check arguments
        nargoutchk(0, 1);

        % (u,v) is simply (x,y)
        uv = x(:, 1:2);

%--------------------------------------------------------------------------

    case 'pca'

        % this function doesn't provide the "out" output
        nargoutchk(0, 1);

        % rotate valve points to make the valve surface as horizontal as
        % possible
        m = mean(x, 1);
        uv = x - m(ones(1, size(x, 1)), :);
        eigv = pts_pca(uv');
        uv = uv * eigv;
        uv = uv(:, 1:2);

%--------------------------------------------------------------------------

    case 'isomap'

        % check arguments
        nargoutchk(0, 1);

        % IsomapII takes the following distance matrices:
        %  (1) a full N x N matrix (as in isomap.m)
        %  (2) a sparse N x N matrix (missing entries are treated as INF)
        %  (3) the name of a function (e.g. 'd_fun') that takes
        %        one argument, i, and returns a row vector containing the
        %        distances from all N points to point i.

        % default parameters
        if (~isfield(param, 'neigh') || isempty(param.neigh))
            param.neigh = 'epsilon';
        end
        if (~isfield(param, 'size') || isempty(param.size))
            param.size = Inf;
        end

        % if distance matrix is not provided by the user, it is computed
        % internally as the full Euclidean distance matrix
        if (~isfield(param, 'd') || isempty(param.d))
            param.d = dmatrix(x', x', 'euclidean');
        end

        % compute 2-d projection of the 3-d data
        if (~isfield(param, 'options') || isempty(param.options))
            param.options.dims = 2;
            param.options.display = 0;
            param.options.overlay = 0;
            param.options.verbose = 0;
            param.options.dijkstra = 1;
        end
        uv = IsomapII(param.d, param.neigh, param.size, param.options);
        uv = uv.coords{1}';
        if (size(uv, 1) ~= size(x, 1))
            error('The neighbourhood size (param.size) is too small to connect all points')
        end

%--------------------------------------------------------------------------

    case 'cmdsmap'

        % check arguments
        nargoutchk(0, 1);

        % if distance matrix is not provided we need to compute it from the
        % mesh description
        if (~isfield(param, 'd') || isempty(param.d))

            % but if the user hasn't provided the mesh triangulation
            % either, we cannot, so we give an error
            if (~isfield(param, 'tri') || isempty(param.tri))
                error('Either PARAM.d or PARAM.tri must be provided')
            end

            % default for fast marching method options
            if (~isfield(param, 'dmethod') || isempty(param.dmethod))
                param.dmethod = 'fastmarching';
            end
            if (~isfield(param, 'options') || isempty(param.options))
                param.options = [];
            end

            % remove fast marching options that don't apply to this method
            if (isfield(param.options, 'constraint_map'))
                warning('Gerardus:InvalidInputArg', 'PARAM.options.constraint_map ignored, this method only works with a full distance matrix')
                param.options = rmfield(param.options, 'constraint_map');
            end
            if (isfield(param.options, 'end_points'))
                warning('Gerardus:InvalidInputArg', 'PARAM.options.end_points ignored, this method only works with a full distance matrix')
                param.options = rmfield(param.options, 'end_points');
            end
            if (isfield(param.options, 'nb_iter_max'))
                warning('Gerardus:InvalidInputArg', 'PARAM.options.nb_iter_max ignored, this method only works with a full distance matrix')
                param.options = rmfield(param.options, 'nb_iter_max');
            end

            % compute full distance matrix from the mesh
            [~, param.d] ...
                = dmatrix_mesh(param.tri, x, param.dmethod, param.options);

            if (issparse(param.d))
                error('Assertion fail: param.d should be a ');
            end

        end

        % if the user provided a sparse distance matrix, we assume that
        % it's a mistake
        if (issparse(param.d))
            error('PARAM.d must be a full distance matrix, not a sparse mesh adjacency-distance matrix');
        end

        % use classic MDS for the optimization
        uv = cmdscale(param.d, 2);

%--------------------------------------------------------------------------

    case 'lmdscale'

        % check arguments
        nargoutchk(0, 2);

        % if distance matrix is not provided we need to compute it from the
        % mesh description
        if (~isfield(param, 'd') || isempty(param.d))

            % but if the user hasn't provided the mesh triangulation
            % either, we cannot, so we give an error
            if (~isfield(param, 'tri') || isempty(param.tri))
                error('Either PARAM.d or PARAM.tri must be provided')
            end

            % default for distance method
            if (~isfield(param, 'options') || isempty(param.options))
                param.options = [];
            end

            % default for fast marching method options
            if (~isfield(param, 'dmethod') || isempty(param.dmethod))
                param.dmethod = 'fastmarching';
            end
            if (~isfield(param, 'options') || isempty(param.options))
                param.options = [];
            end

            % compute neighbourhood distance matrix from the mesh
            [~, param.d] ...
                = dmatrix_mesh(param.tri, x, param.dmethod, param.options);

        end

        % at this point, the distance matrix can be full or sparse, both
        % are valid, unlike in the cases above, where it had to be full

        % compute parameterization using local neighbourhood method
        [u, v, out.stopCondition, out.err] ...
            = lmdscale(param.d, [], [], param.options2);
        uv = [u, v];

%--------------------------------------------------------------------------
% Closed surface
%--------------------------------------------------------------------------

    case 'sphproj'

        % check arguments
        nargoutchk(0, 2);

        % init output
        uv = zeros(size(x, 1), 1);

        % project 3D Cartesian coordinates onto the surface of a sphere
        % lon, lat given in radians, centered around 0
        [uv(:, 2), uv(:, 1), r] = cart2sph(...
            x(:, 1) - mean(x(:, 1)), ...
            x(:, 2) - mean(x(:, 2)), ...
            x(:, 3) - mean(x(:, 3)));

        % we use the median radius of the points in the configuration as the radius
        % of the sphere that best can contains their projections
        out.medrad = median(r);

%--------------------------------------------------------------------------

    case 'smdscale'

        % check arguments
        nargoutchk(0, 2);

        % number of points to embed
        N = size(x, 1);

        % if distance matrix is not provided we need to compute it from the
        % mesh description
        if (~isfield(param, 'd') || isempty(param.d))

            % but if the user hasn't provided the mesh triangulation
            % either, we cannot, so we give an error
            if (~isfield(param, 'tri') || isempty(param.tri))
                error('Either PARAM.d or PARAM.tri must be provided')
            end

            % if no options are provided for fastmarching, we need to have
            % at least an empty field
            if (~isfield(param, 'options') || isempty(param.options))
                param.options = [];
            end

            % default for fast marching method options
            if (~isfield(param, 'dmethod') || isempty(param.dmethod))
                param.dmethod = 'fastmarching';
            end
            if (~isfield(param, 'options') || isempty(param.options))
                param.options = [];
            end

            % compute neighbourhood distance matrix from the mesh
            [~, param.d] ...
                = dmatrix_mesh(param.tri, x, param.dmethod, param.options);

        end

        % defaults
        if (~isfield(param, 'init') || isempty(param.init))
            param.init = 'sphproj';
        end

        % sMDS initialisation
        if ischar(param.init) % initial parametrization as a string

            switch param.init
                case 'sphproj'

                    % initial guess for the sphere embedding by simply
                    % projecting each point along the radius to the
                    % centroid
                    [lat, lon, sphrad] = proj_on_sphere(x);

                    % if the user didn't provide an estimate for the
                    % parameterization sphere's radius, we can use sphrad
                    if (~isfield(param, 'sphrad') || isempty(param.sphrad))
                        param.sphrad = sphrad;
                    end

                case 'random'

                    % counter-intuitively, starting from a completely
                    % random distribution usually gives good results
                    lat = (rand(N, 1) - .5) * pi;
                    lon = 2 * (rand(N, 1) - .5) * pi;

                otherwise
                    error('Not valid initialisation for spherical Isomap parameterisation')
            end

        else % initial parametrization as a point set

            if (size(param.init, 1) ~= N || size(param.init, 1) ~= 2)
                error('Initial parametrization provided, but it is not a 2-column matrix with the same number of points as X')
            end

            lat = param.init(:, 1);
            lon = param.init(:, 2);

        end

        % if the user has not provided an estimate for the parameterization
        % sphere's radius, we have to compute it internally
        if (~isfield(param, 'sphrad') || isempty(param.sphrad))
            [~, ~, param.sphrad] = proj_on_sphere(x);
        end

        % if no options are provided for the MDS algorithm, we need to have
        % at least an empty field
        if (~isfield(param, 'options2') || isempty(param.options2))
            param.options = [];
        end

        % compute the parametrization that optimises isometry
        [lat, lon, out.stopCondition, out.err] = ...
            smdscale(param.d, param.sphrad, lat, lon, param.options2);

        % create output with the parameterisation values for each point
        % lat = em(1, :)
        % lon = em(2, :)
        uv = [lat lon];

%--------------------------------------------------------------------------

    case 'cald'

        % check arguments
        nargoutchk(0, 1);

        % required parameters
        if (~isfield(param, 'tri') || isempty(param.tri))
            error('PARAM.tri must be provided')
        end

        % set up parameters for the CALD algorithm. We have to create a
        % structure with the right format. We can then replace default
        % values by values provided by the user
        param.options.vars = {'MeshGridSize', 'MaxSPHARMDegree', 'Tolerance', ...
            'Smoothing', 'Iteration', 'LocalIteration', 't_major', ...
            'SelectDiagonal', 'OutDirectory'};
        param.options.args = [1 1 1 1 1 1 10 10 200];
        param.options.inFilter = {
            '*_bim.mat;*_fix.mat;*_obj.mat' 'Binary and Mesh Objects (*_bim.mat;*_fix.mat;*_obj.mat)'
            '*_obj.mat'                     'Mesh Objects (*_obj.mat)'
            '*_bim.mat;*_fix.mat'           'Binary Objects (*_bim.mat, *_fix.mat)'
            '*.mat'                         'All Objects (*.mat)'
            };
        param.options.default = {'50'  '6'  '2'  '2'  '100'  '10'  ''  ''  './'};
        if (~isfield(param.options, 'MeshGridSize') || isempty(param.options.MeshGridSize))
            param.options.MeshGridSize = 50;
        end
        if (~isfield(param.options, 'MaxSPHARMDegree') || isempty(param.options.MaxSPHARMDegree))
            param.options.MaxSPHARMDegree = 6;
        end
        if (~isfield(param.options, 'Tolerance') || isempty(param.options.Tolerance))
            param.options.Tolerance = 2;
        end
        if (~isfield(param.options, 'Smoothing') || isempty(param.options.Smoothing))
            param.options.Smoothing = 2;
        end
        if (~isfield(param.options, 'Iteration') || isempty(param.options.Iteration))
            param.options.Iteration = 100;
        end
        if (~isfield(param.options, 'LocalIteration') || isempty(param.options.LocalIteration))
            param.options.LocalIteration = 10;
        end
        param.options.t_major = 'x';
        param.options.SelectDiagonal = 'ShortDiag';
        param.options.OutDirectory = '.';
        param.options = orderfields(param.options, ...
            {'vars', 'args', 'inFilter', 'default', ...
            'MeshGridSize', 'MaxSPHARMDegree', 'Tolerance', ...
            'Smoothing', 'Iteration', 'LocalIteration', 't_major', ...
            'SelectDiagonal', 'OutDirectory'});

        % initial parametrization
        [sph_verts, name3] ...
            = initParamCALD(x, param.tri, '', param.options);

        if (any(isnan(sph_verts)))
            error('Initial parameterization produced NaN values')
        end

        % smooth the parameterization
        [~, ~, sph_verts, new_name] ...
            = smootheCALD(x, param.tri, sph_verts, name3, param.options);

        % convert xyz coordinates to spherical coordinates
        [lon, lat] ...
            = cart2sph(sph_verts(:, 1), sph_verts(:, 2), sph_verts(:, 3));

        % delete files created by the SPHARM functions
        if exist(new_name, 'file')
            delete(new_name) % CALD_smo.mat
        end
        if exist('initParamCALD', 'file')
            rmdir('initParamCALD', 's')
        end

        % create output
        uv = [lat lon];

%--------------------------------------------------------------------------

    otherwise

        error('Parametrization method not implemented')
end


function [acre] = m22acre(m2)
% Convert area from square meters to acres.
% Chad A. Greene 2012
acre = m2*0.0002471053814672;


function M = rotmat(deg)
% M = rotmat(deg)

rads = radians(deg) ;

M = eye(3) ;
M(1,1) = cos(rads) ;
M(1,2) = sin(rads) ;
M(2,1) = -sin(rads) ;
M(2,2) = cos(rads) ;


% Fig. 5.9   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
% Script to generate Figure 5.9

clf
n=1;
d=[1 8 32 0];
rlocus(n,d)
axis([-10 6 -6 6])
hold on
x=[ 0 -2 0 -2];
y=[0  2*sqrt(3)  0 -2*sqrt(3) ];
plot(x,y)
r=roots([1 4 16]);
plot(r,'*')
title('Fig. 5.9 Locus for L=1/[s(s+4)^2+16)]')
z=0:.1:.9;
 wn=1:1:10;
 sgrid(z, wn)
 hold off


function [center,radius] = minboundcircle(x,y,hullflag)
% minboundcircle: Compute the minimum radius enclosing circle of a set of (x,y) pairs
% usage: [center,radius] = minboundcircle(x,y,hullflag)
%
% arguments: (input)
%  x,y - vectors of points, describing points in the plane as
%        (x,y) pairs. x and y must be the same size. If x and y
%        are arrays, they will be unrolled to vectors.
%
%  hullflag - boolean flag - allows the user to disable the
%        call to convhulln. This will allow older releases of
%        matlab to use this code, with a possible time penalty.
%        It also allows minboundellipse to call this code
%        efficiently.
%
%        hullflag = false --> do not use the convex hull
%        hullflag = true  --> use the convex hull for speed
%
%        default: true
%
%
% arguments: (output)
%  center - 1x2 vector, contains the (x,y) coordinates of the
%        center of the minimum radius enclosing circle
%
%  radius - scalar - denotes the radius of the minimum
%        enclosing circle
%
%
% Example usage:
%   x = randn(50000,1);
%   y = randn(50000,1);
%   tic,[c,r] = minboundcircle(x,y);toc
%
%   Elapsed time is 0.171178 seconds.
%
%   c: [-0.2223 0.070526]
%   r: 4.6358
%
%
% See also: minboundrect
%
%
% Author: John D'Errico
% E-mail: woodchips@rochester.rr.com
% Release: 1.0
% Release date: 1/10/07

% default for hullflag
if (nargin<3) || isempty(hullflag)
  hullflag = true;
elseif ~islogical(hullflag) && ~ismember(hullflag,[0 1])
  error 'hullflag must be true or false if provided'
end

% preprocess data
x=x(:);
y=y(:);

% not many error checks to worry about
n = length(x);
if n~=length(y)
  error 'x and y must be the same sizes'
end

% start out with the convex hull of the points to
% reduce the problem dramatically. Note that any
% points in the interior of the convex hull are
% never needed.
if hullflag && (n>3)
  edges = convhulln([x,y],{'QJ' 'Pp'});

  % list of the unique points on the convex hull itself
  % convhulln returns them as edges
  edges = unique(edges(:));

  % exclude those points inside the hull as not relevant
  x = x(edges);
  y = y(edges);

end

% now we must find the enclosing circle of those that
% remain.
n = length(x);

% special case small numbers of points. If we trip any
% of these cases, then we are done, so return.
switch n
  case 0
    % empty begets empty
    center = [];
    radius = [];
    return
  case 1
    % with one point, the center has radius zero
    center = [x,y];
    radius = 0;
    return
  case 2
    % only two points. center is at the midpoint
    center = [mean(x),mean(y)];
    radius = norm([x(1),y(1)] - center);
    return
  case 3
    % exactly 3 points
    [center,radius] = enc3(x,y);
    return
end

% more than 3 points.

% Use an active set strategy.
aset = 1:3; % arbitrary, but quite adequate
iset = 4:n;

% pick a tolerance
tol = 10*eps*(max(abs(mean(x) - x)) + max(abs(mean(y) - y)));

flag = true;
while flag
  % get the enclosing circle for the current set
  [center,radius] = enc3(x(aset),y(aset));

  % are all the inactive set points inside the circle?
  r = sqrt((x(iset) - center(1)).^2 + (y(iset) - center(2)).^2);
  [rmax,k] = max(r);
  if (rmax - radius) <= tol
    % the active set enclosing circle also enclosed
    % all of the inactive points.
    flag = false;
  else
    % it must be true that we can replace one member of aset
    % with iset(k). Which one?
    s1 = [aset([2 3]),iset(k)];
    [c1,r1] = enc3(x(s1),y(s1));
    if (norm(c1 - [x(aset(1)),y(aset(1))]) <= r1)
      center = c1;
      radius = r1;

      % update the active/inactive sets
      swap = aset(1);
      aset = [iset(k),aset([2 3])];
      iset(k) = swap;

      % bounce out to the while loop
      continue
    end
    s1 = [aset([1 3]),iset(k)];
    [c1,r1] = enc3(x(s1),y(s1));
    if (norm(c1 - [x(aset(2)),y(aset(2))]) <= r1)
      center = c1;
      radius = r1;

      % update the active/inactive sets
      swap = aset(2);
      aset = [iset(k),aset([1 3])];
      iset(k) = swap;

      % bounce out to the while loop
      continue
    end
    s1 = [aset([1 2]),iset(k)];
    [c1,r1] = enc3(x(s1),y(s1));
    if (norm(c1 - [x(aset(3)),y(aset(3))]) <= r1)
      center = c1;
      radius = r1;

      % update the active/inactive sets
      swap = aset(3);
      aset = [iset(k),aset([1 2])];
      iset(k) = swap;

      % bounce out to the while loop
      continue
    end

    % if we get through to this point, then something went wrong.
    % Active set problem. Increase tol, then try again.
    tol = 2*tol;

  end

end

% =======================================
%  begin subfunction
% =======================================
function [center,radius] = enc3(X,Y)
% minimum radius enclosing circle for exactly 3 points
%
% x, y are 3x1 vectors

% convert to complex
xy = X + sqrt(-1)*Y;

% just in case the points are collinear or nearly so, get
% the interpoint distances, and test the farthest pair
% to see if they work.
Dij = @(XY,i,j) abs(XY(i) - XY(j));
D12 = Dij(xy,1,2);
D13 = Dij(xy,1,3);
D23 = Dij(xy,2,3);

% Find the most distant pair. Test if their circumcircle
% also encloses the third point.
if (D12>=D13) && (D12>=D23)
  center = (xy(1) + xy(2))/2;
  radius = D12/2;
  if abs(center - xy(3)) <= radius
    center = [real(center),imag(center)];
    return
  end
elseif (D13>=D12) && (D13>=D23)
  center = (xy(1) + xy(3))/2;
  radius = D13/2;
  if abs(center - xy(2)) <= radius
    center = [real(center),imag(center)];
    return
  end
elseif (D23>=D12) && (D23>=D13)
  center = (xy(2) + xy(3))/2;
  radius = D23/2;
  if abs(center - xy(1)) <= radius
    center = [real(center),imag(center)];
    return
  end
end

% if we drop down to here, then the points cannot
% be collinear, so the resulting 2x2 linear system
% of equations will not be singular.
A = 2*[X(2)-X(1), Y(2)-Y(1); X(3)-X(1), Y(3)-Y(1)];
rhs = [X(2)^2 - X(1)^2 + Y(2)^2 - Y(1)^2; ...
       X(3)^2 - X(1)^2 + Y(3)^2 - Y(1)^2];

center = (A\rhs)';
radius = norm(center - [X(1),Y(1)]);


V = (max(newt2.Latitude) - min(newt2.Latitude)) * (max(newt2.Longitude) - min(newt2.Longitude)) * 111*111 *1000*1000 *100 *100*30*1000*100;
dt = max(newt2.Date) - min(newt2.Date);
c = sum( 10.^(1.5*newt2.Magnitude + 16.1));
Msum = 2/3*( log10(c) - 16.1 )
strain = 1/(2*3*10^11*V*dt) * c
% annual deformation in mm
def = strain*(max(newt2.Latitude) - min(newt2.Latitude))*111*1000*100*10

%bdiff2
plotfmdsA1A2
c = 0; RE = [];
dt = 1;
for m = 2:0.1:9
    N = 10^(aw-bw*m);
    c = c + N*( 10.^(1.5*m + 16.1));
    Msum = 2/3*( log10(c) - 16.1 );
    strain = 1/(2*3*10^11*V*dt) * c;
    def = strain*(max(newt2.Latitude) - min(newt2.Latitude))*111*1000*100*10;
    RE = [RE ; m Msum strain def];
end

figure
pl = plot(RE(:,1),RE(:,4),'sk');
set(gca,'Yscale','log')
set(pl,'markerfacecolor','b');
xlabel('Mw')
ylabel('annual deformation (mm)');


function coeff = calc_energy_coeff(dim,N,s,energy)
%CALC_ENERGY_COEFF Coefficient of second term in expansion of energy
%
%Syntax
% coeff = calc_energy_coeff(d,N,s,energy);
%
%Description
% COEFF = CALC_ENERGY_COEFF(dim,N,s,ENERGY) sets COEFF to be the coefficient of
% the second term of an expansion of ENERGY with the same form as the expansion
% of E(dim,N,s), the minimum r^(-s) energy of a set of N points on S^dim.
%
% Specifically, for s not equal to 0, COEFF is the solution to
%
% ENERGY == (SPHERE_INT_ENERGY(dim,s)/2) N^2 + COEFF N^(1+s/dim),
%
% and for s == 0 (the logarithmic potential), COEFF is the solution to
%
% ENERGY == (SPHERE_INT_ENERGY(dim,0)/2) N^2 + COEFF N LOG(N).
%
% The argument dim must be a positive integer.
% The argument N must be a positive integer or an array of positive integers.
% The argument ENERGY must an array of real numbers of the same array size as N.
% The result COEFF will be an array of the same size as N.
%
%Notes
% 1) The energy expansion is not valid for N == 1, and in particular,
%
% EQ_ENERGY_COEFF(dim,N,0,energy) := 0.
%
% 2) For s > 0, [KuiS98 (1.6) p524] has
%
% E(dim,N,s) == (SPHERE_INT_ENERGY(dim,s)/2) N^2 + COEFF N^(1+s/dim) + ...
%
% where SPHERE_INT_ENERGY(dim,s) is the energy integral of the r^(-s) potential
% on S^dim.
%
% The case s == 0 (logarithmic potential) can be split into subcases.
% For s == 0 and dim == 1, E(1,N,0) is obtained by equally spaced points on S^1,
% and the formula for the log potential for N equally spaced points on a circle
% gives
%
% E(1,N,0) == (-1/2) N LOG(N) exactly.
%
% For s == 0 and dim == 2, [SafK97 (4) p7] has
%
% E(2,N,0) == (SPHERE_INT_ENERGY(2,0)/2) N^2 + COEFF N LOG(N) + o(N LOG(N)).
%
% In general, for s == 0,
%
% E(dim,N,0) == (SPHERE_INT_ENERGY(dim,0)/2) N^2 + COEFF N LOG(N) + ...
%
% with sphere_int_energy(1,0) == 0.
%
% CALC_ENERGY_COEFF just uses this general formula for s == 0, so for s == 0 and
% dim == 1, the coefficient returned is actually the coefficient of the first
% non-zero term.
%
%Examples
% > N=2:6
%  N =
%       2     3     4     5     6
%
% > energy=eq_energy_dist(2,N,0)
%  energy =
%     -0.6931   -1.3863   -2.7726   -4.4205   -6.2383
%
% > calc_energy_coeff(2,N,0,energy)
%  ans =
%     -0.2213   -0.1569   -0.2213   -0.2493   -0.2569
%
%See also
% EQ_ENERGY_DIST, EQ_ENERGY_COEFF

% Copyright 2004-2005 Paul Leopardi for the University of New South Wales.
% $Revision 1.10 $ $Date 2005-06-01 $
% Documentation files renamed
% $Revision 1.00 $ $Date 2005-02-12 $
%
% For licensing, see COPYING.
% For references, see AUTHORS.
% For revision history, see CHANGELOG.

%
% Compute the energy coefficient: subtract the first term in the expansion of
% the minimum energy and divide by the power of N in the second term.
%
if s > 0
    %
    % The first term in the expansion of the minimum energy.
    % Ref: [KuiS98 (1.6) p524]
    %
    first_term = (sphere_int_energy(dim,s)/2) * N.^2;
    coeff = (energy-first_term) ./ (N.^(1+s/dim));
else
    %
    % Flatten N into a row vector.
    %
    shape = size(N);
    n_partitions = prod(shape);
    N = reshape(N,1,n_partitions);
    %
    % Refs for s==0, dim == 2:
    % [SafK97 (4) p. 7] [Zho95 (5.6) p. 68, 3.11 - corrected) p. 42]
    %
    first_term = (sphere_int_energy(dim,s)/2) * N.^2;
    %
    % Avoid division by zero.
    %
    coeff = zeros(size(N));
    neq1 = (N ~= 1);
    coeff(neq1) = (energy(neq1)-first_term(neq1)) ./ (N(neq1) .* log(N(neq1)));
    %
    % Reshape output to same array size as original N.
    %
    coeff = reshape(coeff,shape);
    %
end
%
% end function

function energy = sphere_int_energy(dim,s)
%SPHERE_INT_ENERGY Energy integral of r^(-s) potential
%
%Syntax
% energy = sphere_int_energy(d,s);
%
%Description
% ENERGY = SPHERE_INT_ENERGY(dim,s) sets ENERGY to be the energy integral
% on S^dim of the r^(-s) potential, defined using normalized Lebesgue measure.
%
% Ref for s > 0: [KuiS98 (1.6) p524]
% Ref for s == 0 and dim == 2: SafK97 (4) p. 7]
% For s == 0 and dim >= 2, integral was obtained using Maple:
%     energy = (1/2)*(omega(dim)/omega(dim+1)* ...
%          int(-log(2*sin(theta/2)*(sin(theta))^(dim-1),theta=0..Pi),
%     where omega(dim+1) == area_of_sphere(dim).

if s ~= 0
    energy = (gamma((dim+1)/2)*gamma(dim-s)/(gamma((dim-s+1)/2)*gamma(dim-s/2)));
elseif dim ~= 1
    energy = (psi(dim)-psi(dim/2)-log(4))/2;
else
    energy = 0;
end
%
% end function


%
% Usage:  V=mexSparseProject(U,param);
%
% Name: mexSparseProject
%
% Description: mexSparseProject solves various optimization
%     problems, including projections on a few convex sets.
%     It aims at addressing the following problems
%     for all columns u of U in parallel
%       1) when param.mode=1 (projection on the l1-ball)
%           min_v ||u-v||_2^2  s.t.  ||v||_1 <= thrs
%       2) when param.mode=2
%           min_v ||u-v||_2^2  s.t. ||v||_2^2 + lamuda1||v||_1 <= thrs
%       3) when param.mode=3
%           min_v ||u-v||_2^2  s.t  ||v||_1 + 0.5lamuda1||v||_2^2 <= thrs
%       4) when param.mode=4
%           min_v 0.5||u-v||_2^2 + lamuda1||v||_1  s.t  ||v||_2^2 <= thrs
%       5) when param.mode=5
%           min_v 0.5||u-v||_2^2 + lamuda1||v||_1 +lamuda2 FL(v) + ...
%                                                   0.5lamuda_3 ||v||_2^2
%          where FL denotes a "fused lasso" regularization term.
%       6) when param.mode=6
%          min_v ||u-v||_2^2 s.t lamuda1||v||_1 +lamuda2 FL(v) + ...
%                                             0.5lamuda3||v||_2^2 <= thrs
%
%        When param.pos=true and param.mode <= 4,
%        it solves the previous problems with positivity constraints
%
% Inputs: U:  double m x n matrix   (input signals)
%               m is the signal size
%               n is the number of signals to project
%         param: struct
%           param.thrs (parameter)
%           param.lambda1 (parameter)
%           param.lambda2 (parameter)
%           param.lambda3 (parameter)
%           param.mode (see above)
%           param.pos (optional, false by default)
%           param.numThreads (optional, number of threads for exploiting
%             multi-core / multi-cpus. By default, it takes the value -1,
%             which automatically selects all the available CPUs/cores).
%
% Output: V: double m x n matrix (output matrix)
%
% Note: this function admits a few experimental usages, which have not
%     been extensively tested:
%         - single precision setting
%
% Author: Julien Mairal, 2009


function value = r4_asin ( x )

%*****************************************************************************80
%
%% R4_ASIN evaluates the arc-sine of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the arc-sine of X.
%
  persistent asincs
  persistent nterms
  persistent sqeps

  if ( isempty ( nterms ) )
    asincs = [ ...
      0.10246391753227159, ...
      0.054946487221245833, ...
      0.004080630392544969, ...
      0.000407890068546044, ...
      0.000046985367432203, ...
      0.000005880975813970, ...
      0.000000777323124627, ...
      0.000000106774233400, ...
      0.000000015092399536, ...
      0.000000002180972408, ...
      0.000000000320759842, ...
      0.000000000047855369, ...
      0.000000000007225128, ...
      0.000000000001101833, ...
      0.000000000000169476, ...
      0.000000000000026261, ...
      0.000000000000004095, ...
      0.000000000000000642, ...
      0.000000000000000101, ...
      0.000000000000000016 ]';
    nterms = r4_inits ( asincs, 20, 0.1 * r4_mach ( 3 ) );
    sqeps = sqrt ( 6.0 * r4_mach ( 3 ) );
  end

  y = abs ( x );

  if ( x < - 1.0 - sqeps )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_ASIN - Fatal error!\n' );
    fprintf ( 1, '  X < - 1.0\n' );
    error ( 'R4_ASIN - Fatal error!' )

  elseif ( x < - 1.0 )

    value = - 0.5 * pi;

  elseif ( x < 1.0 )

    z = 0.0;
    if ( sqeps < y )
      z = y * y;
    end

    if ( z <= 0.5 )
      value = x * ( 1.0 + r4_csevl ( 4.0 * z - 1.0, asincs, nterms ) );
    else
      value = 0.5 * pi - sqrt ( 1.0 - z ) * ( 1.0 + ...
        r4_csevl ( 3.0 - 4.0 * z, asincs, nterms ) );
    end

    if ( x < 0.0 )
      value = - abs ( value );
    elseif ( 0.0 < x )
      value = + abs ( value );
    end

  elseif ( x < 1.0 + sqeps )

    value = 0.5 * pi;

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_ASIN - Fatal error!\n' );
    fprintf ( 1, '  1.0 < X\n' );
    error ( 'R4_ASIN - Fatal error!' )

  end

  return
end


function alt = triaxial(rsc)

% geodetic altitude relative
% to a triaxial ellipsoid

% input

%  rsc = geocentric position vector (kilometers)

% output

%  alt = geodetic altitude (kilometers)

% Orbital Mechanics with Matlab

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global flat cx cy cz vcon ucon caxis

% semi-axes

aaxis = 6378.138;
baxis = 6367.0;
caxis = aaxis * (1 - flat);

ucon = aaxis * aaxis - caxis * caxis;

vcon = baxis * baxis - caxis * caxis;

cx = (aaxis * caxis * rsc(1)) ^ 2;
cy = (baxis * caxis * rsc(2)) ^ 2;
cz = caxis ^ 2 * rsc(3);

g = sqrt((rsc(1) / aaxis) ^ 2 + (rsc(2) / baxis) ^ 2 + ...
         (rsc(3) / caxis) ^ 2);

zp0 = rsc(3) / g;

% bracketing interval

zp1 = zp0 - 10;

zp2 = zp0 + 10;

rtol = 1.0e-8;

zp = brent('trifunc', zp1, zp2, rtol);

xp = (aaxis ^ 2 * rsc(1) * zp) / (caxis ^ 2 * rsc(3) + ucon * zp);

yp = (baxis ^ 2 * rsc(2) * zp) / (caxis ^ 2 * rsc(3) + vcon * zp);

dx = rsc(1) - xp;
dy = rsc(2) - yp;
dz = rsc(3) - zp;

alt = sqrt(dx ^ 2 + dy ^ 2 + dz ^ 2);


function Jb = JacobianBody_sym(Blist, thetalist)
% *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS ***
% Takes Blist: The joint screw axes in the end-effector frame when the
%              manipulator is at the home position, in the format of a
%              matrix with the screw axes as the columns,
%       thetalist: A list of joint coordinates.
% Returns the corresponding body Jacobian (6xn real numbers).
% Example Input:
%
% clear; clc;
% Blist = [[0; 0; 1;   0; 0.2; 0.2], ...
%        [1; 0; 0;   2;   0;   3], ...
%        [0; 1; 0;   0;   2;   1], ...
%        [1; 0; 0; 0.2; 0.3; 0.4]];
% thetalist = [0.2; 1.1; 0.1; 1.2];
% Jb = JacobianBody(Blist, thetalist)
%
% Output:
% Jb =
%   -0.0453    0.9950         0    1.0000
%    0.7436    0.0930    0.3624         0
%   -0.6671    0.0362   -0.9320         0
%    2.3259    1.6681    0.5641    0.2000
%   -1.4432    2.9456    1.4331    0.3000
%   -2.0664    1.8288   -1.5887    0.4000

Jb = sym(Blist);
T = eye(4);
for i = length(thetalist) - 1: -1: 1
    T = T * simplify(MatrixExp6_sym(VecTose3(Blist(:, i + 1)),-1*thetalist(i + 1)));
	Jb(:, i) = Adjoint(T) * Blist(:, i);
end
end


function lagrange_nd_test10 ( )

%*****************************************************************************80
%
%% LAGRANGE_ND_TEST10 tests LAGRANGE_PARTIAL2 in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LAGRANGE_ND_TEST10\n' );
  fprintf ( 1, '  LAGRANGE_PARTIAL2 determines\n' );
  fprintf ( 1, '  the Lagrange interpolating polynomials L(x)\n' );
  fprintf ( 1, '  for ND points in D dimensions, assuming that\n' );
  fprintf ( 1, '  the number of points is less than or equal to\n' );
  fprintf ( 1, '  R = Pi(D,N), the number of monomials of degree N or less\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  For this example, the data points are the same as those\n' );
  fprintf ( 1, '  used by the level 2 Clenshaw Curtis sparse grid in 2D.\n' );

  d = 2;
  n = 4;
  r = mono_upto_enum ( d, n );
  nd = 13;
  xd = [ 0.0,  0.0;
        -1.0,  0.0;
         1.0,  0.0,
         0.0, -1.0,
         0.0,  1.0,
        -1.0,  1.0,
         1.0,  1.0,
        -1.0, -1.0,
         1.0, -1.0,
        -0.5,  0.0,
         0.0, -0.5,
         0.0, +0.5,
         0.5,  0.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension D = %d\n', d );
  fprintf ( 1, '  Maximum degree N = %d\n', n );
  fprintf ( 1, '  Number of monomials R = %d\n', r );
  fprintf ( 1, '  Number of data points ND = %d\n', nd );

  r8mat_transpose_print ( d, nd, xd, '  Data points XD:' );

  [ po, pc, pe ] = lagrange_partial2 ( d, n, r, nd, xd );
%
%  Print the polynomials.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Lagrange polynomials for XD data points:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : nd
    o = po(i);
    label = sprintf ( '  P(%d)(x) =', i );
    polynomial_print ( d, o, pc(i,1:o), pe(i,1:o), label );
  end
%
%  Evaluate the polynomials at XD.
%
  value = zeros ( nd, nd );

  for j = 1 : nd
    o = po(j);
    label = sprintf ( '  P(%d)(x) =', j );
    value(1:nd,j) = polynomial_value ( d, o, pc(j,1:o), pe(j,1:o), nd, xd );
  end

  err = r8mat_is_identity ( nd, value );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm of Lagrange matrix error = %g\n', err );
%
%  Now evaluate a function at the data points.
%
  pd = zeros ( nd, 1 );
  for i = 1 : nd
    pd(i) = sin ( xd(1,i) ) * cos ( xd(2,i) );
  end
%
%  Compare exact function and interpolant at a grid of points.
%
  ni = 11 * 11;
  xyi = zeros ( 2, ni );

  k = 0;
  for j = 1 : 11
    for i = 1 : 11
      k = k + 1;
      xyi(1,k) = ( ( 11 - i     ) * ( - 1.0 )   ...
                 + (      i - 1 ) * ( + 1.0 ) ) ...
                 / ( 11     - 1 );
      xyi(2,k) = ( ( 11 - j     ) * ( - 1.0 )   ...
                 + (      j - 1 ) * ( + 1.0 ) ) ...
                 / ( 11     - 1 );
    end
  end

  pn = nd;
  zi = interpolant_value ( d, r, pn, po, pc, pe, pd, ni, xyi );

  err = 0.0;
  for k = 1 : ni
    f = sin ( xyi(1,k) ) * cos ( xyi(2,k) );
    err = max ( err, abs ( zi(k) - f ) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Maximum absolute interpolant error on 11x11 grid = %g\n', err );

%
%  Plot the interpolant at a grid of points.
%
%  Reshuffling the shapes of MATLAB vectors is my definition of hell.
%
  x1 = linspace ( -1, +1, 11 );
  y1 = linspace ( -1, +1, 11 );
  [ x2, y2 ] = meshgrid ( x1, y1 );
  x3 = reshape ( x2, 1, 11 * 11 );
  y3 = reshape ( y2, 1, 11 * 11 );
  xyi = [ x3; y3 ];
  ni = 11 * 11;
  zi = interpolant_value ( d, r, pn, po, pc, pe, pd, ni, xyi );
  z2 = reshape ( zi, 11, 11 );

  surf ( x2, y2, z2 );
  title ( 'Lagrange interpolant to sin(x)*cos(y)', 'FontSize', 16 )
  xlabel ( '<---X--->', 'FontSize', 16 )
  ylabel ( '<---Y--->', 'FontSize', 16 )
  zlabel ( '<---F(X,Y)--->', 'FontSize', 16 )

  filename = 'test10_interpolant.png';
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saving plot file in "%s"\n', filename );

  return
end


function [sY] = smooth2(Y,va)
% smooth 2D image with gaussian kernel
% function [sY] = smooth2(Y,va)
% IN:
%   - Y: the input image
%   - va: the smoothing value

if ~exist('va','var') || isempty(va)
    va = 0;
end

if va > 0
    nv = 4;
    w = zeros(2*nv+1);
    for i=-nv:nv
        for j=-nv:nv
            w(i+nv+1,j+nv+1) = exp(-0.5.*(i.^2+j.^2)/va);
        end
    end
    w = w./sum(w(:));
    sY = conv2(Y,w,'same');
else
    sY = Y;
end


function theta = polyhedronNormalAngle(varargin)
%POLYHEDRONNORMALANGLE Compute normal angle at a vertex of a 3D polyhedron.
%
%   THETA = polyhedraNormalAngle(NODES, EDGES, FACES, IND);
%   THETA = polyhedraNormalAngle(NODES, FACES, IND);
%   where NODES is a set of 3D points, and FACES a set of faces, whose
%   elements are indices to NODES array, compute the normal angle at the
%   vertex whose index is given by IND.
%
%   THETA = polyhedraNormalAngle(GRAPH, IND);
%   Uses a graph structure. GRAPH should contain at least fields : 'nodes'
%   and 'faces'.
%
%   Example :
%   % create a simple (irregular) tetrahedra
%   nodes = [0 0 0;1 0 0;0 1 0;0 0 1];
%   faces = [1 2 3;1 2 4;1 3 4;2 3 4];
%   % compute normal angle at each vertex
%   theta = polyhedronNormalAngle(nodes, faces, 1:size(nodes, 1));
%   % sum of normal angles should be equal to 4*pi :
%   sum(theta)
%
%   To-do
%       Works only for polyhedra with convex faces!!!
%
%   See also
%   polyhedra, polygon3dNormalAngle

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2005-11-30
% Copyright 2005-2022 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas)

if length(varargin)==4
    nodes = varargin{1};
    faces = varargin{3};
    ind   = varargin{4};

elseif length(varargin)==3
    nodes = varargin{1};
    faces = varargin{2};
    ind   = varargin{3};

elseif length(varargin)==2
    graph = varargin{1};
    nodes = graph.nodes;
    faces = graph.faces;
    ind   = varargin{2};
else
    error('wrong number of arguments');
end

% number of angles to compute
na = length(ind);

theta = zeros(na, 1);
for i=1:na

    thetaf = [];

    % find faces containing given vertex,
    % and compute normal angle at each face containing vertex
    if iscell(faces)
        for j=1:length(faces)
            if ismember(ind(i), faces{j})
                % create 3D polygon
                face = nodes(faces{j}, :);

                % index of point in polygon
                indp = find(faces{j}==i);

                % compute normal angle of vertex
                thetaf = [thetaf polygon3dNormalAngle(face, indp)]; %#ok<AGROW>
            end
        end
    else
        indf = find(sum(ismember(faces, ind(i)), 2));

        thetaf = zeros(length(indf), 1);
        for j=1:length(indf)
            ind2 = faces(indf(j), :);
            face = nodes(ind2, :);
            indp = find(ind2==ind(i));
            thetaf(j) = pi - polygon3dNormalAngle(face, indp);
        end
    end

    % compute normal angle of polyhedron, by use of angle defect formula
    theta(i) = 2*pi - sum(thetaf);

end


function [pos, tri] = octahedron(varargin)

% OCTAHEDRON
%
% Use as
%   [pos tri] = octahedron;
%
% See also TETRAHEDRON ICOSAHEDRON

pos = [
  0  0 +1
  +1  0  0
  0 +1  0
  -1  0  0
  0 -1  0
  0  0 -1
  ];

tri = [
  1 2 3
  1 3 4
  1 4 5
  1 5 2
  6 3 2
  6 4 3
  6 5 4
  6 2 5
  ];

if nargin>0
  n = varargin{1};
  % perform an n-fold refinement
  for i=1:n
    [pos, tri] = refine(pos, tri, 'banks');
  end
  % scale all vertices to the unit sphere
  pos = pos ./ repmat(sqrt(sum(pos.^2,2)), 1,3);
end


function mse = eval_MTL_mse (Y, X, W)
%% FUNCTION eval_MTL_mse
%   computation of mean squared error given a specific model.
%   the value is the lower the better.
%
%% FORMULATION
%
%  multi-task mse = sum_t (mse(t) * N_t) / sum_t N_t
%
%  where
%     mse(t) = sum((Yt_pred - Y{t})^2))/ N_t
%     Yt_pred = X{t} * W(:, t)
%     N_t     = length(Y{t})
%
%  which can be simplified as:
%
%  multi-task mse = sum_t sum((Yt_pred - Y{t})^2)) / sum_t N_t
%
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   percent: percentage of the splitting range (0, 1)
%
%% OUTPUT
%   X_sel: the split of X that has the specifid percent of samples
%   Y_sel: the split of Y that has the specifid percent of samples
%   X_res: the split of X that has the 1-percent of samples
%   Y_res: the split of Y that has the 1-percent of samples
%   selIdx: the selection index of for X_sel and Y_sel for each task
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on Jan 6, 2016.
%
    task_num = length(X);
    mse = 0;

    total_sample = 0;
    for t = 1: task_num
        y_pred = X{t} * W(:, t);
        %mse = mse + (sum((y_pred - Y{t}).^2)/length(y_pred)) * length(y_pred);
        mse = mse + sum((y_pred - Y{t}).^2); % length of y cancelled.
        total_sample = total_sample + length(y_pred);
    end
    mse = mse/total_sample;
end


function pass = test_rhs( pref )
% Check that non-zero righthand forcing terms are being dealt with
% correctly.
% Alex Townsend, March 2013.

if ( nargin < 1 )
    pref = chebfunpref();
end
tol = 100*pref.cheb2Prefs.chebfun2eps;

d = [0 pi 0 pi];
N = chebop2(@(u) laplacian(u), d);
N.lbc = 0;
N.rbc = @(y) pi*y.^3./6;
N.dbc = 0;
N.ubc = @(x) x*pi^3/6 + sin(x).*sinh(pi);

rhs = chebfun2(@(x,y) x.*y, d);

u = N \ rhs;

exact = chebfun2(@(x,y) x.*y.^3/6 + sin(x).*sinh(y), d);

pass(1) = ( norm(u - exact) < 10*tol );

d = [0 pi 0 pi];
N = chebop2(@(u) laplacian(u), d);
N.lbc = 0;
N.rbc = @(y) pi^4*y./12 + sinh(pi)*(cos(y) + sin(y));
N.dbc = @(x) sinh(x);
N.ubc = @(x) pi*x.^4/12 - sinh(x);

rhs = chebfun2(@(x,y) x.^2.*y, d);

u = N \ rhs;

exact = chebfun2(@(x,y) x.^4.*y./12 + sinh(x).*(cos(y) + sin(y)), d);

pass(2) = ( norm(u - exact) < 10*tol );

% right hand side when we are on a rectangular domain.
% real part of analytic plus forcing term
d = [0 pi 0 1];
exact = chebfun2(@(x,y) real(exp(x+1i*y)) + x.^3.*y.^3, d);
N = chebop2(@(u) laplacian(u), d);
N.lbc = exact(d(1),:);
N.rbc = exact(d(2),:);
N.dbc = exact(:,d(3));
N.ubc = exact(:,d(4));

u = N \ chebfun2(@(x,y) 6*x.*y.^3  + 6*y.*x.^3, d);

pass(3) = ( norm(u - exact) < 10*tol );


function [MATreordered,MATindices,MATcost] = reorderMAT(MAT,H,cost)
%REORDERMAT         Reorder matrix for visualization
%
%   [MATreordered,MATindices,MATcost] = reorderMAT(MAT,H,cost);
%
%   This function reorders the connectivity matrix in order to place more
%   edges closer to the diagonal. This often helps in displaying community
%   structure, clusters, etc.
%
%   Inputs:     MAT,            connection matrix
%               H,              number of reordering attempts
%               cost,           'line' or 'circ', for shape of lattice
%                               (linear or ring lattice)
%
%               MATreordered    reordered connection matrix
%               MATindices      reordered indices
%               MATcost         cost of reordered matrix
%
%
%   Olaf Sporns, Indiana University

N = length(MAT);
diagMAT = diag(diag(MAT));
MAT = MAT-diagMAT;

% generate cost function
if strcmp(cost,'line');
    profil = fliplr(normpdf(1:N,0,N/2));
end;
if strcmp(cost,'circ');
    profil = fliplr(normpdf(1:N,N/2,N/4));
end;
COST = toeplitz(profil,profil);

% initialize lowCOST
lowMATcost = sum(sum(COST.*MAT));

% keep track of starting configuration
MATstart = MAT;
starta = 1:N;

% reorder
for h=1:H
    a = 1:N;
    % choose two positions at random and flip them
    r = randperm(N);
    a(r(1)) = r(2);
    a(r(2)) = r(1);
    MATcostnew = sum(sum(MAT(a,a).*COST));
    if (MATcostnew < lowMATcost)
        MAT = MAT(a,a);
        r2 = starta(r(2));
        r1 = starta(r(1));
        starta(r(1)) = r2;
        starta(r(2)) = r1;
        lowMATcost = MATcostnew;
    end;
end;	% h

MATreordered = MATstart(starta,starta) + diagMAT(starta,starta);
MATindices = starta;
MATcost = lowMATcost;


function output = linear_transform_animation(data,A)
% data = N x 2 matrix
% A = 2 x 2 matrix
% output = N x 2 matrix

dot_colors = jet(size(data,1));

output = transpose(A*data');

minmin = min(min(data));
maxmax = max(max(data));

xy_min = -max(abs(minmin),abs(maxmax));
xy_max = max(abs(minmin),abs(maxmax));

figure;
set(gcf,'position',[316.2000  215.4000  904.8000  420.0000]);
subplot(1,2,1);
scatter(data(:,1),data(:,2),30,dot_colors,'filled')
grid on;
xlim([xy_min, xy_max]); ylim([xy_min, xy_max]);
set(gcf,'color','white');

subplot(1,2,2);
scatter(data(:,1),data(:,2),30,dot_colors,'filled')
grid on;
xlim([xy_min, xy_max]); ylim([xy_min, xy_max]);
set(gcf,'color','white');

n_steps = 100;
for i_steps = 1:n_steps
    step_mtx = (A-eye(2))/n_steps*i_steps;
    new_xy = (eye(2)+step_mtx)*[data(:,1), data(:,2)]';
    scatter(new_xy(1,:), new_xy(2,:),30,dot_colors,'filled')
    grid on;
    xlim([xy_min, xy_max]); ylim([xy_min, xy_max]);
    set(gcf,'color','white')
    pause(0.001);
end


function [ cx, cy ] = csrot ( n, cx, incx, cy, incy, c, s )

%*****************************************************************************80
%
%% CSROT applies a complex plane rotation.
%
%  Discussion:
%
%    The cosine C and sine S are real and the vectors CX and CY are complex.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 April 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, complex CX(*), one of the vectors to be rotated.
%
%    Input, integer INCX, the increment between successive entries of CX.
%
%    Input, complex CY(*), one of the vectors to be rotated.
%
%    Input, integer INCY, the increment between successive elements of CY.
%
%    Input, real C, S, parameters (presumably the cosine and sine of
%    some angle) that define a plane rotation.
%
%    Output, complex CX(*), one of the vectors to be rotated.
%
%    Output, complex CY(*), one of the vectors to be rotated.
%
  temp(1:n)               =   c * cx(1:incx:1+(n-1)*incx) ...
                            + s * cy(1:incy:1+(n-1)*incy);

  cy(1:incy:1+(n-1)*incy) = - s * cx(1:incx:1+(n-1)*incx) ...
                            + c * cy(1:incy:1+(n-1)*incy);

  cx(1:incx:1+(n-1)*incx) = temp(1:n);

  return
end


% Mathematics Q2706108
% https://math.stackexchange.com/questions/2706108
% Lasso ADMM with Positive Constraint
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     25/03/2018
%   *   First release.

%% General Parameters

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Simulation Parameters

paramLambda = 0.5;

numRows = 12;
numCols = 8;

numIterations   = 250;
stepSize        = 0.0075;

%% Generate Data

mA = randn([numRows, numCols]);
vB = 10 * randn([numRows, 1]);

hObjFun = @(vX) (0.5 * sum(((mA * vX) - vB) .^ 2)) + (paramLambda * norm(vX, 1));
hL2SubGrad = @(vX) vX ./ max(norm(vX, 2), 1e-9);

hSoftThresholdL1 = @(vX, paramLambda) sign(vX) .* max(abs(vX) - paramLambda, 0);
hProjectRPlus = @(vX, paramLambda) max(vX, 0);

%% Solution by CVX

cvx_begin('quiet')
    cvx_precision('best');
    variable vX(numCols)
    minimize( (0.5 * square_pos(norm((mA * vX) - vB, 2))) + (paramLambda * norm(vX, 1)) );
    subject to
        vX >= 0;
cvx_end

disp([' ']);
disp(['CVX Solution Summary']);
disp(['The CVX Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(cvx_optval)]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX.'), ' ]']);
disp([' ']);

%% Solution by Sub Gradient Method

vObjValSgm = zeros([numIterations, 1]);

vX = zeros([numCols, 1]);
vObjValSgm(1) = hObjFun(vX);

mAA = mA.' * mA;
vAb = mA.' * vB;

for ii = 2:numIterations
    vG = (mAA * vX) - vAb + (paramLambda * sign(vX));
    vX = vX - (stepSize * vG);
    vX = max(vX, 0); %<! Project onto R+

    vObjValSgm(ii) = hObjFun(vX);
end

disp([' ']);
disp(['Sub Gradient Method Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(vObjValSgm(numIterations))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX.'), ' ]']);
disp([' ']);

%% Solution Proximal Projected Descent

vObjValPgm = zeros([numIterations, 1]);

[vX, mX] = SolveLsL1Prox(mA, vB, paramLambda, numIterations);

for ii = 1:numIterations
    vObjValPgm(ii) = hObjFun(mX(:, ii));
end

disp([' ']);
disp(['Proximal Gradient Method Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(vObjValPgm(numIterations))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX.'), ' ]']);
disp([' ']);

%% Solution by ADMM

vObjValAdmm = zeros([numIterations, 1]);

[vX, mX] = SolveLsL1Admm(mA, vB, paramLambda, numIterations);

for ii = 1:numIterations
    vObjValAdmm(ii) = hObjFun(mX(:, ii));
end

disp([' ']);
disp(['ADMM Method Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(vObjValAdmm(numIterations))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX.'), ' ]']);
disp([' ']);

%% Display Reesults

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);
hAxes       = axes();
hLineSeries = plot([1:numIterations], [cvx_optval * ones([numIterations, 1]), vObjValSgm, vObjValPgm, vObjValAdmm]);
set(hLineSeries, 'LineWidth', lineWidthNormal);
set(hLineSeries(2:end), 'LineStyle', ':');
set(get(hAxes, 'Title'), 'String', ['Least Squares with {L}_{1} Regularization (LASSO) with Positivity Constraint'], ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', 'Iteration Index', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', 'Objective Function Value', ...
    'FontSize', fontSizeAxis);
hLegend = ClickableLegend({['CVX'], ['Sub Gradient Method'], ['Proximal Gradient Method'], ['ADMM']});
set(hAxes, 'LooseInset', [0.07, 0.07, 0.07, 0.07]);

if(generateFigures == ON)
    saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
end

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function truncated_normal_b_cdf_test ( )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_B_CDF_TEST tests TRUNCATED_NORMAL_AB_CDF;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRUNCATED_NORMAL_B_CDF_TEST\n' );
  fprintf ( 1, '  TRUNCATED_NORMAL_B_CDF evaluates the upper Truncated Normal CDF.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The "parent" normal distribution has\n' );
  fprintf ( 1, '    mean = mu\n' );
  fprintf ( 1, '    standard deviation = sigma\n' );
  fprintf ( 1, '  The parent distribution is truncated to\n' );
  fprintf ( 1, '  the interval (-oo,b]\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '                                                  Stored         Computed\n' );
  fprintf ( 1, '       X        Mu         S         B             CDF             CDF\n' );
  fprintf ( 1, '\n');

  n_data = 0;

  while ( 1 )

    [ n_data, mu, sigma, b, x, cdf1 ] ...
      = truncated_normal_b_cdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    cdf2 = truncated_normal_b_cdf ( x, mu, sigma, b );

    fprintf( 1, '  %8.1f  %8.1f  %8.1f  %8.1f  %14g  %14g\n', x, mu, sigma, b, cdf1, cdf2 );

  end

  return
end


function y = perform_thresholding(x, t, type)

% perform_thresholding - perform hard or soft thresholding
%
%   y = perform_thresholding(x, t, type);
%
%   type is either 'hard' or 'soft' or 'semisoft'
%   t is the threshold
%
%   works also for complex data, and for cell arrays.
%
%   if type is 'strict' then it keeps the t largest entry in each
%   column of x.
%
%   Copyright (c) 2006 Gabriel Peyre

if nargin<3
    type = 'hard';
end

if iscell(x)
    % for cell arrays
    for i=1:size(x,1)
        for j=1:size(x,2)
            y{i,j} = perform_thresholding(x{i,j},t, type);
        end
    end
    return;
end

switch lower(type)
    case {'hard', ''}
        y = perform_hard_thresholding(x,t);
    case 'soft'
        y = perform_soft_thresholding(x,t);
    case 'semisoft'
        y = perform_semisoft_thresholding(x,t);
    case 'strict'
        y = perform_strict_thresholding(x,t);
    otherwise
        error('Unkwnown thresholding type.');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function X = perform_strict_thresholding(X,s)
%% keep only the s largest coefficients in each column of X

v = sort(abs(X)); v = v(end:-1:1,:);
v = v(round(s),:);
v = repmat(v, [size(X,1) 1]);
X = X .* (abs(X)>=v);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function y = perform_hard_thresholding(x,t)

t = t(1);
y = x .* (abs(x) > t);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function y = perform_soft_thresholding(x,t)

if not(isreal(x))
    % complex threshold
    d = abs(x);
    d(d<eps) = 1;
    x = x./d .* perform_soft_thresholding(d,t);
end

t = t(1);
s = abs(x) - t;
s = (s + abs(s))/2;
y = sign(x).*s;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function y = perform_semisoft_thresholding(x,t)

if length(t)==1
    t = [t 2*t];
end
t = sort(t);

y = x;
y(abs(x)<t(1)) = 0;
I = find(abs(x)>=t(1) & abs(x)<t(2));
y( I ) = sign(x(I)) .* t(2)/(t(2)-t(1)) .* (abs(x(I))-t(1));


% a tutorial adapted from the fipy diffusion 1D example
% see: http://www.ctcms.nist.gov/fipy/examples/diffusion/index.html

% Written by Ali A. Eftekhari
% Last checked: June 2021
clc

%% define the domain
L = 5;  % domain length
Nx = 100; % number of cells
meshstruct = createMesh1D(Nx, L);
BC = createBC(meshstruct); % all Neumann boundary condition structure
BC.left.a(:) = 0; BC.left.b(:)=1; BC.left.c(:)=1; % left boundary
BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=0; % right boundary
x = meshstruct.cellcenters.x;
%% define the transfer coeffs
D_val = 1;
alfa = createCellVariable(meshstruct, 1);
Dave = createFaceVariable(meshstruct, D_val);
%% define initial values
c_old = createCellVariable(meshstruct, 0, BC); % initial values
c = c_old;
%% loop
dt = 0.001; % time step
final_t = 0.5;
for t=dt:dt:final_t
    % step 1: calculate divergence term
    RHS = divergenceTerm(Dave.*gradientTerm(c_old));
    % step 2: calculate the new value for internal cells
    c_old_int=internalCells(c_old);
    c_val_int = dt*excludeGhostRHS(meshstruct, RHS+constantSourceTerm(c_old))+c_old_int(:);
    c_analytical = 1-erf(x/(2*sqrt(D_val*t)));
    figure(1);plot(x, c_val_int, x, c_analytical, 'r--');drawnow;
    % Step 3: calculate the ghost cell values
    c.value=cellBoundary(c_val_int, BC);
    c_old = c;
    disp(t)
end


function w = composite_weights ( order )

%*****************************************************************************80
%
%% COMPOSITE_WEIGHTS computes weights for a composite midpoint rule.
%
%  Discussion:
%
%     Right now, our composite rule is assumed to be a composition of
%     1 point rules, applied to the interval [-1,1], with equal spacing.
%
%     Except for the rule of order 1, the endpoints are included
%     as abscissas.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ORDER, the order of the rule.
%
%    Output, real W(ORDER), the weights of the rule.
%
  a = -1.0;
  b = +1.0;

  if ( order == 1 )
    w(1) = b - a;
  else
    w(1)         = ( b - a ) / ( order - 1 ) / 2.0;
    w(2:order-1) = ( b - a ) / ( order - 1 );
    w(order)     = ( b - a ) / ( order - 1 ) / 2.0;
  end

  return
end


function n = square_arbq_size ( degree )

%*****************************************************************************80
%
%% SQUARE_ARBQ_SIZE: size of quadrature rule for the symmetric square.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 July 2014
%
%  Author:
%
%    John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the desired degree of exactness.
%    1 <= DEGREE <= 30.
%
%    Output, integer N, the number of points in the
%    corresponding rule.
%
  n_save = [ ...
      1,   3,   4,   6,   7, ...
     10,  12,  16,  17,  22, ...
     24,  31,  33,  41,  44, ...
     52,  55,  64,  68,  78, ...
     82,  93,  98, 109, 115, ...
    127, 132, 147, 152, 167 ];

  if ( degree < 1 | 30 < degree )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SQUARE_ARBQ_SIZE - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of DEGREE.\n' );
    error ( 'SQUARE_ARBQ_SIZE - Fatal error!' );
  end

  n = n_save ( degree );

  return
end


% test_dijkstra.m

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Basic Dijkstra

% distance matrix to describe the graph in
% http://en.wikipedia.org/wiki/Dijkstra's_algorithm
% (saved to matlab/test/Dijkstra_Animation_wikipedia.gif)

d = [...
    %  1   2   3   4   5   6
       0   7   9   0   0  14; ...
       7   0  10  15   0   0; ...
       9  10   0  11   0   2; ...
       0  15  11   0   6   0; ...
       0   0   0   6   0   9; ...
      14   0   2   0   9   0];

% run disjkstra's algorithm on first node
[dd, p] = dijkstra(sparse(d'), 1)

% dd' =
%
%      0     7     9    20    20    11
%
% p' =
%
%      0     1     1     3     6     3

% query several source nodes at the same time
[dd, p] = dijkstra(sparse(d'), [1 3 6])

% dd' =
%
%     0     7     9    20    20    11
%     9    10     0    11    11     2
%    11    12     2    13     9     0
%
% p' =
%
%      0     1     1     3     6     3
%      3     3     0     3     6     3
%      3     3     6     3     6     0

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Dijkstra with limited number of targets

% query a source node, but we are only interested in the path to a single
% target node
[dd, p] = dijkstra(sparse(d'), 1, 3)

% dd' =
%
%     0     7     9    20   Inf    11
%
% p' =
%
%      0     1     1     3   NaN     3

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Dijkstra with piggyback

% secondary distance matrix. The algorithm will ignore this distances in
% terms of searching for shortest-paths, but will create another distance
% output adding the values in D2 while following the path given by D

d2 = [...
    %  1   2   3   4   5   6
       0  10   7   0   0   6; ...
      10   0   2  11   0   0; ...
       7   2   0   9   0   9; ...
       0  11   9   0  14   0; ...
       0   0   0  14   0  15; ...
       6   0   9   0  15   0];

[dd, p, dd2] = dijkstra(sparse(d'), 1:6, 1:6, sparse(d2))

% dd2 =
%
%      0    10     7    16    31    16
%     10     0     2    11    25    11
%      7     2     0     9    24     9
%     16    11     9     0    14    18
%     31    26    24    14     0    15
%     16    11     9    18    15     0

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Dijkstra with non-symmetric input matrix

% we start with a symmetric matrix
d = [...
    %  1   2   3   4   5   6
       0   7   9   0   0  14; ...
       7   0  10  15   0   0; ...
       9  10   0  11   0   2; ...
       0  15  11   0   6   0; ...
       0   0   0   6   0   9; ...
      14   0   2   0   9   0];

% the path from 1 to 5 is 1, 3, 6, 5, and has cost 20
[dd, p] = dijkstra(sparse(d'), 1);
dd(5)
graphpred2path(p', 5)

% in the opposite direction, same path, 5, 6, 3, 1
[dd, p] = dijkstra(sparse(d'), 5);
dd(1)
graphpred2path(p', 1)

% change the distance from 6 to 5 to be very expensive, so that the best
% path now is 1, 3, 4, 5, with cost 26
d(6, 5) = 20;
[dd, p] = dijkstra(sparse(d'), 1);
dd(5)
graphpred2path(p', 5)

% note that in the opposite direction nothing has changed, because the
% matrix is asymmetric
[dd, p] = dijkstra(sparse(d'), 5);
dd(1)
graphpred2path(p', 1)


function ydata = tsne_d(D, labels, no_dims, perplexity)
%TSNE_D Performs symmetric t-SNE on the pairwise Euclidean distance matrix D
%
%   mappedX = tsne_d(D, labels, no_dims, perplexity)
%   mappedX = tsne_d(D, labels, initial_solution, perplexity)
%
% The function performs symmetric t-SNE on the NxN pairwise Euclidean
% distance matrix D to construct an embedding with no_dims dimensions
% (default = 2). An initial solution obtained from an other dimensionality
% reduction technique may be specified in initial_solution.
% The perplexity of the Gaussian kernel that is employed can be specified
% through perplexity (default = 30). The labels of the data are not used
% by t-SNE itself, however, they are used to color intermediate plots.
% Please provide an empty labels matrix [] if you don't want to plot
% results during the optimization.
% The data embedding is returned in mappedX.
%
%
% (C) Laurens van der Maaten, 2010
% University of California, San Diego

    if ~exist('labels', 'var')
        labels = [];
    end
    if ~exist('no_dims', 'var') || isempty(no_dims)
        no_dims = 2;
    end
    if ~exist('perplexity', 'var') || isempty(perplexity)
        perplexity = 30;
    end

    % First check whether we already have an initial solution
    if numel(no_dims) > 1
        initial_solution = true;
        ydata = no_dims;
        no_dims = size(ydata, 2);
    else
        initial_solution = false;
    end

    % Compute joint probabilities
    D = D / max(D(:));                                                      % normalize distances
    P = d2p(D .^ 2, perplexity, 1e-5);                                      % compute affinities using fixed perplexity

    % Run t-SNE
    if initial_solution
        ydata = tsne_p(P, labels, ydata);
    else
        ydata = tsne_p(P, labels, no_dims);
    end


% composit.m        September 2, 2013

% sun-synchronous, repeating ground track,
% frozen orbit design - Kozai J2 perturbations

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;

global j2 j3 mu req omega argper fi xldot xqp

% astrodynamic and utility constants

j2 = 0.00108263;
j3 = -0.00000254;
mu = 398600.4415;
req = 6378.14;
omega = 0.000072921151467;

pi2 = 2.0 * pi;
dtr = pi / 180.0;
rtd = 180.0 / pi;

x = zeros(3, 1);

clc; home;

fprintf('\n                    program composit');

fprintf('\n\n < sun-synchronous, repeating ground track, frozen orbits >\n\n');

% request initial guesses

while(1)

   fprintf('\nplease input an initial guess for the semimajor axis (kilometers)\n');

   x(1) = input('? ');

   if (x(1) > 0.0)
      break;
   end

end

while(1)

   fprintf('\nplease input an initial guess for the eccentricity (non-dimensional)');
   fprintf('\n(0 <= eccentricity < 1)\n');

   x(2) = input('? ');

   if (x(2) >= 0.0 && x(2) < 1.0)
      break;
   end

end

while(1)

    fprintf('\nplease input an initial guess for the inclination (degrees)');
    fprintf('\n(90 < inclination <= 180)\n');

    x(3) = input('? ');

    if (x(3) > 90.0 && x(3) <= 180.0)
       break;
    end

end

x(3) = x(3) * dtr;

while(1)

   fprintf('\nplease input the number of integer orbits in the repeat cycle\n');

   norbits = input('? ');

   if (norbits > 1)
      break;
   end

end

while(1)

   fprintf('\nplease input the number of integer days in the repeat cycle\n');

   ndays = input('? ');

   if (ndays > 0)
      break;
   end

end

% compute repetition factor

xqp = norbits / ndays;

% fundamental interval

fi = pi2 / xqp;

% required nodal regression rate

xldot = (360.0 / 365.25) * dtr / 86400.0;

argper = 90.0 * dtr;

% solve system of nonlinear equations

n = 3;

maxiter = 200;

[xf, niter, icheck] = snle ('compfunc', x, n, maxiter);

sma = xf(1);

ecc = xf(2);

inc = rtd * xf(3);

% keplerian period

tkepler = pi2 * sqrt(sma ^ 3 / mu);

tnode = pi2 * (1.0 / xqp) / (omega - xldot);

% print results

clc; home;

fprintf('\n                    program composit');

fprintf('\n\n < sun-synchronous, repeating ground track, frozen orbits >\n\n');

fprintf('mean semimajor axis                %12.4f  kilometers \n\n', sma);

fprintf('mean eccentricity                  %12.10f \n\n', ecc);

fprintf('mean orbital inclination           %12.4f  degrees \n\n', inc);

fprintf('mean argument of perigee           %12.4f  degrees \n\n', argper*rtd);

fprintf('keplerian period                   %12.4f  minutes \n\n', tkepler/60);

fprintf('nodal period                       %12.4f  minutes \n\n', tnode/60);

fprintf('number of orbits in repeat cycle   %12.4f  \n\n', norbits);

fprintf('number of days in repeat cycle     %12.4f  \n\n', ndays);

fprintf('ground trace repetition factor     %12.4f  \n\n', xqp);


function f = f01_f0 ( n, x, y )

%*****************************************************************************80
%
%% F01_F0 returns the value of function 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N,1), Y(N,1), the evalution points.
%
%    Output, real F(N,1), the function values.
%
  f(1:n,1) = ...
      0.75 * exp ( - ( ( 9.0 * x(1:n,1) - 2.0 ).^2            ...
                     + ( 9.0 * y(1:n,1) - 2.0 ).^2 ) / 4.0 )  ...
    + 0.75 * exp ( - ( ( 9.0 * x(1:n,1) + 1.0 ).^2 ) / 49.0   ...
                     - ( 9.0 * y(1:n,1) + 1.0 ) / 10.0 )      ...
    + 0.5  * exp ( - ( ( 9.0 * x(1:n,1) - 7.0 ).^2            ...
                     + ( 9.0 * y(1:n,1) - 3.0 ).^2 ) / 4.0 )  ...
    - 0.2  * exp ( - (   9.0 * x(1:n,1) - 4.0 ).^2            ...
                     - ( 9.0 * y(1:n,1) - 7.0 ).^2 );

  return
end


%solve the inverse of matrix M,and MOut is the outcome Matrix.
function [MOut] = InverseM(M)
[nRow,nCol] = size(M);
is = zeros(nRow,1);
js = zeros(nRow,1);
fDet = 1.0;
f = 1;
for k = 1:nRow
    fMax = 0;
    for i = k:nRow
        for j = k:nRow
            f1 = abs(M(i,j));
            if f1 > fMax
                fMax = f1;
                is(k,1) = i;
                js(k,1) = j;
            end
        end
    end
    if abs(fMax) < 0.0001
        disp('Error happen 0.0001');
    end
    if is(k,1) ~= k
        f = -f;
        for i = 1:nRow
            [M(k,i),M(is(k,1),i)] = swap(M(k,i),M(is(k,1),i));
        end
    end
    if js(k,1) ~= k
        f = -f;
        for i = 1:nRow
            [M(i,k),M(i,js(k,1))] = swap(M(i,k),M(i,js(k,1)));
        end
    end
    fDet = fDet*M(k,k);
    M(k,k) = 1/M(k,k);
    for j = 1:nRow
        if j ~= k
            M(k,j) = M(k,j)*M(k,k);
        end
    end
    for i = 1:nRow
        if i ~= k
            for j = 1:nRow
                if j ~= k
                    M(i,j) = M(i,j) - M(i,k)*M(k,j);
                end
            end
        end
    end
    for i = 1:nRow
        if i ~= k
            M(i,k) = M(i,k)*(-M(k,k));
        end
    end
end
for k = nRow:-1:1
    if js(k,1) ~= k
        for i = 1:nRow
            [M(k,i),M(js(k,1),i)] = swap(M(k,i),M(js(k,1),i));
        end
    end
    if is(k,1) ~= k
        for i = 1:nRow
            [M(i,k),M(i,is(k,1))] = swap(M(i,k),M(i,is(k,1)));
        end
    end
end
MOut = M;
clear M;


function [out] = interflow_10(S,p1,p2,p3)
%interflow_10

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Scaled linear interflow if storage exceeds a threshold
% Constraints:  f = 0, for S < p2
% @(Inputs):    p1   - time coefficient [d-1]
%               p2   - threshold for flow generation [mm]
%               p3   - linear scaling parameter [-]
%               S    - current storage [mm]

out = p1*max(0,S-p2)/(p3);

end


function value = i4_divp ( i, j )

%*****************************************************************************80
%
%% I4_DIVP returns the smallest multiple of J greater than or equal to I.
%
%  Example:
%
%    I  J  VALUE
%
%    0  4    0
%    1  4    1
%    2  4    1
%    3  4    1
%    4  4    1
%    5  4    2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the number to be analyzed.
%
%    Input, integer J, the number, multiples of which will
%    be compared against I.  J may not be zero.
%
%    Output, integer VALUE, the smallest multiple of J that
%    is greater than or equal to I.
%
  if ( j ~= 0 )
    value = 1 + floor ( ( i - 1 ) / j );
  else
    value = 0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_DIVP - Fatal error!\n' );
    fprintf ( 1, '  The input value of J was zero!\n' );
    error ( 'I4_DIVP - Fatal error!\n' );
  end

  return
end


%% 1D Heat Transfer LBM D1Q2
% by Manuel Diaz
clc; clear; %close all;

%% Domain Grid
L = 30;    % Length
n = 30;    % Nodes
dx = L/n; dt = 1;
x  = 1:dx:L;

%% Initial Condition
f1 = zeros(1,n);
f2 = zeros(1,n);
rho= zeros(1,n); % initial value of the dependent variable rho = T(x,t)
feq1= zeros(1,n);
feq2= zeros(1,n);

%% Constants
csq   = (dx^2)/(dt^2);
alpha = 0.25;
omega = 1/(alpha/(dt*csq)+0.5);
w     = [1/2,1/2];
cx    = [  1, -1];
cy    = [  0,  0];
links = [  1,  2];
tEnd  = 200; % time steps
tPlot = 10;
frames= tEnd/tPlot;
count = 0;

%% Movie Parameters
tPlot = 10; frames= tEnd/tPlot; count = 0;
figure(1)
colordef white

%% Boundary Conditions
twall = 1;

%% Main Loop
for cycle = 1:tEnd;
    % Collision process
    rho = f1+f2;

    % even that for this case k1=k2=1/2m then feq1=feq2=feq
    feq1 = 0.5*rho;
    feq2 = 0.5*rho;

    f1 = (1-omega) * f1 + omega * feq1;
    f2 = (1-omega) * f2 + omega * feq2;
    % Streaming process
    f1 = stream2d( f1 , [cy(1),cx(1)]);
    f2 = stream2d( f2 , [cy(2),cx(2)]);

%   % Original streaming process in A.A.Mohamad
%     for i = 2:n-1;  % Double streaming in one loop
%         f1(n-i+1) = f1(n-i);  % Streaming
%         f2( i-1 ) = f2( i );  % Streaming
%     end

    % Boundary condition
    f1(1) = twall-f2(1);    %Dirichlet BC
    f1(n) = f1(n-1);        %Neumann BC
    f2(n) = f2(n-1);        %Neumann BC

    % Visualization every tPlot
    if mod(cycle,tPlot) == 1
        count = count + 1;
        plot(x,rho,'.');
        title '1D Heat Equation using LBM D1Q2'
        xlabel 'x cell'
        ylabel 'Temperature'
        M(count)=getframe;
    end

    % Animated gif file
    if mod(cycle,tPlot) == 1
        F = getframe;
        if count == 1
            [im,map] = rgb2ind(F.cdata,256,'nodither');
            im(1,1,1,tEnd/tPlot) = 0;
        end
        im(:,:,1,count) = rgb2ind(F.cdata,map,'nodither');
    end
end

%% Make pretty figures
%plot(x,rho,'.');

%% Make Movie
movie(M,3,10); % movie(M,n,fps)

%% Export to Gif
imwrite(im,map,'heat_eq_d1q2.gif','DelayTime',0,'LoopCount',3)


function [RHSdiv, RHSdivx, RHSdivy] = divergenceTerm2D(F)
% This function calculates the divergence of a field using its face
% average flux vector facevariable, which is a face vector
%
% SYNOPSIS:
%   [RHSdiv, RHSdivx, RHSdivy] = divergenceTerm2D(F)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nx = F.domain.dims(1);
Ny = F.domain.dims(2);
G=reshape(1:(Nx+2)*(Ny+2), Nx+2, Ny+2);
DX = repmat(F.domain.cellsize.x(2:end-1), 1, Ny);
DY = repmat(F.domain.cellsize.y(2:end-1)', Nx, 1);

% define the vector of cell index
row_index = reshape(G(2:Nx+1,2:Ny+1),Nx*Ny,1); % main diagonal (only internal cells)

% reassign the east, west, north, and south flux vectors for the
% code readability
Fe = F.xvalue(2:Nx+1,:);		Fw = F.xvalue(1:Nx,:);
Fn = F.yvalue(:,2:Ny+1);       Fs = F.yvalue(:,1:Ny);

% compute the divergence
div_x = (Fe - Fw)./DX;
div_y = (Fn - Fs)./DY;

% define the RHS Vector
RHSdiv = zeros((Nx+2)*(Ny+2),1);
RHSdivx = zeros((Nx+2)*(Ny+2),1);
RHSdivy = zeros((Nx+2)*(Ny+2),1);

% assign the values of the RHS vector
RHSdiv(row_index) = reshape(div_x+div_y,Nx*Ny,1);
RHSdivx(row_index) = reshape(div_x,Nx*Ny,1);
RHSdivy(row_index) = reshape(div_y,Nx*Ny,1);


function [loc_assort_pos,loc_assort_neg] = local_assortativity_wu_sign(W)
%LOCAL_ASSORTATIVITY_WU_SIGN     Local Assortativity
%
%   [loc_assort_pos,loc_assort_neg] = local_assortativity_wu_sign(W);
%
%   Local Assortativity measures the extent to which nodes are connected to
%   nodes of similar strength (vs. higher or lower strength). Adapted from
%   Thedchanamoorthy et al. (2014)'s formula to allow weighted/signed
%   networks (node degree replaced with node strength). Note, output values
%   sum to total assortativity.
%
%   Inputs:     W,        undirected connection matrix with positive and
%                         negative weights
%
%   Output:     loc_assort_pos, local assortativity from positive weights
%
%               loc_assort_neg, local assortativity from negative weights
%
%   Reference: Thedchanamoorthy G, Piraveenan M, Kasthuriratna D,
%              Senanayake U. Proc Comp Sci (2014) 29:2449-2461.
%
%
%   Jeff Spielberg, Boston University

%   Modification History:
%   May 2015: Original

W(1:(size(W,1)+1):end) = 0;
r_pos = assortativity_wei(W.*(W>0),0);
r_neg = assortativity_wei(-W.*(W<0),0);
[str_pos,str_neg] = strengths_und_sign(W);
loc_assort_pos = nan(size(W,1),1);
loc_assort_neg = nan(size(W,1),1);

for curr_node = 1:size(W,1)
    [~,j_pos] = find(W(curr_node,:)>0);
    loc_assort_pos(curr_node,1) = sum(abs(str_pos(j_pos)-str_pos(curr_node)))/str_pos(curr_node);

    [~,j_neg] = find(W(curr_node,:)<0);
    loc_assort_neg(curr_node,1) = sum(abs(str_neg(j_neg)-str_neg(curr_node)))/str_neg(curr_node);
end

loc_assort_pos = ((r_pos+1)/size(W,1))-(loc_assort_pos/sum(loc_assort_pos));
loc_assort_neg = ((r_neg+1)/size(W,1))-(loc_assort_neg/sum(loc_assort_neg));


function [R, tau] = CholeskyMultIdentity(H)
%  Implements Cholesky with added multiple of the identity.  This attempts to
%  to find a scalar tau > 0 such that H + tau * I is sufficiently positive
%  definite, where I is the identity matrix.

global numFact  % Number of Cholesky factorizations attempted.

%  First, we intially try a Cholesky factorization.
[R, fail] = chol(H);
numFact = numFact + 1;
%  If it does not fail, we're done.
if fail == 0
    tau = 0;
    return;
end

%  If the initial Cholesky factorization fails, we attempt to find a scalar
%  tau > 0 such that H + tau * I is sufficiently positive definite.
beta = 0.001;  % Heuristic for increasing tau.
min_H_diag = min(diag(H));  % Smallest diagonal of H.

%  If smallest diagonal of H is positive, set tau to 0; otherwise, set to
%  nonnegative version of the smallest diagonal plus the beta heuristic.
if min_H_diag > 0
    tau = 0;
else
    tau = -min_H_diag + beta;
end

I = eye(size(H,1));  % Identity matrix.

%  Repeatedly add a tau-multiple of the identity to H until the Cholesky
%  factorization succeeds.  Upon each failure, double tau.
while 1
    [R, fail] = chol(H + tau * I);
    numFact = numFact + 1;
    if fail == 0
        return;
    else
        %  NOTE:  In order to decrease number of factorizations, we may want to
        %         increase tau by a factor of 10 instead of 2.
        tau = max(2*tau, beta);
    end
end
end