function [x, cost, info, options] = conjugategradient(problem, x, options)
% Conjugate gradient minimization algorithm for Manopt.
%
% function [x, cost, info, options] = conjugategradient(problem)
% function [x, cost, info, options] = conjugategradient(problem, x0)
% function [x, cost, info, options] = conjugategradient(problem, x0, options)
% function [x, cost, info, options] = conjugategradient(problem, [], options)
%
% Apply the conjugate gradient minimization algorithm to the problem
% defined in the problem structure, starting at x0 if it is provided
% (otherwise, at a random point on the manifold). To specify options whilst
% not specifying an initial guess, give x0 as [] (the empty matrix).
%
% The outputs x and cost are the best reached point on the manifold and its
% cost. The struct-array info contains information about the iterations:
%   iter : the iteration number (0 for the initial guess)
%   cost : cost value
%   time : elapsed time in seconds
%   gradnorm : Riemannian norm of the gradient
%   stepsize : norm of the last tangent vector retracted
%   beta : value of the beta parameter (see options.beta_type)
%   linesearch : information logged by options.linesearch
%   And possibly additional information logged by options.statsfun.
% For example, type [info.gradnorm] to obtain a vector of the successive
% gradient norms reached.
%
% The options structure is used to overwrite the default values. All
% options have a default value and are hence optional. To force an option
% value, pass an options structure with a field options.optionname, where
% optionname is one of the following and the default value is indicated
% between parentheses:
%
%   tolgradnorm (1e-6)
%       The algorithm terminates if the norm of the gradient drops below this.
%   maxiter (1000)
%       The algorithm terminates if maxiter iterations have been executed.
%   maxtime (Inf)
%       The algorithm terminates if maxtime seconds elapsed.
%   minstepsize (1e-10)
%       The algorithm terminates if the linesearch returns a displacement
%       vector (to be retracted) smaller in norm than this value.
%   beta_type ('H-S')
%       Conjugate gradient beta rule used to construct the new search
%       direction, based on a linear combination of the previous search
%       direction and the new (preconditioned) gradient. Possible values
%       for this parameter are:
%           'S-D', 'steep' for beta = 0 (preconditioned steepest descent)
%           'F-R' for Fletcher-Reeves's rule
%           'P-R' for Polak-Ribiere's modified rule
%           'H-S' for Hestenes-Stiefel's modified rule
%           'H-Z' for Hager-Zhang's modified rule
%           'L-S' for Sato's Liu-Storey rule
%       See Hager and Zhang 2006, "A survey of nonlinear conjugate gradient
%       methods" for a description of these rules in the Euclidean case and
%       for an explanation of how to adapt them to the preconditioned case.
%       The adaption to the Riemannian case is straightforward: see in code
%       for details. Modified rules take the max between 0 and the computed
%       beta value, which provides automatic restart, except for H-Z and L-S
%       which use a different modification. Sato's Liu-Storey rule is
%       described in Sato 2021, "Riemannian conjugate gradient methods:
%       General framework and specific algorithms with convergence analyses"
%   orth_value (Inf)
%       Following Powell's restart strategy (Math. prog. 1977), restart CG
%       (that is, make a -preconditioned- gradient step) if two successive
%       -preconditioned- gradients are "too" parallel. See for example
%       Hager and Zhang 2006, "A survey of nonlinear conjugate gradient
%       methods", page 12. An infinite value disables this strategy. See in
%       code formula for the specific criterion used.
%   linesearch (@linesearch_adaptive or @linesearch_hint)
%       Function handle to a line search function. The options structure is
%       passed to the line search too, so you can pass it parameters. See
%       each line search's documentation for info. Another available line
%       search in manopt is @linesearch, in /manopt/linesearch/linesearch.m
%       If the problem structure includes a line search hint, then the
%       default line search used is @linesearch_hint.
%   statsfun (none)
%       Function handle to a function that will be called after each
%       iteration to provide the opportunity to log additional statistics.
%       They will be returned in the info struct. See the generic Manopt
%       documentation about solvers for further information.
%   stopfun (none)
%       Function handle to a function that will be called at each iteration
%       to provide the opportunity to specify additional stopping criteria.
%       See the generic Manopt documentation about solvers for further
%       information.
%   verbosity (3)
%       Integer number used to tune the amount of output the algorithm
%       generates during execution (mostly as text in the command window).
%       The higher, the more output. 0 means silent.
%   storedepth (2)
%       Maximum number of different points x of the manifold for which a
%       store structure will be kept in memory in the storedb. If the
%       caching features of Manopt are not used, this is irrelevant. For
%       the CG algorithm, a store depth of 2 should always be sufficient.
%
%
% In most of the examples bundled with the toolbox (see link below), the
% solver can be replaced by the present one if need be.
%
% See also: steepestdescent trustregions manopt/solvers/linesearch manopt/examples

% An explicit, general listing of this algorithm, with preconditioning,
% can be found in the following paper:
%     @Article{boumal2015lowrank,
%       Title   = {Low-rank matrix completion via preconditioned optimization on the {G}rassmann manifold},
%       Author  = {Boumal, N. and Absil, P.-A.},
%       Journal = {Linear Algebra and its Applications},
%       Year    = {2015},
%       Pages   = {200--239},
%       Volume  = {475},
%       Doi     = {10.1016/j.laa.2015.02.027},
%     }

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, Dec. 30, 2012.
% Contributors: Nicolas Boumal, Nick Vannieuwenhoven
% Change log:
%
%   March 14, 2013, NB:
%       Added preconditioner support : see Section 8 in
%       https://www.math.lsu.edu/~hozhang/papers/cgsurvey.pdf
%
%   Sept. 13, 2013, NB:
%       Now logging beta parameter too.
%
%   Nov. 7, 2013, NB:
%       The search direction is no longer normalized before it is passed
%       to the linesearch. This way, it is up to the designers of the
%       linesearch to decide whether they want to use the norm of the
%       search direction in their algorithm or not. There are reasons
%       against it, but practical evidence that it may help too, so we
%       allow it. The default linesearch_adaptive used does exploit the
%       norm information. The base linesearch does not. You may select it
%       by setting options.linesearch = @linesearch;
%
%   Nov. 29, 2013, NB:
%       Documentation improved: options are now explicitly described.
%       Removed the Daniel rule for beta: it was not appropriate for
%       preconditioned CG and I could not find a proper reference for it.
%
%   April 3, 2015 (NB):
%       Works with the new StoreDB class system.
%
%   Aug. 2, 2018 (NB):
%       Now using storedb.remove() to keep the cache lean.
%
%   Feb. 7, 2022 (NV):
%       Added support for Liu-Storey rule (L-S).

M = problem.M;

% Verify that the problem description is sufficient for the solver.
if ~canGetCost(problem)
    warning('manopt:getCost', ...
        'No cost provided. The algorithm will likely abort.');
end
if ~canGetGradient(problem) && ~canGetApproxGradient(problem)
    warning('manopt:getGradient:approx', ...
           ['No gradient provided. Using an FD approximation instead (slow).\n' ...
            'It may be necessary to increase options.tolgradnorm.\n' ...
            'To disable this warning: warning(''off'', ''manopt:getGradient:approx'')']);
    problem.approxgrad = approxgradientFD(problem);
end

% Set local defaults here
localdefaults.minstepsize = 1e-10;
localdefaults.maxiter = 1000;
localdefaults.tolgradnorm = 1e-6;
localdefaults.storedepth = 20;
% Changed by NB : H-S has the "auto restart" property.
% See Hager-Zhang 2005/2006 survey about CG methods.
% The auto restart comes from the 'max(0, ...)', not so much from the
% reason stated in Hager-Zhang I think. P-R also has auto restart.
localdefaults.beta_type = 'H-S';
localdefaults.orth_value = Inf; % by BM as suggested in Nocedal and Wright

% Depending on whether the problem structure specifies a hint for
% line-search algorithms, choose a default line-search that works on
% its own (typical) or that uses the hint.
if ~canGetLinesearch(problem)
    localdefaults.linesearch = @linesearch_adaptive;
else
    localdefaults.linesearch = @linesearch_hint;
end

% Merge global and local defaults, then merge w/ user options, if any.
localdefaults = mergeOptions(getGlobalDefaults(), localdefaults);
if ~exist('options', 'var') || isempty(options)
    options = struct();
end
options = mergeOptions(localdefaults, options);

timetic = tic();

% If no initial point x is given by the user, generate one at random.
if ~exist('x', 'var') || isempty(x)
    x = M.rand();
end

% Create a store database and generate a key for the current x
storedb = StoreDB(options.storedepth);
key = storedb.getNewKey();

% Compute cost-related quantities for x
[cost, grad] = getCostGrad(problem, x, storedb, key);
gradnorm = M.norm(x, grad);
Pgrad = getPrecon(problem, x, grad, storedb, key);
gradPgrad = M.inner(x, grad, Pgrad);

% Iteration counter (at any point, iter is the number of fully executed
% iterations so far)
iter = 0;

% Save stats in a struct array info and preallocate.
stats = savestats();
info(1) = stats;
info(min(10000, options.maxiter+1)).iter = [];

if options.verbosity >= 2
    fprintf(' iter\t               cost val\t    grad. norm\n');
end

% Compute a first descent direction (not normalized)
desc_dir = M.lincomb(x, -1, Pgrad);

% Start iterating until stopping criterion triggers
while true

    % Display iteration information
    if options.verbosity >= 2
        fprintf('%5d\t%+.16e\t%.8e\n', iter, cost, gradnorm);
    end

    % Start timing this iteration
    timetic = tic();

    % Run standard stopping criterion checks
    [stop, reason] = stoppingcriterion(problem, x, options, info, iter+1);

    % Run specific stopping criterion check
    if ~stop && abs(stats.stepsize) < options.minstepsize
        stop = true;
        reason = sprintf(['Last stepsize smaller than minimum '  ...
                          'allowed; options.minstepsize = %g.'], ...
                          options.minstepsize);
    end

    if stop
        if options.verbosity >= 1
            fprintf([reason '\n']);
        end
        break;
    end

    % The line search algorithms require the directional derivative of the
    % cost at the current point x along the search direction.
    df0 = M.inner(x, grad, desc_dir);

    % If we didn't get a descent direction: restart, i.e., switch to the
    % negative gradient. Equivalent to resetting the CG direction to a
    % steepest descent step, which discards the past information.
    if df0 >= 0

        % Or we switch to the negative gradient direction.
        if options.verbosity >= 3
            fprintf(['Conjugate gradient info: got an ascent direction '...
                     '(df0 = %2e), reset to the (preconditioned) '...
                     'steepest descent direction.\n'], df0);
        end
        % Reset to negative gradient: this discards the CG memory.
        desc_dir = M.lincomb(x, -1, Pgrad);
        df0 = -gradPgrad;

    end

    % Execute line search
    [stepsize, newx, newkey, lsstats] = options.linesearch( ...
                   problem, x, desc_dir, cost, df0, options, storedb, key);

    % Compute the new cost-related quantities for newx
    [newcost, newgrad] = getCostGrad(problem, newx, storedb, newkey);
    newgradnorm = M.norm(newx, newgrad);
    Pnewgrad = getPrecon(problem, newx, newgrad, storedb, newkey);
    newgradPnewgrad = M.inner(newx, newgrad, Pnewgrad);

    % Apply the CG scheme to compute the next search direction.
    %
    % This paper https://www.math.lsu.edu/~hozhang/papers/cgsurvey.pdf
    % by Hager and Zhang lists many known beta rules. The rules defined
    % here can be found in that paper (or are provided with additional
    % references), adapted to the Riemannian setting.
    %
    if strcmpi(options.beta_type, 'steep') || ...
       strcmpi(options.beta_type, 'S-D')              % Gradient Descent

        beta = 0;
        desc_dir = M.lincomb(newx, -1, Pnewgrad);

    else

        oldgrad = M.transp(x, newx, grad);
        orth_grads = M.inner(newx, oldgrad, Pnewgrad) / newgradPnewgrad;

        % Powell's restart strategy (see page 12 of Hager and Zhang's
        % survey on conjugate gradient methods, for example)
        if abs(orth_grads) >= options.orth_value
            beta = 0;
            desc_dir = M.lincomb(x, -1, Pnewgrad);

        else % Compute the CG modification

            desc_dir = M.transp(x, newx, desc_dir);

            switch upper(options.beta_type)

                case 'F-R'  % Fletcher-Reeves
                    beta = newgradPnewgrad / gradPgrad;

                case 'P-R'  % Polak-Ribiere+
                    % vector grad(new) - transported grad(current)
                    diff = M.lincomb(newx, 1, newgrad, -1, oldgrad);
                    ip_diff = M.inner(newx, Pnewgrad, diff);
                    beta = ip_diff / gradPgrad;
                    beta = max(0, beta);

                case 'H-S'  % Hestenes-Stiefel+
                    diff = M.lincomb(newx, 1, newgrad, -1, oldgrad);
                    ip_diff = M.inner(newx, Pnewgrad, diff);
                    beta = ip_diff / M.inner(newx, diff, desc_dir);
                    beta = max(0, beta);

                case 'H-Z' % Hager-Zhang+
                    diff = M.lincomb(newx, 1, newgrad, -1, oldgrad);
                    Poldgrad = M.transp(x, newx, Pgrad);
                    Pdiff = M.lincomb(newx, 1, Pnewgrad, -1, Poldgrad);
                    deno = M.inner(newx, diff, desc_dir);
                    numo = M.inner(newx, diff, Pnewgrad);
                    numo = numo - 2*M.inner(newx, diff, Pdiff)*...
                                     M.inner(newx, desc_dir, newgrad) / deno;
                    beta = numo / deno;

                    % Robustness (see Hager-Zhang paper mentioned above)
                    desc_dir_norm = M.norm(newx, desc_dir);
                    eta_HZ = -1 / ( desc_dir_norm * min(0.01, gradnorm) );
                    beta = max(beta, eta_HZ);

                case 'L-S' % Liu-Storey+ from Sato
                    diff = M.lincomb(newx, 1, newgrad, -1, oldgrad);
                    ip_diff = M.inner(newx, Pnewgrad, diff);
                    denom = -1*M.inner(x, grad, desc_dir);
                    betaLS = ip_diff / denom;
                    betaCD = newgradPnewgrad / denom;
                    beta = max(0, min(betaLS, betaCD));

                otherwise
                    error(['Unknown options.beta_type. ' ...
                           'Should be steep, S-D, F-R, P-R, H-S, H-Z, or L-S.']);
            end

            desc_dir = M.lincomb(newx, -1, Pnewgrad, beta, desc_dir);

        end

    end

    % Transfer iterate info.
    storedb.removefirstifdifferent(key, newkey);
    x = newx;
    key = newkey;
    cost = newcost;
    grad = newgrad;
    Pgrad = Pnewgrad;
    gradnorm = newgradnorm;
    gradPgrad = newgradPnewgrad;

    % iter is the number of iterations we have accomplished.
    iter = iter + 1;

    % Make sure we don't use too much memory for the store database.
    storedb.purge();

    % Log statistics for freshly executed iteration.
    stats = savestats();
    info(iter+1) = stats;

end

info = info(1:iter+1);

if options.verbosity >= 1
    fprintf('Total time is %f [s] (excludes statsfun)\n', info(end).time);
end

% Routine in charge of collecting the current iteration stats
function stats = savestats()
    stats.iter = iter;
    stats.cost = cost;
    stats.gradnorm = gradnorm;
    if iter == 0
        stats.stepsize = nan;
        stats.time = toc(timetic);
        stats.linesearch = [];
        stats.beta = 0;
    else
        stats.stepsize = stepsize;
        stats.time = info(iter).time + toc(timetic);
        stats.linesearch = lsstats;
        stats.beta = beta;
    end
    stats = applyStatsfun(problem, x, storedb, key, options, stats);
end

end


%% Alignment of the Crystal Axes
%
% Default is $\vec c$ axis of highest symmetry.
%
% TODO: Explain the default setting in more detail.
%
%% Switching between different Alignment Options
%
% Since, especialy for lower symmetry groups, different conventions for
% aligning the crystal axes are used it might be necessary to transform
% data, e.g, orientations or tensors, from one convention into another.
% This can be done using the command <tensor.transformReferenceFrame.html
% transformReferenceFrame> as it illustrated below.
%
% First we import the stiffness tensor Forsterite with respect to the axes
% alignment

cs = crystalSymmetry('mmm',[4.7646 10.2296 5.9942],'mineral','Olivin');

% import some stiffness tensor
fname = fullfile(mtexDataPath,'tensor','Olivine1997PC.GPa');
C = stiffnessTensor.load(fname,cs)

plot(C)

%%
% Let us now consider a different setup of the Forsterite symmetry, where
% the $\vec a$ axis is the longest and the $\vec c$-axis is the shortest.

cs_new = crystalSymmetry('mmm',[10.2296 5.9942 4.7646],'mineral','Olivin')

%%
% In order to represent the stiffness tensor |C| with respect to this
% setupt we use the command <tensor.transformReferenceFrame.html
% transformReferenceFrame>.

C_new = C.transformReferenceFrame(cs_new)

nextAxis
plot(C_new)


function fx = circle ( n, x )

%*****************************************************************************80
%
%% CIRCLE evaluates the circle function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N,2), the point coordinates.
%
%    Output, real FX(N), the function values.
%
  global c
  global r

  if ( isempty ( c ) )
    c = [ 0.0, 0.5 ];
  end

  if ( isempty ( r ) )
    r = 0.5;
  end

  nc = ones ( n, 1 ) * c;
  nr = ones ( n, 1 ) * r;

  fx = nr - sqrt ( sum ( ( x - nc ).^2, 2 ) );

  return
end


function bound = AnglesLocalMaxMin(f,N)

%================================================================
% function bound = AnglesLocalMaxMin(f,N)
%
% This function segments f into a maximum of N supports by taking
% the middle point between the N largest local maxima.
% Note: the detected boundaries are given in term of indices
%
% Inputs:
%   -f: the function to segment
%   -N: maximal number of bands
%
% Outputs:
%   -bound: list of detected boundaries
%
% Author: Jerome Gilles
% Institution: UCLA - Department of Mathematics
% Year: 2013
% Version: 1.0
%===============================================================

locmax=zeros(size(f));
locmin=max(f)*ones(size(f));
% We detect the local maxima
for i=2:length(f)-1
    if ((f(i-1)<f(i)) && (f(i)>f(i+1)))
        locmax(i)=f(i);
    end

    if ((f(i-1)>f(i)) && (f(i)<=f(i+1)))
        locmin(i)=f(i);
    end
end

% We check if the endpoint are local maxima or minima (we work on the torus)
if ((f(end)<f(1)) && (f(1)>f(2)))
    locmax(1)=f(1);
end
if ((f(end)>f(1)) && (f(1)<=f(2)))
    locmin(1)=f(1);
end

if ((f(end-1)<f(end)) && (f(end)>f(1)))
    locmax(end)=f(end);
end
if ((f(end-1)>f(end)) && (f(end)<=f(1)))
    locmin(end)=f(end);
end

% We keep the N-th highest maxima and their index
[lmax,Imax]=sort(locmax,1,'descend');
if length(lmax)>N
    Imax=sort(Imax(1:N));
else
    Imax=sort(Imax);
    N=length(lmax);
end

% We detect the lowest minima between two consecutive maxima
bound=zeros(1,N);
for i=1:N
   if i==N
       [lmin,ind]=sort([locmin(Imax(i):end);locmin(1:Imax(1))]);
       tmp=lmin(1);
       n=1;
       if n<length(lmin)
            n=2;
            while ((n<=length(lmin)) && (tmp==lmin(n)))
                n=n+1;
            end
       end
        bound(i)=Imax(i)+ind(ceil(n/2))-1;
       if bound(i)>length(f)
           bound(i)=bound(i)-length(f);
       end
   else
       [lmin,ind]=sort(locmin(Imax(i):Imax(i+1)));
       tmp=lmin(1);
       n=1;
       if n<length(lmin)
            n=2;
            while ((n<=length(lmin)) && (tmp==lmin(n)))
                n=n+1;
            end
       end
       bound(i)=Imax(i)+ind(ceil(n/2))-1;
   end
end

bound=sort(bound);


 function ys = ifftn_fast(xs)
%function ys = ifftn_fast(xs)
%|
%| For some reason, matlab's ifftn routine is suboptimal
%| for the case of 2D FFTs, at least on some machines.
%| The improvement herein was found by Hugo Shi.
%|
%| Note: matlab's ifft() and ifftn() handle an optional second "N" argument in
%| different ways!  So to be safe I am not allowing any second argument here.
%|
%| Copyright 2004-6-28, Jeff Fessler, University of Michigan

if nargin ~= 1, ir_usage, end
if streq(xs, 'test'), ifftn_fast_test, return, end

% around version 7.4, ifftn was fastest, so hardwire that!
ys = ifftn(xs);
return

if ndims(xs) == 2 % 2D or 1D cases
	if min(size(xs)) == 1 % 1D
		ys = ifft(xs);
	else
		ys = ifftn_fast_fftfft(xs);
	end
else
	ys = ifftn(xs);
end

function ys = ifftn_fast_fftfft(xs)
ys = ifft(ifft(xs).').';

% test configuration of ifftn_fast for this machine
function ifftn_fast_test
ifftn_fast_test2
%ifftn_fast_test3

% test configuration of ifftn_fast for this machine for 2D
function ifftn_fast_test2

n = 2^9;
rng(0)
x = rand(n,n);

printm('starting test; be patient.')

% first loop is to get everything in cache or whatever.
% doing it twice is the only way to get an accurate comparison!
for nloop = [2 40];
	tic, for ii=1:nloop, y{1} = ifftn_fast(x); end
	tt(1) = toc; ty{1} = 'fftn_fast';

	tic, for ii=1:nloop, y{2} = ifftn(x); end
	tt(2) = toc; ty{2} = 'fftn';

	tic, for ii=1:nloop, y{3} = ifft(ifft(x).').'; end
	tt(3) = toc; ty{3} = 'fftfft_transpose';

	tic, for ii=1:nloop, y{4} = ifft(ifft(x, [], 1), [], 2); end
	tt(4) = toc; ty{4} = 'fftfft_brack';

	tic, for ii=1:nloop, y{5} = ifft2(x); end
	tt(5) = toc; ty{5} = 'ifft2';

	tic, for ii=1:nloop, y{6} = ifftn_fast_fftfft(x); end
	tt(6) = toc; ty{6} = 'fftfft_func';
end

for ii = 1:length(tt)
	printm('time %19s = %g', ty{ii}, tt(ii))
	if max_percent_diff(y{1}, y{ii}) > 1e-11, error 'bug', end
end

printm('ifftn / ifftn_fast = %g%% ', tt(2) / tt(1) * 100.)
if tt(1) > 1.40 * min(tt(2:end))
	warn 'ifftn_fast is configured supoptimally for your machine!'
else
	printm('ifftn_fast is configured appropriately for your machine')
end


% logimagesc() - make an imagesc(0) plot with log y-axis values (ala semilogy())
%
% Usage:  >> [logfreqs,dataout] = logimagesc(times,freqs,data);
%
% Input:
%   times = vector of x-axis values
%   freqs = vector of y-axis values
%   data  = matrix of size (freqs,times)
%
% Optional Input:
%   plot = ['on'|'off'] plot image or return output (default 'on').
%
% Note: Entering text() onto the image requires specifying (x,log(y)).

% Author: Scott Makeig, SCCN/INC/UCSD, La Jolla, 4/2000

% Copyright (C) 4/2000 Scott Makeig, SCCN/INC/UCSD, scott@sccn.ucsd.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

% 08-07-00 made ydir normal -sm
% 01-25-02 reformated help & license -ad

function [lgfreqs,datout, h, yt, yl] = logimagesc(times,freqs,data,varargin)

  if nargin < 1
      help logimagesc;
      return
  end;
  if size(data,1) ~= length(freqs)
      fprintf('logfreq(): data matrix must have %d rows!\n',length(freqs));
      datout = data;
      return
  end
  if size(data,2) ~= length(times)
      fprintf('logfreq(): data matrix must have %d columns!\n',length(times));
      datout = data;
      return
  end
  if min(freqs)<= 0
      fprintf('logfreq(): frequencies must be > 0!\n');
      datout = data;
      return
  end

  try, icadefs; catch, warning('Using MATLAB default colormap'); end

  lfreqs = log(freqs);
  lgfreqs = linspace(lfreqs(1),lfreqs(end),length(lfreqs));
  lgfreqs = lgfreqs(:);
  lfreqs = lfreqs(:);
  [mesht meshf] = meshgrid(times,lfreqs);
  try
      datout = griddata(mesht,meshf,double(data),times,lgfreqs);
  catch
      fprintf('error in logimagesc.m calling griddata.m, trying v4 method.');
      datout = griddata(mesht,meshf,data,times,lgfreqs,'v4');
  end
  datout(find(isnan(datout(:)))) = 0;

  if ~isempty(varargin)
      plot = varargin{2};
  else
      plot = 'on';
  end

  if strcmp(plot, 'on')
      imagesc(times,freqs,data);
      try colormap(DEFAULT_COLORMAP); catch, end;
      nt = ceil(min(freqs)); % new tick - round up min y to int
      ht = floor(max(freqs)); % high freq - round down

      yt=get(gca,'ytick');
      yl=get(gca,'yticklabel');

      h=imagesc(times,lgfreqs,datout); % plot the image
      set(gca,'ydir','normal')

      i = 0; yt = [];
      yl = cell(1,100);

      tickscale = 1.618; % log scaling power for frequency ticks
      while (nt*tickscale^i < ht )
        yt = [yt log(round(nt*tickscale^i))];
        yl{i+1}=int2str(round(nt*tickscale^i));
        i=i+1;
      end

      if ht/(nt*tickscale^(i-1)) > 1.35
         yt = [yt log(ht)];
         yl{i+1} = ht;
      else
         i=i-1;
      end
     yl = {yl{1:i+1}};
     set(gca,'ytick',yt);
     set(gca,'yticklabel',yl);

%     if nt > min(yt),
%         set(gca,'ytick',log([nt yt]));
%         set(gca,'yticklabel',{int2str(nt) yl});
%      else
%         set(gca,'ytick',log([yt]));
%         set(gca,'yticklabel',{yl});
%      end

  end


% Fig. 6.47   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

num=[20 1];
den=[1 0 0];
w=logspace(-3,1,100);
[m,p]=bode(num,den,w);
loglog(w,m);
grid;
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.47 Compensated open-loop transfer function.');


% DEMBRENDANFGPLVM3 Use the GP-LVM to model the Frey face data with DTCVAR.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'brendan';
experimentNo = 3;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('dtcvar');

latentDim = 2;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
modelWriteResult(model, dataSetName, experimentNo);

if exist('printDiagram') & printDiagram
  lvmPrintPlot(model, lbls, dataSetName, experimentNo);
end

% Load the results and display dynamically.
lvmResultsDynamic(model.type, dataSetName, experimentNo, 'image', [20 28], 1, 0, 1)


% function [x,y,gx,gy,par,threshold,mag,mage,g,FIe,FIo,mago] = quadedgep(I,par,threshold);
% Input:
%    I = image
%    par = vector for 4 parameters
%      [number of filter orientations, number of scales, filter size, elongation]
%      To use default values, put 0.
%    threshold = threshold on edge strength
% Output:
%    [x,y,gx,gy] = locations and gradients of an ordered list of edgels
%       x,y could be horizontal or vertical or 45 between pixel sites
%       but it is guaranteed that there [floor(y) + (floor(x)-1)*nr]
%       is ordered and unique.  In other words, each edgel has a unique pixel id.
%    par = actual par used
%    threshold = actual threshold used
%    mag = edge magnitude
%    mage = phase map
%    g = gradient map at each pixel
%    [FIe,FIo] = odd and even filter outputs
%    mago = odd filter output of optimum orientation
%
% Stella X. Yu, 2001

function [x,y,gx,gy,par,threshold,mag,mage,g,FIe,FIo,mago] = quadedgep(I,par,threshold);

if nargin<3 | isempty(threshold),
    threshold = 0.2;
end

[r,c] = size(I);
def_par = [8,1,20,3];

% take care of parameters, any missing value is substituted by a default value
if nargin<2 | isempty(par),
   par = def_par;
end
par(end+1:4)=0;
par = par(:);
j = (par>0);
have_value = [ j, 1-j ];
j = 1; n_filter = have_value(j,:) * [par(j); def_par(j)];
j = 2; n_scale  = have_value(j,:) * [par(j); def_par(j)];
j = 3; winsz    = have_value(j,:) * [par(j); def_par(j)];
j = 4; enlong   = have_value(j,:) * [par(j); def_par(j)];

% always make filter size an odd number so that the results will not be skewed
j = winsz/2;
if not(j > fix(j) + 0.1),
    winsz = winsz + 1;
end

% filter the image with quadrature filters
FBo = make_filterbank_odd2(n_filter,n_scale,winsz,enlong);
FBe = make_filterbank_even2(n_filter,n_scale,winsz,enlong);
n = ceil(winsz/2);
f = [fliplr(I(:,2:n+1)), I, fliplr(I(:,c-n:c-1))];
f = [flipud(f(2:n+1,:)); f; flipud(f(r-n:r-1,:))];
FIo = fft_filt_2(f,FBo,1);
FIo = FIo(n+[1:r],n+[1:c],:);
FIe = fft_filt_2(f,FBe,1);
FIe = FIe(n+[1:r],n+[1:c],:);

% compute the orientation energy and recover a smooth edge map
% pick up the maximum energy across scale and orientation
% even filter's output: as it is the second derivative, zero cross localize the edge
% odd filter's output: orientation
mag = sqrt(sum(FIo.^2,3)+sum(FIe.^2,3));
mag_a = sqrt(FIo.^2+FIe.^2);
[tmp,max_id] = max(mag_a,[],3);
base_size = r * c;
id = [1:base_size]';
mage = reshape(FIe(id+(max_id(:)-1)*base_size),[r,c]);
mage = (mage>0) - (mage<0);

ori_incr=pi/n_filter; % to convert jshi's coords to conventional image xy
ori_offset=ori_incr/2;
theta = ori_offset+([1:n_filter]-1)*ori_incr; % orientation detectors
% [gx,gy] are image gradient in image xy coords, winner take all
mago = reshape(FIo(id+(max_id(:)-1)*base_size),[r,c]);
ori = theta(max_id);
ori = ori .* (mago>0) + (ori + pi).*(mago<0);
gy = mag .* cos(ori);
gx = -mag .* sin(ori);
g = cat(3,gx,gy);

% phase map: edges are where the phase changes
mag_th = max(mag(:)) * threshold;
eg = (mag>mag_th);
h = eg & [(mage(2:r,:) ~= mage(1:r-1,:)); zeros(1,c)];
v = eg & [(mage(:,2:c) ~= mage(:,1:c-1)), zeros(r,1)];
[y,x] = find(h | v);
k = y + (x-1) * r;
h = h(k);
v = v(k);
y = y + h * 0.5; % i
x = x + v * 0.5; % j
t = h + v * r;
gx = g(k) + g(k+t);
k = k + (r * c);
gy = g(k) + g(k+t);


function BS2 = fit(v,varargin)
% function to fit Bingham parameters
%
% Description
% confidence ellipse for the mean direction based on Tanaka
% (1999) https://doi.org/10.1186/BF03351601
%
% Syntax
%   BS2 = BinghamS2.fit(v)
%
% Input
%  v - vector3d
%
% Output
%  BS2 - @BinghamS2
%
%
% Options
%  ConfElli - confidence level p (default at 0.95)
%
%
% Example
%
%   % simulate some directions
%   odf = unimodalODF(quaternion.id,'halfwidth',10*degree);
%   N = 100;
%   v = odf.discreteSample(N) .* ...
%     rotation.byAxisAngle(vector3d.X,rand(N,1)*2*pi) * vector3d.Y;
%
%   % fit a Bingham distribution
%   S2F = BinghamS2.fit(v)
%
%   % visualization
%   plot(S2F)
%   mtexColorMap LaboTeX
%   hold on
%   plot(t,'Markercolor','k','MarkerSize',3)
%   hold off
%

[a,kappa] = eig3(v*v);
kappa = kappa./sum(kappa);
Z =estimateZ(kappa);
BS2 = BinghamS2(Z, a);
BS2.N = BS2.normalizationConst;

% add the estimate of confidence level, given as ellipse half
% axes e.g.
% plot(v)
% ellipse(rotation('matrix',BS2.a.xyz'),BS2.cEllipse(1),BS2.cEllipse(2))
p = get_option(varargin,'ConfElli',0.95);
J = sqrt(chi2inv(p,2))/2;
BS2.cEllipse = [J/(-Z(2)*(kappa(3)-kappa(2))), ...
  J/(-Z(1)*(kappa(3)-kappa(1)))];

  function Z = estimateZ(kappa)
    % adapted from https://github.com/libDirectional/libDirectional
    % Igor Gilitschenski, Gerhard Kurz, Simon J. Julier, Uwe D. Hanebeck,
    % Efficient Bingham Filtering based on Saddlepoint Approximations, 2014
    % Proceedings of the 2014 IEEE International Conference on Multisensor Fusion and Information Integration (MFI 2014), Beijing, China, September 2014.

    f = @(z) findZ(z, kappa);
    Z = fsolve(f, -ones(2,1), optimset('display', 'off', 'algorithm', 'levenberg-marquardt'));
    Z = [Z; 0];
    [Z, ~] = sort(Z,'ascend');

    function R = findZ(Z, rhs)  % needs esternal mex
      Z=[Z;0];
      d = size(Z,1);

      % normalization constant
      A = numericalSaddlepointWithDerivatives(sort(-Z)+1)*exp(1);
      A = A(3);

      % derivative of normalization constant
      B = zeros(1,3);
      dim = size(Z,1);
      for i=1:dim
        mZ = Z([1:i i i:dim]);
        T = numericalSaddlepointWithDerivatives(sort(-mZ)+1)*exp(1)/(2*pi);
        B(i) = T(3);
      end

      R = zeros(d-1,1);
      for i=1:(d-1)
        R(i) = B(i)/A - rhs(i);
      end
    end
  end

end


% For FDSA, this code provides values of a, A, and c in the gains a_k=a/(k+1+A)^.602 and
% c_k = c/(k+1)^.101. Code does not have built-in provision for gradient averaging (unlike
% SPSA) due to the relatively large no. of loss evals. required per gradient.
%
% Specify the dimension, loss function, and i.c.
%
global p sigma
p=10;
loss='loss4thnoise';
theta=ones(10,1);
alpha=0.602;
%
% User input on measurement noise level, expected no. of iterations in the SPSA run,
% desired magnitude of change in the theta elements, the number of SPSA gradient approximations
% that will be averaged, and the no. of loss evaluations to be used in the gain calculations
% here (note this no. should be divisible by twice the no. of averaged gradients).
%
step= input('What is the initial desired magnitude of change in the theta elements? ');
A = .10*input('What is the expected number of loss evaluations per run? ')...
   /(2*p);
sigma = input('What is the standard deviation of the measurement noise at i.c.? ');
c = max(sigma, .0001);
NL = input('How many loss function evaluations do you want to use in this gain calculation? ');
%
% Calculate the NL/(2*p) FD gradient estimates
%
rand('seed',31415927)
randn('seed',111113); %used in setting seed for noise in loss measurements
gbar=0;
e=eye(p);
atemp=zeros(p,1);
for i=1:NL/(2*p)
   ghat=0;
   for j=1:p
      thetaplus = theta + c*e(:,j);
      thetaminus = theta - c*e(:,j);
      yplus=feval(loss,thetaplus);
      yminus=feval(loss,thetaminus);
      ghat(j) = (yplus - yminus)/(2*c);
   end
   gbar=gbar+abs(ghat);
end
gbar=gbar/(NL/(2*p));
for i=1:p
   atemp(i)=step*((A+1)^alpha)/gbar(i);
end
atemp
a=min(atemp);
c
A
a


function Distance = ComputeDistance( obj, pos )

meta = obj.Metadata{1};

%compute line distance and heading in the ground plane
StartPos = obj.axescoords2native(pos(1,:));
StopPos = obj.axescoords2native(pos(2,:));

DeltaX = (StartPos(1)-StopPos(1))*meta.Grid.Col.SS/cosd(meta.SCPCOA.TwistAng);
DeltaY = (StartPos(2)-StopPos(2))*meta.Grid.Row.SS/cosd(meta.SCPCOA.GrazeAng);

Distance = sqrt(DeltaX*DeltaX+DeltaY*DeltaY);

%alternate method...get lat/lon for each point and then compute the
%distance between the points
lla1 = point_slant_to_ground([StartPos(2), StartPos(1)]',meta);
lla2 = point_slant_to_ground([StopPos(2), StopPos(1)]',meta);

[d1km d2km]=lldistkm(lla1(1:2),lla2(1:2));

Distance = d1km*1000;

end


classdef RankTwoLaminateHomogenizerFromAllaireTestedWebPageCode

    properties
    end

    methods (Static)
        function Chomog = ChinvOld(lambda,mu,d1,d2,m1,m2,theta,epsil)

            sqrt2 = sqrt(2);

            e1x = d1(1);
            e1y = d1(2);

            e2x = d2(1);
            e2y = d2(2);

            K = (mu+lambda)/(mu*(2*mu+lambda));

            muVoigt = mu;

            A =	m1 * ( (lambda+2*mu) - 1/mu*(lambda^2*e1y^2+(lambda+2*mu)^2*e1x^2) + K*((lambda+2*mu)*e1x^2+lambda*e1y^2)^2 ) + ...
                +m2* ( (lambda+2*mu) - 1/mu*(lambda^2*e2y^2+(lambda+2*mu)^2*e2x^2) + K*((lambda+2*mu)*e2x^2+lambda*e2y^2)^2 );
            B =	m1 * ( (lambda+2*mu) - 1/mu*(lambda^2*e1x^2+(lambda+2*mu)^2*e1y^2) + K*((lambda+2*mu)*e1y^2+lambda*e1x^2)^2 )  + ...
                +m2* ( (lambda+2*mu) - 1/mu*(lambda^2*e2x^2+(lambda+2*mu)^2*e2y^2) + K*((lambda+2*mu)*e2y^2+lambda*e2x^2)^2 );
            C =	m1 * ( 4*mu - 1/mu*(2*mu)^2 + K*(4*mu*e1x*e1y)^2 )/2  + ...
                +m2* ( 4*mu - 1/mu*(2*mu)^2 + K*(4*mu*e2x*e2y)^2 )/2;
            D = 	m1 * ( 2*lambda - 1/mu*(2*lambda*(lambda+2*mu)) + K*2*((lambda+2*mu)*e1y^2+lambda*e1x^2)*((lambda+2*mu)*e1x^2+lambda*e1y^2) )/2  + ...
                +m2* ( 2*lambda - 1/mu*(2*lambda*(lambda+2*mu)) + K*2*((lambda+2*mu)*e2y^2+lambda*e2x^2)*((lambda+2*mu)*e2x^2+lambda*e2y^2) )/2;
            E =	m1 * ( -1/mu*(4*mu*(2*lambda+2*mu)*e1x*e1y) + K*2*(4*mu*e1x*e1y*((lambda+2*mu)*e1y^2+lambda*e1x^2)) )/(2*sqrt2)  + ...
                +m2* ( -1/mu*(4*mu*(2*lambda+2*mu)*e2x*e2y) + K*2*(4*mu*e2x*e2y*((lambda+2*mu)*e2y^2+lambda*e2x^2)) )/(2*sqrt2);
            F = 	m1 * ( -1/mu*(4*mu*(2*lambda+2*mu)*e1x*e1y) + K*2*(4*mu*e1x*e1y*((lambda+2*mu)*e1x^2+lambda*e1y^2)) )/(2*sqrt2)  + ...
                +m2* ( -1/mu*(4*mu*(2*lambda+2*mu)*e2x*e2y) + K*2*(4*mu*e2x*e2y*((lambda+2*mu)*e2x^2+lambda*e2y^2)) )/(2*sqrt2);

            %// Ajout du matériau mou (qui simule le vide)
            A = epsil/(1.-epsil)*(lambda+2*mu)	+ theta*A;
            B = epsil/(1.-epsil)*(lambda+2*mu)	+ theta*B;
            C = epsil/(1.-epsil)*muVoigt		+ theta*C;
            D = epsil/(1.-epsil)*lambda		+ theta*D;
            E = epsil/(1.-epsil)*0			+ theta*E;
            F = epsil/(1.-epsil)*0			+ theta*F;

            %//Première inversion
            DET = A*B*C-A*E*E-B*F*F-C*D*D+2*D*E*F;
            A1=(B*C-E*E)/DET;
            B1=(A*C-F*F)/DET;
            C1=(A*B-D*D)/DET;
            D1=(E*F-C*D)/DET;
            E1=(D*F-A*E)/DET;
            F1=(D*E-B*F)/DET;

            A = (1-theta)*A1+(lambda+2*mu)/(4*mu*(lambda+mu));
            B = (1-theta)*B1+(lambda+2*mu)/(4*mu*(lambda+mu));
            C = (1-theta)*C1+1/(muVoigt);
            D = (1-theta)*D1-lambda/(4*mu*(lambda+mu));
            E = (1-theta)*E1;
            F = (1-theta)*F1;

            %//Deuxième inversion
            DET = A*B*C-A*E*E-B*F*F-C*D*D+2*D*E*F;
            A1=(B*C-E*E)/DET;
            B1=(A*C-F*F)/DET;
            C1=(A*B-D*D)/DET;
            D1=(E*F-C*D)/DET;
            E1=(D*F-A*E)/DET;
            F1=(D*E-B*F)/DET;
            A=A1;
            B=B1;
            C=C1;
            D=D1;
            E=E1;
            F=F1;

            Chomog = [A   D	 F;
                D   B  E;
                F   E  C];

        end

    end

end


% TD | Tucker-ADAL | Tucker Decomposition solved by ADAL (Goldfarb and Qin, 2013)
% process_video('TD', 'Tucker-ADAL', 'dataset/demo.avi', 'output/demo_Tucker-ADAL.avi');

alg_path_aux = fullfile(lrs_conf.td_path,'RLRT');
addpath(genpath(alg_path_aux));

pdata.T = T;
pdata.X = T;
N = ndims(pdata.T);
r = 1/sqrt(max(size(pdata.T)));
params.E0 = tenzeros(size(pdata.T));
params.X0 = tenzeros(size(pdata.T));
params.V0 = cell(1, N);
for i = 1:N
  params.V0{i} = tenzeros(size(pdata.T));
end
params.mu0 = 1/(N+1);
params.mode = N;
params.IsTC = false; % is tensor completion
params.rRatio = 1/4;
params.opt_tol = 1e-3;
params.eta = 1/(N+1);
params.max_iter = 1000;
params.mu1fac = 10;
params.mu1 = params.mu1fac*std(T(:));
params.mu2 = params.mu1;
params.mu_min = 1e-4;
params.mu_max = 1e2;
params.lambdaS = 1;
params.lambda = params.lambdaS*r*params.rRatio;
params.verbose = 1;
params.use_cont = true;
params.k = [size(T,1) size(T,2) 1];
%%%%%%%%%% for PROPACK %%%%%%%%%%%%
% declare global var 'sv'
global sv;
global tmode;
global use_propack;
global curr_mu;
sv =  ceil(min(size(pdata.T)) * 0.1) * ones( 1, N );
use_propack = true;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
results = tensor_tucker_adal_ncx(pdata, params);
L = double(results.X);
S = double(results.E);
clear sv tmode use_propack curr_mu;

rmpath(genpath(alg_path_aux));


function [W, p, t] = build1(im, angles)
t_s = toc;
%Pad image
[im_pad,D] = impad(im,'diag',0);
sz = size(im);
m = sz(1); %rows
n = sz(2); %cols
n_proj = length(angles);
N = m*n; %number of unknown variables
M = D*n_proj; %number of equations
try
    W = zeros(M,N); %weighting factor matrix
catch expr
    fprintf([expr.message '\nGenerates a sparse.\r'])
    W = sparse(M,N);
end
%proj_info = zeros(M,2); %[theta No.]
p = zeros(M,1); %projection vector
ix = false(M,1);
rc = [m;n]/2+0.5;
%cnt = 0;
fprintf('Building Weight Matrix...\r')
for kp = 1:n_proj
    im_rot = imrotate(im_pad,-angles(kp),'bilinear','crop');
    pvec = sum(im_rot,1);
    t = -angles(kp)/180*pi;
    R = [cos(t) -sin(t);sin(t) cos(t)];
    %L = m*abs(sin(t))+n*abs(cos(t));
    %offset = ceil((D-L)/2);
    fprintf('\nAngle No.%d(%d Degree)\r',kp,angles(kp))
    for kn = 1:N
        [x,y] = ind2sub(sz,kn);
        xy_rot = R*([x;y]-rc)+D/2+0.5;
        idx = round(xy_rot(2));%#
        %corresponding indice in W and p matrix
        ixM = D*(kp-1)+idx;
        W(ixM,kn) = 1;
        %W(D*(kp-1)+idx,kn) = 1;
        %W(cnt+idx-offset,kn) = 1;
        %p(cnt+idx-offset) = projmat(kp,idx);%#
        if ~ix(ixM)
            p(ixM) = pvec(idx);
            ix(ixM) = true;
        end
        %p(D*(kp-1)+idx) = projmat(kp,idx);
        %proj_info(cnt+idx-offset,:) = [kp,idx];
    end
    %cnt = cnt+(D-2*offset);
    %fprintf('%d equations built.\r',D-2*offset)
end
% %
W = sparse(W);

%Delete all-zero rows in W
fprintf('\nDelete all-zero rows in W...\r')
%ix = sum(W,2)==0;
W(~ix,:) = [];
p(~ix) = [];
n_eq = size(W,1);
fprintf('\nFinish building A. \n%d equations in total.\r',n_eq);
figure('name','Sparsity pattern of matrix W');
spy(W)
t_e = toc;
t = t_e-t_s;
fprintf('\nTotal: %f seconds\n\r',t);
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 pathS = MC_VGGOU(S0,r,d,T,C,G,M,lambda,a,b,NTime,NSim,NBatches)
    intNt = 20;                 % steps in between orginial grid points
    allsteps = intNt * NTime;   % grid containing all necsseary steps
    dT = T / NTime;             % time step
    time = 0 : dT : T;          % tiem for martingale correction

    pathS = zeros(NSim,NTime+1,NBatches); % output
    lnS = zeros(NSim,NTime+1);      % used in batch
    lnS(:,1) = log(S0*exp(-d*T));   % set S(0) dividend adjusted

    %precompute the constants used for martingale correction
    y0 = 1;
    psiVG = (-1i)*C*log(G*M/(G*M+(M-G)-1));     % char exp                                     %characteristic exponent
    phiGOU = 1i*psiVG*y0/lambda*(1-exp(-lambda*time)) + ...
         lambda*a./(1i*psiVG-lambda*b).* ...
         (b*log(b./(b-1i*psiVG/lambda* ...
         (1-exp(-lambda*time))))-1i*psiVG*time); % char func
    omegaT = -phiGOU;           % martingale correction                                                         %martingale correction value
    omegaT(1) = 0;              % martingale correction in 0

for l = 1:NBatches              % batch loop
    yy = ones(NSim,allsteps+1); % init stochastic clock
    Np = poissrnd(a*lambda/allsteps*T,[NSim,allsteps]);

    for k = 1 : NSim
        for j = 1 : allsteps     % generating OU process
            if Np(k,j) > 0
                Ex = -log(rand(Np(k,j),1))/b; % exponential law
                U = exp(-lambda * T / allsteps * rand(Np(k,j),1));          % Uniforms
                yy(k,j+1) = (1-lambda*T/allsteps)*yy(k,j) ...
                    + sum(Ex .* U);
         else
                yy(k,j+1) = (1-lambda*T/allsteps)*yy(k,j);
            end
        end
    end

    ZZ = T*cumsum(yy,2)/allsteps;       %Integrated Time
    Y = zeros(NSim,NTime+1);
    for m = 2:NTime+1
        Y(:,m) = ZZ(:,(m-1)*intNt);
    end

    Intensity = C * (Y(:,2:end)-Y(:,1:end-1));      % intensity
    DGam = gamrnd(Intensity,1/M) - gamrnd(Intensity,1/G);
    diffomegaT = omegaT(2:end) - omegaT(1:end-1);   % maringale corr

    for m=2:NTime+1             % time loop
        lnS(:,m) = lnS(:,m-1) + (r-d)*dT ...
            + diffomegaT(m-1) + DGam(:,m-1);
    end
    pathS(:,:,l) = exp(lnS);
end


function value = he_double_product_integral ( i, j )

%*****************************************************************************80
%
%% HE_DOUBLE_PRODUCT_INTEGRAL: integral of He(i,x)*He(j,x)*e^(-x^2/2).
%
%  Discussion:
%
%    VALUE = integral ( -oo < x < +oo ) He(i,x)*He(j,x) exp(-x^2/2) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, J, the polynomial indices.
%
%    Output, real VALUE, the value of the integral.
%
  if ( i ~= j )
    value = 0.0;
  else
    value = r8_factorial ( i );
  end

  return
end


function delay = fast_slicedelay(TR,nSlices,Slice,AcqOrder)
% delay = fast_slicedelay(TR,nSlices,Slice,AcqOrder)
%
% Computes the amount of time after the start of a TR that a slice
% is aquired given the TR, the number of slices, and the order of
% acquisition (linear or interleaved).
%

if(nargin ~= 4)
     msg = 'USAGE: delay = fast_slicedelay(TR,nSlices,Slice,AcqOrder)';
  qoe(msg);error(msg);
end

if( ~strcmpi(AcqOrder,'linear') & ~strcmpi(AcqOrder,'interleaved'))
  msg = sprintf('AcqOrder = %s, must be either linear or interleaved',AcqOrder);
  qoe(msg);error(msg);
end

dt = TR/nSlices;

if( strcmpi(AcqOrder,'linear') )
  SliceOrder = [0:nSlices-1];
end

if( strcmpi(AcqOrder,'interleaved') )
  SliceOrder = [[0:2:nSlices-1] [1:2:nSlices] ];
end

nthAcq = find(SliceOrder == Slice) - 1;
delay = dt*nthAcq;

return;


function d = p09_fd ( p )

%*****************************************************************************80
%
%% P20_FD is a signed distance function for problem 9.
%
%  Discussion:
%
%    ICAM is represented by 21 regions: 20 rectangles and one polygon.
%
%    Region 12 is not a room, and should be excluded.
%    Region 13 is a staircase to the second floor, and might be excluded.
%
%    The building is contained in the rectangle
%
%      -7 <= X <= 34
%       0 <= Y <= 98
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 December 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P, one or more points.
%
%    Output, real D, the signed distance of each point to the boundary of the region.
%
  v = [ ...
     7.25,  0.25; ... % Lower left corner of R1.
    14.75,  0.25; ...
    14.75,  7.75; ...
    13.00,  7.75; ...
    13.00,  8.25; ...
    25.00,  8.25; ...
    25.00,  7.75; ...
    15.25,  7.75; ...
    15.25,  0.25; ... % Lower left corner of R2.
    28.75,  0.25; ...
    28.75,  7.75; ...
    28.75,  8.25; ...
    29.25,  8.25; ...
    29.25,  7.75; ...
    29.25,  0.25; ... % Lower left corner of R3.
    40.75,  0.25; ...
    40.75,  7.75; ...
    33.00,  7.75; ...
    33.00,  8.25; ...
    33.25,  8.25; ... % Lower left corner of R5.
    40.75,  8.25; ...
    40.75, 17.75; ...
    33.25, 17.75; ...
    33.25, 12.00; ...
    32.75, 12.00; ...
    32.75, 21.75; ...
    20.75, 21.75; ...
    20.75, 22.25; ...
    21.25, 22.25; ... % Lower left corner of R8.
    32.75, 22.25; ...
    32.75, 25.25; ...
    35.75, 25.25; ...
    35.75, 34.75; ...
    32.75, 34.75; ...
    32.75, 37.75; ...
    21.25, 37.75; ...
    21.25, 26.00; ...
    20.75, 26.00; ...
    20.75, 38.25; ...
    25.75, 38.25; ...
    26.25, 38.25; ... % Lower left corner of R11.
    32.75, 38.25; ...
    32.75, 45.75; ...
    26.25, 45.75; ...
    26.25, 41.75; ...
    25.75, 41.75; ...
    25.75, 46.25; ...
    26.25, 46.25; ... % Lower left corner of R15.
    32.75, 46.25; ...
    32.75, 53.75; ...
    26.25, 53.75; ...
    26.25, 49.25; ...
    25.75, 49.25; ...
    25.75, 53.75; ...
    23.75, 53.75; ...
    23.75, 62.75; ...
    23.75, 97.75; ...
    16.25, 97.25; ...
    16.25, 88.00; ...
    15.75, 88.00; ...
    15.75, 97.25; ...
     0.25, 97.25; ...
     0.25, 75.25; ... % Lower left corner of R21
    15.75, 75.25; ...
    15.75, 85.00; ...
    16.25, 85.00; ...
    16.25, 74.75; ...
     0.25, 74.75; ...
     0.25, 63.25; ... % Lower left corner of R19
    16.25, 63.25; ...
    16.25, 62.75; ...
    16.25, 58.25; ... % Lower left corner of R18
    19.25, 58.25; ...
    19.25, 54.25; ...
    20.25, 54.25; ...
    20.25, 53.75; ...
    20.25, 50.75; ... % Lower left corner of R16.
    23.25, 50.75; ...
    23.25, 41.75; ...
    17.25, 41.75; ...
    16.75, 41.75; ...
    16.75, 53.75; ...
     7.25, 53.75; ...
     7.25, 38.25; ...
    17.25, 38.25; ...
    17.25, 37.75; ...
     7.25, 37.75; ...
     7.25, 22.25; ...
    16.75, 22.25; ...
    16.75, 34.25; ...
    17.25, 34.25; ...
    17.25, 21.75; ...
     7.25, 21.75; ...
     7.25,  8.25; ... % Lower left corner of R4.
     9.00,  8.25; ...
     9.00,  7.75; ...
     7.25,  7.75; ...
     7.25,  0.25 ];

  d = dpoly ( p, v );

  return
end


clear all; close all; clc;

% pull=[3 5 7 9 11];
% start=1:20;
% q=abs(start-pull(1));
% [x,n2]=min(q);
%
% for j=1:4
%    q=abs(start-pull(j));
%    [x,n2]=min(q)
%    start=start([1:n2-1 n2+1:(20-j)]);
%    start'
% end
%
% break

%%
L=10; x3=-L:0.1:L; n=length(x3)-1; % define domain
x2=x3(1:n); k=(2*pi/(2*L))*[0:n/2-1 -n/2:-1]; % k-vector
ye=exp(-(x2.^2)); ye2=exp((x2.^2)/2); % define Gaussians
for j=0:9      % loop through 10 modes
  yd=real(ifft(((i*k).^j).*fft(ye))); % 2nd derivative
  mode=((-1)^(j))*(((2^j)*factorial(j)*sqrt(pi))^(-0.5))*ye2.*yd;
  y(:,j+1)=(mode).';  % store modes as columns
end

x=x2(n/2+1-40:n/2+1+40);  % keep only -4<x<4
yharm=y(n/2+1-40:n/2+1+40,:);

n=length(x);
f=(exp(-(x-0.5).^2)+3*exp(-2*(x+1.5).^2))';

%%  Willcox alg.

% sensor 1
for jloop=1:81

s=zeros(n,1);
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,1), pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f1=yharm*atild;
Er(1)=norm(f1-f);

%con(jloop)=cond(M2);
con(jloop)=2*sum(diag(M2))-sum(M2(:));

end

%[s1,n1]=min(con)
[s1,n1]=max(con)

% sensor 2
jlook=[1:n1-1 n1+1:81];
for jloop=jlook

s=zeros(n,1);
s(n1)=1;
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,2), pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f2=yharm*atild;
Er(2)=norm(f2-f);

%con2(jloop)=cond(M2);
con2(jloop)=2*sum(diag(M2))-sum(M2(:));

end
%[s2,n2]=min(con2(jlook))
[s2,n2]=max(con2(jlook))

% sensor 3
if n2>n1
   jlook=[1:n1-1 n1+1:n2-1 n2+1:81];
else
   jlook=[1:n2-1 n2+1:n1-1 n1+1:81];
end

jlook
break
for jloop=jlook

s=zeros(n,1);
s(n1)=1; s(n2)=1;
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,3), pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f3=yharm*atild;
Er(3)=norm(f3-f);

%con3(jloop)=cond(M2);
con3(jloop)=2*sum(diag(M2))-sum(M2(:));

end
%[s3,n3]=min(con3(jlook))
[s3,n3]=max(con3(jlook))

% sensor 4
jlook=[1:n1-1 n1+1:n2-1 n2+1:n3-1 n3+1:81];
for jloop=jlook

s=zeros(n,1);
s(n1)=1; s(n2)=1;
s(jloop)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%figure(3), subplot(2,2,4),pcolor(10:-1:1,1:10,(M2'));, colormap(hot)

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f4=yharm*atild;
Er(4)=norm(f4-f);

%con4(jloop)=cond(M2);
con4(jloop)=2*sum(diag(M2))-sum(M2(:));

end
%[s4,n4]=min(con4(jlook))
[s4,n4]=max(con4(jlook))

figure(1)
subplot(4,1,1), bar((con)), axis([1 81 0 1])
subplot(4,1,2), bar((con2)), axis([1 81 0 1])
subplot(4,1,3), bar((con3)), axis([1 81 0 1])
subplot(4,1,4), bar((con4)), axis([1 81 0 1])

break

%%  Willcox:  condition number

% pull=[3 5 7 9 11];
% start=1:20;
% q=abs(start-pull(1));
% [x,n2]=min(q);
%
% for j=1:4
%    q=abs(start-pull(j));
%    [x,n2]=min(q)
%    start=start([1:n2-1 n2+1:(20-j)]);
%    start'
% end
%
% break
clear f1
clear f2
clear ns
ns=[]; jlook=1:81;

count=1;
for jsense=1:20

for jloop=1:(82-jsense)
s=zeros(n,1); s(ns)=1;
s(jlook(jloop))=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
con(jloop)=cond(M2);

end

length(con)
[s1,n1]=min(con)
kond(jsense)=s1;
clear con
ns=[ns n1];

jlook=jlook([1:n1-1 n1+1:(81-count+1)]);
count=count+1;

% reconstruct
s=zeros(n,1);
s(ns)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f1(:,jsense)=yharm*atild;
Errr(jsense)=norm(f1(:,jsense)-f);
scum(:,jsense)=s;

end

figure(4)
subplot(2,1,2), bar(log(Errr+1))
subplot(2,1,1), bar(log(kond))

subplot(2,1,2), set(gca,'Xlim',[0 21],'Xtick',[1 5 10 15 20],'Xticklabel',{'','','','',''},'Ylim',[0 12],'Ytick',[0 4 8 12],'Yticklabel',{'','','',''})
subplot(2,1,1), set(gca,'Xlim',[0 21],'Xtick',[1 5 10 15 20],'Xticklabel',{'','','','',''},'Ylim',[0 42],'Ytick',[0 20 40],'Yticklabel',{'','',''})

%figure(6), bar(s)

figure(7),
titer=[1:20 25]; f1=[f1 f]; titer2=[9:20 25], f2=[f1(:,9:20) f];
subplot(2,2,1), waterfall(x,titer,f1.'), colormap([0 0 0]), view(-150,50)
subplot(2,2,2), waterfall(x,titer2,f2.'), colormap([0 0 0]), view(-150,50)

subplot(2,2,1), set(gca,'Zlim',[-2 8],'Xlim',[-4 4],'Xtick',[-4 0 4],'Xticklabel',{'','',''}, ...
    'Ylim',[0 25],'Ytick',[1 10 20],'Yticklabel',{'','',''},'Ztick',[-2 0 4 8],'Zticklabel',{'','','',''})
subplot(2,2,2), set(gca,'Zlim',[-.2 3],'Xlim',[-4 4],'Xtick',[-4 0 4],'Xticklabel',{'','',''}, ...
    'Ylim',[0 25],'Ytick',[1 10 20],'Yticklabel',{'','',''},'Ztick',[0 1 2 3],'Zticklabel',{'','','',''})

figure(8)
%bar3(scum)
axes('position',[.05 .05 .9 .35])
scum2=[scum(:,[1:11 13:19])];
pcolor(-scum2.'), colormap(hot), axis off

%bar3(scum2)

%caxis([0 1]), axis off

%%  Willcox:  diagonal sum

% pull=[3 5 7 9 11];
% start=1:20;
% q=abs(start-pull(1));
% [x,n2]=min(q);
%
% for j=1:4
%    q=abs(start-pull(j));
%    [x,n2]=min(q)
%    start=start([1:n2-1 n2+1:(20-j)]);
%    start'
% end
%
% break
clear f1
clear f2
clear ns
clear s
ns=[]; jlook=1:81;

count=1;
for jsense=1:20

for jloop=1:(82-jsense)
s=zeros(n,1); s(ns)=1;
s(jlook(jloop))=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end
%con(jloop)=cond(M2);
con(jloop)=2*sum(diag(M2))-sum(M2(:));

end

%[s1,n1]=min(con)
[s1,n1]=max(con)
kond(jsense)=s1;
clear con
ns=[ns n1];

jlook=jlook([1:n1-1 n1+1:(81-count+1)]);
count=count+1;

% reconstruct
s=zeros(n,1);
s(ns)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end

for j=1:10  % reconstruction using gappy
  ftild(j,1)=trapz(x,s.*(f.*yharm(:,j)));
end
atild=M2\ftild;   % compute error
f1(:,jsense)=yharm*atild;
Errr(jsense)=norm(f1(:,jsense)-f);
scum(:,jsense)=s;

end

figure(4)
subplot(2,1,2), bar(Errr)
subplot(2,1,1), bar((kond))

subplot(2,1,2), set(gca,'Xlim',[0 21],'Xtick',[1 5 10 15 20],'Xticklabel',{'','','','',''},'Ylim',[0 12],'Ytick',[0 4 8 12],'Yticklabel',{'','','',''})
subplot(2,1,1), set(gca,'Xlim',[0 21],'Xtick',[1 5 10 15 20],'Xticklabel',{'','','','',''},'Ylim',[0 42],'Ytick',[0 20 40],'Yticklabel',{'','',''})

%figure(6), bar(s)

figure(7),
titer=[1:20 25]; f1=[f1 f]; titer2=[9:20 25], f2=[f1(:,9:20) f];
subplot(2,2,1), waterfall(x,titer,f1.'), colormap([0 0 0]), view(-150,50)
subplot(2,2,2), waterfall(x,titer2,f2.'), colormap([0 0 0]), view(-150,50)

subplot(2,2,1), set(gca,'Zlim',[-2 8],'Xlim',[-4 4],'Xtick',[-4 0 4],'Xticklabel',{'','',''}, ...
    'Ylim',[0 25],'Ytick',[1 10 20],'Yticklabel',{'','',''},'Ztick',[-2 0 4 8],'Zticklabel',{'','','',''})
subplot(2,2,2), set(gca,'Zlim',[-.2 3],'Xlim',[-4 4],'Xtick',[-4 0 4],'Xticklabel',{'','',''}, ...
    'Ylim',[0 25],'Ytick',[1 10 20],'Yticklabel',{'','',''},'Ztick',[0 1 2 3],'Zticklabel',{'','','',''})

figure(8)
%bar3(scum)
axes('position',[.05 .05 .9 .35])
scum2=[scum(:,[1:11 13:19])];
pcolor(-scum2.'), colormap(hot), axis off

%bar3(scum2)

%caxis([0 1]), axis off

%% Test Random trials with P% of measurements
% spectrum of mu
break
per=[20 40 60 81];
for thresh=1:4

n2=randsample(n,per(thresh));
s=zeros(n,1); s(n2)=1;

for j=1:10
   for jj=1:j
       Area=trapz(x,s.*(yharm(:,j).*yharm(:,jj)));
       M2(j,jj)=Area; M2(jj,j)=Area;
   end
end

[v,d]=eigs(M2);
subplot(2,2,thresh)
plot(real(diag(d)),imag(diag(d)),'ko','Linewidth',[2])
axis([0 2 -1 1])
end


function output =EH_AHE(input,conf)

output=input; %output is the input after AHE
[m,n,~]=size(input);

%ECR
grid_cols=conf.grid;
grid_rows=conf.grid;
limit = conf.limit;

grid_width=int32(fix(m/grid_cols));
grid_height=int32(fix(n/grid_rows));

map=zeros(grid_cols,grid_rows,256);

%for each grid,we create their mapping function
for i=1:grid_cols
    for j=1:grid_rows
        map(i,j,:)=MakeHistogram(input,1+(i-1)*grid_width,1+(j-1)*grid_height,grid_width,grid_height,limit);
    end
end

%interpolate
%boundary cases I followed the Karel Zuiderveld's implement(C version)
xi = 1;
for i = 1:grid_cols+1
    if i == 1
        subx = grid_width/2;
        xu = 1;
        xd = 1;
    elseif i == grid_cols+1
        subx = grid_width/2;
        xu =  grid_cols;
        xd =  grid_cols;
    else
        subx = grid_width;
        xu = i - 1;
        xd = i;
    end
    yi = 1;
    for j = 1:grid_rows+1
        if j == 1
            suby = grid_height/2;
            yl = 1;
            yr = 1;
        elseif j == grid_rows+1
            suby = grid_height/2;
            yl = grid_rows;
            yr = grid_rows;
        else
            suby = grid_height;
            yl = j - 1;
            yr = j;
        end
        UL = map(xu,yl,:);
        UR = map(xu,yr,:);
        DL = map(xd,yl,:);
        DR = map(xd,yr,:);

        subinput = input(xi:xi+subx-1,yi:yi+suby-1);
        subinput = Interpolate(subinput,UL,UR,DL,DR,subx,suby);
        output(xi:xi+subx-1,yi:yi+suby-1) = subinput;
        yi = yi + suby;
    end
    xi = xi + subx;
end

end


function [F10ycp, F01ycp] = Q1001ycpowp(F10, F01, p, c)
%------------------------------------------------------------------------------
%
% This function computes the gridfunction {F10ycp U F01ycp} of size {F10 U F01}
% with values (x-c)^p at its gridpoints. Piecewise constant approximation is
% assumed.
%
%   Orientation
%
%         x
%     o---->
%     |
%   y |
%     v
%
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: December 12, 2000.
% (c) 1999-2000 Stichting CWI, Amsterdam
%------------------------------------------------------------------------------
if isempty(F10) || isempty(F01)
  error(' Q1001ycpowp - at least one colour not present (empty) ')
else
  [n10, m10] = size(F10);
  [n01, m01] = size(F01);
  if p == 0
    F10ycp = ones(n10, m10);
    F01ycp = ones(n01, m01);
  else
    [hx, hy] = Q1001gridfdims(F10, F01);
%
    yfirst =  0 + hy/2 - c;
    ylast  =  yfirst + (m01 - 1) * 2 * hy;
    lineofycp = linspace(yfirst, ylast, n01)';
    F01ycp = lineofycp(:,ones(m01,1));
    if p ~= 1
      F01ycp = F01ycp.^p;
    end
%
    yfirst =  0 + 3*hy/2 - c;
    ylast  =  yfirst + (m10 - 1) * 2 * hy;
    lineofycp = linspace(yfirst, ylast, n10)';
    F10ycp = lineofycp(:,ones(m10,1));
    if p ~= 1
      F10ycp = F10ycp.^p;
    end
  end
end
%------------------------------------------------------------------------------


function A2 = double_diagonal_blocks(A, row_range, col_range)
% function A = double_diagonal_blocks(A, row_range, col_range)
% Double diagonal blocks of a block matrix A whose row range and col range
% are `row_range` and `col_range`
% required: `numel(row_range) == numel(col_range)
% -----------------------------------------------
% Author: Tiep Vu, thv102@psu.edu, 5/11/2016
%         (http://www.personal.psu.edu/thv102/)
% -----------------------------------------------
	if nargin == 0 % test mode
		row_range = [0 3 5 8];
		col_range = [0 4 7 10];
		A = rand(row_range(end), col_range(end));
		Ain = A;
		subplot(1, 3, 1); imagesc(A); title('input'); colormap jet;
	end
	%% check requirements
	if numel(row_range) ~= numel(col_range)
		error('number of blocks in each dimension of the input matrix must be the same');
	end
	%% MAIN
	mask_diagonal_id = build_diagonal_mask(row_range, col_range);
	A(mask_diagonal_id) = 2*A(mask_diagonal_id);
	%% test mode
	if nargin == 0
		subplot(1, 3, 2); imagesc(A); title('output');
		subplot(1, 3, 3); imagesc(A - Ain); title('difference');
		A2 = [];
	end
end


function linpack_d_test31 ( )

%*****************************************************************************80
%
%% TEST31 tests DTRSL.
%
%  Discussion:
%
%    DTRSL solves triangular linear systems.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  lda = n;

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST31\n' );
  fprintf ( 1, '  For a triangular matrix,\n' );
  fprintf ( 1, '  DTRSL solves a linear system.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Lower triangular matrix A.
%
  [ a, seed ] = r8mat_uniform_01 ( n, n, seed );

  for i = 1 : n
    for j = i+1 : n
      a(i,j) = 0.0;
    end
  end

  for i = 1 : n
    x(i,1) = i;
  end

  b(1:n,1) = a(1:n,1:n) * x(1:n,1);

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For a lower triangular matrix A,\n' );
  fprintf ( 1, '  solve A * x = b\n' );

  job = 00;

  [ b, info ] = dtrsl ( a, lda, n, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The solution (should be 1,2,3,4,5):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %6d  %14f\n', i, b(i,1) );
  end

  b(1:n,1) = a(1:n,1:n)' * x(1:n,1);

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For a lower triangular matrix A,\n' );
  fprintf ( 1, '  solve A'' * x = b\n' );

  job = 10;

  [ b, info ] = dtrsl ( a, lda, n, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The solution (should be 1,2,3,4,5):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %6d  %14f\n', i, b(i,1) );
  end
%
%  Upper triangular matrix A.
%
  [ a, seed ] = r8mat_uniform_01 ( n, n, seed );

  for i = 1 : n
    for j = 1 : i - 1
      a(i,j) = 0.0;
    end
  end

  for i = 1 : n
    x(i,1) = i;
  end

  b(1:n,1) = a(1:n,1:n) * x(1:n,1);

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For an upper triangular matrix A,\n' );
  fprintf ( 1, '  solve A * x = b\n' );

  job = 01;

  [ b, info ] = dtrsl ( a, lda, n, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The solution (should be 1,2,3,4,5):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %6d  %14f\n', i, b(i,1) );
  end

  b(1:n,1) = a(1:n,1:n)' * x(1:n,1);

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For an upper triangular matrix A,\n' );
  fprintf ( 1, '  solve A'' * x = b\n' );

  job = 11;

  [ b, info ] = dtrsl ( a, lda, n, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The solution (should be 1,2,3,4,5):\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %6d  %14f\n', i, b(i,1) );
  end

  return
end


function quad_mesh_order1_display ( prefix )

%*****************************************************************************80
%
%% QUAD_MESH_ORDER1_DISPLAY plots piecewise constant quad mesh data.
%
%  Discussion:
%
%    This program reads three data files defining piecewise constant
%    data over a mesh of quadrilaterals, and displays a 3D MATLAB plot
%    of the data.
%
%  Usage:
%
%    quad_mesh_order1_display ( 'prefix' )
%
%    where
%
%    * 'prefix'_nodes.txt contains the node coordinates;
%    * 'prefix'_elements.txt contains the element definitions.
%    * 'prefix'_values.txt contains the values associated with each element.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string PREFIX, the common file prefix.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUAD_MESH_ORDER1_DISPLAY:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read and plot piecewise constant data on a quadrilateral mesh.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  This program expects to find three files to read:\n' );
  fprintf ( 1, '  * a node file,\n' );
  fprintf ( 1, '  * an element file,\n' );
  fprintf ( 1, '  * a value file (one value per element)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  It reads the files and displays a plot.\n' );
%
%  The command line argument is the common filename prefix.
%
  if ( nargin < 1 )

    fprintf ( 1, '\n' );

    prefix = input ( 'Enter the filename prefix, in quotes:  ' );

  end
%
%  Create the filenames.
%
  node_filename = strcat ( prefix, '_nodes.txt' );
  element_filename = strcat ( prefix, '_elements.txt' );
  value_filename = strcat ( prefix, '_values.txt' );
%
%  Read the node data.
%
  [ dim_num, node_num ] = r8mat_header_read ( node_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".', node_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension DIM_NUM = %d\n', dim_num );
  fprintf ( 1, '  Number of points NODE_NUM = %d\n', node_num );

  if ( dim_num ~= 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'QUAD_MESH_ORDER1_DISPLAY - Fatal error!\n' );
    fprintf ( 1, '  Dataset must have spatial dimension 2.\n' );
    error ( 'QUAD_MESH_ORDER1_DISPLAY - Fatal error!' );
  end

  node_xy = r8mat_data_read ( node_filename, dim_num, node_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', node_filename );

  r8mat_transpose_print_some ( dim_num, node_num, node_xy, 1, 1, dim_num, 5, ...
    '  First 5 nodes:' );
%
%  Read the element data.
%
  [ element_order, element_num ] = i4mat_header_read ( element_filename );

  if ( element_order ~= 4 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'QUAD_MESH_ORDER1_DISPLAY - Fatal error!\n' );
    fprintf ( 1, '  Data is not for a 4-node quadrilateral mesh.\n' );
    error ( 'QUAD_MESH_ORDER1_DISPLAY - Fatal error!' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".\n', element_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Element order = %d\n', element_order );
  fprintf ( 1, '  Number of elements ELEMENT_NUM  = %d\n', element_num );

  element_node = i4mat_data_read ( element_filename, element_order, element_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', element_filename );

  i4mat_transpose_print_some ( element_order, element_num, ...
    element_node, 1, 1, element_order, 10, '  First 10 elements:' );
%
%  Detect and correct 0-based indexing.
%
  element_node = mesh_base_one ( node_num, element_order, element_num, ...
    element_node );
%
%  Read the values.
%
  [ value_dim, value_num ] = r8mat_header_read ( value_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".', value_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension = %d\n', value_dim );
  fprintf ( 1, '  Number of values  = %d\n', value_num );

  if ( value_dim ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'QUAD_MESH_ORDER1_DISPLAY - Fatal error!\n' );
    fprintf ( 1, '  VALUE data must be scalar.\n' );
    error ( 'QUAD_MESH_ORDER1_DISPLAY - Fatal error!' );
  end

  if ( value_num ~= element_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'QUAD_MESH_ORDER1_DISPLAY - Fatal error!\n' );
    fprintf ( 1, '  Number of values must equal number of elements.\n' );
    error ( 'QUAD_MESH_ORDER1_DISPLAY - Fatal error!' );
  end

  value = r8mat_data_read ( value_filename, value_dim, value_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', value_filename );

  r8mat_transpose_print_some ( value_dim, value_num, value, 1, 1, value_dim, 5, ...
    '  First 5 values:' );
%
%  Display the mesh.
%
  figure ( 1 )
  clf
  hold on

  value_min = min ( value(1:element_num) );
  value_max = max ( value(1:element_num) );

  caxis ( [ value_min, value_max ] );

  for e = 1 : element_num

    n1 = element_node(1,e);
    n2 = element_node(2,e);
    n3 = element_node(3,e);
    n4 = element_node(4,e);

    x1 = node_xy(1,n1);
    x2 = node_xy(1,n2);
    x3 = node_xy(1,n3);
    x4 = node_xy(1,n4);

    y1 = node_xy(2,n1);
    y2 = node_xy(2,n2);
    y3 = node_xy(2,n3);
    y4 = node_xy(2,n4);

    fill ( [ x1, x2, x3, x4 ], [ y1, y2, y3, y4 ], value(e) );

  end

  xlabel ( 'X', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16 );

  ylabel ( 'Y', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16, 'Rotation', 0 );

  title ( 'Quadrilateral Mesh', 'FontName', 'Helvetica', 'FontWeight', ...
    'bold', 'FontSize', 16 );

  colorbar ( );

  hold off

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Press return for 3D image:\n' );

  pause
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Here is a 3D image of the data.\n' );
  fprintf ( 1, '  Use the 3D-Rotate menu item to examine the picture.\n' );

  quad_mesh_order1_display_image ( node_xy, element_num, ...
    element_node, value );

  hold off
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUAD_MESH_ORDER1_DISPLAY:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function column_num = file_column_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_COLUMN_COUNT counts the columns in the first line of a file.
%
%  Discussion:
%
%    The file is assumed to be a simple text file.
%
%    Most lines of the file are presumed to consist of COLUMN_NUM words,
%    separated by spaces.  There may also be some blank lines, and some
%    comment lines, which have a "#" in column 1.
%
%    The routine tries to find the first non-comment non-blank line and
%    counts the number of words in that line.
%
%    If all lines are blanks or comments, it goes back and tries to analyze
%    a comment line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the file.
%
%    Output, integer COLUMN_NUM, the number of columns in the file.
%
  FALSE = 0;
  TRUE = 1;
%
%  Open the file.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_COLUMN_COUNT - Error!' );
  end
%
%  Read one line, but skip blank lines and comment lines.
%  Use FGETL so we drop the newline character!
%
  got_one = FALSE;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

    elseif ( line(1) == '#' )

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

  if ( got_one == FALSE )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Warning!\n' );
    fprintf ( 1, '  The file does not seem to contain any data.\n' );
    column_num = -1;
    return;
  end

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_ROW_COUNT counts the number of row records in a file.
%
%  Discussion:
%
%    Each input line is a "RECORD".
%
%    The records are divided into three groups:
%
%    * BLANK LINES (nothing but blanks)
%    * COMMENT LINES (begin with a '#')
%    * DATA RECORDS (anything else)
%
%    The value returned by the function is the number of data records.
%
%    By the way, if the MATLAB routine FGETS is used, instead of
%    FGETL, then the variable LINE will include line termination
%    characters, which means that a blank line would not actually
%    have zero characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the input file.
%
%    Output, integer ROW_NUM, the number of rows found.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_ROW_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_ROW_COUNT - Error!' );
  end

  blank_num = 0;
  comment_num = 0;
  row_num = 0;

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    record_num = record_num + 1;
    record_length = s_len_trim ( line );

    if ( record_length <= 0 )
      blank_num = blank_num + 1;
    elseif ( line(1) == '#' )
      comment_num = comment_num + 1;
    else
      row_num = row_num + 1;
    end

  end

  fclose ( input_unit );

  return
end
function table = i4mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% I4MAT_DATA_READ reads data from an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns in the data.
%
%    Output, integer TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %d' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the input file.\n' );
    error ( 'I4MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
      fprintf ( 1, '  End of input while reading data.\n' );
      error ( 'I4MAT_DATA_READ - Error!' );
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = i4mat_header_read ( input_filename )

%*****************************************************************************80
%
%% I4MAT_HEADER_READ reads the header from an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function i4mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% I4MAT_TRANSPOSE_PRINT_SOME prints some of an I4MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, an optional title.
%
  incx = 10;

  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d  ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );
    fprintf ( 1, '\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d  ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%7d  ', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function element_node = mesh_base_one ( node_num, element_order, ...
  element_num, element_node )

%*****************************************************************************80
%
%% MESH_BASE_ONE ensures that the element definition is one-based.
%
%  Discussion:
%
%    The ELEMENT_NODE array contains nodes indices that form elements.
%    The convention for node indexing might start at 0 or at 1.
%    Since a MATLAB program will naturally assume a 1-based indexing, it is
%    necessary to check a given element definition and, if it is actually
%    0-based, to convert it.
%
%    This function attempts to detect 0-based node indexing and correct it.
%
%    Thanks to Feifei Xu for pointing out that I was subtracting 1 when I
%    should have been adding 1!  29 November 2012.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input/output, integer ELEMENT_NODE(ELEMENT_ORDE,ELEMENT_NUM), the element
%    definitions.
%
  node_min = min ( min ( element_node(1:element_order,1:element_num) ) );
  node_max = max ( max ( element_node(1:element_order,1:element_num) ) );

  if ( node_min == 0 && node_max == node_num - 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE:\n' );
    fprintf ( 1, '  The element indexing appears to be 0-based!\n' );
    fprintf ( 1, '  This will be converted to 1-based.\n' );
    element_node(1:element_order,1:element_num) = ...
      element_node(1:element_order,1:element_num) + 1;
  elseif ( node_min == 1 && node_max == node_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE:\n' );
    fprintf ( 1, '  The element indexing appears to be 1-based!\n' );
    fprintf ( 1, '  No conversion is necessary.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE - Warning!\n' );
    fprintf ( 1, '  The element indexing is not of a recognized type.\n' );
    fprintf ( 1, '  NODE_MIN = %d\n', node_min );
    fprintf ( 1, '  NODE_MAX = %d\n', node_max );
    fprintf ( 1, '  NODE_NUM = %d\n', node_num );
  end

  return
end
function quad_mesh_order1_display_image ( node_xy, element_num, ...
  element_node, value )

%*****************************************************************************80
%
%% QUAD_MESH_ORDER1_DISPLAY_IMAGE plots piecewise constant data.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer NODE_XY(2,NODE_NUM), the coordinates of the nodes.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_NODE(4,ELEMENT_NUM),
%    the nodes that made up each element.
%
%    Input, real VALUE(ELEMENT_NUM), the value assigned to each element
%
  zmax = max ( value(1:element_num) );
  zmin = min ( value(1:element_num) );

  caxis ( [ zmin, zmax ] );
%
%  Pick the colors that will correspond to the minimum and maximum
%  values of Z.
%
  rmax = 0.8;
  gmax = 0.2;
  bmax = 0.1;

  rmin = 0.1;
  gmin = 0.3;
  bmin = 0.7;

  figure ( 2 )
  clf
  hold on

  for element = 1 : element_num
%
%  Pick out the nodes of the triangle.
%
    x1 = node_xy(1,element_node(1,element));
    y1 = node_xy(2,element_node(1,element));
    x2 = node_xy(1,element_node(2,element));
    y2 = node_xy(2,element_node(2,element));
    x3 = node_xy(1,element_node(3,element));
    y3 = node_xy(2,element_node(3,element));
    x4 = node_xy(1,element_node(4,element));
    y4 = node_xy(2,element_node(4,element));

    z = value(element);
%
%  Draw the top of the prism, using a color corresponding to the height.
%
    r = ( ( zmax - z ) * rmin + ( z - zmin ) * rmax ) / ( zmax - zmin );
    g = ( ( zmax - z ) * gmin + ( z - zmin ) * gmax ) / ( zmax - zmin );
    b = ( ( zmax - z ) * bmin + ( z - zmin ) * bmax ) / ( zmax - zmin );

    fill3 ( [ x1, x2, x3, x4 ], [ y1, y2, y3, y4 ], [ z, z, z, z ], z )
%
%  Draw the bottom of the prism, using black.
%
    fill3 ( [ x1, x2, x3, x4 ], [ y1, y2, y3, y4 ], [ 0, 0, 0, 0 ], z )
%
%  Draw the sides of the prism, using a lighter shade of the top color.
%
    r = sqrt ( r );
    g = sqrt ( g );
    b = sqrt ( b );

    fill3 ( [ x1, x2, x2, x1 ], [ y1, y2, y2, y1 ], [ 0, 0, z, z ], z )
    fill3 ( [ x2, x3, x3, x2 ], [ y2, y3, y3, y2 ], [ 0, 0, z, z ], z )
    fill3 ( [ x3, x4, x4, x3 ], [ y3, y4, y4, y3 ], [ 0, 0, z, z ], z )
    fill3 ( [ x4, x1, x1, x4 ], [ y4, y1, y1, y4 ], [ 0, 0, z, z ], z )

  end

  xlabel ( 'X', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16 );

  ylabel ( 'Y', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
    'FontSize', 16, 'Rotation', 0 );

  title ( 'Z(X,Y)', 'FontName', 'Helvetica', 'FontWeight', ...
    'bold', 'FontSize', 16 );

  colorbar ( )

  view ( 3 )

  return
end
function table = r8mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% R8MAT_DATA_READ reads data from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns of data.
%
%    Output, real TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %f' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file.\n' );
    error ( 'R8MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = r8mat_header_read ( input_filename )

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function r8mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% R8MAT_TRANSPOSE_PRINT_SOME prints some of an R8MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, an optional title.
%
  incx = 5;

  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d       ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%12f', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
%% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LEN, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function word_num = s_word_count ( s )

%*****************************************************************************80
%
%% S_WORD_COUNT counts the number of "words" in a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be examined.
%
%    Output, integer WORD_NUM, the number of "words" in the string.
%    Words are presumed to be separated by one or more blanks.
%
  FALSE = 0;
  TRUE = 1;

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

    if ( s(i) == ' ' )
      blank = TRUE;
    elseif ( blank == TRUE )
      word_num = word_num + 1;
      blank = FALSE;
    end

  end

  return
end
function timestamp ( )

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

  return
end


function [occGrad,occlusionErrors] = occlusionGradOptimal(S,state,p2d)

%mask = double(state.mask);
[Y,X]=size(state.mask);
%mkp = mean(state.kps,2);% we had shifted everything by this when computing projected points so shift back now
%occlusionErrors = zeros(numpoints,1);

%% Finding points projected outside the image
%points2d = state.cameraScale*state.cameraRot*S'; %% Verify this later
%points2d = points2d' + repmat(mkp',size(S,1),1);
%points2d = round(points2d(:,1:2)); %% Verify correctness later
if(nargin<3)
    p2d = transform2d(S,state.cameraRot,state.cameraScale,state.translation);
    p2d = round(p2d); %% Verify correctness later
end

badX = (p2d(:,1)>X);p2d(badX,1)=X;
badX = (p2d(:,1)<1);p2d(badX,1)=1;
badY = (p2d(:,2)>Y);p2d(badY,2)=Y;
badY = (p2d(:,2)<1);p2d(badY,2)=1;

%% Computing nearest silhoutte point and gradient
%tic
diff3d = zeros(size(S));
IDX_2 = state.occlusionIDX;
sil_nbridx = IDX_2(sub2ind([Y X],p2d(:,2),p2d(:,1)));
[Ys,Xs] = ind2sub([Y X],sil_nbridx);
diff2d = double([Xs Ys]) - p2d;
diff3d(:,1:2)=diff2d;

occlusionErrors = double(sum(abs(diff3d),2)>=2);
occGrad = 1/state.cameraScale*diff3d*state.cameraRot;

end


function [Entropy] = Energy_Entropy_Block(f,winLength,winStep,numOfShortBlocks)

f = f / max(abs(f));
Eol = sum(f.^2);
L = length(f);

if (winLength==0)
    winLength = floor(L);
    winStep = floor(L);
end

numOfBlocks = (L-winLength)/winStep + 1;
curPos = 1;
for (i=1:numOfBlocks)
    curBlock = f(curPos:curPos+winLength-1);
    for (j=1:numOfShortBlocks)
        s(j) = sum(curBlock((j-1)*(winLength/numOfShortBlocks)+1:j*(winLength/numOfShortBlocks)).^2)/Eol;
    end

    Entropy(i) = -sum(s.*log2(s));
    curPos = curPos + winStep;
end


function [ddCs, rhsCs] = electrodeConcentration(dCs,cs_barrato,T,jflux,param)
% electrodeConcentration describes the ODE of the concentration of lithium ions within the
% electrodes.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

if(param.SolidPhaseDiffusion==1 || param.SolidPhaseDiffusion==2)
    % Cathode
    rhsCs_p  =((-3/param.Rp_p)*jflux(1:param.Np));
    ddCs_p   = dCs(1:param.Np) - rhsCs_p;

    % Anode
    rhsCs_n  = ((-3/param.Rp_n)*jflux(param.Np+1:end));
    ddCs_n   = dCs(param.Np+1:end) - rhsCs_n;
else
    switch param.SolidPhaseDiffusionNumericalScheme
        % Use the FDM method for the solid phase diffusion
        case 1
            [rhsCs_p, rhsCs_n, ddCs_p, ddCs_n] = FDM9orderElectrodeDiffusion(T, cs_barrato, jflux, dCs, param);
        % Use the spectral method for the discretization of the solid phase
        % diffusion
        case 2
            [rhsCs_p, rhsCs_n, ddCs_p, ddCs_n] = spectralMethodElectrodeDiffusion(T, cs_barrato, jflux, dCs, param);
    end
end
rhsCs   = [rhsCs_p;rhsCs_n];
ddCs    = [ddCs_p;ddCs_n];
end


function x=stdtinv(p,v)
% Inverse Cumulative Distribution Function (CDF) of the Standardized T
% distribution; Maps [0,1] to a standardized Students-t with V degrees of freedom
%
% USAGE:
%   X = stdtinv(P,V)
%
% INPUTS:
%   P     - Values to be inverted, P in [0,1]
%   V     - Degree of freedom parameters, either scalar or size(X)
%
% OUTPUTS:
%   X     - Standardized T distributed random variables corresponding to P
%
% COMMENTS:
%   V>2
%
% REFERENCES:
%   [1] Cassella and Berger (1990) 'Statistical Interence'
%
% See also STDTCDF, STDTINV, STDTRND, STDTLOGLIK, TPDF

% Copyright:
% Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

if nargin~=2
    error('2 inputs required')
end

[err, errtext, sizeOut, v] = iscompatible(1,v,size(p));
if err
    error(errtext)
end

x=tinv(p,v);

stdev=sqrt(v./(v-2));
stdev(v<=2)=NaN;
x=x./stdev;