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) ;


function [ x, obj_val,exitflag ] = QCQP_LB2( H_wave,y_wave,N,l,u,obj_prev)
%UNTITLED9 Summary of this function goes here
%   Detailed explanation goes here
Q = 2*H_wave*H_wave.';
f = -2*H_wave*y_wave;
c = y_wave.'*y_wave;
A = zeros(N,2*N);
for ii = 1:N
    A(ii,ii) = cos((l(ii)+u(ii))/2);
    A(ii,ii+N) = sin((l(ii)+u(ii))/2);         %Linear Constraint
end

function [y,grady] = quadobj(x,Q,f,c)         %Objective Function
    y = 1/2*x.'*Q*x+f.'*x+c;
    if nargout>1
        grady = Q*x + f;
    end
end

function [y_con,yeq,grady,gradyeq] = quadconstr(x)      %Nonlinear Constraint
    for nn = 1:N
        y_con(nn) = x(nn)^2+x(nn+N)^2 - 1;
    end
    y_con(N+1) = 1/2*x.'*Q*x+f.'*x+c-obj_prev;
    yeq = [];
    if nargout>2
       gradyeq = zeros(length(x),N+1);
       for nn = 1:N
           grady(nn,nn) = 2*x(nn);
           grady(nn+N,nn) = 2*x(nn+N);
       end
       grady(:,N+1) = Q*x + f;
    end
    gradyeq = [];
end

Acon = -A;

for ii = 1:N
    b(ii) = -cos((u(ii)-l(ii))/2);
end

% options = optimoptions(@fmincon,'Algorithm','sqp',...
%     'SpecifyObjectiveGradient',true,'SpecifyConstraintGradient',true,...
%     'Display','off','OptimalityTolerance',1e-6,'MaxIterations',400);

options = optimoptions('fmincon','Algorithm','sqp', 'GradObj','on');

fun = @(x)quadobj(x,Q,f,c);
nonlconstr = @(x)quadconstr(x);
x1 = ones(2*N,1);
[x,ignore,exitflag] = fmincon(fun,x1,Acon,b,[],[],[],[],nonlconstr,options);
obj_val = objval_func(x,H_wave,y_wave);
end

github-repos/MATLAB/yuanhao-cui-Must-Reading-on-ISAC/Must-Reading-on-ISAC-34cd6615c52ebca121428a979e756c608b195040/Codes/Fan2018TSP/Codes for DFRC Waveform Design/Constant Modulus/QCQP_LB2.m


% 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


function [] = gDiscrPdfRnd();

% generic Discrete Pdf Random numbers generator
%
% this function extracts numbers that are randomly distributed over a discrete set of N elements,
% the probability distribution function is user-defined.
%
%
% syntax:
%
% Y = gDiscrPdfRnd(pdf)
% Y = gDiscrPdfRnd(pdf,n)
% Y = gDiscrPdfRnd(pdf,n,m)
%
% pdf is a vector; the i-th component of pdf is the
% probability for the i-th element to be extracted
%
% REMARK: the value sum(pdf) does NOT have to be equal to one. Due to the limited
% precision of double numbers, such condition is often hard to be verified. For this
% reason, THE ONLY CONDITION FOR THE ELEMENT OF pdf IS TO BE NONNEGATIVE
% (i.e. pdf(i) >= 0 , for each 1 <= i <= length(pdf))
%
%
%
% Y = gDiscrPdfRnd(pdf)
% Y is a integer from 1 to length(pdf), randomly generated, according to pdf
%
% Y = gDiscrPdfRnd(pdf,n)
% Y is a  n-by-n  matrix of intergers from 1 to length(pdf), randomly generated, according to pdf
%
% Y = gDiscrPdfRnd(pdf,n,m)
% Y is a  n-by-m  matrix of intergers  from 1 to length(pdf), randomly generated, according to pdf
%
%
%
% examples 1
%
% The following line creates a 3-by-2 matrix of intergers equally distributed from 1 to 6
% gDiscrPdfRnd([1/6 1/6 1/6 1/6 1/6 1/6],3,2)
%
% which is identical to the following
% gDiscrPdfRnd([1 1 1 1 1 1],3,2)
%
%
%
%
%
% example 2
%
% the following example generates 10000 samples
% a triangular pdf and displys the frequencies
%
% ^
% |
% |           *
% |       *   *   *
% |___*___*___*___*___*______
%     1   2   3   4   5
%
%
% pdf = [1/9 2/9 3/9 2/9 1/9];  % or simply pdf = [1 2 3 2 1];
% Y = gDiscrPdfRnd(pdf,10000,1);
% hist(Y,[1:length(pdf)]);
%
%
%
%
% example 3
% similar to the previous, but this time pdf look
% rather sinusoidal...
%
% t = [1:.2:3*pi]; pdf = 1 + t.*sin(t); pdf = pdf - min(pdf);
%
% figure
% bar(pdf,'r');
%
% Y = gDiscrPdfRnd(pdf,100000,1);
%
% figure;
% hist(Y,[1:length(pdf)]);
%
%
%
% author Gianluca Dorini
%
% g.dorini@ex.ac.uk
%
% for compiling type
%
% mex gDiscrPdfRnd.c

error('gDiscrPdfRnd mex file absent, type mex gDiscrPdfRnd.c to compile');


%[x,y]=find(fixations);
%densityMap= make_gauss_masks(y,x,[video_res_y,video_res_x]);

function my_mask= make_gauss_masks(x,y,my_size,fixTime)
%This function drop Gaussians centered at (x,y) locations. my_size is the
%size of your original image (also the size of the output of this function).

% Author    : Umesh Rajashekar
% Date      : 4 Jan. 2008

if (nargin < 4)
    fixTime = ones(size(x));
end

W = 1; %Full width at half max in visual degrees of angle for the Gaussian
W = W*30; %For my experiments, 1 degree of visual angle is 60 pixels

%Convert full width at half max to variance
my_sigma = W/(2*sqrt(2*log(2)));

R = my_size(1);C= my_size(2);
big_R=2*R+1;big_C= 2*C+1; %Double your gauss size

[X Y] = meshgrid(0:big_C-1,0:big_R-1);
my_mask = zeros(R,C);

temp_num = (X-C).^2 + (Y-R).^2;
temp = exp(-temp_num/(2*my_sigma^2));
big_gauss = temp /(2*pi*my_sigma);

for i = 1: length(x)
    temp_x = floor(x(i));temp_y = floor(y(i));
    temp = big_gauss(R-temp_y:R-temp_y+R-1 ,C-temp_x:C-temp_x+C-1);
    my_mask = my_mask + temp*fixTime(i);   %Weight by time of fixation
end

my_mask = my_mask./max(my_mask(:));


% test for radial basis function interpolation in 1D.

% number of points
m = 20;

n = 2024;

scaling = 2;
x = rand(m,1)*(scaling-1)+1;
x = rescale(cumsum(x));

xi = linspace(0,1,n)';

% random values
d = rescale( rand(m,1), .1,.9 );

type = {'abs', 'poly3', 'sqrt', 'gwenn'};

clf;
for i=1:length(type)

    options.rbf = type{i};
    v = perform_rbf_interpolation(xi,x,d, options);
    subplot(length(type),1,i);
    hold on;
    plot(xi, v); plot(x, d, 'r.');
    axis([0 1 0 1]);
    hold off;
    title(['RBF ' type{i}]);

end


%
% 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 W = randInitializeWeights(L_in, L_out)
%RANDINITIALIZEWEIGHTS Randomly initialize the weights of a layer with L_in
%incoming connections and L_out outgoing connections
%   W = RANDINITIALIZEWEIGHTS(L_in, L_out) randomly initializes the weights
%   of a layer with L_in incoming connections and L_out outgoing
%   connections.
%
%   Note that W should be set to a matrix of size(L_out, 1 + L_in) as
%   the first row of W handles the "bias" terms
%

% You need to return the following variables correctly
W = zeros(L_out, 1 + L_in);

% ====================== YOUR CODE HERE ======================
% Instructions: Initialize W randomly so that we break the symmetry while
%               training the neural network.
%
% Note: The first row of W corresponds to the parameters for the bias units
%

epsilon_init = 0.12;
W = rand(L out, 1 + L in) * 2 * epsilon init − epsilon init;

% =========================================================================

end


function c = tapas_unitsq_sgm_mu3_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the unit square sigmoid observation model for binary responses
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The unit square sigmoid (ussgm) is the function
%
% f(x) = x^zeta/(x^zeta + (1-x)^zeta) = ustapas_sgm(x; zeta),
%
% where x is in the unit interval, and zeta > 0 is a parameter that determines the shape of the
% sigmoid. Since both its argument and value are always in the unit interval, its graph is
% restricted to the unit square, hence the name unit square sigmoid.
%
% In the application here, the ussgm is the probability of observing a decision y=1 (rather than
% the only alternative y=0) given the current probability mu1hat (or value) of input u=1:
%
% p(y=1|mu1hat) = ustapas_sgm(mu1hat; zeta)
%
% The parameter zeta regulates the steepness of the sigmoid such that it forms the diagonal of
% the unit square for zeta=1 and approaches a step function at 0.5 as zeta approaches infinity.
% Values of 0 < zeta < 1 lead to sigmoids with reverse concavity than usual, but they still
% represent valid observation models.
%
% Zeta can be interpreted as inverse decision noise. To have a shrinkage prior on this, choose a
% high value. It is estimated log-space since it has a natural lower bound at zero.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'tapas_unitsq_sgm_mu3';

% This model has no free parameters

% Gather prior settings in vectors
c.priormus = [
         ];

c.priorsas = [
         ];

% Model filehandle
c.obs_fun = @tapas_unitsq_sgm_mu3;

% Handle to function that transforms observation parameters to their native space
% from the space they are estimated in
c.transp_obs_fun = @tapas_unitsq_sgm_mu3_transp;

return;


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


fprintf('\nHere we train Sparse and Denoising Autoencoders on the MNIST dataset.\n');

% LOAD THE MINIST DATASET
load('mnistSmall.mat','trainData','testData');

[nObs,nIn]= size(trainData);

nHid = 500;

% DEFINE SINGLE-LAYERED ARCHITECTURE
arch.size = [nIn nHid];
arch.actFun = {'sigmoid','sigmoid'}; % ALL SIGMOIDS
arch.lRate = [.1 .1];

% TRAINING OPTIONS
arch.opts = {'nEpoch',10, ...
             'sparsity',.01, ...
             'costFun','xent'};

% INITIALIZE AUTOENCODER
a1 = ae(arch);

% TRAIN & TEST
fprintf('\nTraining a Sparse Autoencoder...\n');
a1 = a1.train(trainData);
[~,xent1] = a1.test(testData);

nVis = 225;
figure; visWeights(a1.layers{1}.W',0,[-1 1],1,nVis);
title(sprintf('Encoding Features of the Sparse Autoencoder\n(sorted by L2 norm)'));
drawnow

% DEFINE DENOISING ARCHITECTURE
arch2 = arch;

% ADD 20% DENOISING RATIO
arch2.opts = {'denoise',.2, ...
              'nEpoch',10, ...
              'costFun','xent'};

% INITIALIZE NETWORK
a2 = ae(arch2);

% TRIAN & TEST
fprintf('\n\nNow training a Denoising Autoencoder...\n');
a2 = a2.train(trainData);
[~,xent2] = a2.test(testData);

figure; visWeights(a2.layers{1}.W',0,[-1 1],1,nVis);
title(sprintf('Encoding Features of the Denoising Autoencoder\n(sorted by L2 norm)'))
drawnow

% DISPLAY
fprintf('\nTesting Mean Squared Error for Autoencoder: %1.2f\n',xent1);
fprintf('\nTesting Mean Squared Error for Denoising Autoencoder: %1.2f\n',xent2);


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 [ loss ] = modified_hata( freq, urban_suburban_rural, tx_height, rx_height, distance)
%modified_hata Implementation of the modified Hata model based on R-ITU-R  SM.2028-1
    rHm = min(rx_height, tx_height);
    rHb = max(rx_height, tx_height);

    rCorr = log10(distance + (tx_height - rx_height)^2/10^6);
    rA = (1.1 * log10(freq) - 0.7) * min(10, rHm) - (1.56 * log10(freq) - 0.8) + max(0, 20 * log10(rHm / 10));
    rB = min(0, 20 * log10(rHb / 30));

    %urban result
    loss = 69.6 + 26.2 * log10(freq) - 13.82 * log10(max(30, rHb)) + (44.9 - 6.55 * log10(max(30, rHb))) * rCorr - rA - rB;

    if urban_suburban_rural == 2
        new_freq = log10(min(max(150, freq), 2000) / 28);
        loss = loss - 2 * (new_freq * new_freq) - 5.4;
    elseif urban_suburban_rural == 3
        adjustedFrequency  = log10(min(max(150, freq), 2000));
        loss = loss - 4.78 * (adjustedFrequency * adjustedFrequency) + 18.33 * adjustedFrequency - 40.94;
    end

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;


clear all; close all;
I=imread('rice.png');
I=im2double(I);
LEN=20;
THETA=10;
PSF=fspecial('motion', LEN, THETA);
J=imfilter(I, PSF, 'conv', 'circular');
figure;
subplot(121);  imshow(I);
subplot(122);  imshow(J);
noise=0.03*randn(size(I));
K=imadd(J, noise);
NP=abs(fft2(noise)).^2;
NPower=sum(NP(:))/prod(size(noise));
NCORR=fftshift(real(ifft2(NP)));
IP=abs(fft2(I)).^2;
IPower=sum(IP(:))/prod(size(I));
ICORR=fftshift(real(ifft2(IP)));
L=deconvwnr(K, PSF, NCORR, ICORR);
figure;
subplot(121);  imshow(K);
subplot(122);  imshow(L);


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 [t,s]=zerocros(y,m,x)
%ZEROCROS finds the zeros crossings in a signal [T,S]=(Y,M,X)
% Inputs:  y = input waveform
%          m = mode string containing:
%              'p' - positive crossings only
%              'n' - negative crossings only
%              'b' - both (default)
%              'r' - round to sample values
%          x = x-axis values corresponding to y [default 1:length(y)]
%
% Outputs: t = x-axis positions of zero crossings
%          s = estimated slope of y at the zero crossing
%
% This routine uses linear interpolation to estimate the position of a zero crossing
% A zero crossing occurs between y(n) and y(n+1) iff (y(n)>=0) ~= (y(n+1)>=0)

% Example: y=sin(2*pi*(0:1000)/200); y(1:100:1001)=0; zerocros(y);
% Note that we get a zero crossing at the end but not at the start.

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

if nargin<2 || ~numel(m)
    m='b';
end
s=y>=0;
k=s(2:end)-s(1:end-1);
if any(m=='p')
    f=find(k>0);
elseif any(m=='n')
    f=find(k<0);
else
    f=find(k~=0);
end
s=y(f+1)-y(f);
t=f-y(f)./s;
if any(m=='r')
    t=round(t);
end
if nargin>2
    tf=t-f; % fractional sample
    t=x(f).*(1-tf)+x(f+1).*tf;
    s=s./(x(f+1)-x(f));
end
if ~nargout
    n=length(y);
    plot(1:n,y,'-',t,zeros(length(t),1),'o');
end


function pass = test_diff( )
% Test diff commands

tol = 1e3*chebfunpref().cheb2Prefs.chebfun2eps;
j=1;
% Check empty
u = diskfunv;
f = diffx(u);
pass(j) = isempty(f) & isa(f,'diskfunv'); j=j+1;
f = diffy(u);
pass(j) = isempty(f) & isa(f,'diskfunv'); j=j+1;
f = diff(u, 2, 2);
pass(j) = isempty(f) & isa(f,'diskfunv'); j=j+1;

%check definitions:
f = diskfun(@(x,y) cos(3*y.^2).*sin(2*x));
g = diskfun(@(x,y) sin(x.^4).*cos(5*y));
F = diskfunv(f, g);
G = diskfunv(diff(f, 1,1), diff(g, 1, 1));
pass(j) = ( norm(2*diffx(F)-diff(F, 1, 1)-G) < tol); j = j+1;
G = diskfunv(diff(f, 1, 3), diff(g, 1, 3));
pass(j) = ( norm(2*diffx(F, 3) - diff(F, 1, 3)-G) < tol); j = j+1;
G = diskfunv(diff(f, 2, 1), diff(g, 2, 1));
pass(j) = ( norm(2*diffy(F) - diff(F, 2, 1)-G) < tol); j = j+1;
G = diskfunv(diff(f, 2, 2), diff(g, 2, 2));
pass(j) = ( norm(2*diffy(F, 2)-diff(F, 2, 2)-G)  < tol); j = j+1;

% Simple examples:
F = diskfunv(@(x,y) cos(x), @(x,y) sin(y));
exact = diskfunv(@(x,y) -sin(x), @(x,y) 0*x);
pass(j) = (norm( diffx(F) - exact ) < tol); j = j+1;
exact = diskfunv(@(x,y) 0*x, @(x,y) cos(y));
pass(j) = ( norm(diffy(F)-exact) < tol ); j = j+1;

%
f = diskfun(@(x,y) cos(3*y.^2).*sin(2*x));
g = diskfun(@(x,y) sin(pi*x.^4).*cos(5*y));
F = diskfunv(f, g);
exact = diskfunv( @(x,y) 2*cos(3*y.^2).*cos(2*x), ...
    @(x,y) 4*pi*x.^3.*cos(pi*x.^4).*cos(5*y) );
pass(j) = ( norm( diffx(F)-exact )  < tol); j = j+1;
exact = diskfunv( @(x,y) -6*y.*sin(3*y.^2).*sin(2*x), ...
    @(x,y) -5*sin(5*y).*sin(pi*x.^4));
pass(j) = ( norm(diffy(F)-exact) ) < tol;
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[varargout]=vdiff(varargin)
%VDIFF	Length-preserving first central difference.
%
%   DX=VDIFF(X,DIM) differentiates X along dimension DIM using the first
%   central difference; DX is the same size as X.
%
%   [D1,D2,...,DN]=VDIFF(X1,X2,...,XN,DIM) for multiple input variables
%   also works.
%
%   VDIFF(X1,X2,...,DIM); with no output arguments overwrites the
%   original input variables.
%
%   DXDT=VDIFF(DT,...) optionally uses scalar timestep DT to approximate
%   a time derivative, i.e. DXDT equals DX divided by DT.
%   _____________________________________________________________________
%
%   First and last points
%
%   The first and last points must be treated differently, as the central
%   difference is not defined there.  Three different methods can be used.
%
%   VDIFF(...,STR) specifies which method to use.
%
%        'endpoint'  uses the first forwards / first backwards difference
%                    at the first and last point, respectively.
%        'periodic'  treats the array as being periodic along dimension DIM,
%                    so that the central difference is defined at endpoints.
%        'nans'      fills in the first and last values with NANs.
%
%   The default behavior is 'endpoint'.
%   _____________________________________________________________________
%
%   'vdiff --t' runs some tests.
%
%   Usage:  x=vdiff(x,dim);
%           x=vdiff(dt,x,dim);
%           x=vdiff(dt,x,dim,'periodic');
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2000--2015 J.M. Lilly --- type 'help jlab_license' for details

%   I am so irritated by diff

if strcmpi(varargin{1}, '--t')
  vdiff_test,return
end

if length(varargin{1})==1
    dt=varargin{1};
    varargin=varargin(2:end);
else
    dt=1;
end

if ischar(varargin{end})
    str=varargin{end};
    varargin=varargin(1:end-1);
else
    str='endpoints';
end

n=varargin{end};
varargin=varargin(1:end-1);

for i=1:length(varargin)
    varargout{i}=vdiff1(varargin{i},n,str);
    if dt~=1
       varargout{i}=varargout{i}/dt;
    end
end

eval(to_overwrite(length(varargin)))

function[y]=vdiff1(x,n,str)

if ~isempty(x)
    %y=vshift(x,1,n)/2-vshift(x,-1,n)/2;

    %/*********************************************************************
    %This block is just a slightly faster way to take the first central difference
    sizex=size(x);
    %Correct for the fact that sizex ignores singleton dimensions
    if n>length(sizex)
        for i=length(sizex)+1:n
            sizex(i)=1;
        end
    end
    sizey=sizex;
    sizey(n)=sizex(n)-2;

%     dx=diff(x,1,n)/2;
%     array=zeros(length(size(x)),1);
%     array(n)=-1;
%     y=vindex(dx,1:size(x,n)-2,n)+vindex(dx,2:size(x,n)-1,n);
%
    y=zeros(sizey);
    i1=1:2:size(x,n);
    if iseven(size(x,n))
        i2=i1+1;
    else
        i2=i1(1:end-1)+1;
    end

    %This is just the same as .... y=vshift(x,1,n)/2-vshift(x,-1,n)/2;
    y=vindexinto(y,diff(vindex(x,i1,n),1,n)/2,i1(1:end-1),n)+vindexinto(y,diff(vindex(x,i2,n),1,n)/2,i2(1:end-1),n);
    %\*********************************************************************

    if strcmpi(str(1:3),'end')
        dx1=vindex(x,2,n)-vindex(x,1,n);
        dxend=vindex(x,size(x,n),n)-vindex(x,size(x,n)-1,n);
        y=cat(n,dx1,cat(n,y,dxend));
    elseif strcmpi(str(1:3),'nan')
        dx1=nan*vindex(x,1,n);
        y=cat(n,dx1,cat(n,y,dx1));
    elseif strcmpi(str(1:3),'per')
        dx1=(vindex(x,2,n)-vindex(x,1,n))/2+(vindex(x,1,n)-vindex(x,size(x,n),n))/2;
        dxend=(vindex(x,size(x,n),n)-vindex(x,size(x,n)-1,n))/2+(vindex(x,1,n)-vindex(x,size(x,n),n))/2;
        y=cat(n,dx1,cat(n,y,dxend));
    end
else
    y=[];
end

function[]=vdiff_test

y1=(1:4)';
y2=2*(1:4)';
[x1,x2]=vdiff(y1,y2,1);
bool=aresame(x1,[1 1 1 1]').*aresame(x2,2*[1 1 1 1]');
reporttest('VDIFF', bool)
vdiff(y1,y2,1);
bool=aresame(y1,[1 1 1 1]').*aresame(y2,2*[1 1 1 1]');
reporttest('VDIFF output overwrite', bool)

dt=pi;
y1=(1:4)';
y2=2*(1:4)';
[x1,x2]=vdiff(pi,y1,y2,1);
bool=aresame(x1,[1 1 1 1]'./dt).*aresame(x2,2*[1 1 1 1]'./dt,1e-10);
reporttest('VDIFF with non-unit time step', bool)

rng(1);
xxx=randn(100000,2);
tic;y=vshift(xxx,1,1)/2-vshift(xxx,-1,1)/2;etime1=toc;
tic;y2=vdiff(xxx,1,'periodic');etime2=toc;
reporttest('VDIFF new and former algorithms match, row dimension, even length', aresame(y,y2,1e-10))
disp(['New algorithm was ' num2str(etime2./etime1) ' times faster than former algorithm.'])

xxx=randn(2,100000);
tic;y=vshift(xxx,1,2)/2-vshift(xxx,-1,2)/2;etime1=toc;
tic;y2=vdiff(xxx,2,'periodic');etime2=toc;
reporttest('VDIFF new and former algorithms match, column dimension, even length', aresame(y,y2,1e-10))
disp(['New algorithm was ' num2str(etime2./etime1) ' times faster than former algorithm.'])

rng(1);
xxx=randn(100001,2);
tic;y=vshift(xxx,1,1)/2-vshift(xxx,-1,1)/2;etime1=toc;
tic;y2=vdiff(xxx,1,'periodic');etime2=toc;
reporttest('VDIFF new and former algorithms match, row dimension, odd length', aresame(y,y2,1e-10))
disp(['New algorithm was ' num2str(etime2./etime1) ' times faster than former algorithm.'])

xxx=randn(2,100001);
tic;y=vshift(xxx,1,2)/2-vshift(xxx,-1,2)/2;etime1=toc;
tic;y2=vdiff(xxx,2,'periodic');etime2=toc;
reporttest('VDIFF new and former algorithms match, column dimension, odd length', aresame(y,y2,1e-10))
disp(['New algorithm was ' num2str(etime2./etime1) ' times faster than former algorithm.'])


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


function f = flops_solve(a,b,c)
% FLOPS_SOLVE    Flops for matrix left division.
% FLOPS_SOLVE(a,b) returns the number of flops for a\b.
% FLOPS_SOLVE(n,m,c) returns the number of flops for ones(n,m)\ones(m,c).

if nargin == 2
  f = flops_solve(rows(a),cols(a),cols(b));
  return;
end
if a == b
  if a == 1
    % scalar division
    f = c*flops_div;
    return;
  end
  f = flops_chol(a) + 2*flops_solve_tri(a,b,c);
elseif a > b
  % this comes from Ax=b, x = (A'*A)\(A'*b)
  f = flops_mul(b,a,b) + flops_mul(b,a,c) + flops_solve(b,b,c);
else
  % this comes from Ax=b, x = A'*(A*A')\b
  f = flops_mul(a,b,a) + flops_mul(b,a,c) + flops_solve(a,a,c);
end


%% Quadrature
% Using quad to integrate f(x)

clear
clc

%% Evaluating quad
n=0;
for i=[2,5.5,9]
    n=n+1;
    F=@(x)((1./x)-1).^(i-1).*exp(1-(1./x))./(x.^2);
    [q(n,1) p(n,1)]=quad(F,0,1,1.0e-8);
end
[q p]