function M = oned_bilinear ( kernel, phi, test, w_g )

%*****************************************************************************80
%
%% ONE_BILINEAR evaluates a bilinear form.
%
%  Discussion:
%
%    This function computes integral ( kernel * phi * test }
%
%    The calculation that is carried out is equivalent to:
%
%      [ n_gauss, n_test ] = size ( test );
%      [ n_gauss, n_dof ] = size ( phi );
%
%      M = zeros ( n_test, n_dof );
%      for i = 1 : n_test
%        for j = 1 : n_dof
%          M(i,j) = ( kernel' .* test(:,i)' ) * ( phi(:,j) .* w_g );
%        end
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Parameters:
%
%    Input, real KERNEL(N_GAUSS), the kernel function in the integral,
%    evaluated at the Gauss points.
%
%    Input, real PHI(N_GAUSS,N_DOF), the element test functions evaluated
%    at the Gauss points.
%
%    Input, real TEST(N_GAUSS,N_TEST), the test functions evaluated at the
%    Gauss points.
%
%    Input, real W_G(N_GAUSS,1), a column vector of Gauss weights.
%
%    Output, real M(N_TEST,N_DOF), the bilinear form.
%
  M = test' * diag ( kernel .* w_g ) * phi;

  return
end


function yawm = mag_compass_update(m_n, dec, inc, DCMnb, roll, pitch)
% magh_update: calculates magnetic heading angle (yaw) from magnetometer data.
%
% INPUT
%   mag, 1x3 magnetic flux density (Tesla).
%   dec, magnetic declination angle (rad).
%   inc, magnetic inclination angle (rad).
%   DCMnb, 3x3 DCM nav-to-body.
%   roll, roll angle (rad).
%   pitch, pitch angle (rad).
%
% OUTPUT
%   yawm, yaw angle from magnetometer data.
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% Reference:
%
%   Paul D. Groves. Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems. Second Edition.
% Eq. 6.2 to 6.6, page 219.
%
%   NOAA, Magnetic Field Calculators.
% https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml
%
% Version: 002
% Date:    2021/03/20
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

B = norm( m_n );                    % Magnitude of the flux density

D = [ cos(dec) * cos(inc); sin(dec) * cos(inc); sin(inc); ];

m_b = ( DCMnb * D * B ) ;

x =  -m_b(2) * cos(roll)  + m_b(3) * sin(roll) ;
y =   m_b(1) * cos(pitch) + m_b(2) * sin(roll) * sin(pitch) ...
    + m_b(3) * cos(roll) * sin(pitch) ;

yawm = correct_yaw (atan2(x , y) + dec);    % Four-quadrant arctangent function

end


function L = evaluate_log_posterior(this, uv)
%EVALUATE_LOG_POSTERIOR computes the log-posterior (negative energy) of the
%   flow fields UV
%   Actually only proportional to the log posterior since the variance of neither the
%   spatial nor the data terms is considered
%
%   This is a member function of the class 'hs_optical_flow'.
%
%   Author: Deqing Sun, Department of Computer Science, Brown University
%   Contact: dqsun@cs.brown.edu
%   $Date: 2007-11-30 $
%   $Revision: $
%
% Copyright 2007-2010, Brown University, Providence, RI. USA
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

% spatial term

S = {[1 -1], [1; -1]};
p = 0;
for i = 1:length(S)
    u_  = conv2(uv(:,:,1), S{i}, 'valid');
    v_  = conv2(uv(:,:,2), S{i}, 'valid');
    p   = p - sum(u_(:).^2) - sum(v_(:).^2);
end;

% data term
It  = partial_deriv(this.images, uv, this.interpolation_method, this.deriv_filter);
l   = -sum(It(:).^2);

L = this.lambda/this.sigmaS2*p + l/this.sigmaD2;

if this.display
    fprintf('spatial\t%3.2e\tdata\t%3.2e\n', this.lambda*p/this.sigmaS2, l/this.sigmaD2);
end;


function A = warmUpExercise()
%WARMUPEXERCISE Example function in octave
%   A = WARMUPEXERCISE() is an example function that returns the 5x5 identity matrix

A = [];
% ============= YOUR CODE HERE ==============
% Instructions: Return the 5x5 identity matrix
%               In octave, we return values by defining which variables
%               represent the return values (at the top of the file)
%               and then set them accordingly.
A = eye(5,5);

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

end


function element_num = sphere_grid_q4_element_num ( nelemx, nelemy )

%*****************************************************************************80
%
%% SPHERE_GRID_Q4_ELEMENT_NUM counts the elements in a Q4 sphere grid.
%
%  Example:
%
%    Input:
%
%      NELEMX = 3, NELEMY = 2
%
%    Output:
%
%      ELEMENT_NUM = NELEMX * NELEMY = 6
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.
%
%    Output, integer ELEMENT_NUM, the number of elements in the grid.
%
  element_num = nelemx * nelemy;

  return
end


function W = FormingConvFilter(Datacube,ConvFilter,num_of_filters)
% For each pixel in the Datacube, extract a small
% cube: ConvFilter(Height*Width*Channel), vectorizing into a column vector
% For all pixels in the Datacube, performing PCA/WPCA to gather
% num_of_filters eigenvecs, then transforming each eigenvecs which corresponds to
% a column of W into a small cube as our 3D ConvFilter.

X = im2colstep(Datacube,[ConvFilter.PatchSize,ConvFilter.PatchSize,ConvFilter.Channel]); % Dominated by column to extract
mu = mean(X,2);
% mu = mean(X,1);
X = bsxfun(@minus, X, mu);
Rx = X*X'/size(X,2);
[E,D] = eig(Rx);
[~, ind] = sort(diag(D),'descend');
% W = E(:,ind(1:num_of_filters));  % principal eigenvectors
W = E(:,ind(1:3));
% WPCA
% D = diag(D);
% D = D(ind(1:num_of_filters));
% W = bsxfun(@rdivide,W,sqrt(D)');

% W = cell(num_of_filters,1);
% for i=1:size(V,2)
%     W{i} = reshape(V(:,i),[Height,Width,Channel]);
% end


function pck = computePCK(dist,range)

pck = zeros(numel(range),size(dist,2)+1);

for jidx = 1:size(dist,2)
    % compute PCK for each threshold
    for k = 1:numel(range)
        pck(k,jidx) = 100*mean(squeeze(dist(1,jidx,:))<=range(k));
    end
end

% compute average PCK
for k = 1:numel(range)
    pck(k,end) = 100*mean(reshape(squeeze(dist(1,:,:)),size(dist,2)*size(dist,3),1)<=range(k));
end

end


function [x, cost, info, options] = rlbfgs(problem, x0, options)
% Riemannian limited memory BFGS solver for smooth objective functions.
%
% function [x, cost, info, options] = rlbfgs(problem)
% function [x, cost, info, options] = rlbfgs(problem, x0)
% function [x, cost, info, options] = rlbfgs(problem, x0, options)
% function [x, cost, info, options] = rlbfgs(problem, [], options)
%
%
% This is a Riemannian limited memory BFGS solver (quasi-Newton method),
% which aims to minimize the cost function in the given problem structure.
% It requires access to the gradient of the cost function.
%
% Parameter options.memory can be used to specify the number of iterations
% the algorithm remembers and uses to approximate the inverse Hessian of
% the cost. Default value is 30.
% For unlimited memory, set options.memory = Inf.
%
%
% For a description of the algorithm and theorems offering convergence
% guarantees, see the references below.
%
% The initial iterate is x0 if it is provided. Otherwise, a random point on
% the manifold is picked. To specify options whilst not specifying an
% initial iterate, give x0 as [] (the empty matrix).
%
% The two outputs 'x' and 'cost' are the last reached point on the manifold
% and its cost.
%
% The output 'info' is a struct-array which contains information about the
% iterations:
%   iter (integer)
%       The iteration number. The initial guess is 0.
%   cost (double)
%       The corresponding cost value.
%   gradnorm (double)
%       The (Riemannian) norm of the gradient.
%   time (double)
%       The total elapsed time in seconds to reach the corresponding cost.
%   stepsize (double)
%       The size of the step from the previous to the new iterate.
%   accepted (Boolean)
%       true if step is accepted in the cautious update. 0 otherwise.
%   And possibly additional information logged by options.statsfun.
% For example, type [info.gradnorm] to obtain a vector of the successive
% gradient norms reached at each iteration.
%
% 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. For well-scaled problems, a rule of thumb is that you can
%       expect to reduce the gradient norm by 8 orders of magnitude
%       (sqrt(eps)) compared to the gradient norm at a "typical" point (a
%       rough initial iterate for example). Further decrease is sometimes
%       possible, but inexact floating point arithmetic will eventually
%       limit the final accuracy. If tolgradnorm is set too low, the
%       algorithm may end up iterating forever (or at least until another
%       stopping criterion triggers).
%   maxiter (1000)
%       The algorithm terminates if maxiter iterations were executed.
%   maxtime (Inf)
%       The algorithm terminates if maxtime seconds elapsed.
%   minstepsize (1e-10)
%     The minimum norm of the tangent vector that points from the current
%     point to the next point. If the norm is less than minstepsize, the
%     program will terminate.
%   memory (30)
%     The number of previous iterations the program remembers. This is used
%     to approximate the inverse Hessian at the current point. Because of
%     difficulty of maintaining a representation of operators in terms of
%     coordinates, a recursive method is used. The number of steps in the
%     recursion is at most options.memory. This parameter can take any
%     integer value >= 0, or Inf, which is taken to be options.maxiter. If
%     options.maxiter has value Inf, then it will take value 10000 and a
%     warning will be displayed.
%   strict_inc_func (@(t) 1e-4*t)
%     The Cautious step needs a real function that has value 0 at t = 0,
%     and  is strictly increasing. See details in Wen Huang's paper
%     "A Riemannian BFGS Method without Differentiated Retraction for
%     Nonconvex Optimization Problems"
%   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. statsfun is
%       called with the point x that was reached last.
%   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 (2)
%       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. 3 and above includes a
%       display of the options structure at the beginning of the execution.
%   debug (false)
%       Set to true to allow the algorithm to perform additional
%       computations for debugging purposes. If a debugging test fails, you
%       will be informed of it, usually via the command window. Be aware
%       that these additional computations appear in the algorithm timings
%       too, and may interfere with operations such as counting the number
%       of cost evaluations, etc. (the debug calls get storedb too).
%   storedepth (2)
%       Maximum number of different points x of the manifold for which a
%       store structure will be kept in memory in the storedb for caching.
%       If memory usage is an issue, you may try to lower this number.
%       Profiling may then help to investigate if a performance hit was
%       incurred as a result.
%   ls_initial_scale (@(gradnorm) 1/gradnorm)
%       A function handle that takes as input a real number (a gradient norm)
%       and outputs a real number for how to scale the initialization of
%       line-search.
%
%
% Please cite the Manopt paper as well as the research paper:
% @InBook{Huang2016,
%   title     = {A {R}iemannian {BFGS} Method for Nonconvex Optimization Problems},
%   author    = {Huang, W. and Absil, P.-A. and Gallivan, K.A.},
%   year      = {2016},
%   publisher = {Springer International Publishing},
%   editor    = {Karas{\"o}zen, B{\"u}lent and Manguo{\u{g}}lu, Murat and Tezer-Sezgin, M{\"u}nevver and G{\"o}ktepe, Serdar and U{\u{g}}ur, {\"O}m{\"u}r},
%   address   = {Cham},
%   booktitle = {Numerical Mathematics and Advanced Applications ENUMATH 2015},
%   pages     = {627--634},
%   doi       = {10.1007/978-3-319-39929-4_60}
% }
%
% We point out that, at the moment, this implementation of RLBFGS can be
% slower than the implementation in ROPTLIB by Wen Huang et al. referenced
% above. For the purpose of comparing to their work, please use their
% implementation.

% This file is part of Manopt: www.manopt.org.
% Original author: Changshuo Liu, July 19, 2017.
% Contributors: Nicolas Boumal
% Change log:
%
%   Nov. 27, 2017 (Wen Huang):
%       Changed the default strict_inc_func to @(t) 1e-4*t from @(t) t.
%
%   Jan. 18, 2018 (NB):
%       Corrected a bug related to the way the line search hint was defined
%       by default.
%
%   Aug. 2, 2018 (NB):
%       Using the new storedb.remove features to keep storedb lean, and
%       reduced the default value of storedepth from 30 to 2 as a result.
%
%   Aug. 2, 2022 (sfrcorne):
%       Added option ls_initial_scale, with default setting to ensure
%       the method is invariant to positive scaling of the cost function.

    % 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)
        % Note: we do not give a warning if an approximate gradient is
        % explicitly given in the problem description, as in that case the user
        % seems to be aware of the issue.
        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

    % This solver uses linesearch_hint as a line search algorithm. By
    % default, try a step size of 1, so that if the BFGS approximation of
    % the Hessian (or inverse Hessian) is good, then the iteration is close
    % to a Newton step.
    if ~canGetLinesearch(problem)
        problem.linesearch = @(x, d) 1;
    end

    % Local defaults for the program
    localdefaults.minstepsize = 1e-10;
    localdefaults.maxiter = 1000;
    localdefaults.tolgradnorm = 1e-6;
    localdefaults.memory = 30;
    localdefaults.strict_inc_func = @(t) 1e-4*t;
    localdefaults.ls_max_steps = 25;
    localdefaults.ls_initial_scale = @(gradnorm) 1/gradnorm;
    localdefaults.storedepth = 2;

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

    % To make sure memory in range [0, Inf)
    options.memory = max(options.memory, 0);
    if options.memory == Inf
        if isinf(options.maxiter)
            options.memory = 10000;
            warning('rlbfgs:memory', ['options.memory and options.maxiter' ...
              ' are both Inf; options.memory has been changed to 10000.']);
        else
            options.memory = options.maxiter;
        end
    end

    M = problem.M;

    timetic = tic();

    % Create a random starting point if no starting point is provided.
    if ~exist('x0', 'var')|| isempty(x0)
        xCur = M.rand();
    else
        xCur = x0;
    end

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

    % __________Initialization of variables______________
    % Number of iterations since the last restart
    k = 0;
    % Total number of BFGS iterations
    iter = 0;

    % This cell stores step vectors which point from x_{t} to x_{t+1} for t
    % indexing the last iterations, capped at options.memory.
    % That is, it stores up to options.memory of the most recent step
    % vectors. However, the implementation below does not need step vectors
    % in their respective tangent spaces at x_{t}'s. Rather, it requires
    % them transported to the current point's tangent space by vector
    % transport. For details regarding the requirements on the the vector
    % transport, see the reference paper by Huang et al.
    % In this implementation, those step vectors are iteratively
    % transported to the current point's tangent space after every
    % iteration. Thus, at every iteration, vectors in sHistory are in the
    % current point's tangent space.
    sHistory = cell(1, options.memory);

    % This cell stores the differences for latest t's of the gradient at
    % x_{t+1} and the gradient at x_{t}, transported to x_{t+1}'s tangent
    % space. The memory is also capped at options.memory.
    yHistory = cell(1, options.memory);

    % rhoHistory{t} stores the reciprocal of the inner product between
    % sHistory{t} and yHistory{t}.
    rhoHistory = cell(1, options.memory);

    % Scaling of direction given by getDirection for acceptable step
    alpha = 1;

    % Norm of the step
    stepsize = 1;

    % Stores whether the step is accepted by the cautious update check.
    accepted = true;

    % Query the cost function and its gradient
    [xCurCost, xCurGradient] = getCostGrad(problem, xCur, storedb, key);

    xCurGradNorm = M.norm(xCur, xCurGradient);

    % Scaling of initial matrix, Barzilai-Borwein.
    scaleFactor = options.ls_initial_scale(xCurGradNorm);

    % Line-search statistics for recording in info.
    lsstats = [];

    % Flag to control restarting scheme to avoid infinite loops (see below)
    ultimatum = false;

    % 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                   cost val            grad. norm           alpha\n');
    end

    % Main iteration
    while true

        % Display iteration information
        if options.verbosity >= 2
        fprintf('%5d    %+.16e        %.8e      %.4e\n', ...
                iter, xCurCost, xCurGradNorm, alpha);
        end

        % Start timing this iteration
        timetic = tic();

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

        % If none triggered, run specific stopping criterion check
        if ~stop
            if stats.stepsize < options.minstepsize
                % To avoid infinite loop and to push the search further
                % in case BFGS approximation of Hessian is off towards
                % the end, we erase the memory by setting k = 0;
                % In this way, it starts off like a steepest descent.
                % If even steepest descent does not work, then it is
                % hopeless and we will terminate.
                if ~ultimatum
                    if options.verbosity >= 2
                        fprintf(['stepsize is too small, restarting ' ...
                            'the bfgs procedure at the current point.\n']);
                    end
                    k = 0;
                    ultimatum = true;
                else
                    stop = true;
                    reason = sprintf(['Last stepsize smaller than '  ...
                        'minimum allowed; options.minstepsize = %g.'], ...
                        options.minstepsize);
                end
            else
                % We are not in trouble: lift the ultimatum if it was on.
                ultimatum = false;
            end
        end

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

        % Compute BFGS direction
        p = getDirection(M, xCur, xCurGradient, sHistory,...
                yHistory, rhoHistory, scaleFactor, min(k, options.memory));

        % Execute line-search
        [stepsize, xNext, newkey, lsstats] = ...
            linesearch_hint(problem, xCur, p, xCurCost, ...
                            M.inner(xCur, xCurGradient, p), ...
                            options, storedb, key);

        % Record the BFGS step-multiplier alpha which was effectively
        % selected. Toward convergence, we hope to see alpha = 1.
        alpha = stepsize/M.norm(xCur, p);
        step = M.lincomb(xCur, alpha, p);

        % Query cost and gradient at the candidate new point.
        [xNextCost, xNextGrad] = getCostGrad(problem, xNext, storedb, newkey);

        % Compute sk and yk
        sk = M.transp(xCur, xNext, step);
        yk = M.lincomb(xNext, 1, xNextGrad, ...
                             -1, M.transp(xCur, xNext, xCurGradient));

        % Computation of the BFGS step is invariant under scaling of sk and
        % yk by a common factor. For numerical reasons, we scale sk and yk
        % so that sk is a unit norm vector.
        norm_sk = M.norm(xNext, sk);
        sk = M.lincomb(xNext, 1/norm_sk, sk);
        yk = M.lincomb(xNext, 1/norm_sk, yk);

        inner_sk_yk = M.inner(xNext, sk, yk);
        inner_sk_sk = M.norm(xNext, sk)^2;    % ensures nonnegativity

        % If the cautious step is accepted (which is the intended
        % behavior), we record sk, yk and rhok and need to do some
        % housekeeping. If the cautious step is rejected, these are not
        % recorded. In all cases, xNext is the next iterate: the notion of
        % accept/reject here is limited to whether or not we keep track of
        % sk, yk, rhok to update the BFGS operator.
        cap = options.strict_inc_func(xCurGradNorm);
        if inner_sk_sk ~= 0 && (inner_sk_yk / inner_sk_sk) >= cap

            accepted = true;

            rhok = 1/inner_sk_yk;

            scaleFactor = inner_sk_yk / M.norm(xNext, yk)^2;

            % Time to store the vectors sk, yk and the scalar rhok.
            % Remember: we need to transport all vectors to the most
            % current tangent space.

            % If we are out of memory
            if k >= options.memory

                % sk and yk are saved from 1 to the end with the most
                % current recorded to the rightmost hand side of the cells
                % that are occupied. When memory is full, do a shift so
                % that the rightmost is earliest and replace it with the
                % most recent sk, yk.
                for  i = 2 : options.memory
                    sHistory{i} = M.transp(xCur, xNext, sHistory{i});
                    yHistory{i} = M.transp(xCur, xNext, yHistory{i});
                end
                if options.memory > 1
                    sHistory = sHistory([2:end, 1]);
                    yHistory = yHistory([2:end, 1]);
                    rhoHistory = rhoHistory([2:end 1]);
                end
                if options.memory > 0
                    sHistory{options.memory} = sk;
                    yHistory{options.memory} = yk;
                    rhoHistory{options.memory} = rhok;
                end

            % If we are not out of memory
            else

                for i = 1:k
                    sHistory{i} = M.transp(xCur, xNext, sHistory{i});
                    yHistory{i} = M.transp(xCur, xNext, yHistory{i});
                end
                sHistory{k+1} = sk;
                yHistory{k+1} = yk;
                rhoHistory{k+1} = rhok;

            end

            k = k + 1;

        % The cautious step is rejected: we do not store sk, yk, rhok but
        % we still need to transport stored vectors to the new tangent
        % space.
        else

            accepted = false;

            for  i = 1 : min(k, options.memory)
                sHistory{i} = M.transp(xCur, xNext, sHistory{i});
                yHistory{i} = M.transp(xCur, xNext, yHistory{i});
            end

        end

        % Update variables to new iterate.
        storedb.removefirstifdifferent(key, newkey);
        xCur = xNext;
        key = newkey;
        xCurGradient = xNextGrad;
        xCurGradNorm = M.norm(xNext, xNextGrad);
        xCurCost = xNextCost;

        % 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
        % (this is independent from the BFGS memory.)
        storedb.purge();

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

    end

    % Housekeeping before we return
    info = info(1:iter+1);
    x = xCur;
    cost = xCurCost;

    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 = xCurCost;
        stats.gradnorm = xCurGradNorm;
        if iter == 0
            stats.stepsize = NaN;
            stats.time = toc(timetic);
            stats.accepted = NaN;
        else
            stats.stepsize = stepsize;
            stats.time = info(iter).time + toc(timetic);
            stats.accepted = accepted;
        end
        stats.linesearch = lsstats;
        stats = applyStatsfun(problem, xCur, storedb, key, options, stats);
    end

end

% BFGS step, see Wen's paper for details. This function takes in a tangent
% vector g, and applies an approximate inverse Hessian P to it to get Pg.
% Then, -Pg is returned.
%
% Theory requires the vector transport to be isometric and to satisfy the
% locking condition (see paper), but these properties do not seem to be
% crucial in practice. If your manifold provides M.isotransp, it may be
% good to do M.transp = M.isotransp; after loading M with a factory.
%
% This implementation operates in the tangent space of the most recent
% point since all vectors in sHistory and yHistory have been transported
% there.
function dir = getDirection(M, xCur, xCurGradient, sHistory, yHistory, ...
                            rhoHistory, scaleFactor, k)

    q = xCurGradient;

    inner_s_q = zeros(1, k);

    for i = k : -1 : 1
        inner_s_q(1, i) = rhoHistory{i} * M.inner(xCur, sHistory{i}, q);
        q = M.lincomb(xCur, 1, q, -inner_s_q(1, i), yHistory{i});
    end

    r = M.lincomb(xCur, scaleFactor, q);

    for i = 1 : k
         omega = rhoHistory{i} * M.inner(xCur, yHistory{i}, r);
         r = M.lincomb(xCur, 1, r, inner_s_q(1, i)-omega, sHistory{i});
    end

    dir = M.lincomb(xCur, -1, r);

end


function varargout = imsmooth(varargin)
% VL_IMSMOOTH  Smooth image
%   J = VL_IMSMOOTH(I,SIGMA) convolves the image I by an isotropic
%   Gaussian kernel of standard deviation SIGMA.  I must be an array
%   of doubles. IF the array is three dimensional, the third dimension
%   is assumed to span different channels (e.g. R,G,B). In this case,
%   each channel is convolved independently.
%
%   VL_IMSMOOTH() accepts the following options:
%
%   Kernel::
%     Selects between GAUSSIAN and TRIANGULAR kernels. The triangular
%     kernel support has 2*SIGMA-1 sampels. Kernels have unit mass.
%
%   Padding::
%     Selects between ZERO or CONTINUITY padding method to handle the
%     image boundaries. ZERO extends the input image with zeroes
%     around the border, and CONTINUITY extends the image with
%     constant pixels.
%
%   Step::
%     Sets the subsampling step. A subsampling step of STEP pixels
%     causes J(1:STEPS:end, 1:STEPS:end, :) to be computed. This is
%     useful to downsample the image.
%
%   See also: VL_HELP().
[varargout{1:nargout}] = vl_imsmooth(varargin{:});


function y = vl_nnpdist(x, x0, p, varargin)
%VL_NNPDIST CNN p-distance from target.
%   VL_NNPDIST(X, X0, P) computes the P distance raised of each feature
%   vector in X to the corresponding feature vector in X0:
%
%     Y(i,j,1) = (SUM_d (X(i,j,d) - X0(i,j,d))^P)^(1/P)
%
%   X0 should have the same size as X; the outoput Y has the same
%   height and width as X, but depth equal to 1. Optionally, X0 can
%   be a 1 x 1 x D x N array, in which case the same target feature
%   vector in X0 is compared to all feature vectors in X.
%
%   Setting the `noRoot` option to `true` does not take the 1/P power
%   in the formula, computing instead
%
%     Y(i,j,1) = SUM_d (X(i,j,d) - X0(i,j,d))^P
%
%   For example, `vl_nnpdist(x, x0, 2, 'noRoot', true)` computes the
%   squared L2 distance.
%
%   DZDX = VL_NNPDISTP(X, X0, P, DZDY) computes the derivative of the
%   block projected onto DZDY. DZDX and DZDY have the same dimensions
%   as X and Y, respectively.
%
%   VL_NNPDIST(___, 'OPT', VAL, ...) accepts the following options:
%
%   `NoRoot`:: `false`
%      If set to true, compute the P-distance to the P-th power.
%
%   `Epsilon`:: 1e-6
%      When computing derivatives, quantities that are divided in are
%      lower boudned by this value. For example, the L2 distance is
%      not smooth at the origin; this option prevents the
%      derivative from diverging.
%
%   `Aggregate`:: false
%      Instead of returning one scalar for each spatial location in
%      the inputs, sum all of them into a single scalar.
%
%   `InstanceWeights``:: `[]`
%      Optionally weight individual instances. This parameter can be
%      eigther a scalar or a weight mask, one for each pixel in the
%      input tensor.

% Copyright (C) 2015  Karel Lenc and Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

% -------------------------------------------------------------------------
%                                                             Parse options
% -------------------------------------------------------------------------

opts.noRoot = false ;
opts.epsilon = 1e-6 ;
opts.aggregate = false ;
opts.instanceWeights = [] ;
backMode = numel(varargin) > 0 && ~ischar(varargin{1}) ;
if backMode
  dzdy = varargin{1} ;
  opts = vl_argparse(opts, varargin(2:end)) ;
else
  dzdy = [] ;
  opts = vl_argparse(opts, varargin) ;
end

% -------------------------------------------------------------------------
%                                                             Parse options
% -------------------------------------------------------------------------

d = bsxfun(@minus, x, x0) ;

if ~isempty(dzdy) && ~isempty(opts.instanceWeights)
  dzdy = bsxfun(@times, opts.instanceWeights, dzdy) ;
end

if ~opts.noRoot
  if isempty(dzdy)
    if p == 1
      y = sum(abs(d),3) ;
    elseif p == 2
      y = sqrt(sum(d.*d,3)) ;
    else
      y = sum(abs(d).^p,3).^(1/p) ;
    end
  else
    if p == 1
      y = bsxfun(@times, dzdy, sign(d)) ;
    elseif p == 2
      y = max(sum(d.*d,3), opts.epsilon).^(-0.5) ;
      y = bsxfun(@times, bsxfun(@times, dzdy, y),  d) ;
    elseif p < 1
      y = sum(abs(d).^p,3).^((1-p)/p) ;
      y = bsxfun(@times, bsxfun(@times, dzdy, y), max(abs(d), opts.epsilon).^(p-1) .* sign(d)) ;
    else
      y = max(sum(abs(d).^p,3), opts.epsilon).^((1-p)/p) ;
      y = bsxfun(@times, bsxfun(@times, dzdy, y), abs(d).^(p-1) .* sign(d)) ;
    end
  end
else
  if isempty(dzdy)
    if p == 1
      y = sum(abs(d),3) ;
    elseif p == 2
      y = sum(d.*d,3) ;
    else
      y = sum(abs(d).^p,3) ;
    end
  else
    if p == 1
      y = bsxfun(@times, dzdy, sign(d)) ;
    elseif p == 2
      y = bsxfun(@times, 2 * dzdy, d) ;
    elseif p < 1
      y = bsxfun(@times, p * dzdy, max(abs(d), opts.epsilon).^(p-1) .* sign(d)) ;
    else
      y = bsxfun(@times, p * dzdy, abs(d).^(p-1) .* sign(d)) ;
    end
  end
end

if isempty(dzdy)
  if ~isempty(opts.instanceWeights)
    y = bsxfun(@times, opts.instanceWeights, y) ;
  end
  if opts.aggregate
    y = sum(sum(y)) ;
  end
end


function [ abd, info ] = cpbfa ( abd, lda, n, m )

%*****************************************************************************80
%
%% CPBFA factors a complex hermitian positive definite band matrix.
%
%  Discussion:
%
%    CPBFA is usually called by CPBCO, but it can be called
%    directly with a saving in time if RCOND is not needed.
%
%  Band storage:
%
%    If A is a hermitian positive definite band matrix,
%    the following program segment will set up the input.
%
%      m = (band width above diagonal)
%      do j = 1, n
%        i1 = max ( 1, j-m )
%        do i = i1, j
%          k = i-j+m+1
%          abd(k,j) = a(i,j)
%        end do
%      end do
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex ABD(LDA,N); the matrix to be factored.
%    The columns of the upper triangle are stored in the columns of ABD
%    and the diagonals of the upper triangle are stored in the rows of ABD.
%
%    Input, integer LDA, the leading dimension of ABD.
%    LDA must be at least M+1.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer M, the number of diagonals above the main diagonal.
%    0 <= M < N.
%
%    Output, integer INFO.
%    0, for normal return.
%    K, if the leading minor of order K is not positive definite.
%
%    Output, complex ABD(LDA,N); an upper triangular matrix R, stored in
%    band form, so that A = hermitian(R)*R.
%
  info = 0;

  for j = 1 : n

    s = 0.0;
    ik = m + 1;
    jk = max ( j - m, 1 );
    mu = max ( m + 2 - j, 1 );

    for k = mu : m
      t = abd(k,j) - abd(ik:ik+k-mu-1,jk)' * abd(mu:mu+k-mu-1,j);
      t = t / abd(m+1,jk);
      abd(k,j) = t;
      s = s + real ( t * conj ( t ) );
      ik = ik - 1;
      jk = jk + 1;
    end

    s = real ( abd(m+1,j) ) - s;

    if ( s <= 0.0 | imag ( abd(m+1,j) ) ~= 0.0 )
      info = j;
      break
    end

    abd(m+1,j) = sqrt ( s );

  end

  return
end


function varargout = liop(varargin)
% VL_LIOP Local Intensity Order Pattern descriptor
%   D = VL_LIOP(I) computes the LIOP descriptor of an image I, as
%   described by [1]. I is a gray-scale square image with odd side
%   length of class SINGLE. D is a column vector containing the LIOP
%   descriptor of I. Note that LIOP is also integrated in the VL_COVDET()
%   function for feature extraction.
%
%   VL_LIOP() accepts the following options:
%
%   NumNeighbours:: 4
%     Set the number of neighbours sampled to construct the order
%     pattern of each image pixel.
%
%   Radius:: 5
%     Set the radius of the circular neighbourhood used to sampled
%     the local order pattern of each pixel.
%
%   NumSpatialBins:: 6
%     Set the number of spatial pooling regions. The LIOP descriptor
%     has dimension factorial(NumNeighbours) * NumSpatialBins.
%
%   IntensityThreshold:: -0.02
%     Set the intensity threshold used to weight order patterns as they
%     are pooled into a histogram. A negative value is interpreted
%     as a fraction of the difference between the maximum and minimum
%     intensity in each local patch.
%
%   Verbose::
%     If specified, be verbose
%
%   REFERENCES::
%   [1] Z. Wang, B. Fan, F. Wu. Local Intensity Order Pattern for feature
%   description. In ICCV, 2011
%
%   See: LIOP, VL_COVDET(),
%   VL_HELP().
[varargout{1:nargout}] = vl_liop(varargin{:});


%  Script file: microphone.m
%
%  Purpose:
%    This program plots the gain pattern of a cardioid
%    microphone.
%
%  Record of revisions:
%      Date       Programmer          Description of change
%      ====       ==========          =====================
%    01/15/07    S. J. Chapman        Original code
%
% Define variables:
%   g         -- Microphone gain constant
%   gain      -- Gain as a function of angle
%   theta     -- Angle from microphone axis (radians)

% Calculate gain versus angle
g = 0.5;
theta = 0:pi/20:2*pi;
gain = 2*g*(1+cos(theta));

% Plot gain
polar (theta,gain,'r-');
title ('\bfGain versus angle \theta');


function Xr = pcaRandVec( U, mu, vars, k, n, hypershpere, show )
% Generate random vectors in PCA subspace.
%
% Used to generate random vectors from the subspace spanned by the first k
% principal components.  The points generated come from the gaussian
% distribution from within the subspace. Can optionally generate points on
% the subspace that are also on a hypershpere centered on the origin.  This
% may be useful if the original data points were all from a hypershpere --
% for example they were normalized via imNormalize.  Set the optional
% hypershpere flag to 1 to generate points only on the hypersphere.
%
% USAGE
%  Xr = pcaRandVec( U, mu, vars, k, n, [hypershpere], [show] )
%
% INPUTS
%  U           - returned by pca.m
%  mu          - returned by pca.m
%  vars        - returned by pca.m
%  k           - number of principal coordinates to use
%  n           - number of points to generate
%  hypershpere - [0] generate points on hypersphere (see above)
%  show        - [1] figure to use for display (no display if == 0)
%
% OUTPUTS
%  Xr          - resulting randomly generated vectors
%
% EXAMPLE
%
% See also PCA IMNORMALIZE
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<6 || isempty(hypershpere) ); hypershpere=0; end
if( nargin<7 || isempty(show) ); show=0; end

siz1 = size(mu);   nd = ndims(mu);
sizX = [siz1, n];  D = prod(siz1);

% generate random vectors inside of subspace.
C = diag( vars(1:k).^-1 );
Yr = C * randn(k,n);
Uk = U(:,1:k);
Xr = Uk * Yr;

if(hypershpere)
  % The final point must lie on hypershpere.  Need to scale each element xr
  % of Xr so that after adding it to mu the resulting vector has length D.
  % So need to find k>0 such that (k*xr + mu) has length D.  To find k
  % simply solve the quadratic equation induced by ||(k*xr + mu)||=D,
  % choosing the root such that k>0.  Note that the mean of resulting
  % vector will not necessarily be 0, but the variance will pretty much be
  % 1! Regardless, renomralize at the end.
  muv = mu(:);  muMag = dot(muv,muv);
  for i=1:n
    xr = Xr(:,i);
    rs = roots( [dot(xr,xr), 2 * dot(xr,muv), muMag-D] );
    Xr(:,i) = muv + max(rs)*xr;
  end
  Xr = reshape( Xr, sizX );
  Xr = fevalArrays( Xr, @imNormalize );
else
  % simply add the mean to reshaped Xr
  Xr = reshape( Xr, sizX );
  muRep = repmat(mu, [ones(1,nd), n ] );
  Xr = Xr + muRep;
end

% optionaly show resulting vectors
if(show && (nd==2 || nd==3)); figure(show); montage2(Xr); end

%%% Little test - see if eigenvectors induced by randomly generated vectors
%%% are the same as the original eigenvectors.  Assumes [U,mu,vars] exist.
%   Xr = pcaRandVec( U, mu, vars, 3, 100 );
%   [ Ur, mur, varsr ] = pca( Xr );
%   ind = 3;
%   Uim = reshape( U(:,ind), [ size(mu,1), size(mu,2) ]  );
%   Uimr = reshape( Ur(:,ind), [ size(mu,1), size(mu,2) ]  );
%   if( sum(abs(Uim-Uimr))>sum(abs(Uim+Uimr))) Uimr=Uimr*-1; end; %sign?
%   clf; subplot(3,1,1); im( Uim ); subplot(3,1,2); im( Uimr );
%   subplot(3,1,3); im( Uim - Uimr);
%%%


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

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

% Flux function
% ------------------
% Description:  Saturation excess flow from a store with different degrees
%               of saturation (min-max exponential variant)
% Constraints:  -
% @(Inputs):    p1   - maximum contributing fraction area [-]
%               p2   - minimum contributing fraction area [-]
%               p3   - exponentia scaling parameter [-]
%               S    - current storage [mm]
%               In   - incoming flux [mm/d]

out = min(p1,p2+p2.*exp(p3.*S)).*In;

end


function pass = test_subsref( pref )

if ( nargin < 1 )
    pref = chebfunpref;
end

tol = 1000*pref.cheb2Prefs.chebfun2eps;

% Evaluation at x,y,z
ff = @(x,y,z) z.*sin(x.*(y-.1));
f = spherefun(ff);
x = 1/sqrt(3); y = 1/sqrt(3); z = -1/sqrt(3);
pass(1) = ( abs( f(x,y,z) - ff(x,y,z) ) < tol );

% Evaluation at (lambda,theta)
ff = @(lam,th) exp(cos(lam).*sin(th).*cos(th));
f = spherefun(ff);
lam = pi/3; th = pi/4;
pass(2) = ( abs( f(lam,th ) - ff(lam,th) ) < tol );

% Slices in theta
ff = @(lam,th) exp(cos(lam).*sin(th).*cos(th));
f = spherefun(ff);
th1 = 0.2;
slice1 = chebfun(@(lam) feval(ff,lam,th1),[-pi pi], 'trig');
th2 = 0.7;
slice2 = chebfun(@(lam) feval(ff,lam,th2),[-pi pi], 'trig');
pass(3) = ( norm( f(:,[th1 th2]).' - [slice1 slice2] ) < tol );

% Slices in lambda
ff = @(lam,th) exp(cos(lam).*sin(th).*cos(th));
f = spherefun(ff);
lam1 = 0.2;
slice1 = chebfun(@(th) feval(ff,lam1,th),[-pi pi], 'trig');
lam2 = 0.7;
slice2 = chebfun(@(th) feval(ff,lam2,th),[-pi pi], 'trig');
pass(4) = ( norm( f([th1 th2],:) - [slice1 slice2] ) < tol );

% Slice in z
ff = @(x,y,z) z.*sin(x.*(y-.1));
f = spherefun(ff);
z = 0.1; th = acos(z);
slice = chebfun(@(lam) feval(ff,cos(lam).*sin(th),sin(lam).*sin(th),z),[-pi pi], 'trig');
pass(5) = ( norm( f(:,:,z) - slice ) < tol );

% Slice in x
ff = @(x,y,z) z.*sin(x.*(y-.1));
f = spherefun(ff);
x = 0.1;
slice = chebfun(@(t) feval(ff,x,sqrt(1-x.^2).*cos(t),sqrt(1-x.^2).*sin(t)),[-pi pi], 'trig');
pass(6) = ( norm( f(x,:,:) - slice ) < tol );

% Slice in y
ff = @(x,y,z) z.*sin(x.*(y-.1));
f = spherefun(ff);
y = -0.4;
slice = chebfun(@(t) feval(ff,sqrt(1-y.^2).*cos(t),y,sqrt(1-y.^2).*sin(t)),[-pi pi], 'trig');
pass(7) = ( norm( f(:,y,:) - slice ) < tol );

% GET properties
f = spherefun(@(lam,th) cos(lam).*sin(th));
pass(8) = ( norm(f.rows - chebfun(@(lam) cos(lam),[-pi pi],'trig')) < tol || ...
    norm(f.rows + chebfun(@(lam) cos(lam),[-pi pi],'trig')) < tol );
pass(9) = ( norm(f.cols - chebfun(@(th) sin(th),[-pi pi],'trig')) < tol || ...
    norm(f.cols + chebfun(@(th) sin(th),[-pi pi],'trig')) < tol );
pass(10) = ( norm( f.domain - [-pi pi 0 pi] ) < tol );

% Composition of a spherefun with a chebfun (1 and 3 columns):
f = spherefun(@(x,y,z) z + sin(pi*x.*y));
g = chebfun(@(t) t.^2, [ -1.5, 1.5 ]);
h_true = spherefun(@(x,y,z) (z + sin(pi*x.*y)).^2);
h = g(f);
pass(11) = ( norm(h - h_true) < tol );

G = chebfun(@(t) [ t.^2, t, -t.^2 ], [ -1.5, 1.5 ]);
H_true = spherefunv(@(x,y,z) (z + sin(pi*x.*y)).^2, ...
    @(x,y,z) z + sin(pi*x.*y), @(x,y,z) -(z + sin(pi*x.*y)).^2);
H = G(f);
pass(12) = ( norm(H - H_true) < tol );

end


function [xo,yo,ind] = polybool(x1,y1,x2,y2,flag,flag2)
% POLYBOOL Boolean operations on polygons.
%	[XO,YO] = POLYBOOL(X1,Y1,X2,Y2,FLAG)
%	calulates results of Boolean operations on
%	a pair of polygons.
%	FLAG Specifies the type of the operation:
%	1 - Intersection (P1 & P2)
%	2 - Union (P1 | P2)
%	3 - Difference (P1 & ~P2)

%  Copyright (c) 1995 by Kirill K. Pankratov,
%       kirill@plume.mit.edu.
%       06/25/95, 09/07/95

%	This program calls the following functions:
%	AREA, ISINTPL, ISCROSS, INTSECL.

% Algorithm:
%  1. Check boundary contour directions (area).
%     For intersection and union make all
%     counter-clockwise. For difference make the second
%    contour clock-wise.
%  2. Calculate matrix of intersections (function ISINTPL).
%     Quick exit if no intersections.
%  3. For intersecting segments calculate intersection
%     coordinates (function INTSECL).
%  4. Sort intersections along both contours.
%  5. Calculate sign of cross-product between intersectiong
%     segments. This will give which contour goes "in" and
%     "out" at intersections.
%
%  6. Start with first intersection:
%     Determine direction to go ("in" for intersection,
%     "out" for union).
%     Move until next intersection, switch polygons at each
%     intersection until coming to the initial point.
%     If not all intersections are encountered, the
%     resulting polygon is disjoint. Separate output
%     coordinates by NaN and repeat procedure until all
%     intersections are counted.
xo=[];yo=[];ind=[];
% Default for flag
flag_dflt = 1; % 1- intersec., 2-union, 3 - diff.
if ~exist('flag2','var')
flag2=1;
end
% Handle input
if nargin==0, help polybool, return, end
if nargin < 4
    error(' Not enough input arguments')
end
if nargin<5, flag = flag_dflt; end

x1 = x1(:); y1 = y1(:);
x2 = x2(:); y2 = y2(:);
l1 = length(x1);
l2 = length(x2);

%  % Check areas and reverse if negative
% nn1 = area(x1,y1);
% if nn1<0, x1 = flipud(x1); y1 = flipud(y1); end
% nn2 = area(x2,y2);
% if (nn2<0 & flag<3) | (nn2>0 & flag==3)
%   x2 = flipud(x2); y2 = flipud(y2);
% end

% Check areas and reverse if negative
x_temp = [x1;x1(1)]; y_temp = [y1;y1(1)];
nn1 = (sum(x_temp(1:end-1).*y_temp(2:end))...
    -sum(x_temp(2:end).*y_temp(1:end-1)))/2;
if nn1<0, x1 = flipud(x1); y1 = flipud(y1); end
x_temp = [x2;x2(1)]; y_temp = [y2;y2(1)];
nn2 = (sum(x_temp(1:end-1).*y_temp(2:end))...
    -sum(x_temp(2:end).*y_temp(1:end-1)))/2;
if (nn2<0 && flag<3) | (nn2>0 && flag==3)
    x2 = flipud(x2); y2 = flipud(y2);
end

% If both polygons are identical ........
if l1==l2
    if all(x1==x2) && all(y1==y2)
        if flag<3, xo = x1; yo = y1; ind = 1:l1;
        else, xo = []; yo = []; ind = []; end
        return
    end
end

% Calculate matrix of intersections .....
[is,C] = isintpl(x1,y1,x2,y2);
is = any(any(C));

% Quick exit if no intersections ........
if ~is
    if flag==1       % Intersection
        xo=[]; yo = [];
    elseif flag==2   % Union
        xo = [x1; nan; x2];
        yo = [y1; nan; y2];
    elseif flag==3   % Difference
        xo = x1; yo = y1;
    end
    return
end

% Mark intersections with unique numbers
i1 = find(C);
ni = length(i1);
C(i1) = 1:ni;

% Close polygon contours
x1 = [x1; x1(1)]; y1 = [y1; y1(1)];
x2 = [x2; x2(1)]; y2 = [y2; y2(1)];
l1 = length(x1);  l2 = length(x2);

% Calculate intersections themselves
[i1,i2,id] = find(C);
xs1 = [x1(i1) x1(i1+1)]'; ys1 = [y1(i1) y1(i1+1)]';
xs2 = [x2(i2) x2(i2+1)]'; ys2 = [y2(i2) y2(i2+1)]';

% Call INTSECL ............................
[xint,yint] = intsecl(xs1,ys1,xs2,ys2);

% For sements belonging to the same line
% find interval of intersection ...........
ii = find(xint==inf);
if length(ii)~=0
    [is,inx] = interval(xs1(:,ii),xs2(:,ii));
    [is,iny] = interval(ys1(:,ii),ys2(:,ii));
    xint(ii) = mean(inx);
    yint(ii) = mean(iny);
end

% Coordinate differences of intersecting segments
xs1 = diff(xs1); ys1 = diff(ys1);
xs2 = diff(xs2); ys2 = diff(ys2);

% Calculate cross-products
cp = xs1.*ys2-xs2.*ys1;
cp = cp>0;
if flag==2, cp=~cp; end % Reverse if union
cp(ii) = 2*ones(size(ii));

% Sort intersections along the contours
ind = (xint-x1(i1)').^2+(yint-y1(i1)').^2;
ind = ind./(xs1.^2+ys1.^2);
cnd = min(ind(ind>0));
ind = ind+i1'+i2'/(ni+1)*cnd*0;
[xo,ii] = sort(ind);
xs1 = id(ii);
[xo,ind] = sort(xs1);
ind = rem(ind,ni)+1;
xs1 = xs1(ind);

ind = (xint-x2(i2)').^2+(yint-y2(i2)').^2;
ind = ind./(xs2.^2+ys2.^2);
cnd = min(ind(ind>0));
[xo,ii] = sort(i2'+ind+i1'/(ni+1)*cnd*0);
xs2 = id(ii);
[xo,ind] = sort(xs2);
ind = rem(ind,ni)+1;
xs2 = xs2(ind);

% Combine coordinates in one vector
x1 = [x1; x2]; y1 = [y1; y2];

% Find max. possible length of a chain
xo = find(any(C'));
xo = diff([xo xo(1)+l1]);
mlen(1) = max(xo);
xo = find(any(C));
xo = diff([xo xo(1)+l2]);
mlen(2) = max(xo);

% Check if multiple intersections in one segment
xo = diff([i1 i2]);
is_1 = ~all(all(xo));

% Begin counting intersections *********************

% Initialization ..................
int = zeros(size(xint));
nn = 1;   % First intersection
nn1 = i1(nn); nn2 = i2(nn);
b = cp(nn);
is2 = b==2;
xo = []; yo = []; ind = [];
closed = 0;

% Proceed until all intersections are counted
while ~closed  % begin counting `````````````````````0

    % If contour closes, find new starting point
    if int(nn) & ~all(int)
        ii = find(int);
        C(id(ii)) = zeros(size(ii));
        nn = min(find(~int));  % Next intersection
        nn1 = i1(nn);
        nn2 = i2(nn);
        xo = [xo; nan];        % Separate by NaN
        yo = [yo; nan];
        ind = [ind; nan];
        % Choose direction ......
        b = cp(nn);
    end

    % Add current intersection ......
    xo = [xo; xint(nn)];
    yo = [yo; yint(nn)];
    ind = [ind; 0];
    int(nn) = 1;
    closed = all(int);

    % Find next segment
    % Indices for next intersection
    if ~b, if numel(xs1)>0 nn = xs1(nn); end          %%%%%%%changed here varsha
    else,  nn = xs2(nn);
    end
    if ~b, pt0 = nn1; else,  pt0 = nn2; end

    nn1 = i1(nn);
    nn2 = i2(nn);

    if b, pt = nn2; else, pt = nn1; end

    if b, pt0 = pt0+l1; pt = pt+l1; end
    ii = (pt0+1:pt);

    % Go through the beginning ..............
    cnd = pt<pt0 | (pt==pt0 & is_1 & flag>1);
    if cnd
        if ~b,  ii = [pt0+1:l1 1:pt];
        else,   ii = [pt0+1:l1+l2 l1+1:pt];
        end
    end
    len = length(ii);
    cnd = b & len>mlen(2);
    cnd = cnd | (~b & len>mlen(1));
    if is2 | cnd, ii=[]; end

    % Add new segment
    xo = [xo; x1(ii)];
    yo = [yo; y1(ii)];
    ind = [ind; ii'];

    % Switch direction
    if cp(nn)==2, b = ~b; is2 = 1;
    else, b = cp(nn); is2 = 0;
    end

end    % End while (all intersections) '''''''''''''''0

% %if you are running intersection of image with field polygons uncomment
% %this and comment below
% % Remove coincident successive points
if flag2

if sum(isnan(xo))>0 | sum(isnan(yo))>0
     xo=[];yo=[];ind=[];
     return;
 end
ii = find(~diff(xo) & ~diff(yo));
xo(ii) = []; yo(ii) = []; ind(ii) = [];

 % Remove points which are
ii = find(isnan(xo));
if length(ii)~=0
  i2 = ones(size(xo));
  ii = [ii; length(xo)+1];

  i1 = find(diff(ii)==3);
  i1 = ii(i1);
  i1 = [i1; i1+1; i1+2];
  i2(i1) = zeros(size(i1));

  i1 = find(diff(ii)==2);
  i1 = ii(i1);
  i1 = [i1; i1+1];
  i2(i1) = zeros(size(i1));

  xo = xo(i2); yo = yo(i2); ind = ind(i2);
else
%if you are running intersection with superpixels uncomment this and
%comment above
out_xo=[]; out_yo=[]; out_ind=[];
if sum(isnan(xo))>0 | sum(isnan(yo))>0

    ii=find(isnan(xo));
    ii=[0;ii;length(xo)+1];
    for i=1:numel(ii)-1
        seg_xo=xo(ii(i)+1:ii(i+1)-1);
        seg_yo=yo(ii(i)+1:ii(i+1)-1);
        seg_ind=ind(ii(i)+1:ii(i+1)-1);
        iii = find(~diff(seg_xo) & ~diff(seg_yo));
        seg_xo(iii) = []; seg_yo(iii) = []; seg_ind(iii) = [];
        if numel(seg_xo)> 2
        out_xo=[out_xo;NaN ;seg_xo];
        out_yo=[out_yo;NaN; seg_yo];
        out_ind=[out_ind;NaN ;seg_ind];
        end

    end

    xo=out_xo;
    yo=out_yo;
    ind=out_ind;
else

    ii = find(~diff(xo) & ~diff(yo));
    xo(ii) = []; yo(ii) = []; ind(ii) = []; %both polygons are convex

end
end

end


function centroids = kMeansInitCentroids(X, K)
%KMEANSINITCENTROIDS This function initializes K centroids that are to be
%used in K-Means on the dataset X
%   centroids = KMEANSINITCENTROIDS(X, K) returns K initial centroids to be
%   used with the K-Means on the dataset X
%

% You should return this values correctly
centroids = zeros(K, size(X, 2));

% ====================== YOUR CODE HERE ======================
% Instructions: You should set centroids to randomly chosen examples from
%               the dataset X
%

randidx = randperm(size(X, 1));
centroids = X(randidx(1:K), :);

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

end


function triangulation_test31 ( )

%*****************************************************************************80
%
%% TEST31 tests VORONOI_POLYGON_AREA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 August 2006
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  neighbor_num = 4;
  node_num = 5;

  area_correct = 0.5;
  center = 5;
  neighbor_index = [ 1, 2, 3, 4 ];
  node_xy = [ ...
    0.0, 0.0; ...
    1.0, 0.0; ...
    1.0, 1.0; ...
    0.0, 1.0; ...
    0.5, 0.5 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST31\n' );
  fprintf ( 1, '  VORONOI_POLYGON_AREA computes the area of\n' );
  fprintf ( 1, '  a finite Voronoi polygon.\n' );

  area = voronoi_polygon_area ( center, neighbor_num, neighbor_index, ...
    node_num, node_xy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The computed area is %f\n', area );
  fprintf ( 1, '  The correct area is  %f\n', area_correct );

  return
end


function [xc,good,bad,type] = cornerfinder(xt,I,wintx,winty,wx2,wy2);

%[xc] = cornerfinder(xt,I);
%
%Finds the sub-pixel corners on the image I with initial guess xt
%xt and xc are 2xN matrices. The first component is the x coordinate
%(horizontal) and the second component is the y coordinate (vertical)
%
%Based on Harris corner finder method
%
%Finds corners to a precision below .1 pixel!
%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!!
%Sept 1998 - UPDATED to handle diverged points: we keep the original points
%good is a binary vector indicating wether a feature point has been properly
%found.
%
%Add a zero zone of size wx2,wy2
%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher
%resolution and larger number of iterations.

% California Institute of Technology
% (c) Jean-Yves Bouguet -- Oct. 14th, 1997

line_feat = 1; % set to 1 to allow for extraction of line features.

xt = xt';
xt = fliplr(xt);

if nargin < 4,
   winty = 5;
   if nargin < 3,
      wintx = 5;
   end;
end;

if nargin < 6,
   wx2 = -1;
   wy2 = -1;
end;

%mask = ones(2*wintx+1,2*winty+1);
mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2);

% another mask:
[X,Y] = meshgrid(-winty:winty,-wintx:wintx);
mask2 = X.^2 + Y.^2;
mask2(wintx+1,winty+1) = 1;
mask2 = 1./mask2;
%mask - mask2;

if (wx2>0) & (wy2>0),
   if ((wintx - wx2)>=2)&((winty - wy2)>=2),
      mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1);
   end;
end;

offx = [-wintx:wintx]'*ones(1,2*winty+1);
offy = ones(2*wintx+1,1)*[-winty:winty];

resolution = 0.005;

MaxIter = 10;

[nx,ny] = size(I);
N = size(xt,1);

xc = xt; % first guess... they don't move !!!

type = zeros(1,N);

for i=1:N,

   v_extra = resolution + 1; 		% just larger than resolution

   compt = 0; 				% no iteration yet

   while (norm(v_extra) > resolution) & (compt<MaxIter),

      cIx = xc(i,1); 			%
      cIy = xc(i,2); 			% Coords. of the point
      crIx = round(cIx); 		% on the initial image
      crIy = round(cIy); 		%
      itIx = cIx - crIx; 		% Coefficients
      itIy = cIy - crIy; 		% to compute
      if itIx > 0, 			% the sub pixel
	 vIx = [itIx 1-itIx 0]'; 	% accuracy.
      else
	 vIx = [0 1+itIx -itIx]';
      end;
      if itIy > 0,
	 vIy = [itIy 1-itIy 0];
      else
	 vIy = [0 1+itIy -itIy];
      end;

      % What if the sub image is not in?

      if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5;
      elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4;
      else
	 xmin = crIx-wintx-2; xmax = crIx+wintx+2;
      end;

      if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5;
      elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4;
      else
	 ymin = crIy-winty-2; ymax = crIy+winty+2;
      end;

      SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood
      SI = conv2(conv2(SI,vIx,'same'),vIy,'same');
      SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood
      [gy,gx] = gradient(SI); 		% The gradient image
      gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only
      gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients

      px = cIx + offx;
      py = cIy + offy;

      gxx = gx .* gx .* mask;
      gyy = gy .* gy .* mask;
      gxy = gx .* gy .* mask;

      bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))];

      a = sum(sum(gxx));
      b = sum(sum(gxy));
      c = sum(sum(gyy));

      dt = a*c - b^2;

      xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;

      %keyboard;

      if line_feat,

	 G = [a b;b c];
	 [U,S,V]  = svd(G);

	 %keyboard;

	 % If non-invertible, then project the point onto the edge orthogonal:

	 if (S(1,1)/S(2,2) > 50),
	    % projection operation:
	    xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)';
	    type(i) = 1;
	 end;

      end;

      %keyboard;

%      G = [a b;b c];
%      [U,S,V]  = svd(G);

%      if S(1,1)/S(2,2) > 150,
%	 bb2 = U'*bb;
%	 xc2 = (V*[bb2(1)/S(1,1) ;0])';
%      else
%	 xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
%      end;

      %if (abs(a)> 50*abs(c)),
%	 xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)];
%      elseif (abs(c)> 50*abs(a))
%	 xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt];
%      else
%	 xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
%      end;

      %keyboard;

      v_extra = xc(i,:) - xc2;

      xc(i,:) = xc2;

%      keyboard;

      compt = compt + 1;

   end
end;

% check for points that diverge:

delta_x = xc(:,1) - xt(:,1);
delta_y = xc(:,2) - xt(:,2);

%keyboard;

bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty);
good = ~bad;
in_bad = find(bad);

% For the diverged points, keep the original guesses:

xc(in_bad,:) = xt(in_bad,:);

xc = fliplr(xc);
xc = xc';

bad = bad';
good = good';


function [abpl, ubpl] = tapas_ti_bpl(llh, t)
%% Computes the Bayesiand predictive log likelihood.
%
%   Input
%       llh     Multidimensional array of preditive likelihood. Dimensions are
%               Number of subjects x number of chains x number of samples
%       t       Temperature schedule of size 1 x number of chains
%   Output
%       abpl    Adjusted Bayesian preditive likelihood
%       ubpl    Unadjusted Bayesian predictive likelihood
%

% aponteeduardo@gmail.com
% copyright (C) 2017
%

[ns, nc, np] = size(llh);
%if all([1, nc] == size(t))
%    error('tapas:ti:bpl', 'Dimensions of t and llh do not match');
%end

% Compute the free energy

lnz = trapz(t, sum(mean(llh, 3), 1));

% Make sure that the first dimension is the temperature
ubpl = trapz(t', mean(llh, 3)')';

% Do post hoc importance sampling
abpl = zeros(ns, 1);
for s = 1:ns
    sllh = bsxfun(@times, -t', squeeze(llh(s, :, :)));
    sllh = bsxfun(@minus, sllh, mean(sllh, 2));
    sllh = bsxfun(@minus, sllh, log(sum(exp(sllh), 2)));
    % Change the dynamic range to max being 100
    adja = -max(sllh')' + log(1000);
    sllh = bsxfun(@plus, sllh, adja);
    sllh = exp(sllh) ;
    nllh = squeeze(sum(skip_column(llh, s), 1));
    nllh = sum(bsxfun(@times, nllh, sllh), 2) .* exp(-adja);
    abpl(s) = lnz - trapz(t, nllh);
end

end

function [nllh] = skip_column(llh, sbj)
% Removes an slice and return array minux slice

[ns, nc, np] = size(llh);
i = 1:ns;
i(sbj) = [];

nllh = llh(i, :, :);

end


function h=entropyDist(npd)
%
% Compute entropy estimate using nearest neighbor estimate
%

% Copyright (C) 2003 Alexander Ihler; distributable under GPL -- see README.txt

Ce = .57721566490153286;

pts = getPoints(npd);

[N1,N2] = size(pts);
[tmp,D] = knn(npd,pts,2);

Sr = N1* pi^(N1/2) / gamma((N1/2) + 1);
h = N1/N2 * sum( log(D) ) + log(Sr * (N2-1)/N1 ) + Ce;


function [DI, k, mad, w, chi2]  = detectChange(obj, t1, t2)
% Refer to http://people.compute.dtu.dk/alan/software.html
[rows, cols, chns1] = size(t1);
chns2 = size(t2, 3);
chns = min(chns1, chns2);
N = rows*cols;
w = ones(N, 1); % Initial weights
t1_ = reshape(double(t1), N, chns1);
t2_ = reshape(double(t2), N, chns2);
rhos = [];

for ii = 1:obj.nIters
    [sigma, xc] = covW(obj, [t1_, t2_], w);
    x1 = xc(:,1:chns1);
    x2 = xc(:,chns1+1:end);
    sigma11 = sigma(1:chns1, 1:chns1);
    sigma12 = sigma(1:chns1, chns1+1:end);
    sigma21 = sigma(chns1+1:end, 1:chns1);
    sigma22 = sigma(chns1+1:end, chns1+1:end);

    [a, e1] = eig(sigma12 / sigma22 * sigma21, sigma11);
    e1 = sqrt(diag(e1));
    [b, e2] = eig(sigma21 / sigma11 * sigma12, sigma22);
    e2 = sqrt(diag(e2));
    [e1, idx1] = sort(e1, 'descend');
    a = a(:,idx1);
    a = a(:,chns:-1:1);
    e1 = e1(chns:-1:1);
    [e2, idx2] = sort(e2, 'descend');
    b = b(:,idx2);
    b = b(:,chns:-1:1);
    e2 = e2(chns:-1:1);

    % Normalize a for unit dispersion
    % Ensure that a'*s11*a=I to meet the constraints
    vars1 = (a'*sigma11*a);
    a = a ./ sqrt(diag(vars1))';
    % Similiar operations on b
    vars2 = (b'*sigma22*b);
    b = b ./ sqrt(diag(vars2))';

    % Ensure sum of positive correlations between x1 and x1*a is positive
    invStd1 = diag(1./std(x1));
    sgn = diag(sign(sum(invStd1*sigma11*a)));
    a = a*sgn;

    % Assure positive correlation between pair of canonical variates
    b = b .* diag(sign(a'*sigma12*b))';

    mad = x1*a - x2*b;

    % Normalize mad
    % This is no regular operation, yet appears to improve the result
    mad = (mad - mean(mad)) ./ (std(mad)+eps);
    chi2 = sum(mad.^2, 2);

    %         chi2 = sum(mad.^2 ./ (2*(1-e1')), 2);   % Should be no-change std only

    w = 1 - chi2cdf(chi2, chns);

    if ii > 1
        err = max(abs(e1'-rhos(end,:)));
        if err < obj.epsilon, break; end
    end
    rhos = [rhos; e1'];
end

if ii == obj.nIters
    warning('Exceeded max number of iterations.')
end

%     % Use L2 pooling
%     DI = reshape(sqrt(sum(mad.*mad, 2)), rows, cols);

mad = reshape(mad, rows, cols, chns);
w = reshape(w, rows, cols);
chi2 = reshape(chi2, rows, cols);
k = min(chns, obj.nMADUsed);
DI = abs(mad(:,:,1:k));
end


function [ rotImg, R ] = rotatePanorama( img, vp, R )
%ROTATEPANORAMA Rotate panorama
%   if R is given, vp (vanishing point) will be overlooked
%   otherwise R is computed from vp

[sphereH, sphereW, C] = size(img);
% rotImg = zeros( sphereH, sphereW, C);

%% new uv coordinates
[TX TY] = meshgrid(1:sphereW, 1:sphereH);
TX = TX(:);
TY = TY(:);
ANGx = (TX- sphereW/2 -0.5)/sphereW * pi *2 ;
ANGy = -(TY- sphereH/2 -0.5)/sphereH * pi;
uvNew = [ANGx ANGy];
xyzNew = uv2xyzN(uvNew,1);

%% rotation matrix
if nargin<3
    R = diag([1 1 1])/(vp');
end

xyzOld = (R\xyzNew')';
uvOld = xyz2uvN(xyzOld, 1);

% Px = uvOld(:,1)/2/pi*sphereW + 0.5 + sphereW/2;
% Py = -uvOld(:,2)/pi*sphereH + 0.5 + sphereH/2;
Px = (uvOld(:,1)+pi) / (2*pi) * sphereW + 0.5;
Py = (-uvOld(:,2) + pi/2) / pi * sphereH + 0.5;

Px = reshape(Px, [sphereH sphereW]);
Py = reshape(Py, [sphereH sphereW]);

% boundary
imgNew = double(zeros(sphereH+2, sphereW+2, C));
imgNew(2:end-1, 2:end-1, :) = img;
imgNew(2:end-1,1,:) = img(:,end,:);
imgNew(2:end-1,end,:) = img(:,1,:);
imgNew(1,2:sphereW/2+1,:) = img(1,sphereW:-1:sphereW/2+1,:);
imgNew(1,sphereW/2+2:end-1,:) = img(1,sphereW/2:-1:1,:);
imgNew(end,2:sphereW/2+1,:) = img(end,sphereW:-1:sphereW/2+1,:);
imgNew(end,sphereW/2+2:end-1,:) = img(1,sphereW/2:-1:1,:);
imgNew(1,1,:) = img(1,1,:);
imgNew(end,end,:) = img(end,end,:);
imgNew(1,end,:) = img(1,end,:);
imgNew(end,1,:) = img(end,1,:);

rotImg = warpImageFast(imgNew, Px+1, Py+1);
% rotImg = warpImageFast(img, Px, Py);

end


function [label, Theta, w, llh] = mixGaussGb(X, opt)
% Collapsed Gibbs sampling for Dirichlet process (infinite) Gaussian mixture model (a.k.a. DPGM).
% This is a wrapper function which calls underlying Dirichlet process mixture model.
% Input:
%   X: d x n data matrix
%   opt(optional): prior parameters
% Output:
%   label: 1 x n cluster label
%   Theta: 1 x k structure of trained Gaussian components
%   w: 1 x k component weight vector
%   llh: loglikelihood
% Written by Mo Chen (sth4nth@gmail.com).
[d,n] = size(X);
mu = mean(X,2);
Xo = bsxfun(@minus,X,mu);
s = sum(Xo(:).^2)/(d*n);
if nargin == 1
    kappa0 = 1;
    m0 = mean(X,2);
    nu0 = d;
    S0 = s*eye(d);
    alpha0 = 1;
else
    kappa0 = opt.kappa;
    m0 = opt.m;
    nu0 = opt.nu;
    S0 = opt.S;
    alpha0 = opt.alpha;
end
prior = GaussWishart(kappa0,m0,nu0,S0);
[label, Theta, w, llh] = mixDpGb(X,alpha0,prior);


classdef TP6 < PROBLEM
% <multi> <real> <large/none> <robust>
% Test problem for robust multi-objective optimization
% delta --- 0.05 --- Maximum disturbance degree
% H     ---   50 --- Number of disturbances

%------------------------------- Reference --------------------------------
% A. Gaspar-Cunha, J. Ferreira, and G. Recio, Evolutionary robustness
% analysis for multi-objective optimization: benchmark problems, Structural
% and Multidisciplinary Optimization, 2014, 49: 771-793.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        delta;      % Maximum disturbance degree
        H;          % Number of disturbances
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.delta,obj.H] = obj.ParameterSet(0.05,50);
            obj.M = 2;
            if isempty(obj.D); obj.D = 5; end
            obj.lower    = [0,-ones(1,obj.D-1)];
            obj.upper    = [1, ones(1,obj.D-1)];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(~,PopDec)
            PopObj(:,1) = PopDec(:,1);
            h = 1 - PopDec(:,1).^2;
            g = sum(10-10*cos(4*pi*(PopDec(:,2:end)))+PopDec(:,2:end).^2,2);
            S = 1./(0.2+PopDec(:,1)) + PopDec(:,1).^2;
            PopObj(:,2) = h + g.*S;
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - R(:,1).^2 ;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case {'Mean_IGD','Mean_HV','Worst_IGD','Worst_HV'}
                    score = feval(metName,Population,obj);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Perturb solutions multiple times
        function PopX = Perturb(obj,PopDec,N)
            if nargin < 3; N = obj.H; end
            Delta = repmat(obj.delta.*(obj.upper-obj.lower),N*size(PopDec,1),1);
            w     = UniformPoint(N,obj.D,'Latin');
            Dec   = 2*Delta.*w(reshape(repmat(1:end,size(PopDec,1),1),1,[]),:) + repmat(PopDec,N,1) - Delta;
            Dec   = obj.CalDec(Dec);
            PopX  = SOLUTION(Dec,obj.CalObj(Dec),obj.CalCon(Dec));
        end
    end
end


function [C,nc] = graph_coloring(A,nc)
  % GRAPH_COLORING Color a graph given an adjacency matrix. This heuristic is
  % designed to efficiently find a conservative coloring (i.e., using too many
  % colors). It does *not* attempt to find the minimal coloring. It is best used
  % when the provided nc is a few more than the optimal coloring. For example,
  % planar graphs can be colored with 4 colors, (slow) heuristics for finding the
  % optimal coloring will often find 6 colors, but this function will very
  % quickly find a 7-coloring (nc=7). On such a graph, if you passed nc=4, this
  % function will likely waste time attemping to find a 4-coloring, 5-coloring,
  % 6-coloring, and then very quickly find a 7-coloring. It is designed to give
  % up searching for parsimonious colorings and increase the number of colors
  % (throwing a warning).
  %
  % [C,nc] = graph_coloring(A,nc)
  %
  % Inputs:
  %   A  #V by #V adjacency matrix
  %   nc  desired number of colors (for planar/triangle meshes, 7 is a fast
  %     choice; for tetrahedral meshes, 13 appears to often be fast)
  % Outputs:
  %   C  #V list of color ids into (1:nc)
  %   nc  effective number of colors >= input nc

  n = size(A,1);
  [FI,FJ] = find(triu(A,1));
  % always marking the higher valence vertex as bad definitely makes things
  % worse 50x.
  %
  % if nc is much greater than optimal, always marking the lower valence vertex
  % doesn't much effect. But for nc close to optimal, this seems to make a very
  % big difference (300x)
  val = sum(A,2);
  swap = val(FI)<val(FJ);
  EI = FI.*swap + FJ.*~swap;
  EJ = FJ.*swap + FI.*~swap;

  onc = nc;
  % worst case just increase number of colors
  max_outer = 10;
  for outer = 1:max_outer
    % Might as well retry a few times for this nc
    for retry = 1:10
      C = ceil(nc*rand(n,1));
      for iter = 1:2*ceil(sqrt(size(A,1)))
        bad = unique(EI(C(EI)==C(EJ)));
        if numel(bad) == 0
          break;
        end
        C(bad) = nan;
        C(bad) = ceil(nc*rand(numel(bad),1));
      end
      if numel(bad) == 0
        break;
      end
    end
    if numel(bad) == 0
      break;
    end
    nc = nc+1;
    if nc>max_outer
      error('Failed to converge.\n');
    end
  end
  if nc ~= onc
    warning('Had to increasing number of colors')
  end

  %V2C = sparse(1:n,C,1,n,nc);
  % find all edges with the same color
  %A*V2C;
end


function rule_num = lyness_rule_num ( )

%*****************************************************************************80
%
%% LYNESS_RULE_NUM returns the number of Lyness quadrature rules.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    James Lyness, Dennis Jespersen,
%    Moderate Degree Symmetric Quadrature Rules for the Triangle,
%    Journal of the Institute of Mathematics and its Applications,
%    Volume 15, Number 1, February 1975, pages 19-32.
%
%  Parameters:
%
%    Output, integer RULE_NUM, the number of rules.
%
  rule_num = 21;

  return
end


function dy = lotka(t,y,a,b,d,g)
dy = [
    a*y(1) - b*y(1)*y(2);
    d*y(1)*y(2) - g*y(2);
    ];


function cost = FDDL_fidelity(Y, Y_range, D, D_range, X)

    nClasses = numel(Y_range) - 1;
    cost = 0;
    for c = 1: nClasses
        Yc = get_block_col(Y, c, Y_range);
        Dc = get_block_col(D, c, D_range);
        Xc = get_block_row(X, c, D_range);
        Xcc = get_block_col(Xc, c, Y_range);
        cost = cost + normF2(Yc - Dc *Xcc);
        for j = 1:nClasses
            if j == c
                continue;
            else
                Xcj = get_block_col(Xc, j, Y_range);
                cost = cost + normF2(Dc*Xcj);
            end
        end
    end

end


%% Naive Bayes with independent Bernoulli
close all; clear;
d = 10;
k = 2;
n = 2000;
[X,t,mu] = mixBernRnd(d,k,n);
m = floor(n/2);
X1 = X(:,1:m);
X2 = X(:,(m+1):end);
t1 = t(1:m);
t2 = t((m+1):end);
model = nbBern(X1,t1);
y2 = nbBernPred(model,X2);
err = sum(t2~=y2)/numel(t2);


function subWins = get_augmentation_matrix( augmentationType, varargin )
%GET_AUGMENTATION Build augmentation matrix
%
%   augmentationType:: 'nr3'
%       1st field(f|n) indicates whether include flipped copy or not
%       2nd field(s|r) indicates type of region - Square or Rectangle
%       3rd field(1..4) indicates number of levels
%       note: 'none', 'ns1', 'nr1' are equivalent
%   `width`:: [1, 0.75, 0.5, 1/3]
%       width of windows in each layer

opts.width = [1, 0.75, 0.5, 1/3];
opts = vl_argparse(opts,varargin);
width = opts.width;

if strcmp(augmentationType,'none'),
    augmentationType = 'ns1';
end
nLevels = str2double(augmentationType(3));
if nLevels>length(width),
    error('Too many levels.');
end
if augmentationType(2)=='s',
    subWins = [0;1;0;1];
    for l=2:nLevels,
        sv = linspace(0,1-width(l),l);
        [XX,YY] = meshgrid(sv,sv);
        sxy = reshape(permute(cat(3,XX,YY),[3,1,2]),[2,l^2]);
        subWins = [subWins [sxy(1,:) ; width(l)*ones(1,l^2) ;sxy(2,:); ...
            width(l)*ones(1,l^2)]];
    end
elseif augmentationType(2)=='r',
    sv = [];
    w = [];
    for l=1:nLevels,
        sv = [sv linspace(0,1-width(l),l)];
        w = [w width(l)*ones(1,l)];
    end
    [XX_sv,YY_sv] = meshgrid(sv,sv);
    [XX_w,YY_w] = meshgrid(w,w);
    sxy = reshape(permute(cat(3,XX_sv,YY_sv),[3,1,2]),[2,numel(XX_sv)]);
    wxy = reshape(permute(cat(3,XX_w,YY_w),[3,1,2]),[2,numel(XX_w)]);
    subWins = [sxy(1,:) ; wxy(1,:) ; sxy(2,:) ; wxy(2,:)];
else
    error('Unknow augmentation type: %s', augmentationType);
end
subWins(end+1,:) = 0;
if augmentationType(1)=='f',
    subWins = [subWins subWins];
    subWins(end,size(subWins,2)/2+1:end) = 1;
end

end


function p = mstud_lpdf(x, mu, Cov, nu)
% p = mstud_lpdf(x, mu, Cov, nu)

D = length(x);

x = x(:);
mu = mu(:);

invCov = inv(Cov);

p = gammaln((D+nu)/2);
p = p - gammaln(nu/2);
p = p - D/2 * log(nu * pi);
p = p - 0.5 * log(det(Cov));
p = p + -(D+nu)/2 * log(1 + 1/nu*(x-mu)'*invCov*(x-mu));


% CVX: Additional functions added by CVX.
%
%   These functions have been provided to expand the variety of constraints
%   and objectives that can be specified in CVX models. But in fact, they
%   can be used with numeric arguments *outside* of CVX as well. The help
%   text for each of these functions contains general information about the
%   computations it performs, as well as specific information about its
%   proper use in CVX models, as dictated by its convexity/concavity and
%   monotonicity properties.
%
%   Those functions marked with a (*), as well as the exponential and
%   logarithm functions, are supported using a "successive approximation"
%   approach: that is, the solver is called multiple times to refine the
%   solution to the required accuracy. Thus models using these functions
%   should be expected to run more slowly than models of comparable size
%   that do not. See the CVX user guide for details.
%
%   A number of Matlab's built-in functions have been extended to provide
%   CVX support; for example,
%     abs, exp(*), log (*), max, min, norm, prod, sqrt
%   For a full list, type "help cvx/builtins".
%
%   berhu             - Reverse Huber penalty function.
%   det_inv           - Determinant of the inverse of an SPD matrix.
%   det_root2n        - 2nth-root of the determinant of an SPD matrix.
%   det_rootn         - nth-root of the determinant of an SPD matrix.
%   entr              - Scalar entropy. (*)
%   geo_mean          - Geometric mean. (*)
%   huber             - Huber penalty function.
%   huber_circ        - Circularly symmetric version of the Huber penalty.
%   huber_pos         - Monotonic Huber-style function.
%   inv_pos           - Reciprocal of a positive quantity.
%   kl_div            - Scalar Kullback-Leibler distance. (*)
%   lambda_max        - Maximum eigenvalue of a symmetric matrix.
%   lambda_min        - Minimum eigenvalue of a symmetric matrix.
%   log_det           - Logarithm of the determinant. (*)
%   log_normcdf       - Logarithm of the normal CDF. (approximation)
%   log_sum_exp       - log(sum(exp(x))). (*)
%   logsumexp_sdp     - SDP-based approximation of log(sum(exp(x))).
%   matrix_frac       - Matrix fractional function.
%   norm_largest      - Sum of the k largest magnitudes of a vector.
%   norm_nuc          - Nuclear norm of a matrix.
%   norms             - Computation of multiple vector norms.
%   norms_largest     - Computation of multiple norm_largest() norms.
%   poly_env          - Convex or concave envelope of a polynomial.
%   polyval_trig      - Evaluate a trigonometric polynomial.
%   pos               - Positive part.
%   pow_p             - Nonnegative branches of the power function.
%   pow_pos           - Convex/concave branches of the power function.
%   pow_abs           - Absolute value raised to a fixed power.
%   quad_form         - Quadratic form.
%   quad_over_lin     - Sum of squares over linear.
%   quad_pos_over_lin - Sum of squares of positives over linear.
%   rel_entr          - Scalar relative entropy. (*)
%   sigma_max         - Maximum singular value.
%   square            - Square.
%   square_abs        - Square of absolute value.
%   square_pos        - Square of positive part.
%   sum_largest       - Sum of the largest k values of a vector.
%   sum_smallest      - Sum of the smallest k elements of a vector.
%   sum_square        - Sum of squares.
%   sum_square_abs    - sum of squares of absolute values.
%   sum_square_pos    - Sum of squares of positive parts.
%   trace_inv         - Trace of the inverse of a PSD matrix.
%   trace_sqrtm       - Trace of the square root of a PSD matrix.
%   vec               - Vectorize.

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [ index ] = gsp_good_graph_index( G, X, param )
%GSP_GOOD_GRAPH_INDEX Index testing how well a given graph G, matches some data X
%   Usage: gsp_good_graph(G, X);
%
%   Input parameters:
%       G          : the graph
%       X          : a data matrix
%       param      : structure of optional parameters
%   Output parameters:
%       index      : the computed index
%
%   A wrapper function with which one may test how well a given graph G,
%   matches some data X.
%
%   Example:::
%       G = gsp_2dgrid(16);
%       X = pinv(full(G.L)) * randn(G.N, G.N);
%       param.verbose = 1;
%       param.index = 'tcer';
%       index = gsp_good_graph_index(G, X, param)
%       param.index = 'stationarity';
%       index = gsp_good_graph_index(G, X, param)
%
%   Optional paramaters
%   -------------------
%
%   * *param.index*: 'tcer' or 'stationarity' (default 'tcer').
%
%   See also: gsp_learn_tcer, gsp_stationarity_ratio

% Author  : Andreas Loukas
% Date    : 15 Nov 2016

% Handle input
if nargin < 3, param = struct(); end
if not(isfield(param, 'index'));   param.index = 'tcer'; end;

switch param.index,

    case 'tcer',
        index = gsp_learn_tcer(G, X, param);

    case 'stationarity'
        index = gsp_stationarity_ratio(G, X*X', param);

    otherwise,
        error('uknown index.');

end
end


function [cl, cp, cs] = dtiComputeWestinShapes(eigVal, denominator)
%
% [cl, cp, cs] = dtiComputeWestinShapes(eigVal, [denominator='lsum'])
% Computes Westin's shape indices
%'denominator' is a parameter allowing to choose an appropriate definition
%for Westin shapes.
%
% denominator='lsum' (default):
%    cl = (lambda_1 - lambda_2) / (lambda_1 + lambda_2 + lambda_3)
%    cp = 2 * (lambda_2 - lambda_3) / (lambda_1 + lambda_2 + lambda_3)
%    cs = 3 * lambda_3 / (lambda_1 + lambda_2 + lambda_3)
%
% This was the originial formulation described in:
%
% C-F. Westin, S. Peled, H. Gubbjartsson, R. Kikinis, and F.A. Jolesz.
% Geometrical diffusion measures for MRI from tensor basis analysis.
% In Proceedings 5th Annual ISMRM, 1997.
%
% denominator='l1':
%
% In later work, Westin et. al. adopted a simpler normalization
% formulation (e.g., see Westin et. al. 2002 Med. Image Anal.; PMID:
% 12044998) where the constants are dropped and the denominator is simply
% lambda_1. This definition produces cl+cp+cs=1 which is convenient for
% tensor shape representation in barycentric coordinates.
% In practice, the two methods produce very similar maps.
% cl = (lambda_1 - lambda_2) / (lambda_1)
% cp = 2 * (lambda_2 - lambda_3) / (lambda_1)
% cs = 3 * lambda_3 / (lambda_1)
%
% eigVal: XxYxZx3 or nx3xN array of tensor eigenvalues. Or, you can pass
% the dt6 array (XxYxZx6) and we'll compute the eigenvalues for you.
%
%
% HISTORY:
% 2004.07.22 RFD (bobd@stanford.edu) & ASH wrote it.
% 2007.01.02 SHC made changes to allow nx3xN format.
% 2009.01.05 EIR added option of using a more recent definition of Westin shapes

if ~exist('denominator', 'var')
    denominator='lsum';
end

if size(eigVal,4)==6
    [eigVec, eigVal] = dtiEig(eigVal);
    clear eigVec;
end

% Check inputs
switch ndims(eigVal)
    case 2,
        Ind    = 1; % Data in indexed nx6 format
        eigVal = shiftdim(eigVal, -2);
    case 3,
        if size(eigVal,2)==6 && size(eigVal,3)~=6
            Ind    = 1; % Data in indexed nx6xN format
            eigVal = shiftdim(eigVal, -2);
        else
            Ind    = 2; % Data in XxYx6 format
            eigVal = shiftdim(eigVal, -1);
        end
    otherwise,
        Ind = 0; % Data in XxYxZx6xN format
end

epsilon = 1e-10;
switch denominator
    case 'lsum'
        denum=sum(eigVal,4);

    case 'l1'
        denum=eigVal(:, :, :, 1);
    otherwise fprintf('Wrong denominator option');
end

nz = denum>epsilon;

% Avoid divide-by-zero (we'll replace these values with zeros below).
denum(~nz) = 1;

cl = (eigVal(:,:,:,1)-eigVal(:,:,:,2))./denum;
cp = 2*(eigVal(:,:,:,2)-eigVal(:,:,:,3))./denum;
cs = 3*eigVal(:,:,:,3)./denum;

if strcmp(denominator,'l1')
    cp=cp./2;
    cs=cs./3;
end

cl(~nz) = 0; cp(~nz) = 0; cs(~nz) = 0;
cl(cl>1.0) = 1.0;
cp(cp>1.0) = 1.0;
cs(cs>1.0) = 1.0;
cl(cl<0.0) = 0.0;
cp(cp<0.0) = 0.0;
cs(cs<0.0) = 0.0;

% Adjust output
switch Ind
    case 1,
        cl = shiftdim(cl, 2);
        cp = shiftdim(cp, 2);
        cs = shiftdim(cs, 2);
    case 2,
        cl = shiftdim(cl, 1);
        cp = shiftdim(cp, 1);
        cs = shiftdim(cs, 1);
end

return;


function [x, objV] = proxF_l1(x,t)
%% Usage proxF_l1(x,t)
% ProxF_l1(x,t) applies L1 shrinkage to n-dimensional matrix x for
% a shrinkage factor of t.  Returns the shrunk object.
%
%
% INPUTS:
%
% x: n-dimensional matrix
% t: shrinkage threshold
%
% OUTPUTS:
%
% x: n-dimensional matrix after shrinkage.
%
% Authors: G. Ely, S. Aeron, Z. Zhang, ECE, Tufts Univ. 03/16/2015

tq = t * 1;
s  = 1 - min( tq./abs(x), 1 );
x  = x .* s;
objV = sum(x(:));
end


function out = correl_compare_dep(y1,y2,varargin)
% Compare dependent correlations between pairs of vectors in y1 and y2.
%
% :Usage:
% ::
%
%     out = correl_compare_dep(y1,y2,['alpha',myalpha],['rank'],['table'])
%
% Each of y1 and y2 would contain at least 2 columns, which would be
% correlated and saved in r1 and r2 matrices in output.
% Then, the r1 and r2 matrices are subtracted, and P-values are
% returned for the differences.
%   - In the simplest case, y1 would contain vectors [a b] and y2 would
%     contain vectors [a c].  tests are provided on the a-b vs. a-c
%     difference in correlations.
%
% Repeats dep. correl. analysis for each pair of columns
%
% Returns results in correlation matrix form, where number of rows and
% cols. are the number of pairs [y1(:,i) y2(:,i)]
%
% myalpha is 2-tailed alpha value; p-values are 2-tailed
% FDR correction is at .01, 2-tailed
%
% Based on Steiger, 1980, tests for comparing dependent correlations.
%
% :Examples:
% ::
%
%    for i = 1:length(cl), y1(:,i) = cl.CONTRAST.data(:,2); y2(:,i) = cl.CONTRAST.data(:,1); end
%    for i = 1:length(cl), y1(:,i) = cl(i).CONTRAST.data(:,2); y2(:,i) = cl(i).CONTRAST.data(:,1); end
%
%    % y1 is matrix of obs x data vectors for condition 1
%    % y2 is matrix of obs x data vectors for condition 2
%    out = correl_compare_dep(y1,y2)
%
%    figure('Color','w');nmdsfig(c.GroupSpace,c.ClusterSolution.classes, ...
%    c.names,out.sig,1,{'Pos' 'Neg'});
%    nmdsfig_legend(c.ClusterSolution.X,c.r)
%
% :Examples:
% ::
%
%    c_compare = correl_compare_dep(y1avg,y2avg,'alpha',.05,'rank','table','names',c.APPLY_CLUSTER.names);
%
%    out = correl_compare_dep([ypred pain],[ypred temp], 'alpha', .06, 'table', 'names', {'biomarker resp' 'pain or temp'});

    myalpha = .05;
    dorankdata = 0;
    dotable = 0;
    names = [];

    for i = 1:length(varargin)
        if isstr(varargin{i})
            switch varargin{i}
                % functional commands
                case 'alpha', myalpha = varargin{i+1};
                case 'rank', dorankdata = 1;
                case 'table', dotable = 1;
                case 'names', names = varargin{i+1};
                otherwise, warning(['Unknown input string option:' varargin{i}]);
            end
        end
    end

    [N,npairs] = size(y1);

    [rows,cols,ncorr] = corrcoef_indices(npairs);

    if nargin < 4, dorankdata = 0; end
    if dorankdata
        str = sprintf('Ranking data: Nonparametric correlations'); fprintf(1,str);

        for i = 1:npairs
            y1(:,i) = rankdata(y1(:,i));
            y2(:,i) = rankdata(y2(:,i));
        end
    else
        str = sprintf('Assuming continuous data (no ranks).'); fprintf(1,str);
    end

    erase_string(str);

    i = 1; j = 2; k = 3; h = 4;

    str = sprintf('Computing differences among correlations %04d',0); fprintf(1,str);

    diffr = zeros(ncorr,1);
    Zstar2 = zeros(ncorr,1);
    rr = zeros(ncorr,2);

    for cc = 1:ncorr

        fprintf(1,'\b\b\b\b%04d',cc);

        % get full correlation matrix for the 4 variables involved
        dat = [y1(:,[rows(cc) cols(cc)]) y2(:,[rows(cc) cols(cc)])];

        % get differences between correlation z-values (estimate)
        [diffr(cc),r,rr(cc,:),diffrz,z] = correlation_diffs(dat,i,j,k,h);

        s = covcorr(r,i,j,k,h);         % covariance of corr coeffs

        Zstar2(cc) = diffrz * sqrt( (N-3) ./ (2-(2*s)) );

        % bootstrap
        %vals = bootstrp(5000,@correlation_diffs,dat,i,j,k,h);
        %Zboot(cc) = mean(vals) ./ std(vals);

        %if Zstar2(cc) > 2, keyboard, end
    end

    erase_string(str);

    pvec = 2 * (1 - normcdf(abs(Zstar2)));

    r1 = reconstruct(rr(:,1),npairs,ncorr,rows,cols);
    r2 = reconstruct(rr(:,2),npairs,ncorr,rows,cols);

    Z = reconstruct(Zstar2,npairs,ncorr,rows,cols);
    p = reconstruct(pvec,npairs,ncorr,rows,cols);

    dat = [rr diffr Zstar2 pvec];
    dat = [rows cols dat];
    dat = dat(pvec <= myalpha,:);

    diffr = reconstruct(diffr,npairs,ncorr,rows,cols);

    sig = (p <= myalpha - eye(size(p))) .* sign(diffr);

    % FDR corrected
    pthr = FDR(pvec,.05);
    if isempty(pthr), pthr = 0; end

    sigfdr = (p <= pthr) .* sign(diffr);

    out = struct('alpha',myalpha,'r1',r1,'r2',r2,'diffr',diffr, ...
        'Z',Z,'p',p,'sig',sig,'pthr',pthr,'sigfdr',sigfdr,'sigstats',dat);

    % output table...
    if dotable
        if isempty(names)
            disp(['No names entered; try ''names'' keyword to add them.']);
            for i = 1:npairs, names{i} = ['R' num2str(i)]; end
        end

        out.names = names;

        disp(['Uncorrected, p < ' num2str(myalpha)])
        maketable(out,'sig',names);
        disp('')

        disp(['FDR corrected, p < ' num2str(myalpha)])
        maketable(out,'sigfdr',names);
        disp('')
    end

    return

function [diffr,r,rr,diffrz,z] = correlation_diffs(dat,i,j,k,h)
    % get differences between correlation z-values
    r = corrcoef(dat);

    rr = [r(i,j) r(k,h)];                % correls to be compared
    diffr = -diff(rr);              % negative sign means we get (1) - (2)

    if nargout > 3
        z = .5 .* log( (1+rr) ./ (1-rr) );
        diffrz = -diff(z);               % diff btwn correl z values
    end

    return

function [rows,cols,ncorr] = corrcoef_indices(npairs)
    % upper triangle only
    tmp = triu(ones(npairs));
    tmp = tmp - eye(npairs);
    [rows,cols] = find(tmp);
    ncorr = length(rows);
    return

function s = covcorr(R,i,j,k,h)

    % pool correl. coeff for more stable var est.
    % as they are equal under Ho.  Steiger, 1980, eq. 14 for Z*
    pooledr = mean([R(i,j) R(k,h)]);
    s = pearsonf(R,i,j,k,h) ./  ( corvar(pooledr) );

    return

function cv = corvar(r)
    % variance of a correlation coefficient
    % simplified form (special case) of pearsonf
    cv = (1 - r^2) ^ 2;
    return

    % % function cv = co(R,x,y)
    % % % x and y are 2-vector indices of matrix R to compute covariance for
    % % % cv is covariance
    % % cv = pearsonf(R,x(1),x(2),y(1),y(2));
    % % return

function pf = pearsonf(R,i,j,k,h)

    % Pearson-Filon: covariance (or var) of element i,j with element k,h
    % depending on correlation values

    % for variance of 1 correl, works as well, and reduces to:
    % (1 - rr(1)^2)^2

    pf = (1/2) .* R(i,j) .* R(k,h) .* ...
        ( R(i,k).^2 + R(i,h).^2 + R(j,k).^2 + R(j,h).^2 ) + ...
        R(i,k) .* R(j,h) + R(i,h) .* R(j,k) - ...
        R(i,j) .* ( R(j,k) .* R(j,h) + R(i,k) .* R(i,h) ) - ...
        R(k,h) .* ( R(j,k) .* R(i,k) + R(j,h) .* R(i,h) );

    return

    % function pf = pearsonf2(r,j,k,h,m)
    %
    % pf = .5 * ( (r(j,h) - r(j,k)*r(k,h)) * (r(k,m) - r(k,h)*r(h,m))  + ...
    %     (r(j,m) - r(j,h)*r(h,m)) * (r(k,h) - r(k,j)*r(j,h)) + ...
    %     (r(j,h) - r(j,m)*r(m,h)) * (r(k,m) - r(k,j)*r(j,m)) + ...
    %     (r(j,m) - r(j,k)*r(k,m)) * (r(k,h) - r(k,m)*r(m,h))    );
    %
    % return

function valmat = reconstruct(vals,npairs,ncorr,rows,cols)

    valmat = zeros(npairs);
    for i = 1:ncorr
        valmat(rows(i),cols(i)) = vals(i);
    end
    valmat = valmat + valmat';

    return

function erase_string(str1)
    fprintf(1,repmat('\b',1,length(str1))); % erase string
    return

function maketable(c_compare,whfield,names)

    str = {'-' '+'};
    [rows,cols] = find(triu(c_compare.(whfield)));
    if isempty(rows)
        disp('No significant results.')
    else
        fprintf(1,'Name1\tName2\trow\tcol.\t+ or -\tr1\tr2\tZ\tp\n');
        for i = 1:length(rows)
            fprintf(1,'%s\t%s\t%3.0f\t%3.0f\t%s\t%3.3f\t%3.3f\t%3.2f\t%3.4f\n', ...
                names{rows(i)}, names{cols(i)},rows(i),cols(i), ...
                str{1.5 + (.5.*c_compare.(whfield)(rows(i),cols(i)))}, ...          % + or - sign
                c_compare.r1(rows(i),cols(i)),c_compare.r2(rows(i),cols(i)), ...   % correlations
                c_compare.Z(rows(i),cols(i)),c_compare.p(rows(i),cols(i)));        % Z and p
        end
    end
    fprintf(1,'\n');
    return


function qamsys(bin,f)

disp('========================================');
disp(' HAM DIEU CHE DICH BIEN & PHA: QAM');
disp(' VI DU: qamsys([0 0 0 1 1 1 0 1 1 1 1 1],3)');
disp('                Writer: Minh Dai Ca');
disp('========================================');

bin=[0 0 0 1 1 1 0 1 1 1 1 1];f=5;
t=0:2*pi/149:2*pi;

L = length(bin);bit1=ones(1,50);bit0=zeros(1,50);mbit=[];mcw=[];
sig000=cos(f*t);sig001=cos(f*t+pi/2);sig010=cos(f*t+pi);sig011=cos(f*t+3*pi/2);
sig100=2*sig000;sig101=2*sig001;sig110=2*sig010;sig111=2*sig011;

if 3*fix(L/3)~=L
    error('DO DAI CUA CHUOI bin PHAI LA BOI SO CUA 3');
end

for n=1:3:L;
    if bin(n)==0 && bin(n+1)==0 && bin(n+2)==0;
       cw=sig000;bit=[bit0 bit0 bit0];
    elseif bin(n)==0 && bin(n+1)==0 && bin(n+2)==1;
       cw=sig001;bit=[bit0 bit0 bit1];
    elseif bin(n)==0 && bin(n+1)==1 && bin(n+2)==0;
       cw=sig010;bit=[bit0 bit0 bit0];
    elseif bin(n)==0 && bin(n+1)==1 && bin(n+2)==1;
       cw=sig011;bit=[bit0 bit1 bit1];

    elseif bin(n)==1 && bin(n+1)==0 && bin(n+2)==0;
       cw=sig100;bit=[bit0 bit0 bit0];
    elseif bin(n)==1 && bin(n+1)==0 && bin(n+2)==1;
       cw=sig101;bit=[bit0 bit0 bit1];
    elseif bin(n)==1 && bin(n+1)==1 && bin(n+2)==0;
       cw=sig110;bit=[bit0 bit0 bit0];
    elseif bin(n)==1 && bin(n+1)==1 && bin(n+2)==1;
       cw=sig111;bit=[bit0 bit1 bit1];
    end
    mbit=[mbit bit];
   mcw=[mcw cw];
end
qam=mcw;
deqam=mcw;

subplot(3,1,1);plot(mbit,'r','linewidth',2);axis([0  50*L -0.5 1.5]);grid on;title('Data in');
subplot(3,1,2);plot(qam,'m','linewidth',1.5);axis([0  50*L -2.5 2.5]);grid on;title('PSK modulation');
subplot(3,1,3);plot(deqam,'g','linewidth',1.5);axis([0  50*L -2.5 2.5]);grid on;title('PSK demodulation,Data out');


function minD = minDistBlock(v1, v2, blockSize);
%"minDistBlock"
%   Returns the minimum distance between two sets of points in n-D space.
%   Points are specified as a matrix size nPts x nDims, ie for 10 points in
%   3 dimensions, 10x3.
%
%   minDistBlock uses block processing to avoid out of memory errors.  A
%   blockSize parameter can be passed in, but if it does not exist the
%   default value of 5E6 is used.
%
% PEL 04/22/05
%
%Usage:
%   minD = minDistBlock(pts1, pts2, blockSize);
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

%Set default blockSize if needed.
if ~exist('blockSize')

    blockSize = 5E6;
end

%Get number of points in each set.
n1 = size(v1, 1);
n2 = size(v2, 1);

%Get number of dimensions in each set.
ndim1 = size(v1, 2);
ndim2 = size(v2, 2);
if ndim1 ~= ndim2
    error('minDistBlock: both point sets must have the same number of dimensions.')
end

%If block processing needed, make n1 the smallest point set.
if(n1*n2>blockSize),
    if(n1>n2),
        tmp = v2;
        v2 = v1;
        v1 = tmp;
        clear tmp;
        n1 = size(v1, 1);
        n2 = size(v2, 1);
    end
end

%If both point sets have more than blockSize elements...
if(n1>blockSize & n2>blockSize),
    message = ['minDistBlock: Both point sets are larger than blockSize. Calculation may take awhile.  BlockSize can be increased from ' num2str(blockSize) '.'];
    warning(message);
    minD = Inf;
    blockStep = blockSize;
    numBlocks =  ceil(n1/blockStep);
    for i = 1:numBlocks,
        cntr1 = (i-1)*blockStep+1;
        cntr2 = min(i*blockStep, n1);
        for j = 1:n2,
            rTmpSq = sepsq(v1(cntr1:cntr2, :)', v2(j, :)');
            minD = min(minD, min(rTmpSq(:)));
        end
    end

%If one set has less than blockSize elements...
else
    minD = Inf;
    blockStep = floor(blockSize/n1);
    numBlocks = ceil(n2/blockStep);

    for i = 1:numBlocks
        cntr1 = (i-1)*blockStep+1;
        cntr2 = min(i*blockStep, n2);
        rTmpSq = sepsq(v1', v2(cntr1:cntr2,:)');
        minTmp = min(rTmpSq(:));
        minD = min(minD, minTmp);
    end

end

%Convert back to sqrt since sepsq does not take sqrt.
minD = sqrt(minD);