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

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defun chebyshevT (@var{n}, @var{x})
%% Numerically evaluate Chebyshev polynomials of the first kind.
%%
%% Evaluates the Chebyshev polynomial of the first kind of degree
%% @var{n} at the point @var{x}, in double precision.  Both inputs
%% can be arrays but their sizes must be either the same or scalar.
%%
%% Example:
%% @example
%% @group
%% @c doctest: +SKIP_IF(compare_versions (OCTAVE_VERSION(), '6.0.0', '<'))
%% chebyshevT (18, 0.9)
%%   @result{} ans = -0.2614
%% @end group
%% @end example
%%
%% Using this function may be preferable to evaluating the Chebyshev
%% polynomial in monomial form because the latter can give poor
%% accuracy due to numerical instability.
%% See the example in @pxref{@@double/chebyshevU}.
%%
%% This function may be slow for large numbers of inputs.
%% This is because it is not a native double-precision implementation
%% but rather the numerical evaluation of the Python @code{mpmath} function
%% @code{chebyshevt}.
%%
%% @seealso{@@sym/chebychevT, @@double/chebyshevU}
%% @end defun

function y = chebyshevT (n, x)
  if (nargin ~= 2)
    print_usage ();
  end

  if (isequal (size (n), size (x)) || isscalar(n))
    y = zeros (size (x));
  elseif (isscalar (x))
    y = zeros (size (n));
  else
    error ('chebyshevT: inputs N and X must have compatible sizes')
  end

  cmd = { 'Ln = _ins[0]'
          'Lx = _ins[1]'
          'if len(Ln) == 1 and len(Lx) != 1:'
          '    Ln = Ln*len(Lx)'
          'if len(Ln) != 1 and len(Lx) == 1:'
          '    Lx = Lx*len(Ln)'
          'c = [complex(mpmath.chebyt(n, x)) for n,x in zip(Ln, Lx)]'
          'return c,' };
  c = pycall_sympy__ (cmd, num2cell (n(:)), num2cell (x(:)));
  for i = 1:numel (c)
    y(i) = c{i};
  end
end

%!error chebyshevT (1)
%!error chebyshevT (1, 2, 3)

%!error <sizes> chebyshevT ([1 2], [1 2 3])
%!error <sizes> chebyshevT ([1 2], [1; 2])

%!test
%! y = sym(11)/10;
%! t = sym(2);
%! x = 1.1;
%! s = 2;
%! A = chebyshevT (s, x);
%! B = double (chebyshevT (t, y));
%! assert (A, B, -2*eps);

%!test
%! % maple
%! A = -0.304681164165948269030369;
%! B = chebyshevT (18.1, 0.9);
%! assert (A, B, -10*eps)

%!test
%! % maple, complex inputs
%! % ChebyshevT(12.1+3.1*I, 0.5+0.2*I);
%! A = 0.637229289490379273451 - 0.475324703778957991318*1i;
%! B = chebyshevT (12.1+3.1*i, 0.5+0.2i);
%! assert (A, B, -5*eps);

%!test
%! % maple, matrix inputs
%! A = [0.59523064198266880000  0.57727442996887552000];
%! B = chebyshevT ([16 17], [0.9 0.7]);
%! assert (A, B, -10*eps);

%!test
%! % x matrix, s scalar
%! y = [1 2 sym(pi); exp(sym(1)) 5 6];
%! t = sym(2);
%! x = double (y);
%! s = 2;
%! A = chebyshevT (s, x);
%! B = double (chebyshevT (t, y));
%! assert (A, B, -eps);

%!test
%! % s matrix, x scalar
%! t = [1 2 sym(pi); exp(sym(1)) 5 6];
%! y = sym(2);
%! s = double (t);
%! x = 2;
%! A = chebyshevT (s, x);
%! B = double (chebyshevT (t, y));
%! assert (A, B, -eps);

%!xtest
%! % https://github.com/fredrik-johansson/mpmath/issues/469
%! assert (chebyshevT (4, inf), inf)
%! assert (chebyshevT (4, -inf), inf)
%! assert (chebyshevT (3, inf), inf)
%! assert (chebyshevT (3, -inf), -inf)


function gradfun = approxgradientFD(problem, options)
% Gradient approx. fnctn handle based on finite differences of the cost.
%
% function gradfun = approxgradientFD(problem)
% function gradfun = approxgradientFD(problem, options)
%
% Input:
%
% A Manopt problem structure (already containing the manifold and enough
% information to compute the cost) and an options structure (optional),
% containing one option:
%    options.stepsize (positive double; default: 2^-23).
%    options.subspacedim (positive integer; default: [], for M.dim()).
%
% If the cost cannot be computed on 'problem', a warning is issued.
%
% Output:
%
% Returns a function handle, encapsulating a generic finite difference
% approximation of the gradient of the problem cost. The finite difference
% is based on M.dim()+1 computations of the cost.
%
% The returned gradfun has this calling pattern:
%
%   function gradfd = gradfun(x)
%   function gradfd = gradfun(x, storedb)
%   function gradfd = gradfun(x, storedb, key)
%
% x is a point on the manifold problem.M, storedb is a StoreDB object,
% and key is the StoreDB key to point x.
%
% Usage:
%
% Typically, the user will set problem.M and other fields to define the
% cost (typically, problem.cost). Then, to use this generic purpose
% gradient approximation:
%
%   problem.approxgrad = approxgradientFD(problem, options);
%
% See also: steepestdescent conjugategradient

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Nov. 1, 2016.
% Contributors:
% Change log:

    % This gradient approximation is based on the cost:
    % check availability.
    if ~canGetCost(problem)
        warning('manopt:approxgradFD:nocost', ...
                'approxgradFD requires the cost to be computable.');
    end

    % Set local defaults here, and merge with user options, if any.
    localdefaults.stepsize = 2^-23;
    localdefaults.subspacedim = [];
    if ~exist('options', 'var') || isempty(options)
        options = struct();
    end
    options = mergeOptions(localdefaults, options);

    % % Finite-difference parameters
    % How far do we look?
    stepsize = options.stepsize;
    % Approximate the projection of the gradient on a random subspace of
    % what dimension? If [], uses full tangent space.
    subspacedim = options.subspacedim;

    % Build and return the function handle here. This extra construct via
    % funhandle makes it possible to make storedb and key optional.
    gradfun = @funhandle;
    function gradfd = funhandle(x, storedb, key)
        % Allow omission of the key, and even of storedb.
        if ~exist('key', 'var')
            if ~exist('storedb', 'var')
                storedb = StoreDB();
            end
            key = storedb.getNewKey();
        end
        gradfd = gradientFD(stepsize, subspacedim, problem, x, storedb, key);
    end

end

function gradfd = gradientFD(stepsize, subspacedim, problem, x, storedb, key)
% This function does the actual work.
%
% Original code: Nov. 1, 2016 (NB).

    % Evaluate the cost at the root point
    fx = getCost(problem, x, storedb, key);

    % Pick an orthonormal basis for the tangent space at x, or a subspace
    % thereof. The default is a full subspace. If a strict subspace is
    % picked, the returned vector approximates the orthogonal projection of
    % the gradient to that subspace.
    B = tangentorthobasis(problem.M, x, subspacedim);

    % Use finite differences to approximate the directional derivative
    % along each direction in the basis B.
    df = zeros(size(B));
    for k = 1 : numel(B)
        % Move in the B{k} direction
        xk = problem.M.retr(x, B{k}, stepsize);
        keyk = storedb.getNewKey();
        % Evaluate the cost there
        fxk = getCost(problem, xk, storedb, keyk);
        % Don't keep this point in cache
        storedb.remove(keyk);
        % Finite difference
        df(k) = (fxk - fx)/stepsize;
    end

    % Build the gradient approximation.
    gradfd = lincomb(problem.M, x, B, df);

end


function mono_next_grlex_test ( )

%*****************************************************************************80
%
%% MONO_NEXT_GRLEX_TEST tests MONO_NEXT_GRLEX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 February 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MONO_NEXT_GRLEX_TEST\n' );
  fprintf ( 1, '  MONO_NEXT_GRLEX returns the next monomial\n' );
  fprintf ( 1, '  in graded lexicographic order.\n' );

  m = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Let M =  %d\n', m );

  k = 0;
  x = zeros ( m, 1 );

  while ( 1 )

    d = sum ( x(1:m) );
    fprintf ( 1, '  %2d  %2d  |  %2d  %2d  %2d  %2d\n', k, d, x(1:m) );
    if ( x(1) == 3 )
      break
    end
    k = k + 1;
    x = mono_next_grlex ( m, x );
  end

  return
end


function H = sphankel(r)
%SPHANKEL   Sparse Hankel operator.
%   SPHANKEL(R) this forms a sparse Hankel matrix by forming it as an upside-
%   down Toeplitz matrix. This is required by the ultraspherical multiplication
%   operator.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Hankel is an upside-down Toeplitz matrix.
r = flipud(r(:)); % Ensure column vector.
H = fliplr(triu(ultraS.sptoeplitz(r, r)));

end


% dftfilt3() - discrete complex wavelet filters
%
% Usage:
%   >> [wavelet,cycles,freqresol,timeresol] = dftfilt3( freqs, cycles, srate, varargin)
%
% Inputs:
%   freqs    - vector of frequencies of interest.
%   cycles   - cycles array. If cycles=0, then the Hanning tapered Short-term FFT is used.
%              If one value is given and cycles>0, all wavelets have
%              the same number of cycles. If two values are given, the
%              two values are used for the number of cycles at the lowest
%              frequency and at the highest frequency, with linear or
%              log-linear interpolation between these values for intermediate
%              frequencies
%   srate    - sampling rate (in Hz)
%
% Optional Inputs: Input these as 'key/value pairs.
%   'cycleinc' - ['linear'|'log'] increase mode if [min max] cycles is
%              provided in 'cycle' parameter. {default: 'linear'}
%   'winsize'  Use this option for Hanning tapered FFT or if you prefer to set the length of the
%              wavelets to be equal for all of them (e.g., to set the
%              length to 256 samples input: 'winsize',256). {default: [])
%              Note: the output 'wavelet' will be a matrix and it may be
%              incompatible with current versions of timefreq and newtimef.
%   'timesupport' The number of temporal standard deviation used for wavelet lengths {default: 7)
%
% Output:
%   wavelet - cell array or matrix of wavelet filters
%   timeresol - temporal resolution of Morlet wavelets.
%   freqresol - frequency resolution of Morlet wavelets.
%
% Note: The length of the window is always made odd.
%
% Authors: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 3/28/2003
%          Rey Ramirez, SCCN/INC/UCSD, La Jolla, 9/26/2006

% Copyright (C) 3/28/2003 Arnaud Delorme 8, SCCN/INC/UCSD, arno@salk.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

%
% Revision 1.12 2006/09/25  rey r
% Almost complete rewriting of dftfilt2.m, changing both Morlet and Hanning
% DFT to be more in line with conventional implementations.
%
% Revision 1.11  2006/09/07 19:05:34  scott
% further clarified the Morlet/Hanning distinction -sm
%
% Revision 1.10  2006/09/07 18:55:15  scott
% clarified window types in help msg -sm
%
% Revision 1.9  2006/05/05 16:17:36  arno
% implementing cycle array
%
% Revision 1.8  2004/03/04 19:31:03  arno
% email
%
% Revision 1.7  2004/02/25 01:45:55  arno
% sinus test
%
% Revision 1.6  2004/02/15 22:23:08  arno
% implementing morlet wavelet
%
% Revision 1.5  2003/05/09 20:55:10  arno
% adding hanning function
%
% Revision 1.4  2003/04/29 16:02:54  arno
% header typos
%
% Revision 1.3  2003/04/29 01:09:16  arno
% debug imaginary part
%
% Revision 1.2  2003/04/28 23:01:13  arno
% *** empty log message ***
%
% Revision 1.1  2003/04/28 22:46:49  arno
% Initial revision
%

function [wavelet,cycles,freqresol,timeresol] = dftfilt3( freqs, cycles, srate, varargin);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Rey fixed all input parameter sorting.
if nargin < 3
    error(' A minimum of 3 arguments is required');
end;
numargin=length(varargin);
if rem(numargin,2)
    error('There is an uneven number key/value inputs. You are probably missing a keyword or its value.')
end
varargin(1:2:end)=lower(varargin(1:2:end));

% Setting default parameter values.
cycleinc='linear';
winsize=[];
timesupport=7;  % Setting default of 7 temporal standard deviations for wavelet's length.

for n=1:2:numargin
    keyword=varargin{n};
    if strcmpi('cycleinc',keyword)
        cycleinc=varargin{n+1};
    elseif strcmpi('winsize',keyword)
        winsize=varargin{n+1};
        if ~mod(winsize,2)
            winsize=winsize+1; % Always set to odd length wavelets and hanning windows;
        end
    elseif strcmpi('timesupport',keyword)
        timesupport=varargin{n+1};
    else
        error(['What is ' keyword '? The only legal keywords are: type, cycleinc, winsize, or timesupport.'])
    end
end
if isempty(winsize) & cycles==0
    error('If you are using a Hanning tapered FFT, please supply the winsize input-pair.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% compute number of cycles at each frequency
% ------------------------------------------
type='morlet';
if length(cycles) == 1 & cycles(1)~=0
    cycles = cycles*ones(size(freqs));
elseif length(cycles) == 2
    if strcmpi(cycleinc, 'log') % cycleinc
         cycles = linspace(log(cycles(1)), log(cycles(2)), length(freqs));
         cycles = exp(cycles);
         %cycles=logspace(log10(cycles(1)),log10(cycles(2)),length(freqs)); %rey
    else
        cycles = linspace(cycles(1), cycles(2), length(freqs));
    end;
end;
if cycles==0
    type='sinus';
end

sp=1/srate; % Rey added this line (i.e., sampling period).
% compute wavelet
for index = 1:length(freqs)
    fk=freqs(index);
    if strcmpi(type, 'morlet') % Morlet.
        sigf=fk/cycles(index); % Computing time and frequency standard deviations, resolutions, and normalization constant.
        sigt=1./(2*pi*sigf);
        A=1./sqrt(sigt*sqrt(pi));
        timeresol(index)=2*sigt;
        freqresol(index)=2*sigf;
        if isempty(winsize) % bases will be a cell array.
            tneg=[-sp:-sp:-sigt*timesupport/2];
            tpos=[0:sp:sigt*timesupport/2];
            t=[fliplr(tneg) tpos];
            psi=A.*(exp(-(t.^2)./(2*(sigt^2))).*exp(2*i*pi*fk*t));
            wavelet{index}=psi;  % These are the wavelets with variable number of samples based on temporal standard deviations (sigt).
        else % bases will be a matrix.
            tneg=[-sp:-sp:-sp*winsize/2];
            tpos=[0:sp:sp*winsize/2];
            t=[fliplr(tneg) tpos];
            psi=A.*(exp(-(t.^2)./(2*(sigt^2))).*exp(2*i*pi*fk*t));
            wavelet(index,:)=psi; % These are the wavelets with the same length.
            % This is useful for doing time-frequency analysis as a matrix vector or matrix matrix multiplication.
        end
    elseif strcmpi(type, 'sinus') % Hanning
        tneg=[-sp:-sp:-sp*winsize/2];
        tpos=[0:sp:sp*winsize/2];
        t=[fliplr(tneg) tpos];
        win = exp(2*i*pi*fk*t);
        wavelet(index,:) = win .* hanning(winsize)';
        %wavelet{index} = win .* hanning(winsize)';
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    end;
end;

% symmetric hanning function
function w = hanning(n)
if ~rem(n,2)
    w = .5*(1 - cos(2*pi*(1:n/2)'/(n+1)));
    w = [w; w(end:-1:1)];
else
    w = .5*(1 - cos(2*pi*(1:(n+1)/2)'/(n+1)));
    w = [w; w(end-1:-1:1)];
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Released under the MIT License.
% If you use this code, please cite the following paper:
% Mahmoud Afifi, Abdelrahman Abdelhamed, Abdullah Abuolaim, Abhijith
% Punnappurath, and Michael S Brown. CIE XYZ Net: Unprocessing Images for
% Low-Level Computer Vision Tasks. arXiv preprint, 2020.
%
% Author: Mahmoud Afifi | Email: mafifi@eecs.yorku.ca, m.3afifi@gmail.com
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

classdef mae_cos_LossLayer < nnet.layer.RegressionLayer
    properties
        scale
    end
    methods
        function layer = mae_cos_LossLayer(name, scaleFactor)
            layer.Name = name;
            layer.Description = ...
                'Mean abs error + (negative) cos similarity';
            layer.scale = scaleFactor;
        end

        function loss = forwardLoss(layer, Y, T)
            % Calculate MAE + (negative) cos similarity.
            Y(Y<0) = 0;

            sz = size(Y);
            if length(sz) == 3
                R = 1;
                mae_srgb = sum(reshape(abs(Y(:,:,4:6) - T(:,:,4:6)),...
                    [],3),2);
                mae_xyz = sum(reshape(abs(Y(:,:,1:3) - T(:,:,1:3)),...
                    [],3),2);

                % compute cosine simialrity
                cosSim_srgb = zeros(size(mae_srgb), 'like', Y);
                cosSim_xyz = zeros(size(mae_xyz), 'like', Y);

                y_srgb = reshape(Y(:,:,3:6),[],3);
                t_srgb = reshape(T(:,:,4:6),[],3);
                y_xyz = reshape(Y(:,:,1:3),[],3);
                t_xyz = reshape(T(:,:,1:3),[],3);

                cosSim_srgb = cosSim_srgb + sum(y_srgb.*t_srgb,2)./...
                    (sqrt(sum(y_srgb.^2,2)) .* ...
                    sqrt(sum(t_srgb.^2,2)) + eps);

                cosSim_xyz = cosSim_xyz + sum(y_xyz.*t_xyz,2)./...
                    (sqrt(sum(y_xyz.^2,2)) .* ...
                    sqrt(sum(t_xyz.^2,2)) + eps);

            else
                R = sz(4);
                mae_srgb = zeros(sz(1)*sz(2),1,'like',Y);
                cosSim_srgb = zeros(size(mae_srgb),'like',mae_srgb);

                mae_xyz = zeros(sz(1)*sz(2),1,'like',Y);
                cosSim_xyz = zeros(size(mae_xyz),'like',mae_xyz);

                for j = 1 : R

                    mae_srgb = mae_srgb + sum(reshape( ...
                        abs(Y(:,:,4:6,j) - T(:,:,4:6,j)) ,...
                        [],3),2);
                    mae_xyz = mae_xyz + sum(reshape( ...
                        abs(Y(:,:,1:3,j) - T(:,:,1:3,j)) ,...
                        [],3),2);

                    % compute cosine simialrity
                    y_srgb = reshape(Y(:,:,4:6),[],3);
                    t_srgb = reshape(T(:,:,4:6),[],3);
                    y_xyz = reshape(Y(:,:,1:3),[],3);
                    t_xyz = reshape(T(:,:,1:3),[],3);

                    cosSim_srgb = cosSim_srgb + sum(y_srgb.*t_srgb,2)./...
                        (sqrt(sum(y_srgb.^2,2)) .* ...
                        sqrt(sum(t_srgb.^2,2)) + eps);

                    cosSim_xyz = cosSim_xyz + sum(y_xyz.*t_xyz,2)./...
                        (sqrt(sum(y_xyz.^2,2)) .* ...
                        sqrt(sum(t_xyz.^2,2)) + eps);
                end
            end

            loss = (sum(mae_srgb - cosSim_srgb) + ...
                layer.scale * sum(mae_xyz - cosSim_xyz))/R;

        end
    end
end


function [y] = local2globalPosM(x, X)

% SYNTAX:
%   [y] = local2globalPos(x, X);
%
% INPUT:
%   x = local position vector(s)
%   X = origin vector(s)
%
% OUTPUT:
%   y = global position vector(s)
%
% DESCRIPTION:
%   Rototraslation from local-level reference frame to Earth-fixed reference frame

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

%initialize new position vector
y = zeros(size(x));

    %geodetic coordinates
    [phi, lam] = cart2geod(X(1,i), X(2,i), X(3,i));

    %rotation matrix from global to local reference system
    R = [-sin(lam) cos(lam) 0;
         -sin(phi)*cos(lam) -sin(phi)*sin(lam) cos(phi);
         +cos(phi)*cos(lam) +cos(phi)*sin(lam) sin(phi)];

    %rototraslation
    %y(:,i) = R\x(:,i) + X(:,i);
    y = R * x' + X;

end


function [U,N,Y] = exterior_skinning_modes(V,D,H,M,k)
  % EXTERIOR_SKINNING_MODES Perform modal analysis on H,M within the subspace
  % where vertices in "linear blend skinning handle" selected by D of a mesh with
  % vertices V deform affinely.
  %
  % [U,N,Y,H,M] = exterior_skinning_modes(V,D,H,M,k)
  %
  % Inputs:
  %   V  #V by dim
  %   D  #V by #H bone-handle incidence matrix D(i,j) = 1 means vertex i belongs
  %     to bone j
  %   H  #V*dim by #V*dim stiffness matrix
  %   M  #V*dim by #V*dim mass matrix
  %   k  number of eigen modes to compute
  % Outputs:
  %   U  #V*3 by k list of eigen modes in the "maximal" space
  %   N  #V*3 by #N subspace reduction matrix
  %   Y  #N by k list of subspace modes so that U = N * Y
  %
  % Example:
  %   H = arap_hessian(V,F);
  %   M = repdiag(massmatrix(V,F),3);
  %   [~,N,Y] = exterior_skinning_modes(V,D,H,M,20);
  %

  assert(max( sum(D,2) == 1));
  assert(all( any(D,1) ));
  n = size(V,1);
  dim = size(V,2);
  assert(size(H,1) == n*dim);
  assert(size(M,1) == n*dim);
  assert(k <= n);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Build the null-space so that V = N * Y
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  int = find(~any(D,2));
  b = find(any(D,2));
  ni = numel(int);
  m = size(D,2);
  S = sparse(lbs_matrix(V(b,:),D(b,:)));
  NI = speye(dim*ni,dim*ni+size(S,2));
  vec = @(X) X(:);
  b3 = vec(b+(0:dim-1)*n);
  int3 = vec(int+(0:dim-1)*n);
  N = sparse(int3,1:numel(int)*dim,1,n*dim,dim*ni+size(S,2));
  N(b3,:) = [sparse(dim*(n-ni),dim*ni) S];

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Test that applying identity transformations to handles produces V
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %Astack = repmat([eye(3,3) zeros(3,1)],[1 1 m]);
  %% collect transformations into column
  %A = reshape(permute(Astack,[3 1 2]),m*3*(3+1),1);
  %Y = [reshape(V(int,:),[],1);A];
  %tsurf(F,reshape(N*Y,[],3),'CData',1*any(D,2),fphong)
  %axis equal;
  %view(90,0);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Handle constrained Eigen Decomposition
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % wow, this symmetrization is important...
  smash = @(A,P) 0.5*(P'*A*P + (P'*A*P)');
  [Y,YD] = eigs(-0.5*smash(H,N),0.5*smash(M,N),k,'sm');

  U = N*Y;
end


function pass = test_real( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e4*pref.techPrefs.chebfuneps;

% Example 1
f = real(ballfun(@(x,y,z)x+1i*y));
exact = ballfun(@(x,y,z)x);
pass(1) = norm( f - exact ) < tol;

% Example 2
f = real(ballfun(@(x,y,z)sin(z)+1i*cos(y)));
exact = ballfun(@(x,y,z)sin(z));
pass(2) = norm( f - exact ) < tol;

if (nargout > 0)
    pass = all(pass(:));
end
end


function op = proj_linfl2( q )

%PROJ_LINFL2   Projection of each row onto the scaled l2 norm ball.
%    OP = PROJ_LINFL2( Q ) returns an operator implementing the
%    indicator function for the set of l2 norm ball of size q,
%    { X | for all rows i, norm( X(i,:),2) <= q }. Q is optional; if omitted,
%    Q=1 is assumed. But if Q is supplied, it must be a positive
%    real scalar.
% Dual: prox_l1l2.m
% See also: prox_l1, prox_linf, proj_l1

if nargin == 0,
	q = 1;
elseif ~isnumeric( q ) || ~isreal( q ) || numel( q ) ~= 1 || q <= 0,
	error( 'Argument must be positive.' );
end

% In r2007a and later, we can use bsxfun instead of the spdiags trick
if exist('OCTAVE_VERSION','builtin')
    vr = '2000';
else
    vr=version('-release');
end
if str2num(vr(1:4)) >= 2007
    op = @(varargin)proj_linfl2_q_bsxfun( q, varargin{:} );
else
    % the default, using spdiags
    op = @(varargin)proj_linfl2_q( q, varargin{:} );
end

function [ v, x ] = proj_linfl2_q( q, x, t )
v = 0;
switch nargin,
	case 2,
		if nargout == 2,
			error( 'This function is not differentiable.' );
		elseif norm( x(:), Inf ) > q,
			v = Inf;
		end
	case 3,
        % Compute the norms of the rows
        m = size(x,1);
        nrms = sqrt( sum( abs(x).^2 , 2 ) );
        % Scale the rows using left diagonal multiplication
        x = spdiags( min(1,q./nrms), 0, m, m )*x;
	otherwise,
		error( 'Not enough arguments.' );
end

function [ v, x ] = proj_linfl2_q_bsxfun( q, x, t )
v = 0;
switch nargin,
	case 2,
		if nargout == 2,
			error( 'This function is not differentiable.' );
		elseif norm( x(:), Inf ) > q,
			v = Inf;
		end
	case 3,
        nrms = sqrt( sum( abs(x).^2 , 2 ) );
        bsxfun( @times, x, min(1,q./nrms) );
	otherwise,
		error( 'Not enough arguments.' );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
%function fmtt
%FMTT 	Unit test for the function FMT.

%	O. Lemoine - May 1996.

N=128;

% Perfect reconstruction with ifmt
fmin=0.1; fmax=0.5;
sig=amgauss(N).*fmconst(N,.3);
[MELLIN,BETA]=fmt(sig,fmin,fmax,N);
X=ifmt(MELLIN,BETA,N);
err=abs(X-sig);
if any(err>1e-7),
 error('fmt test 1 failed');
end

% Energy conservation
x=fmconst(N);
fmin=0.1; fmax=0.4;
FMT=fmt(x,fmin,fmax,N);
SP=fft(x);
indmin = 1+round(fmin*(N-2));
indmax = 1+round(fmax*(N-2));
SPana=SP(indmin:indmax);
nu=(indmin:indmax)'/N;
SPp=SPana./nu;
Es=SPp'*SPana;
Efmt=norm(FMT)^2;
if abs(Es-Efmt)>sqrt(eps),
 error('fmt test 2 failed');
end;

% Unitarity of the MT
x1=amgauss(N).*fmlin(N,.15,.35);
x2=amgauss(N).*fmconst(N);
fmin=0.01; fmax=0.49;
FMT1=fmt(x1,fmin,fmax,2*N);
FMT2=fmt(x2,fmin,fmax,2*N);
indmin = 1+round(fmin*(2*N-2));
indmax = 1+round(fmax*(2*N-2));
SP1=fft(x1); SP2=fft(x2);
nu=(indmin:indmax)'/N/2;
SP1p=SP1(indmin:indmax)./nu;
cor1=SP1p'*SP2(indmin:indmax);
cor2=conj(FMT1*FMT2');
if abs(cor1-cor2)>N*1e-2,
 error('fmt test 3 failed');
end;

% Covariance by dilation
% Property of the Mellin transform used in scale.
% So as scale works, this property is verified.

% MT of a product = convolution of the MT
x1=amgauss(N).*fmlin(N,.15,.35);
x2=amgauss(N).*fmsin(N,.15,.35);
FMT1=fmt(x1,fmin,fmax,2*N);
FMT2=fmt(x2,fmin,fmax,2*N);
FMT=conv(FMT1,FMT2);
FMT=FMT/max(real(FMT));
X1=fft(x1); X2=fft(x2);
X=X1.*X2;
x=fftshift(ifft(X));
FMTp=fmt(x,fmin,fmax,2*N);
FMTp=FMTp/max(real(FMTp));
Diff=FMTp-FMT(N+1:3*N);
if any(abs(Diff)>1e-4),
 error('fmt test 4 failed');
end

N=121;

% Perfect reconstruction with ifmt
fmin=0.1; fmax=0.5;
sig=amgauss(N).*fmconst(N,.3);
[MELLIN,BETA]=fmt(sig,fmin,fmax,N+1);
X=ifmt(MELLIN,BETA,N);
err=abs(X-sig);
if any(err>1e-2),
 error('fmt test 5 failed');
end

% Energy conservation
x=fmconst(N);
fmin=0.1; fmax=0.4;
FMT=fmt(x,fmin,fmax,N+1);
SP=fft(hilbert(real(x)));
indmin = 1+round(fmin*(N-2));
indmax = 1+round(fmax*(N-2));
SPana=SP(indmin:indmax);
nu=(indmin:indmax)'/(N+1);
SPp=SPana./nu;
Es=SPp'*SPana;
Efmt=norm(FMT)^2;
if abs(Es-Efmt)>sqrt(eps),
 error('fmt test 6 failed');
end;

% Unitarity of the MT
x1=amgauss(N).*fmlin(N,.15,.35);
x2=amgauss(N).*fmconst(N);
fmin=0.01; fmax=0.49;
FMT1=fmt(x1,fmin,fmax,2*N);
FMT2=fmt(x2,fmin,fmax,2*N);
indmin = 1+round(fmin*(2*N-2));
indmax = 1+round(fmax*(2*N-2));
SP1=fft(x1); SP2=fft(x2);
nu=(indmin:indmax)'/N/2;
SP1p=SP1(indmin:indmax)./nu;
cor1=SP1p'*SP2(indmin:indmax);
cor2=conj(FMT1*FMT2');
if abs(cor1-cor2)>N*1e-2,
 error('fmt test 7 failed');
end;

% MT of a product = convolution of the MT
x1=amgauss(N).*fmlin(N,.15,.35);
x2=amgauss(N).*fmsin(N,.15,.35);
FMT1=fmt(x1,fmin,fmax,2*N);
FMT2=fmt(x2,fmin,fmax,2*N);
FMT=conv(FMT1,FMT2);
FMT=FMT/max(real(FMT));
X1=fft(x1); X2=fft(x2);
X=X1.*X2;
x=fftshift(ifft(X));
FMTp=fmt(x,fmin,fmax,2*N);
FMTp=FMTp/max(real(FMTp));
Diff=FMTp-FMT(N+1:3*N);
if any(abs(Diff)>1e-4),
 error('fmt test 8 failed');
end


function value = r8mat_sum ( m, n, a )

%*****************************************************************************80
%
%% R8MAT_SUM returns the sum of the entries of an R8MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), the array.
%
%    Output, real VALUE, the sum of the entries.
%
  value = sum ( sum ( a(1:m,1:n) ) );

  return
end


function graph_to_dot(adj, varargin)
%GRAPH_TO_DOT  Makes a GraphViz (AT&T) file representing an adjacency matrix
% graph_to_dot(adj, ...) writes to the specified filename.
%
% Optional arguments can be passed as name/value pairs: [default]
%
% 'filename' - if omitted, writes to 'tmp.dot'
% 'arc_label' - arc_label{i,j} is a string attached to the i-j arc [""]
% 'node_label' - node_label{i} is a string attached to the node i ["i"]
% 'width'     - width in inches [10]
% 'height'    - height in inches [10]
% 'leftright' - 1 means layout left-to-right, 0 means top-to-bottom [0]
% 'directed'  - 1 means use directed arcs, 0 means undirected [1]
%
% For details on graphviz, See http://www.research.att.com/sw/tools/graphviz
%
% See also dot_to_graph and draw_dot.

% First version written by Kevin Murphy 2002.
% Modified by Leon Peshkin, Jan 2004.
% Bugfix by Tom Minka, Mar 2004.

node_label = [];   arc_label = [];   % set default args
width = 10;        height = 10;
leftright = 0;     directed = 1;     filename = 'tmp.dot';

for i = 1:2:nargin-1                    % get optional args
  switch varargin{i}
    case 'filename', filename = varargin{i+1};
    case 'node_label', node_label = varargin{i+1};
    case 'arc_label', arc_label = varargin{i+1};
    case 'width', width = varargin{i+1};
    case 'height', height = varargin{i+1};
    case 'leftright', leftright = varargin{i+1};
    case 'directed', directed = varargin{i+1};
  end
end
% minka
if ~directed
  adj = triu(adj | adj');
end

fid = fopen(filename, 'w');
if directed
  fprintf(fid, 'digraph G {\n');
  arctxt = '->';
  if isempty(arc_label)
    labeltxt = '';
  else
    labeltxt = '[label="%s"]';
  end
else
  fprintf(fid, 'graph G {\n');
  arctxt = '--';
  if isempty(arc_label)
    labeltxt = '[dir=none]';
  else
    labeltext = '[label="%s",dir=none]';
  end
end
edgeformat = strcat(['%d ',arctxt,' %d ',labeltxt,';\n']);
fprintf(fid, 'center = 1;\n');
fprintf(fid, 'size=\"%d,%d\";\n', width, height);
if leftright
  fprintf(fid, 'rankdir=LR;\n');
end
Nnds = length(adj);
for node = 1:Nnds               %  process nodes
  if isempty(node_label)
    fprintf(fid, '%d;\n', node);
  else
    fprintf(fid, '%d [ label = "%s" ];\n', node, node_label{node});
  end
end
for node1 = 1:Nnds   % process edges
  arcs = find(adj(node1,:));         % children(adj, node);
  for node2 = arcs
    if  ~isempty(arc_label)
      fprintf(fid, edgeformat,node1,node2,arc_label{node1,node2});
    else
      fprintf(fid, edgeformat, node1, node2);
    end
  end
end
fprintf(fid, '}');
fclose(fid);


function meshnd_quality (do_metis)
%MESHND_QUALITY test the ordering quality computed by meshnd.
% The fill-in and flop count for sparse Cholesky factorization using the meshnd
% nested dissection ordering is computed with AMD.  If SuiteSparse is installed
% with METIS, and if requested, then the metis nested dissection ordering is
% also compared.
%
% Example:
%   meshnd_quality          % compare MESHND and AMD
%   meshnd_quality (1)      % also compare with METIS
%
% See also meshnd, meshsparse, nested, amd, metis.

% Copyright 2007, Timothy A. Davis, Univ. of Florida

stencils = [5 9 7 27] ;

if (nargin < 1)
    do_metis = 0 ;
end
if (do_metis)
    if (exist ('metis') ~= 3)                                               %#ok
        % METIS not installed
        do_metis = 0 ;
    end
end

figure (1)
clf

for sk = 1:4

    stencil = stencils (sk) ;

    is3D = (stencil == 7 | stencil == 27) ;     %#ok
    if (is3D)
	s = 2.^(3:.1:7) ;		% mesh size up to 127-by-127-by-127
    else
	s = 2.^(3:.1:10) - 1 ;		% mesh size up to 1023-by-1023
    end
    t = length (s) ;
    lnz = nan * zeros (3,t) ;
    fl  = nan * zeros (3,t) ;

    try

        for t = 1:length (s)

            n = floor (s (t)) ;

            % create the mesh and the matrix, and get nested dissection ordering
            if (is3D)
                fprintf ('3D mesh: %d-by-%d-by-%d, %d-point stencil\n', ...
                    n, n, n, stencil) ;
                [G p] = meshnd (n, n, n) ;
            else
                fprintf ('2D mesh: %d-by-%d, %d-point stencil\n', n, n,stencil);
                [G p] = meshnd (n, n) ;
            end
            A = meshsparse (G, stencil) ;

            % ND results
            c = symbfact (A (p,p)) ;
            lnz (1,t) = sum (c) ;
            fl  (1,t) = sum (c.^2) ;
            fprintf ('    MESHND:            nnz(L) %8.3e  flops %8.3e\n', ...
                lnz (1,t), fl (1,t)) ;
            clear G

            % AMD results
            try
                p = amd (A) ;
            catch
                % assume SuiteSparse is installed
                p = amd2 (A) ;
            end
            c = symbfact (A (p,p)) ;
            lnz (2,t) = sum (c) ;
            fl  (2,t) = sum (c.^2) ;
            fprintf ('    AMD:               nnz(L) %8.3e  flops %8.3e\n', ...
                lnz (2,t), fl (2,t)) ;

            % METIS results (requires SuiteSparse and METIS)
            if (do_metis)
                p = metis (A) ;
                c = symbfact (A (p,p)) ;
                lnz (3,t) = sum (c) ;
                fl  (3,t) = sum (c.^2) ;
                fprintf (...
                    '    METIS:             nnz(L) %8.3e  flops %8.3e\n', ...
                    lnz (3,t), fl (3,t)) ;
            end

            % plot the relative nnz(L) results
            subplot (2, 4, 2*sk - 1) ;
            loglog (s (1:t), lnz (2,1:t) ./ lnz (1,1:t), 'b-') ;
            hold on
            if (do_metis)
                loglog (s (1:t), lnz (3,1:t) ./ lnz (1,1:t), 'r-') ;
            end
            loglog (s (1:t), ones (1,t), 'k-') ;
            if (do_metis)
                ylabel ('nnz(L) for AMD or METIS / nnz(L) for meshnd') ;
                legend ('AMD', 'METIS') ;
            else
                ylabel ('nnz(L) for AMD / nnz(L) for meshnd') ;
            end
            xlabel ('mesh size') ;
            axis ([min(s) max(s) .1 10]) ;
            set (gca, 'YTick', [.1 .25 .5 .8 1 1.25 2 4 10]) ;
            if (is3D)
                set (gca, 'XTick', [1 10 100]) ;
                title (sprintf ('3D mesh, %d-point stencil', stencil)) ;
            else
                set (gca, 'XTick', [1 10 100 1000]) ;
                title (sprintf ('2D mesh, %d-point stencil', stencil)) ;
            end

            % plot the relative flop results
            subplot (2, 4, 2*sk) ;
            loglog (s (1:t), fl (2,1:t) ./ fl (1,1:t), 'b-') ;
            hold on
            if (do_metis)
                loglog (s (1:t), fl (3,1:t) ./ fl (1,1:t), 'r-') ;
            end
            loglog (s (1:t), ones (1,t), 'k-') ;
            ylabel ('flops for AMD or METIS / flops for meshnd') ;
            if (do_metis)
                ylabel ('flops for AMD or METIS / flops for meshnd') ;
                legend ('AMD', 'METIS') ;
            else
                ylabel ('nnz(L) for AMD / nnz(L) for meshnd') ;
            end
            xlabel ('mesh size') ;
            axis ([min(s) max(s) .1 10]) ;
            set (gca, 'YTick', [.1 .25 .5 .8 1 1.25 2 4 10]) ;
            if (is3D)
                set (gca, 'XTick', [1 10 100]) ;
                title (sprintf ('3D mesh, %d-point stencil', stencil)) ;
            else
                set (gca, 'XTick', [1 10 100 1000]) ;
                title (sprintf ('2D mesh, %d-point stencil', stencil)) ;
            end

            drawnow

        end

    catch
        % out-of-memory is OK, other errors are not
        disp (lasterr) ;
        if (isempty (strfind (lasterr, 'Out of memory')))
            error (lasterr) ;                                               %#ok
        else
            fprintf ('test terminated early, but otherwise OK\n') ;
        end
    end

end


function a = acosh(a)
%ACOSH        Gradient inverse hyperbolic cosine acosh(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 03/22/04     S.M. Rump  correction for complex input
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  wng = warning;
  warning off

  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  ax = full(a.x(:));
  ax = 1 ./ ( sqrt( ax-1 ) .* sqrt( ax+1 ) );
  a.x = acosh(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  warning(wng)

  if rndold
    setround(rndold)
  end


function [R, flag, timing, x0] = random_sample_precond(A, params, b)
% [R, flag, timing] = random_sample_precond(A, params)
%
% Builds a least-squares preconditioners for A based on
% random sampling.
%
% "params" - parameters governning the method.
%    params.type - type of mixing transform. Optional values: 'DCT', 'DHT', 'WHT'.
%                  Default is DHT.
%    params.gamma - gamma * n rows will be sampled (A is m-by-n).
%                   Default is 4.
%    params.preprocess_steps - number of mixing steps to do in advance.
%                               Default is 1.
%    params.maxcond - maximum condition number of the preconditioner.
%                     Default is 1 / (5 * epsilon_machine).
%
%
% Output:
%   R - the upper triangular factor of the preconditioner.
%   flag - success flag. If this function fails then probably A is rank
%          deficient.
%   timing - statistics on the time spent on various phases.
%
%
% 6-December 2009, Version 1.3
% Copyright (C) 2009, Haim Avron and Sivan Toledo.

if (nargin < 2)
    params = struct;
end

if (~isfield(params, 'type'));
    params.type = 'DHT';
end

if (~isfield(params, 'gamma'))
    params.gamma = 4;
end

if (~isfield(params, 'preprocess_steps'))
    params.preprocess_steps = 1;
end

if (~isfield(params, 'maxcond'))
    params.maxcond = 1 / (5 * eps);
end

if (~isfield(params, 'slight_coherence'))
    params.slight_coherence = 0;
end

if (~isfield(params, 'improve_start_point'))
    params.improve_start_point = false;
end

[m, n] = size(A);
mm = m;
times = 0;

timing.qr_time = 0;
timing.condest_time = 0;
timing.sample_time = 0;
timing.frut_time = 0;

if (~params.improve_start_point)
    x0 = [];
end

htimes =  params.preprocess_steps;
while (true)

    t0 = wtime;
    for i = 1:htimes
        D = sign(randn(mm, 1));
        A = fast_unitary_transform(A, D, params.type);
        mm = size(A, 1);

        if (params.improve_start_point)
            b = fast_unitary_transform(b, D, params.type);
        end
    end
    frut_time = wtime - t0;
    disp(sprintf(['\t\tRandom unit diagonal + unitary transformation time: ' ...
                  '%.2f sec'], frut_time));
    timing.frut_time = timing.frut_time + frut_time;

    t0 = wtime;
    %t = params.gamma * n / m;
    t = params.gamma * n / mm;
    s = rand(mm, 1);
    rows = find(s < t);
    R = A(rows, :);
    sample_time = wtime - t0;
    disp(sprintf('\t\tRandom sampling time: %.2f sec', sample_time));
    timing.sample_time = timing.sample_time + sample_time;

    if (params.slight_coherence > 0)
        R = [R; max(max(R))* rand(params.slight_coherence, n)];
    end

    t0 = wtime;
    if (~params.improve_start_point)
        R = mex_dgeqrf(R);
        R = triu(R(1:n, 1:n));
    else
        [R, tau] = mex_dgeqrf(R);
        Qtb = mex_dormqr(R, tau, b(rows), 'L', 'T');
        R = triu(R(1:n, 1:n));
        x0 = R \ Qtb(1:n);
    end

    qr_time = wtime - t0;
    disp(sprintf('\t\tQR on random sample time: %.2f sec', qr_time));
    timing.qr_time = timing.qr_time + qr_time;

    % Check if complete
    t0 = wtime;
    ce = mex_dtrcon(R);
    condest_time = wtime - t0;
    disp(sprintf('\t\tCondition estimation: %.2f sec', condest_time));
    timing.condest_time = timing.condest_time + condest_time;

    times = times + 1;

    if (ce > (1/ params.maxcond))
        flag = true;
        return;
    else
        if (times <= 3)
            disp(sprintf(['\t\tFailed to produced a non singular ' ...
                          'preocnditioner... applying one more FRUT...']));
            htimes = 1;
        else
            disp(sprintf('\t\tFailed to produced a non singular preocnditioner... too many FRUTs... giving up...'));
            flag = false;
            return;
        end
    end
end


classdef spotColorKey < orientationColorKey
  %
  %   Detailed explanation goes here

  properties
    center
    color
    psi
  end

  methods
    function oM = spotColorKey(varargin)
      oM = oM@orientationColorKey(varargin{:});

      oM.center = get_option(varargin,'center',orientation.id(oM.CS1));
      oM.CS1 = oM.center.CS;
      oM.color = get_option(varargin,'color',[1 0 0]);
      oM.psi = get_option(varargin,'kernel',...
        SO3DeLaValleePoussinKernel('halfwidth',get_option(varargin,'halfwidth',10*degree)));
    end

    function rgb = orientation2color(oM,ori)

      s = size(ori);
      rgb = ones([s,3]);

      for k=1:length(oM.center)

        w = oM.psi.eval(dot(ori,oM.center(k)))./oM.psi.eval(1);

        cdata = rgb2hsv(repmat(oM.color(k,:),length(ori),1));
        cdata(:,2) = w(:).*cdata(:,2);
        cdata = reshape(hsv2rgb(cdata),[s,3]);
        rgb = rgb.*cdata;
      end
    end
  end
end


function [mod_ref_data, mod_deg_data]= input_filter( ref_data, ref_Nsamples, ...
    deg_data, deg_Nsamples)

mod_ref_data= DC_block( ref_data, ref_Nsamples);
mod_deg_data= DC_block( deg_data, deg_Nsamples);

mod_ref_data= apply_filters( mod_ref_data, ref_Nsamples);
mod_deg_data= apply_filters( mod_deg_data, deg_Nsamples);


function W = spm_vb_adjacency(edges,weights,N)
% (Weighted) adjacency (or weight) matrix of a graph
% FORMAT W = spm_vb_adjacency(edges,weights,N)
%
% edges    [Nedges x 2] list of neighboring voxel indices
% weights  [Nedges x 1] list of edge weights (unity of not specified)
% N        number of nodes (cardinality of node set)
%
% W        [N x N] matrix of (weighted) edges
% Wij      edge weight between nodes i and j if they are neighbors, otherwise 0
%__________________________________________________________________________
% Copyright (C) 2008-2014 Wellcome Trust Centre for Neuroimaging

% Lee Harrison
% $Id: spm_vb_adjacency.m 6079 2014-06-30 18:25:37Z spm $

% Number of edges
Ne = size(edges,1);

% Uniform weights if not specified
if nargin < 2
    weights = ones(Ne,1);
end

% Number of nodes (if N is not specified)
if nargin < 3
    N = max(edges(:));
end

% (Weighted) adjacency matrix
W  = sparse([edges(:,1);edges(:,2)],[edges(:,2);edges(:,1)],...
            [weights;weights],N,N);


function [prob,sol,fmin] = nls_prob(varargin)
%NLS_PROB  Return an OPTI NLS
%
%   prob = nls_prob(no) return a pre-built optiprob of a saved NLS.
%
%   [prob,sol,fmin] = NLS_prob(no) returns the optimum solution and function
%   eval at the optimum
%
%   no = nls_prob() returns the number of problems available for testing.

%   (C) 2012 Jonathan Currie (I2C2)

% Functions are taken from:
% More, J. J., B. S. Garbow, and K. E. Hillstrom. "Testing Unconstrained
% Optimization Software." ACM Transactions on Mathematical Software 7,
% no. 1 (1981): 17-41.

%Check if just returning no problems
if(nargin < 1)
    prob = 20; sol = []; fmin = [];
    return;
else
    no = varargin{1};
end

%Big switch yard
switch(no)
    case 1
        fun = @(x) [10*(x(2)-x(1)^2); 1 - x(1)];
        ydata = zeros(2,1);
        x0 = [-1.2;1];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1,1];
        fmin = 0;

    case 2
        fun = @(x) [-13 + x(1) + ((5 - x(2))*x(2) - 2)*x(2);
                    -29 + x(1) + ((x(2) + 1)*x(2) - 14)*x(2)];
        ydata = zeros(2,1);
        x0 = [0.5;-2];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [5,4];
        fmin = 0;

    case 3
        fun = @(x) [1e4*x(1)*x(2) - 1;
                    exp(-x(1)) + exp(-x(2)) - 1.0001];
        ydata = zeros(2,1);
        x0 = [0;1];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1.098e-5;9.106];
        fmin = 0;

    case 4
        fun = @(x) [x(1) - 1e6; x(2) - 2e-6; x(1)*x(2) - 2];
        ydata = zeros(3,1);
        x0 = [1;1];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1e6,2e-6];
        fmin = 0;

    case 5
        fun = @(x) [1.5 - x(1)*(1 - x(2)); 2.25 - x(1)*(1 - x(2)^2); 2.625 - x(1)*(1 - x(2)^3) ];
        ydata = zeros(3,1);
        x0 = [1;1];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [3;0.5];
        fmin = 0;

    case 6
        i = (1:10)';
        fun = @(x) 2 + 2*i - (exp(0.2578*i) + exp(0.2578*i));
        ydata = zeros(10,1);
        x0 = [0.3;0.4];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [0.2578;0.2578];
        fmin = 124.36226865;

    case 7
        fun = @more7;
        ydata = zeros(3,1);
        x0 = [-1;0;0];
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1;0;0];
        fmin = 0;

    case 8
        u = (1:15)';
        v = 16 - u;
        w = zeros(15,1);
        for i = 1:15
            w(i) = min(u(i),v(i));
        end
        y = [0.14;0.18;0.22;0.25;0.29;0.32;0.35;0.39;0.37;0.58;0.73;0.96;1.34;2.10;4.39];
        fun = @(x) (x(1) + u ./ (v*x(2) + w*x(3)));
        x0 = [1;1;1];
        prob = optiprob('fun',fun,'ydata',y,'x0',x0);
        sol = [];
        fmin = 8.21487e-3;

    case 9
        i = (1:15)';
        y = [0.0009;0.0044;0.0175;0.0540;0.1295;0.2420;0.3521;0.3989;0.3521;0.2420;0.1295;0.0540;0.0175;0.0044;0.0009];
        t = (8 - i)./2;
        fun = @(x) x(1)*exp((-x(2)*(t - x(3)).^2)./2);
        x0 = [0.4;1;0];
        prob = optiprob('fun',fun,'ydata',y,'x0',x0);
        sol = [];
        fmin = 1.12793e-8;

    case {10,11}
        i = (1:16)';
        y = [34780;28610;23650;19630;16370;13720;11540;9744;8261;7030;6005;5147;4427;3820;3307;2872];
        t = 45 + 5*i;
        fun = @(x) x(1)*exp(x(2)./(t + x(3)));
        x0 = [0.02;4000;250];
        prob = optiprob('fun',fun,'ydata',y,'x0',x0);
        sol = [];
        fmin = 87.9458;

%     case 11 %not working??
%         m = 100;
%         i = [1:m]';
%         t = i./100;
%         y = 25 + (-50 * log(t)).^(2/3);
%         fun = @(x) exp(-(abs(y*100.*i*x(2)).^x(3))./x(1)) - t;
%         x0 = [5;2.5;0.15];
%         ydata = zeros(m,1);
%         prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
%         sol = [50;25;1.5];
%         fmin = 0;

    case 12
        m = 3;
        i = (1:m)';
        t = 0.1*i;
        fun = @(x) exp(-t*x(1)) - exp(-t*x(2)) - x(3)*exp(-t) - exp(-10*t);
        x0 = [0;10;20];
        ydata = zeros(m,1);
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1;10;1];
        fmin = 0;

    case 13
        fun = @(x) [x(1) + 10*x(2);
                    sqrt(5)*(x(3) - x(4));
                    (x(3) - 2*x(3))^2;
                    sqrt(10)*(x(1)-x(4))^2];
        x0 = [3;-1;0;1];
        ydata = zeros(4,1);
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [0;0;0;0];
        fmin = 0;

    case 14
        fun = @(x) [10*(x(2)-x(1)^2);
                    1 - x(1);
                    sqrt(90)*(x(4)-x(3)^2);
                    1 - x(3);
                    sqrt(10)*(x(2) + x(4) - 2);
                    10^(-0.5)*(x(2)-x(4))];
        x0 = [3;-1;-3;-1];
        ydata = zeros(6,1);
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [1;1;1;1];
        fmin = 0;

    case 15
        y = [0.1957;0.1947;0.1735;0.16;0.0844;0.0627;0.0456;0.0342;0.0323;0.0235;0.0246];
        u = [4;2;1;0.5;0.25;0.167;0.125;0.1;0.0833;0.0714;0.0625];
        fun = @(x) (x(1)*(u.^2 + u*x(2))) ./ (u.^2 + u*x(3) + x(4));
        x0 = [0.25;0.39;0.415;0.39];
        prob = optiprob('fun',fun,'ydata',-y,'x0',x0);
        sol = [];
        fmin = 3.07505e-4;

    case 16
        m = 20;
        i = (1:m)';
        t = i/5;
        fun = @(x) (x(1) + t*x(2) - exp(t)).^2 + (x(3) + x(4)*sin(t) - cos(t)).^2;
        x0 = [25;5;-5;-1];
        ydata = zeros(m,1);
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [];
        fmin = 85822.2016;

    case 17
        m = 33;
        i = (1:m)';
        t = 10*(i - 1);
        y = [0.844;0.908;0.932;0.936;0.925;0.908;0.881;0.850;0.818;0.784;0.751;0.718;0.685;0.658;0.628;0.603;...
             0.58;0.558;0.538;0.522;0.506;0.490;0.478;0.467;0.457;0.448;0.438;0.431;0.424;0.42;0.414;0.411;0.406];
        fun = @(x) (x(1) + x(2)*exp(-t*x(4)) + x(3)*exp(-t*x(5)));
        x0 = [0.5;1.5;-1;0.01;0.02];
        prob = optiprob('fun',fun,'ydata',-y,'x0',x0);
        sol = [];
        fmin = 5.46489e-5;

    case 18
        m = 13;
        i = (1:m)';
        t = 0.1*i;
        y = exp(-t) - 5*exp(-10*t) + 3*exp(-4*t);
        fun = @(x) x(3)*exp(-t*x(1)) - x(4)*exp(-t*x(2)) + x(6)*exp(-t*x(5));
        x0 = [1;2;1;1;1;1];
        prob = optiprob('fun',fun,'ydata',y,'x0',x0);
        sol = [];
        fmin = 0; %5.65565e-3; seems wrong?

    case 19
        m = 65;
        i = (1:m)';
        t = (i-1)/10;
        y = [1.366;1.191;1.112;1.013;0.991;0.885;0.831;0.847;0.786;0.725;0.746;0.679;0.608;0.655;0.616;0.606;0.602;0.626;0.651;0.724;0.649;0.649;...
            0.694;0.644;0.624;0.661;0.612;0.558;0.533;0.495;0.5;0.423;0.395;0.375;0.372;0.391;0.396;0.405;0.428;0.429;0.523;0.562;0.607;0.653;...
            0.672;0.708;0.633;0.668;0.645;0.632;0.591;0.559;0.597;0.625;0.739;0.710;0.729;0.720;0.636;0.581;0.428;0.292;0.162;0.098;0.054];
        fun = @(x) (x(1)*exp(-t*x(5)) + x(2)*exp(-(t-x(9)).^2*x(6)) + x(3)*exp(-(t-x(10)).^2*x(7)) + x(4)*exp(-(t-x(11)).^2*x(8)));
        x0 = [1.3;0.65;0.65;0.7;0.6;3;5;7;2;4.5;5.5];
        prob = optiprob('fun',fun,'ydata',y,'x0',x0);
        sol = [];
        fmin = 4.01377e-2;

    case 20
        m = 31; n = 12;
        fun = @more20;
        x0 = zeros(n,1);
        ydata = zeros(m,1);
        prob = optiprob('fun',fun,'ydata',ydata,'x0',x0);
        sol = [];
        fmin = 4.72238e-10;

    otherwise
        error('Problem not available or not implemented yet');
end

function j = more7(x)

if(x(1) > 0)
    theta = 1/(2*pi)*atan(x(2)/x(1));
else
    theta = 1/(2*pi)*atan(x(2)/x(1)) + 0.5;
end

j(1,1) = 10*(x(3) - 10*theta);
j(2,1) = 10*(x(1)^2 + x(2)^2)^0.5 - 1;
j(3,1) = x(3);

function f = more20(x)
n = length(x);
j = (2:n)';
j2 = (1:n)';
t = 0.1*(1:29)';
f = zeros(31,1);
for k = 1:29
    f(k) = sum( (j-1).*x(2:end).*t(k).^(j-2) ) - sum( x.*t(k).^(j2-1) ) - 1;
end
f(30) = x(1);
f(31) = x(2) - x(1)^2 - 1;


% SOLVE_KIRCHHOFF_LOVE_SHELL: Solve the Kirchhoff-Love shell model in a NURBS domain.
%
% USAGE:
%
%  [geometry, msh, space, u] = solve_kirchhoff_love_shell (problem_data, method_data)
%
% INPUT:
%
%  problem_data: a structure with data of the problem. It contains the fields:
%    - geo_name:     name of the file containing the geometry
%    - drchlt_sides: sides with Dirichlet boundary condition
%    - drchlt_components: cell-array, the components that are set to zero for each drchlt_side
%    - E_coeff:      function handle for Young's modulus
%    - nu_coeff:     function handle for Poisson's ratio
%    - thickness:    scalar value, thickness of the shell
%    - f:            source term, distributed load
%
%  method_data : a structure with discretization data. Its fields are:
%    - degree:     degree (>=2) of the spline functions.
%    - regularity: continuity (>=1) of the spline functions.
%    - nsub:       number of subelements with respect to the geometry mesh
%                   (nsub=1 leaves the mesh unchanged)
%    - nquad:      number of points for Gaussian quadrature rule
%
% OUTPUT:
%
%  geometry: geometry structure (see geo_load)
%  msh:      mesh object that defines the quadrature rule (see msh_cartesian)
%  space:    space object that defines the discrete basis functions (see sp_vector)
%  u:        the computed degrees of freedom
%
% NOTE: only homogeneous Dirichlet conditions implemented so far
%
% See also EX_KL_SHELL_SCORDELIS_LO_ROOF for an example.
%
% Copyright (C) 2017-2019 Pablo Antolin, Luca Coradello, Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

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

function [geometry, msh, space, u] = solve_kirchhoff_love_shell (problem_data, method_data)

% Extract the fields from the data structures into local variables
data_names = fieldnames (problem_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= problem_data.(data_names{iopt});']);
end
data_names = fieldnames (method_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= method_data.(data_names{iopt});']);
end

if (any(degree <= 1) || any(regularity == 0))
  error ('The degree must be at least two, and the regularity at least C^1')
end

geometry = geo_load (geo_name);
degelev  = max (degree - (geometry.nurbs.order-1), 0);
nurbs    = nrbdegelev (geometry.nurbs, degelev);
[rknots, ~, nknots] = kntrefine (nurbs.knots, nsub-1, nurbs.order-1, regularity);

nurbs    = nrbkntins (nurbs, nknots);
geometry = geo_load (nurbs);

% Construct msh structure
rule     = msh_gauss_nodes (nquad);
[qn, qw] = msh_set_quad_nodes (geometry.nurbs.knots, rule);
msh      = msh_cartesian (geometry.nurbs.knots, qn, qw, geometry);

% Construct space structure
sp_scalar = sp_nurbs (geometry.nurbs, msh);
scalar_spaces = repmat ({sp_scalar}, 1, msh.rdim);
space = sp_vector (scalar_spaces, msh);

% Assemble the stiffness matrix and right-hand side
K = op_KL_shells_tp (space, space, msh, E_coeff, nu_coeff, thickness);
rhs = op_f_v_tp (space, msh, f);

u = zeros (space.ndof, 1);

% Apply boundary conditions
drchlt_dofs = [];
for iside = 1:numel(drchlt_sides)
  side = drchlt_sides(iside);
  if (~exist('drchlt_components','var'))
    components = 1:3;
  else
    components = drchlt_components{iside};
  end
  for icomp = components
    drchlt_dofs = union (drchlt_dofs, space.boundary(side).dofs(space.boundary(side).comp_dofs{icomp}));
  end
end

int_dofs = setdiff (1:space.ndof, drchlt_dofs);
%rhs(int_dofs) = rhs(int_dofs) - K(int_dofs, drchlt_dofs)*u(drchlt_dofs);

% Solve the linear system
u(int_dofs) = K(int_dofs, int_dofs) \ rhs(int_dofs);

end


function [P Fcost Emi Pl]=emield(elddata,emidata,h1,h2,B,Pd)

if nargin ~= 6
     error('Wrong number of input arguments')
end
n=length(elddata(:,1));

     Aeq=ones(1,n);
     a=elddata(:,1);
          b=elddata(:,2);
               c=elddata(:,3);
                    l=elddata(:,4);
                         u=elddata(:,5);
                          a1=emidata(:,1);
          b1=emidata(:,2);
               c1=emidata(:,3);
                           P=l;
                         for i=1:5
                             Pl=P'*B*P;
                             Pd1=Pd+Pl;
                             ll=diag(1-2*B*P);
                             A1=inv(ll)*(h1*a+h2*a1);
                              B1=inv(ll)*(h1*b+h2*b1);
                              H=2*diag(A1);
                              P=quadprog(H,B1,[],[],Aeq,Pd1,l,u);
                              Fcost=a'*(P.*P)+b'*P+sum(c);
                               Emi=a1'*(P.*P)+b1'*P+sum(c1);
                         end


function sphere_cubed_grid_lines_test ( )

%*****************************************************************************80
%
%% SPHERE_CUBED_GRID_LINES_TEST tests SPHERE_CUBED_GRID_LINES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 May 2015
%
%  Author:
%
%    John Burkardt
%
  n = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_CUBED_GRID_LINES_TEST\n' );
  fprintf ( 1, '  SPHERE_CUBED_GRID_LINES defines the lines\n' );
  fprintf ( 1, '  on a cubed sphere grid.\n' );
  fprintf ( 1, '  Each cube face is divided into %dx%d subfaces\n', n, n  );

  point_num = sphere_cubed_grid_point_count ( n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of points is %d\n', point_num );

  xyz = sphere_cubed_grid_points ( n, point_num );

  line_num = sphere_cubed_grid_line_count ( n );

  fprintf ( 1, '  The number of grid lines is %d\n', line_num );

  line_data = sphere_cubed_grid_lines ( n, line_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Line     Start              End\n' );
  fprintf ( 1, '  Index    X    Y   Z         X  Y   Z\n' );
  fprintf ( 1, '\n' );

  for i = 1 : min ( 10, line_num )
    fprintf ( 1, '\n' )
    fprintf ( 1, '  %4d  %10f  %10f  %10f    %10f  %10f  %10f\n', ...
      i, line_data(i,1:3,1), line_data(i,1:3,2) );
  end

  return
end


% This subroutine assigns creates a 3D grid with
% spacing dx,dy, dz (in degreees). The size will
% be selected interactiVELY. The pvalue in each
% volume around a grid point containing ni earthquakes
% will be calculated as well as the magnitude
% of completness
%   Stefan Wiemer 1/98

report_this_filefun(mfilename('fullpath'));
global no1 bo1 inb1 inb2

if sel == 'in'
    % get the grid parameter
    % initial values
    %
    dx = 0.1;
    dy = 0.1 ;
    dz = 5.00 ;
    ni = 300;
    R = 10000;

    def = {'0.1','0.1',num2str(dz),num2str(max(a.Depth)), num2str(min(a.Depth))};

    tit ='Three dimesional z-value analysis';
    prompt={...
        'Spacing in Longitude (dx in [deg])',...
        'Sapcing in Latitude  (dy in [deg])',...
        'Spacing in Depth    (dz in [km ])',...
        'Depth Range: deep limit [km] ',...
        'Depth Range: shallow limit',...
        };

    ni2 = inputdlg(prompt,tit,1,def);

    l = ni2{1}; dx= str2double(l);
    l = ni2{2}; dy= str2double(l);
    l = ni2{3}; dz= str2double(l);
    l = ni2{4}; z1= str2double(l);
    l = ni2{5}; z2= str2double(l);

    sel = 'ca'; zgrid3d

end   % if sel == 'in'

% get the grid-size interactively and
% calculate the b-value in the grid by sorting
% thge seimicity and selectiong the ni neighbors
% to each grid point

if sel == 'ca'
    selgp3dB
    zvect=[z2:dz:z1];
    gz = zvect;
    itotal = length(t5);
    zmap_message_center.set_info(' ','Running... ');think
    %  make grid, calculate start- endtime etc.  ...
    %
    zvg = NaN(length(gx),length(gy),length(gz),300);
    ram  = NaN(length(gx),length(gy),length(gz),300);

    t0b = min(a.Date)  ;
    n = a.Count;
    teb = a(n,3) ;
    tdiff = round((teb - t0b)*365/par1);
    loc = zeros(3, length(gx)*length(gy));

    % loop over  all points
    %
    i2 = 0.;
    i1 = 0.;
    allcount = 0.;
    wai = waitbar(0,' Please Wait ...  ');
    set(wai,'NumberTitle','off','Name',' 3D gridding - percent done');;
    drawnow
    %
    %

    z0 = 0; x0 = 0; y0 = 0; dt = 1;
    % loop over all points
    for il =1:length(t5);

        x = t5(il,1);
        y = t5(il,2);
        z = t5(il,3);
        allcount = allcount + 1.;
        i2 = i2+1;

        % calculate distance from center point and sort wrt distance
        di = sqrt(((a.Longitude-x)*cos(pi/180*y)*111).^2 + ((a.Latitude-y)*111).^2 + ((a.Depth - z)).^2 ) ;
        [s,is] = sort(di);

        l2 = find(is <= 300);

        %[cumu, xt] = hist(b(:,3),(t0b:(teb-t0b)/99:teb));

        zvg(t5(il,5),t5(il,6),t5(il,7),:) = is(1:300);
        ram(t5(il,5),t5(il,6),t5(il,7),:) = di(is(1:300));
        if rem(allcount,20) == 0;  waitbar(allcount/itotal) ;end
    end  % for xt5
    % save data
    %
    catSave3 =...
        [ 'zmap_message_center.set_info(''Save Grid'',''  '');think;',...
        '[file1,path1] = uiputfile(fullfile(hodi, ''eq_data'', ''*.mat''), ''Grid Datafile Name?'') ;',...
        ' sapa2 = [''save '' path1 file1 '' zvg ram gx gy gz dx dy dz  par1 tdiff t0b teb a main faults mainfault coastline yvect xvect tmpgri ll''];',...
        ' if length(file1) > 1, eval(sapa2),end , done']; eval(catSave3)

    close(wai)
    watchoff

    gz = -gz;
    zv2 = zvg;
    sel = 'no';
    lta_winy = 2;
    zv4 = zv2;
    tiz = 10;
    slm = 'new'; slicemapz;

end  % if cal


function varargout = integral2(varargin)
%INTEGRAL2  Double integral of a SPHEREFUN over its domain.
%   I = INTEGRAL2(F) returns a value representing the double integral of a
%   SPHEREFUN.
%
% See also SPHEREFUN/INTEGRAL, SPHEREFUN/SUM2, SPHEREFUN/QUAD2D.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

[varargout{1:nargout}] = sum2(varargin{:});

end


function varargout = implies(varargin)
%IMPLIES Logical implication
%
% IMPLIES(X,Y) creates a mixed integer representation of
% the constraint X --> Y, i.e. Y is true if X is true.
%
% Syntax
%   F = implies(X,Y,tol)
%
% Input
%   X : binary SDPVAR variable or a constraint
%   Y : binary SDPVAR variable or a constraint
%  tol: Optional threshhold for defining zero (see NOTE)
%
% Output
%   F : SET object
%
% Examples
%
%  binvar X Y; F = implies(X,Y);
%  binvar X;sdpvar Y; F = [implies(X,Y>=5), -10 <= Y <= 100];
%  binvar X;Y=sdpvar(3,1); F = [implies(X,[sum(Y);Y(2)]>=[5;0]), -1<= Y <= 10];
%
% Note
%
%  All variables in the expressions have to be explicitly bounded somewhere
%  in the model (implicit constraints are not sufficients such as [Y <= Z,...,Z<= 10]
%
%  Using implies with X non-binary is highly sensitive numerically.
%  The problem comes from the definition of 0 in a floating-point
%  environment, and precision in the solver. To account for this,
%  the user can supply a third argument to define a dead-zone around
%  zero, i.e Implies(X<=0,Y) will be replaced with IMPLIES(X<=-tol,Y)
%  Note, you typically need to tweak this number for your
%  application/solver. By default, YALMIP uses tol = 0, which means you
%  easily can get garbage... A positive number means YALMIP is cautious in
%  terms of activating the condition, while a negative number means YALMIP
%  will be aggressive  in activating the condition.
%
%   See also @SDPVAR/AND, @SDPVAR/OR, IFF

% There are some cases to take care of...
%
% X --> Y     binary/binary :                     Implemented
% X --> Y     binary/(LP,equality,sdp)            Implemented
% X --> Y     (LP,equality,sdp)/binary            Not implemented
% X --> Y     (LP,equality,sdp)/(LP,equality,sdp) Not implemented

X = varargin{1};
Y = varargin{2};

if isempty(X)
    varargout{1} = [];
end

switch class(X)

    case {'sdpvar','constraint','lmi'}
        varargout{1} = setupMeta(lmi([]), mfilename,varargin{:});

    case 'char'
        varargout{1} = implies_internal(varargin{3:end});

    case 'logical'
        if length(X)==1
            if X
                varargout{1} = Y;
            else
                varargout{1} = [];
            end
        else
            if length(X) == length(Y)
                i = find(X);
                if isempty(i)
                    varargout{1} = [];
                else
                    varargout{1} = Y(i);
                end
            else
                error('Size mismatch in input arguments');
            end
        end
end


function figure_num = trim_test ( figure_num )

%*****************************************************************************80
%
%% TRIM_TEST tries to make a plot without all that wasted margin.
%
%  Discussion:
%
%    Surprisingly, a typical MATLAB plot can involve a lot of empty
%    space.  In the simple case where you are plotting a circle, you
%    will get white space in the grid region itself if you enforce
%    equal axis units, since MATLAB wants to display the grid within
%    a rectangle.  But you will also get a substantial margin of white
%    space around the plot area itself.  This can be annoying when you
%    are trying to prepare graphics for use in a publication, or
%    if you want to put several images side by side - the white space,
%    which contains no information, eats up lots of space.
%
%    Can you cut back that white space?  In particular, if you want to
%    plot a circle (or anything else whose shape doesn't correspond to
%    MATLAB's "golden rectangle"), can you make an image that is
%    mathematically correct, and economic in terms of wasted white space?
%
%    You may try various combinations of "axis equal", "axis square",
%    "axis tight", "axis ( [0,1,0,1] )" and so on, all the time coming
%    halfway to your goal, without ever actually achieving an image
%    that is square (or whatever aspect ratio you want), doesn't have
%    wasted graph space, and doesn't have that excessive margin around
%    the plot.
%
%    After wasting a lot of time, I stumbled across a few commands that,
%    used together, seem to achieve the correct result.  However, in my
%    case, the image still looks rectangular in the MATLAB interactive
%    viewer - but comes out nice and square in the PNG image rendered
%    by the PRINT command.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 May 2013
%
%  Author:
%
%    John Burkardt
%
  if ( nargin < 1 )
    figure_num = 0;
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIM_TEST\n' );
  fprintf ( 1, '  Try to suppress the wasted margin.\n' );
%
%  Set up the circle data once.
%
  r = 1.0;
  theta = linspace ( 0.0, 2.0 * pi, 121 );
  x = r * cos ( theta );
  y = r * sin ( theta );
%
%  Plot the circle with the default axis.
%
  figure_num = figure_num + 1;
  gcf = figure ( figure_num );
  plot ( x, y, 'b-', 'LineWidth', 3 );
  grid on
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  title ( 'A circle drawn with the default aspect ratio and margin' )

  filename = 'trim_test01.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved graphics file "%s".\n', filename );
%
%  Plot the circle again, using commands I DON'T REALLY UNDERSTAND.
%  But they do seem to result in a PNG image that has lost the margin junk.
%
  figure_num = figure_num + 1;
  gcf = figure ( figure_num );

  set ( gcf, 'PaperUnits', 'inches','PaperPosition', [0, 0, 10, 10] );
  pbaspect ( [ 1.0, 1.0, 1.0 ] );
  set ( gcf, 'Units', 'normal' );
  set ( gca, 'Position', [ 0.05, 0.05, 0.90, 0.90 ] )

  plot ( x, y, 'r-', 'LineWidth', 3 );
  grid on
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  title ( 'A circle drawn with white space trimmed' )

  filename = 'trim_test02.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Saved graphics file "%s".\n', filename );

  return
end


function i4vec_part_quick_a_test ( )

%*****************************************************************************80
%
%% I4VEC_PART_QUICK_A_TEST tests I4VEC_PART_QUICK_A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 12;
  b = 0;
  c = n;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_PART_QUICK_A_TEST\n' );
  fprintf ( 1, '  I4VEC_PART_QUICK_A reorders an integer vector\n' );
  fprintf ( 1, '  as part of a quick sort.\n' );
  fprintf ( 1, '  Using initial random number seed = %d\n', seed );

  [ a, seed ] = i4vec_uniform_ab ( n, b, c, seed );

  i4vec_print ( n, a, '  Before rearrangement:' );

  [ a, l, r ] = i4vec_part_quick_a ( n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Rearranged array\n' );
  fprintf ( 1, '  Left index =  %d\n', l );
  fprintf ( 1, '  Key index =   %d\n', l+1 );
  fprintf ( 1, '  Right index = %d\n', r );

  i4vec_print ( l,     a(1:l),   '  Left half:' );
  i4vec_print ( 1,     a(l+1),   '  Key:' );
  i4vec_print ( n-l-1, a(l+2:n), '  Right half:' );

  return
end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = sprice_4_2_fast(a, b, r, n, f, k, t,m, mu, nu, l, u, cp)
% sabr prices using the risk neutral density psabr_4
% and integrating this density with respect to the payoff
% fast version since cumsum is applied

eps = 0.001;
nl = length(k);
y = ones(1,nl);

rr = eps/10:eps:.5;

F1 = @(x) x .* psabr_4_2(a, b, r, n, f, x, t, m, mu, nu, l, u);
F2 = @(x) psabr_4_2(a, b, r, n, f, x, t, m, mu, nu, l, u);

y1 = F1(rr);
y2 = F2(rr);

y1 = cumsum(y1(end:-1:1)); y1 = y1(end:-1:1);
y2 = cumsum(y2(end:-1:1)); y2 = y2(end:-1:1);

    % call
for j = 1:nl;
    index = find(rr>=k(j),1,'first');
    y(j) = eps*(y1(index) - k(j) * y2(index));
end

if (cp ~= 1)
     y = k - f + y;
end

end


function [lbm] = g2lbm(g)
% Convert mass from grams to pounds-mass.
% Chad Greene 2012
lbm = g*0.002204622621849 ;


function gBest = gBest_get(AA)
% The gBest (global best) updating strategy of S-ECSO

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    [M,~]     = size(AA.decs);
    r         = rand(1,size(AA.objs,2));
    r_matr    = repmat(r,M,1);
    f_gBest   = sum(r_matr.*AA.objs,2)/sum(r);
    [~,index] = min(f_gBest);
    B         = AA.decs;
    gBest     = B(index,:);
end


function [ x ] = phi_x_inv( y )

global minus_log_phi_inv_table;
global min_minus_log_phi;
global max_minus_log_phi;
global increment_minus_log_phi;
minus_log_phi = -log(y);
minus_log_phi = max(minus_log_phi, min_minus_log_phi);
minus_log_phi = min(minus_log_phi, max_minus_log_phi);
minus_log_phi_index = round((minus_log_phi - min_minus_log_phi)/increment_minus_log_phi - 0.499) + 1;
x = minus_log_phi_inv_table(minus_log_phi_index);

end


function raw_y = adpcm_decoder(adpcm_y)

% This m-file is based on the app note: AN643, Adaptive differential pulse
% code modulation using PICmicro microcontrollers, Microchip Technology
% Inc. The app note is avaialbe from www.microchip.com
% Example:  Y = wavread('test.wav');
%           y = adpcm_encoder(Y);
%           YY = adpcm_decode(y);
IndexTable = [-1, -1, -1, -1, 2, 4, 6, 8, -1, -1, -1, -1, 2, 4, 6, 8];

StepSizeTable = [7, 8, 9, 10, 11, 12, 13, 14, 16, 17, 19, 21, 23, 25, 28, 31, 34, 37, 41, 45, 50, 55, 60, 66, 73, 80, 88, 97, 107, 118, 130, 143, 157, 173, 190, 209, 230, 253, 279, 307, 337, 371, 408, 449, 494, 544, 598, 658, 724, 796, 876, 963, 1060, 1166, 1282, 1411, 1552, 1707, 1878, 2066, 2272, 2499, 2749, 3024, 3327, 3660, 4026, 4428, 4871, 5358, 5894, 6484, 7132, 7845, 8630, 9493, 10442, 11487, 12635, 13899, 15289, 16818, 18500, 20350, 22385, 24623, 27086, 29794, 32767];

prevsample = 0;
previndex = 1;

Ns = length(adpcm_y);
n = 1;

while (n <= Ns)
    predsample = prevsample;
    index = previndex;
    step = StepSizeTable(index);
    code = adpcm_y(n);

    diffq = bitshift(step, -3);
    if (bitand(code, 4))
        diffq = diffq + step;
    end
    if (bitand(code, 2))
        diffq = diffq + bitshift(step, -1);
    end
    if (bitand(code, 1))
        diffq = diffq + bitshift(step, -2);
    end

    if (bitand(code, 8))
        predsample = predsample - diffq;
    else
        predsample = predsample + diffq;
    end

    if (predsample > 32767)
        predsample = 32767;
    elseif (predsample < -32768)
        predsample = -32768;
    end

    index = index + IndexTable(code+1);

    if (index < 1)
        index = 1;
    end
    if (index > 89)
        index = 89;
    end

    prevsample = predsample;
    previndex = index;

    raw_y(n) = predsample / 32767;
    n = n + 1;
end


function linpack_z_test23 ( )

%*****************************************************************************80
%
%% TEST23 tests ZPPCO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST23\n' );
  fprintf ( 1, '  For a double precision complex (C)\n' );
  fprintf ( 1, '  Hermitian positive definite packed matrix (PP),\n' );
  fprintf ( 1, '  ZPPCO estimates the reciprocal condition number.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix order is N = %d\n', n );
%
%  Set the values of the matrix A.
%
  a(1) = complex ( 2.5281,  0.0000 );

  a(2) = complex ( 2.1341, -0.2147 );
  a(3) = complex ( 3.0371,  0.0000 );

  a(4) = complex ( 2.4187,  0.2932 );
  a(5) = complex ( 2.0905,  1.1505 );
  a(6) = complex ( 2.7638,  0.0000 );
%
%  Estimate the condition.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimate the condition number.\n' );

  [ a, rcond, info ] = zppco ( a, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Reciprocal condition number = %f\n', rcond );

  return
end


function [M] = hyperConvert2d(M)
% HYPERCONVERT2D Converts an HSI cube to a 2D matrix
% Converts a 3D HSI cube (m x n x p) to a 2D matrix of points (p X N)
% where N = mn
%
% Usage
%   [M] = hyperConvert2d(M)
% Inputs
%   M - 3D HSI cube (m x n x p)
% Outputs
%   M - 2D data matrix (p x N)

if (ndims(M)>3 || ndims(M)<2)
    error('Input image must be m x n x p or m x n');
end
if (ndims(M) == 2)
    numBands = 1;
    [h, w] = size(M);
else
    [h, w, numBands] = size(M);
end

M = reshape(M, w*h, numBands).';

return;


function varargout = quad2d(varargin)
%QUAD2D  Compute definite integral of a CHEBFUN2.
%   I = QUAD2D( F ) returns the definite integral of a CHEBFUN2
%   over its domain of definition.
%
%   I = QUAD2D(F, a, b, c, d) returns the definite integral of a CHEBFUN2
%   over the rectangle [a,b] x [c,d].
%
% See also INTEGRAL2, SUM2, INTEGRAL.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

[varargout{1:nargout}] = quad2d@separableApprox(varargin{:});

end


function lambda = householder_eigenvalues ( n, x )

%*****************************************************************************80
%
%% HOUSEHOLDER_EIGENVALUES returns the eigenvalues of a HOUSEHOLDER matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 May 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real X(N,1), the vector that defines the
%    Householder matrix.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  x = x(:);

  lambda(1,1)   = -1.0;
  lambda(2:n,1) = +1.0;

  return
end


%L2depths Compute depths of PRMM from basis L.
%
%  No known depths are exploited. These can be different because of noise
%  etc.
%
%  Parameters:
%    opt.verbose(1) .. whether display info

function [Mdepths, lambda] = L2depths(L, M, Idepths, opt)

if nargin < 4, opt = []; end
if ~isfield(opt,'verbose')
  opt.verbose = 1; end

if opt.verbose, fprintf('Computing depths...'); tic; end

Mdepths = M;

[m n] = size(M); m = m/3;
lambda(m, n) = 0;  % memory allocation

for j = 1:n
  full     = find(~isnan(M(1:3:end,j)));
  mis_rows = intersect(find(Idepths(:,j)==0),full);
  if length(mis_rows) > 0
    submatrix = spread_depths_col(M(k2i(full),j),Idepths(full,j));

    % We want submatrix to be in the space L ->
    % we search for combination of columns of the base of L i.e.
    % L(k2i(full),:)*res(1:4)-submatrix*[1 res(5:length(res))] = 0
    right = submatrix(:,1);
    A     = [ L(k2i(full),:) -(submatrix(:,2:size(submatrix,2))) ];
    if rank(A) < size(A, 2)  % depths cannot be computed => kill the data
      kill = full(~Idepths(full,j));
      Mdepths(k2i(kill),j) = NaN; lambda(kill,j) = NaN;
    else
      res   = A \ right;

      %test: er should be near to zero
      %er=L(k2i(full),:)*res(1:4)-submatrix*[1 res(5:length(res))']'

      % depth corresponding to right is/are set to 1
      i = full(find(right(1:3:end))); lambda(i,j) = 1;
      Mdepths(k2i(i),j) = M(k2i(i),j);

      for ii = 1:size(submatrix,2)-1
        i = full(find(submatrix(1:3:end,1+ii))); lambda(i,j) = res(4+ii);
        Mdepths(k2i(i),j) = M(k2i(i),j)*lambda(i,j);
      end
    end
  end
end

if opt.verbose, disp(['(' num2str(toc) ' sec)']); end


function value = r8mat_amax ( m, n, a )

%*****************************************************************************80
%
%% R8MAT_AMAX returns the maximum absolute value entry of an R8MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, real A(M,N), the M by N matrix.
%
%    Output, real VALUE, the maximum absolute value entry of A.
%
  value = max ( max ( abs ( a(1:m,1:n) ) ) );

  return
end


function misclassified = ml_get_misclassified(X,labels, f, options)
%ML_GET_MISCLASSIFIED
%
%   input -----------------------------------------------------------------
%
%       o  X        : (N x D), dataset of N datapoints of dimension D.
%
%       o labels    : (N x 1), ground truth class labels.
%
%       o f         : function handle, classifier f.
%
%               - y = f(X); y are predicted class labels.
%
%   output ----------------------------------------------------------------
%
%       o misclassified : struct,
%
%           - misclassified.index :     indicies of data points X which have
%                                       been misclassified.
%
%           - misclassified.predicted: the predicted class label (which was wrong).
%
%           - misclassified.truth:     class label which should have been
%                                      predicted.

class_id        = unique(labels);
num_classes     = length(class_id);
M               = zeros(num_classes,num_classes);
dim_swaped      = false; %false: (MXD) , true: (DXM)
if isfield(options,'dim_swaped'),       dim_swaped   = options.dim_swaped;   end

% predicted class label
if dim_swaped
    hc          = f(X');
else
    hc          = f(X);
end

% entries which are 0, mean they where correctly classified, otherwise not.
tmp         = hc(:) - labels(:);
idx         = find(tmp ~= 0);

misclassified.idx           = idx;
misclassified.predicted     = hc(idx);
misclassified.truth         = labels(idx);

end


%% This file will be used to compare identified PDE of the SINDy-PI and ground truth.
% Code By: K
% Last Updated: 2019/06/24
%% Clear all
close all;clear all;clc;
%% Add path
[fld_status, fld_msg, fld_msgID]=mkdir('Datas');
[fld_status, fld_msg, fld_msgID]=mkdir('Figures');
addpath('Datas')
addpath('Figures')
addpath('Functions')
%% Define parameters for the simulation
% Define diffusion rate
Dx=0.01;Dz=0.01;Ds=1;Du=1;

% Others
q=0.1;f=1.5;ksi=0.3;alpha=0.3;beta=0.26;gama=0.4;ksi2=1.5;ksi3=0.003;phi=0;

% Define the time horizon
dt=0.001;T=1;
tspan=0:dt:T;

% Define the spatial domain
L=20; % Total length of each dimension
n=128; % Discretization point of each dimension
N=n*n; % Total points used, n^2

x2=linspace(-L/2,L/2,n+1); x=x2(1:n);
y=x;dx=x(2)-x(1);

kx=(2*pi/L)*[0:(n/2-1) -n/2:-1];
ky=kx;

% Get n-dimensional grid
[X,Y]=meshgrid(x,y);
[KX,KY]=meshgrid(kx,ky);
K2=KX.^2+KY.^2; K22=reshape(K2,N,1);
Kx=reshape(KX,N,1);Ky=reshape(KY,N,1);Kxx=reshape(KX,N,1);Kyy=reshape(KY,N,1);

% Create a time matrix
r_t=zeros(n,n,length(tspan));r=zeros(n,n,length(tspan));r_x=zeros(n,n,length(tspan));r_xx=zeros(n,n,length(tspan));r_y=zeros(n,n,length(tspan));r_yy=zeros(n,n,length(tspan));
z_t=zeros(n,n,length(tspan));z=zeros(n,n,length(tspan));z_x=zeros(n,n,length(tspan));z_xx=zeros(n,n,length(tspan));z_y=zeros(n,n,length(tspan));z_yy=zeros(n,n,length(tspan));
s_t=zeros(n,n,length(tspan));s=zeros(n,n,length(tspan));s_x=zeros(n,n,length(tspan));s_xx=zeros(n,n,length(tspan));s_y=zeros(n,n,length(tspan));s_yy=zeros(n,n,length(tspan));
u_t=zeros(n,n,length(tspan));u=zeros(n,n,length(tspan));u_x=zeros(n,n,length(tspan));u_xx=zeros(n,n,length(tspan));u_y=zeros(n,n,length(tspan));u_yy=zeros(n,n,length(tspan));
r_lap=zeros(n,n,length(tspan));z_lap=zeros(n,n,length(tspan));s_lap=zeros(n,n,length(tspan));u_lap=zeros(n,n,length(tspan));
%% Set a Guassian for the initial condition
r0=cos(sqrt(X.^2+Y.^2)).^2+GaussianFilter(X,Y,0.05,0.05,0,0)+GaussianFilter(X,Y,1,0.01,0,0)+GaussianFilter(X,Y,0.01,1,0,0)+GaussianFilter(X,Y,0.25,0.25,5,5)+GaussianFilter(X,Y,0.25,0.25,5,-5)+GaussianFilter(X,Y,0.25,0.25,-5,5)+GaussianFilter(X,Y,0.25,0.25,-5,-5)+0.1;
z0=sin(0.1*sqrt(X.^2+Y.^2)).^2+GaussianFilter(X,Y,0.05,0.01,0,0)+GaussianFilter(X,Y,0.25,0.25,5,5)+GaussianFilter(X,Y,0.25,0.25,5,-5)+GaussianFilter(X,Y,0.25,0.25,-5,5)+GaussianFilter(X,Y,0.25,0.25,-5,-5)+0.1;
s0=sin(sqrt(X.^2+Y.^2)).^2+GaussianFilter(X,Y,0.5,0.5,0,5)+GaussianFilter(X,Y,0.5,0.5,5,0)+GaussianFilter(X,Y,0.5,0.5,0,-5)+GaussianFilter(X,Y,0.5,0.5,-5,0)+0.1;
u0=cos(sqrt(X.^2+Y.^2)).^2+GaussianFilter(X,Y,0.5,0.01,0,0)+GaussianFilter(X,Y,0.01,0.5,0,0)+GaussianFilter(X,Y,0.25,0.25,5,5)+GaussianFilter(X,Y,0.25,0.25,5,-5)+GaussianFilter(X,Y,0.25,0.25,-5,5)+GaussianFilter(X,Y,0.25,0.25,-5,-5)+0.01;

%% Plot the initial conditions
figure(1)
surf(x,y,r0)
set(gca,'FontSize',18);
set(gcf,'Position',[100 100 600 600]);
set(gcf,'PaperPositionMode','auto');
view(-18,35)
box('off')
axis('off')

figure(2)
surf(x,y,z0)
set(gca,'FontSize',18);
set(gcf,'Position',[100 100 600 600]);
set(gcf,'PaperPositionMode','auto');
view(-18,35)
box('off')
axis('off')

figure(3)
surf(x,y,s0)
set(gca,'FontSize',18);
set(gcf,'Position',[100 100 600 600]);
set(gcf,'PaperPositionMode','auto');
view(-18,35)
box('off')
axis('off')

figure(4)
surf(x,y,u0)
set(gca,'FontSize',18);
set(gcf,'Position',[100 100 600 600]);
set(gcf,'PaperPositionMode','auto');
view(-18,35)
box('off')
axis('off')
%%
close all
% Transfer the initial guess to fourier domain
xzsu0t=[reshape(fft2(r0),1,N) reshape(fft2(z0),1,N) reshape(fft2(s0),1,N) reshape(fft2(u0),1,N)].';

%% Simulate the original equation
tic
opts = odeset('RelTol',1e-12,'AbsTol',1e-13);
NeedDev=0;
[time,xzsusol]=ode45(@(time,xzsut)BZ_Reaction_PDE(time,xzsut,Kx,Kxx,Ky,Kyy,K22,n,N,Dx,Dz,Ds,Du,q,f,ksi,alpha,beta,gama,ksi2,ksi3,phi,NeedDev),tspan,xzsu0t,opts);
toc

% Simulate the DL-SINDy discovered equation
tic
opts = odeset('RelTol',1e-12,'AbsTol',1e-13);
NeedDev=0;
[time,xzsusol_DL]=ode45(@(time,xzsut)BZ_Reaction_DL_SINDy_PDE(time,xzsut,Kx,Kxx,Ky,Kyy,K22,n,N,NeedDev),tspan,xzsu0t,opts);
toc

%% Extract values
% After simulation, we extract the derivative
NeedDev=1;

for pin=1:length(tspan)
    % Get the derivative data
    [~,r_t(:,:,pin),z_t(:,:,pin),s_t(:,:,pin),u_t(:,:,pin),r(:,:,pin),z(:,:,pin),s(:,:,pin),u(:,:,pin),r_x(:,:,pin),z_x(:,:,pin),s_x(:,:,pin),u_x(:,:,pin),r_y(:,:,pin),z_y(:,:,pin),s_y(:,:,pin),u_y(:,:,pin),r_xx(:,:,pin),z_xx(:,:,pin),s_xx(:,:,pin),u_xx(:,:,pin),r_yy(:,:,pin),z_yy(:,:,pin),s_yy(:,:,pin),u_yy(:,:,pin),r_lap(:,:,pin),z_lap(:,:,pin),s_lap(:,:,pin),u_lap(:,:,pin)]=...
        BZ_Reaction_PDE(0,xzsusol(pin,:).',Kx,Kxx,Ky,Kyy,K22,n,N,Dx,Dz,Ds,Du,q,f,ksi,alpha,beta,gama,ksi2,ksi3,phi,NeedDev);

    % Do the same for the DL-SINDy
    [~,~,~,~,~,r_DL(:,:,pin),z_DL(:,:,pin),s_DL(:,:,pin),u_DL(:,:,pin),~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~]=...
        BZ_Reaction_SINDy_PI_PDE(0,xzsusol_DL(pin,:).',Kx,Kxx,Ky,Kyy,K22,n,N,NeedDev);
end

%% Plot the result for original data
for pin=2:4
    if pin==1
        Animate=r;
        limits=[0 1];
    elseif pin==2
        Animate=z;
        limits=[0 1];
    elseif pin==3
        Animate=s;
        limits=[0 1];
    elseif pin==4
        Animate=u;
        limits=[0 1];
    end
    % Normalize it
    Dummy=Animate(:,:,1);
    Max=max(max(Dummy))
    Animate=Animate/Max;
    %
    for j=1:200:length(tspan)
        clf
        surf(x,y,Animate(:,:,j))
        colormap(cool)
        %shading interp
        %caxis([0,1])
        %Make plot propotional
        %zlim([0,1.1])
        %set(gca,'FontSize',18);
        set(gcf,'Position',[100 100 600 600]);
        set(gcf,'PaperPositionMode','auto');
        view(-18,35)
        box('off')
        axis('off')
        drawnow limitrate
    end
end

%% Plot the result for DL-SINDy simulation
for pin=2:4
    if pin==1
        Animate=r_DL;
        limits=[0 1];
    elseif pin==2
        Animate=z_DL;
        limits=[0 1];
    elseif pin==3
        Animate=s_DL;
        limits=[0 1];
    elseif pin==4
        Animate=u_DL;
        limits=[0 1];
    end
    % Normalize it
    Dummy=Animate(:,:,1);
    Max=max(max(Dummy))
    Animate=Animate/Max;
    %
    for j=1:200:length(tspan)
        clf
        surf(x,y,Animate(:,:,j))
        colormap(cool)
        %caxis([0,1])
        %Make plot propotional
        %zlim([0,1.1])
        %set(gca,'FontSize',18);
        set(gcf,'Position',[100 100 600 600]);
        set(gcf,'PaperPositionMode','auto');
        view(-18,35)
        box('off')
        axis('off')
        drawnow limitrate
    end
end


function [ out, component ] = ScoreVideoToComponentVideo( scoreVideo )
%  ScoreVideoToComponentVideo - use component analysis to only retain
%  largest component
%--------------------------------------------------------------------------
%   Params: scoreVideo - the score video
%
%   Returns: out - component video only containing biggest component
%            component - number for max component or 0 if no component was
%            large enough
%--------------------------------------------------------------------------

out = 0;
[L,num] = bwlabeln(scoreVideo);
max = 0;
component = 0;

for i = 1:num
    temp = sum(sum(sum(L==i)));
    if (temp>max)
        component=i;
        max = temp;
    end
end
if component == 0
    out = scoreVideo;
else
    %check if a good portion of biggest component is classified correct.
    %If not then don't show it on screen
    %Good values are 0.007, .006, .0065, .005, .004
    percentage = 0.006 * ( size(scoreVideo,1) * size(scoreVideo,2) * size(scoreVideo,3) );
    %display(max);
    %display(percentage);
    if (max < percentage)
        component = 0;
    else
        out = (L==component);
    end
end

end


function initRegularisation(solver,varargin)

epsilon = get_option(varargin,'epsilon',solver.psi.halfwidth*2);

% a discrete Laplacian
A = -dot_outer(solver.S3G,solver.S3G,'epsilon',epsilon);
d = full(sum(A)).';

solver.RM = solver.lambda * spdiags(-1-d,0,A);


report_this_filefun(mfilename('fullpath'));

[lat,lon] = meshgrat(tmap,tmapleg);
[X , Y]  = meshgrid(gx,gy);

ren = interp2(X,Y,re3,lon,lat);
mi = 0;

figure_w_normalized_uicontrolunits('pos',[150 100 1000 700])

hold on; axis off
axesm('MapProjection','mercator',...
    'MapLatLimit',[s4_south s3_north],'MapLonLimit',[s2_west s1_east])

meshm(ren,tmapleg,size(tmap),tmap);

daspectm('m',5);
tightmap
view([0 90])
camlight; lighting phong
% set(gca,'projection','perspective');

plotm(coastline(:,2), coastline(:,1),'w','Linewidth',2);
zdatam(handlem('allline'),10000) % keep line on surface

j = jet(64);
%j = j(64:-1:1,:);
j = [ [ 0.85 0.9 0.9] ; j];

colormap(j); brighten(0.3);
%caxis([mi 1.4])

set(gcf,'color','w')

setm(gca,'ffacecolor','w')
setm(gca,'fedgecolor','w','flinewidth',1);

setm(gca,'mlabellocation',1)
setm(gca,'meridianlabel','on')
setm(gca,'plabellocation',1)
setm(gca,'parallellabel','on')
setm(gca,'Fontcolor','w','Fontweight','bold','FontSize',12)

h5 = colorbar;
set(h5,'position',[0.82 0.35 0.01 0.3])

set(gcf,'Inverthardcopy','off');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   PRRP example arm with 4 DOF.
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche.
%   email: arturo.gil@umh.es date:   03/01/2023
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

%Kinematic parameters
robot.DH.theta= '[0 q(2) q(3) 0]';
robot.DH.d='[q(1) 0  0    q(4)]';
robot.DH.a='[0  1  1   0]';
robot.DH.alpha= '[pi/2  0 pi  0]';

%Jacobian matrix. Variation of (X, Y, Z) as a function of (w1, w2, w3)
robot.J='[];';
robot.name='PRRP';

robot.inversekinematic_fn = 'inversekinematic_PRRP(robot, T)';

%number of degrees of freedom
robot.DOF = 4;

%rotational: R, translational: T
robot.kind=['T' 'R' 'R' 'T'];

%minimum and maximum rotation angle in rad
robot.maxangle =[-1 1; %Axis 1, minimum, maximum
                -pi pi;
                -pi pi; %Axis 3, translational
                0 0.5]; %Axis 4

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(200); %Axis 1, rad/s
                deg2rad(200); %Axis 2, m/s
                2; %Axis 3, m/s
                deg2rad(360)]; %Axis 4, rad/s

% end effectors maximum velocity
robot.linear_velmax = 1.0; %m/s

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.path = pwd;

% GRAPHICS
robot.graphical.has_graphics=0;
robot.graphical.color = [25 20 40];
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-3 3 -3 3 -1 1];
%read graphics files
robot = read_graphics(robot);

%DYNAMICS
robot.has_dynamics=0;


function results = TC_ADAL( data, params )

tic;
Y = data.X;
b = data.b;
Omega = data.Omega;

mu = params.mu0;
N = length( size(Y) );
lambda = params.lambda;

Lamb = cell( 1, N );
Xs = cell(1,N);
for i = 1:N
    Lamb{i} = data.Lamb{i};
    Xs{i} = data.X;
end

for iter = 1:params.max_iter
    % solve for X_{N+1} = Y
    Yprev = Y;
    R = tenzeros( size(Y) );
    R(Omega) = lambda*b;
    R = R + ten_sum_all( Lamb ) + ten_sum_all( Xs )/mu;
    Y = R * mu/N;
    Y(Omega) = R(Omega) / (lambda+N/mu);

    % solve for X_i's
%     Xs = cell(1,N);
    for i = 1:N
        Xs{i} = tensor_shrinkage( Y-mu*Lamb{i}, mu, i );
        Lamb{i} = Lamb{i} - (Y-Xs{i})/mu;
    end

    % compute optimality stats
    pinf = 0;
    for i = 1:N
        pinf = pinf + norm( double(tenmat( Xs{i}-Y, i )), 'fro' )^2;
    end
    ynorm = norm(double(tenmat(Y,1)),'fro');
    pinf = sqrt(pinf / N) / ynorm;
%     pinf = pinf / ynorm;
    dinf = norm( double(tenmat(Y - Yprev, 1 )), 'fro' ) / ynorm;
%     dinf = dinf / ynorm;

    % print
    fprintf('Iter: %d,   pinf: %3.2e,   dinf: %3.2e\n', iter, pinf, dinf);

    if pinf < params.opt_tol && dinf < params.opt_tol
        break;
    end

    % update mu
%     if rem( iter, 10 ) == 0
%         mu = mu / 5;
%     end
end

diffNorm = norm( double( tenmat( Y - data.X, 1 )), 'fro' );
trueNorm = norm( double( tenmat( data.X, 1 )), 'fro' );
rel_err = diffNorm / trueNorm;

% save results
results.X = Y;
results.iter = iter;
results.cpu = toc;
results.rel_err = rel_err;

end


%   getBox:  Given the current state, returns a number from 1 to 162
%             designating the region of the state space encompassing the current state.
%             Returns a value of -1 if a failure state is encountered.

function box = getBox5(theta,thetaDot,x,xDot)
theta = rad2deg(theta);
thetaDot = rad2deg(thetaDot);
if (x < -2.4 || x > 2.4  || theta < -12 || theta > 12)
    box = 379;
else

if(theta>=-12 && theta<=-10)
    thetaBucket = 1;
elseif(theta>-10 && theta<=-8)
    thetaBucket = 2;
elseif(theta>-8 && theta<=-6)
    thetaBucket = 3;
elseif(theta>-6 && theta<=-4)
    thetaBucket = 4;
elseif(theta>-4 && theta<=-2)
    thetaBucket = 5;
elseif(theta>-2 && theta<=-1)
    thetaBucket = 6;
elseif(theta>-1 && theta<=0)
    thetaBucket = 7;
elseif(theta>0 && theta<=1)
    thetaBucket = 8;
elseif(theta>1 && theta<=2)
    thetaBucket = 9;
elseif(theta>2 && theta<=4)
    thetaBucket = 10;
elseif(theta>4 && theta<=6)
    thetaBucket = 11;
elseif(theta>6 && theta<=8)
    thetaBucket = 12;
elseif(theta>8 && theta<=10)
    thetaBucket = 13;
elseif(theta>10 && theta<=12)
    thetaBucket = 14;
end

if (x<-0.8&&x>=-2.4)
	xBucket = 1;
elseif (x<=0.8&&x>=-0.8)
	xBucket = 2;
elseif (x<=2.4&&x>0.8)
	xBucket = 3;
end

if (xDot<-0.5)
	xDotBucket = 1;
elseif (xDot>=-0.5&&xDot<=0.5)
	xDotBucket = 2;
else
	xDotBucket = 3;
end

if (thetaDot<-50)
	thetaDotBucket = 1;
elseif (thetaDot>=-50&&thetaDot<=50)
	thetaDotBucket = 2;
else
	thetaDotBucket = 3;
end

box = sub2ind([14,3,3,3],thetaBucket, thetaDotBucket,xBucket,xDotBucket);
end
return;