function [H,Q,M,W]=matExpIntFam(T,A,B,Qc)
%%MATEXPINTFAM This function explicitly solves a number of real matrix
%       integral problems that arise when solving for optimal linear
%       regulators. The integrals are:
%       H=int_0^T expm(A*s)*B ds
%       Q=int_0^T expm(A'*s)*Qc*expm(A*s) ds
%       M=int_0^T expm(A'*s)*Qc*H(s) ds
%       W=int_0^T H(s)'*Qc*H(s) ds
%       where s is the scalar parameter of integration, expm is the matrix
%       exponential function int refers to a definite integral, and the
%       function
%       H(s) is defined to be
%       H(s)=(int_0^sexpm(A*tau) dtau)*B
%
%INPUTS: T The real, positive, scalar upper bound of the integrals.
%        A A real nXn matrix.
%        B A real nXp matrix. If this parameter is omitted or an empty
%          matrix is passed, then it is assumed that p=0, in which case H,
%          M, and W will be empty matrices.
%       Qc A symmetric positive (semi)definite nXn matrix. If omitted or an
%       empty matrix is passed, then it is assumed that Qc=eye(n,n);
%
%OUTPUTS: H,Q,M,W The values of the desired matrix integrals, which are
%         defined above
%
%Expressions for obtaining the solutions to the integrals by evaluating a
%single matrix exponential are given in [1]. The type of linear regulator
%problem addressed by these integrals is given in [2].
%
%REFERENCES:
%[1] C. F. Van Loan, "Computing Integrals Involving the Matrix
%    Exponential," IEEE Transactions on Automatic Control, vol. AC-23, no.
%    3, pp. 395-404, Jun. 1967.
%[2] E. S. Armstrong and A. K. Caglayan, "An algorithm for the weighting
%    matrices in the sampled-data optimal linear regulator problem,"
%    National Aeronautics and Space Administration, Langley Research
%    Center, Hampton, VA, Tech. Rep. NASA TN D-8372, Dec. 1976.
%
%September 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(A,1);

if(nargin<4||isempty(Qc))
    Qc=eye(n,n);
end

if(isempty(B)||nargin<2)
    p=0;
    B=zeros(n,p);
else
    p=size(B,2);
end

C=[-A.',        eye(n,n), zeros(n,n+p);
    zeros(n,n), -A.',     Qc,   zeros(n,p);
    zeros(n,2*n),         A,    B;
    zeros(p,3*n+p)];

CExp=expm(C*T);

F3=CExp((2*n+1):(3*n),(2*n+1):(3*n));
G2=CExp((n+1):(2*n),(2*n+1):(3*n));
G3=CExp((2*n+1):(3*n),(3*n+1):(3*n+p));
H2=CExp((n+1):(2*n),(3*n+1):(3*n+p));
K1=CExp(1:n,(3*n+1):(3*n+p));

H=G3;
Q=F3.'*G2;
M=F3.'*H2;

temp=B.'*F3.'*K1;
W=temp+temp.';

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function geometry_test016 ( )

%*****************************************************************************80
%
%% GEOMETRY_TEST016 tests CIRCLE_IMP_POINTS_2D, POLYGON_AREA_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  center(1:2,1) =  [ 5.0; -2.0 ];
  r = 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'GEOMETRY_TEST016\n' );
  fprintf ( 1, '  CIRCLE_IMP_POINTS_2D gets points on a circle;\n' );
  fprintf ( 1, '  POLYGON_AREA_2D finds the area of a polygon.\n' );

  circle_imp_print_2d ( r, center, '  The implicit circle:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The area = %f\n', pi * r * r );

  n = 8;

  p = circle_imp_points_2d ( r, center, n );

  r8mat_print ( 2, n, p, '  Sample results:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For any N, the sampled points define a polygon\n' );
  fprintf ( 1, '  whose area approximates the circle area.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  N      Area\n' );
  fprintf ( 1, '\n' );

  for n = 3 : 24

    p = circle_imp_points_2d ( r, center, n );
    result = polygon_area_2d ( n, p );
    fprintf ( 1, '  %6d  %12f\n', n, result );

  end

  return
end


% Surface fit artifact removal
[x,y] = meshgrid(0:.01:1);
z0 = exp(x+y);

znan = z0;
znan(20:50,40:70) = NaN;
znan(30:90,5:10) = NaN;
znan(70:75,40:90) = NaN;

z = inpaint_nans(znan,3);

% Comparison to griddata
k = isnan(znan);
zk = griddata(x(~k),y(~k),z(~k),x(k),y(k));
zg = znan;
zg(k) = zk;

close all
figure
surf(z0)
title 'Original surface'

figure
surf(znan)
title 'Artifacts (large holes) in surface'

figure
surf(zg)
title(['Griddata inpainting (',num2str(sum(isnan(zg(:)))),' NaNs remain)'])

figure
surf(z)
title 'Inpainted surface'

figure
surf(zg-z0)
title 'Griddata error surface'

figure
surf(z-z0)
title 'Inpainting error surface (Note z-axis scale)'


function [U, Uc] = slrangespace(tar, varargin)
%SLRANGESPACE Determines the subspace of the range of X
%
% $ Syntax $
%   - U = slrangespace(tar)
%   - U = slrangespace(tar, ...)
%   - [U, Uc] = slrangespace(...)
%
% $ Arguments $
%   - tar:      the target, can be a sample matrix or covariance
%   - U:        the basis for the range space
%   - Uc:       the basis for the orthogonal complement of the range
%
% $ Description $
%   - U = slrangespace(tar) determines the range space of tar in default
%     settings. That is to set the dimension to be the rank of tar. Note
%     that tar can have two forms: a sample matrix or a covariance
%     given by the syntax {'cov', C}.
%
%   - U = slrangespace(tar, ...) determines the range space of X using
%     the specified dimension determination schemes. The arguments
%     input following X will be delivered to sldim_by_eigval for dimension
%     determination.
%
%   - [U, Uc] = slrangespace(...) also returns orthogonal complement of U.
%
% $ History $
%   - Created by Dahua Lin on Apr 25, 2005
%

%% parse and verify input arguments

if isnumeric(tar)
    target_type = 1;        % target is sample

    if ndims(tar) ~= 2
        error('sltoolbox:invalidarg', ...
            'When tar is sample matrix, it should be a 2D matrix');
    end

    X = tar;
    d = size(X, 1);

elseif iscell(tar)

    if length(tar) == 2 && ischar(tar{1}) ...
            && strcmpi(tar{1}, 'cov')

        target_type = 2;        % target is covariance

        C = tar{2};
        d = size(C, 1);

        if ~isequal(size(C), [d d])
            error('sltoolbox:invalidarg', ...
                'A covariance matrix should be square');
        end

    else
        error('sltoolbox:invalidarg', ...
            'The target should be a sample matrix or a covariance given by cell');
    end

else

    error('sltoolbox:invalidarg', ...
        'The target should be a sample matrix or a covariance given by cell');
end

if nargout == 0
    return;
elseif nargout == 1
    need_complement = false;
else
    need_complement = true;
end

%% compute

switch target_type

    case 1      % sample matrix

        if ~need_complement
            [U, D] = svd(X, 0);
        else
            [U, D] = svd(X);
        end

        evals = diag(D) .^ 2;
        clear D;

    case 2      % covariance

        [evals, U] = slsymeig(C);

end

evals = max(evals, 0);
k = sldim_by_eigval(evals, varargin{:});

%% output

if ~need_complement
    U = U(:, 1:k);
else
    if k == size(U, 2)
        Uc = zeros(d, 0);
    else
        Uc = U(:, k+1:end);
        U  = U(:, 1:k);
    end
end


function [ x, seed ] = hyperball01_sample ( m, n, seed )

%*****************************************************************************80
%
%% HYPERBALL01_SAMPLE uniformly samples the unit hyperball.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Russell Cheng,
%    Random Variate Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998, pages 168.
%
%    Reuven Rubinstein,
%    Monte Carlo Optimization, Simulation, and Sensitivity
%    of Queueing Networks,
%    Krieger, 1992,
%    ISBN: 0894647644,
%    LC: QA298.R79.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real X(M,N), the points.
%
  x = randn ( m, n );
  norm = ones ( 1, m ) * ( x.^2 );
  norm = sqrt ( norm );
  for i = 1 : m
    x(i,1:n) = x(i,1:n) ./ norm(1:n);
  end

  for j = 1 : n
    r = rand ( 1, 1 );
    x(1:m,j) = r ^ ( 1.0 / m ) * x(1:m,j);
  end

  return
end


%DEMO_AUDIOSHRINK  Decomposition into tonal and transient parts
%
%   This demos shows how to do audio coding and "tonal + transient"
%   decomposition using group lasso shrinkage of two |wmdct| transforms
%   with different time-frequency resolutions.
%
%   The signal is transformed using two orthonormal |wmdct| bases.
%   Then group lasso shrinkage is applied to the two transforms
%   in order to:
%
%     * select fixed frequency lines of large |wmdct| coefficients on the
%       wide window |wmdct| transform
%
%     * select fixed time lines of large |wmdct| coefficients on the
%       narrow window |wmdct| transform
%
%   The corresponding approximated signals are computed with the
%   corresponding inverse, |iwmdct|.
%
%   .. figure::
%
%      Plots and time-frequency images
%
%      The upper plots in the figure show the tonal parts of the signal, the
%      lower plots show the transients. The TF-plots on the left are the
%      corresponding wmdct coefficients found by appropriate group lasso
%      shrinkage
%
%   Corresponding reconstructed tonal and transient sounds may be
%   listened from arrays rec1 and rec2 (sampling rate: 44.1 kHz)
%

% Load audio signal and add noise
% -------------------------------
% Use the 'glockenspiel' signal.
sig=gspi;
fs=44100;

% Shorten signal
siglen = 2^16;
sig = sig(1:siglen);

% Add Gaussian white noise
nsig = sig + 0.01*randn(size(sig));

% Tonal layer
% -----------

% Create a WMDCT basis with 256 channels
F1=frametight(frame('wmdct','gauss',256));

% Group lasso and invert
c1 = franagrouplasso(F1,nsig,0.8,'soft','freq');
rec1 = frsyn(F1,c1);

% Transient layer
% ---------------

% Create a WMDCT basis with 32 channels
F2=frametight(frame('wmdct','gauss',32));

c2 = franagrouplasso(F2,nsig,0.5,'soft','time');
rec2 = frsyn(F2,c2);

% Plots
% -----

% Dynamic range for plotting
dr=50;
xplot=(0:siglen-1)/fs;

figure(1);
subplot(2,2,1);
plot(xplot,rec1);
xlabel('Time (s)');
axis tight;

subplot(2,2,2);
plotframe(F1,c1,fs,dr);

subplot(2,2,3);
plot(xplot,rec2);
xlabel('Time (s)');
axis tight;

subplot(2,2,4);
plotframe(F2,c2,fs,dr);

% Count the number of non-zero coefficients
N1=sum(abs(c1)>0);
N2=sum(abs(c2)>0);

p1 = 100*N1/siglen;
p2 = 100*N2/siglen;
p=p1+p2;

fprintf('Percentage of retained coefficients: %f + %f = %f\n',p1,p2,p);

disp('To play the original, type "soundsc(sig,fs)"');
disp('To play the tonal part, type "soundsc(rec1,fs)"');
disp('To play the transient part, type "soundsc(rec2,fs)"');


%% ---------------Truncated Amplitude Spectral Initializer-----------------

% Intializer proposed for Truncated Amplitude Flow as given in Algorithm 1
% of the Truncated Amplitude Flow (TAF) paper. For certain definitions and
% descriptions, user may need to refer to equations (14, 17) and Algorithm
% box 1 for details.
%
% The authors of the paper propose two different initialization schemes
% (this function implements the second approach, described below).
% In the first approach, one throws out the measurements of large
% magnitude, and only keeps the remaining measurement vectors that produce
% small measurements.  These vectors are nearly orthogonal to the signal,
% and so we approximate the signal by finding a vector that is
% un-correlated with these measurement vectors. This requires finding the
% smallest eigenvalue of a large matrix.
%    In the second method, the authors propose to throw out the smallest
% measurements, and find the signal most correlated with the remaining
% measurement vectors (which produce large measurement) by finding the
% largest eigenvalue of a matrix.
%    For certain isometric measurement matrices, these appraoches are both
% equivalent.  However, for non-isometric matrices they differ.  The
% authors of the paper claim that it is difficult to find a small
% eigenvector, and so they adopt the second method that requires a large
% eigenvector.  This second method, which requires the leading (largest)
% eigenvalue/vector, is implemented here.
%   Note:  in practice, small eigenvalues can be computed efficiently using
% an Arnoldi method, and this often leads to better initializers.  For an
% implementation using the smallest eigenvalue/vector, see initNull.m.
%
%  See the script 'testInitOrthogonal.m' for an example of proper usage of
%  this function.

% PAPER TITLE:
%              Solving Systems of Random Quadratic Equations via Truncated
%              Amplitude Flow.

% ARXIV LINK:
%              https://arxiv.org/pdf/1605.08285.pdf

% INPUTS:
%         A:   Function handle/numerical matrix for data matrix A. The rows
%              of this matrix are the measurement vectors that produce
%              amplitude measurements '\psi'.
%         At:  Function handle/numerical matrix for A transpose.
%         b0:  Observed data vector consisting of amplitude measurements
%              generated from b0 = |A*x|. We assign it to 'psi' to be
%              consistent with the notation in the paper.
%         n:   Length of unknown signal to be recovered by phase retrieval.

% OUPTUT :
%         x0:  The initial vector to be used by any solver.

% Note:        When a function handle is used, the value of 'n' (the length
%              of the unknown signal) and 'At' (a function handle for the
%              adjoint of 'A') must be supplied. When 'A' is numeric, the
%              values of 'At' and 'n' are ignored and inferred from the
%              arguments

% DESCRIPTION:
%              Random vectors in high dimensions are almost always
%              orthogonal to each other. Using this very intuitive result,
%              this method computes an initial guess that is orthogonal to
%              as many measurement vectors as possible. The orthogonal
%              promoting initializer solves the limitations of the spectral
%              methods, namely heavy tailed distributions due to 4th moment
%              generating functions by using a truncation step.
%              Specifically, it removes measurement vectors that are not
%              orthogonal to the initial random guess. We form the matrix Y
%              = (1/card_I) * S_bar^T* S_bar using the truncated vectors
%              and compute the leading eigenvector. S_bar is the complement
%              of the set S. Refer to the paper for a description of S.

% METHOD:
%         1.) Find the set 'I_Bar'. I is set of indices of |I| smallest
%             measurement vectors as defined in equation(14) in the paper,
%             where |I| is the cardinality of I.I_bar is the complement of
%             I. This is done because by using I, we are required to find
%             the smallest eignevalue of a matrix formed from the data
%             vectors. However, finding the smallest eigenvalue is often
%             intractible in practice. So we find the largest eigenvalue
%             instead.

%         2.) Using a mask R, form the matrix Y = (1/card_I) * S_bar^T *
%             S_bar where S_bar is the complement of the S which is defined
%             in equation (17) in the Truncated Amplitude Paper.

%         3.) Compute the leading eigenvector of Y (computed in previous
%             step) and scale it according to the norm of x as described in
%             Step 3, Algorithm 1 of the paper.
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START----------------------------------

function [x0] = initAmplitude(A,At,b0,n,verbose)

psi = b0; % To be consistent with the notation used in the paper

% If A is a matrix, infer n and At from A
if isnumeric(A)
    n = size(A, 2);
    % Transform matrix into function form
    At = @(x) A' * x;
    A = @(x) A * x;
end

m = length(psi);    % Number of measurements.

if ~exist('verbose','var') || verbose
fprintf(['Estimating signal of length %d using an orthogonal ',...
        'initializer with %d measurements...\n'],n,m);
end

% Cardinality of I. I is the set that contains the indices of the
% truncated vectors. Namely, it removes measurement vectors that are
% not orthogonal to the initial random guess
card_I = ceil(m/6);

% STEP 1: Construct the set I of indices. We approximate by assuming that
% the norm of each row of A is same.
[~, index_array] = sort(psi, 'descend');  % Sort the data vector
ind = index_array(1: card_I); % Truncate the indexes.

% STEP 2: Form Y
R = zeros(m, 1);
% Defining the mask for truncation
R(ind) = 1;
% Forming the truncated matrix Y according to equation (17) in referenced paper.
Y = @(x) 1/card_I * At(R.*A(x));

% STEP 3: Use eigs to compute leading eigenvector of Y (Y is computed in
% previous step)
opts = struct;
opts.isreal = false; % Create opts struct for eigs
[V, ~] = eigs(Y, n, 1, 'lr', opts);
% Scale the norm to match that of x
AV = abs(A(V));
alpha = (AV'*psi) / ( AV'*AV );
x0 = V*alpha;

if ~exist('verbose','var') || verbose
    fprintf('Initialization finished.\n');
end

end


function value = i4vec_multinomial_pdf ( n, p, m, x )

%*****************************************************************************80
%
%% I4VEC_MULTINOMIAL_PDF evaluates the multinomial PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2013
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, integer N, the number of trials.
%
%    Input, real P(M), the probability of each outcome
%    on any single trial.
%
%    Input, integer M, the number of possible outcomes
%    of a single trial.
%
%    Input, integer X(M), the results of N trials,
%    with X(I) the number of times outcome I occurred.
%
%    Output, real VALUE, the probability
%    density function evaluated at X.
%

%
%  The combinatorial coefficient is an integer.
%
  c = 1;
  top = n;
  for i = 1 : m
    bot = 1;
    do j = 1 : x(i)
      c = ( c * top ) / bot;
      top = top - 1;
      bot = bot + 1;
    end
  end

  value = c;
  for i = 1 : m
    value = value * p(i) ^ x(i);
  end

  return
end


% Computes the gradient using "finite differences" and gives
% us a numerical estimate of the gradient.
function numgrad = debug_numerical_gradient(gradient_step, theta)
    % The following code implements numerical gradient checking, and
    % returns the numerical gradient. It sets numgrad(i) to (a numerical
    % approximation of) the partial derivative of J with respect to the
    % i-th input argument, evaluated at theta. (i.e., numgrad(i) should
    % be the (approximately) the partial derivative of J with respect
    % to theta(i).)
    numgrad = zeros(size(theta));
    perturb = zeros(size(theta));

    e = 1e-4;
    for p = 1:numel(theta)
        % Set perturbation vector
        perturb(p) = e;
        [loss1 gradients1] = gradient_step(theta - perturb);
        [loss2 gradients2] = gradient_step(theta + perturb);

        % Compute numerical gradient.
        numgrad(p) = (loss2 - loss1) / (2 * e);
        perturb(p) = 0;
    end
end


function [shortestPath, totalCost] = dijkstra(netCostMatrix, s, d)
%==============================================================
% shortestPath: the list of nodes in the shortestPath from source to destination;
% totalCost: the total cost of the  shortestPath;
% farthestNode: the farthest node to reach for each node after performing the routing;
% n: the number of nodes in the network;
% s: source node index;
% d: destination node index;
%==============================================================
%  Code by:
% ++by Xiaodong Wang
% ++23 Jul 2004 (Updated 29 Jul 2004)
% ++http://www.mathworks.com/matlabcentral/fileexchange/5550-dijkstra-shortest-path-routing
% Modifications (simplifications) by Meral Shirazipour 9 Dec 2009
%==============================================================
n = size(netCostMatrix,1);
for i = 1:n
    % initialize the farthest node to be itself;
    farthestPrevHop(i) = i; % used to compute the RTS/CTS range;
    farthestNextHop(i) = i;
end

% all the nodes are un-visited;
visited(1:n) = false;

distance(1:n) = inf;    % it stores the shortest distance between each node and the source node;
parent(1:n) = 0;

distance(s) = 0;
for i = 1:(n-1),
    temp = [];
    for h = 1:n,
         if ~visited(h)  % in the tree;
             temp=[temp distance(h)];
         else
             temp=[temp inf];
         end
     end;
     [t, u] = min(temp);      % it starts from node with the shortest distance to the source;
     visited(u) = true;         % mark it as visited;
     for v = 1:n,                % for each neighbors of node u;
         if ( ( netCostMatrix(u, v) + distance(u)) < distance(v) )
             distance(v) = distance(u) + netCostMatrix(u, v);   % update the shortest distance when a shorter shortestPath is found;
             parent(v) = u;     % update its parent;
         end;
     end;
end;

shortestPath = [];
if parent(d) ~= 0   % if there is a shortestPath!
    t = d;
    shortestPath = [d];
    while t ~= s
        p = parent(t);
        shortestPath = [p shortestPath];

        if netCostMatrix(t, farthestPrevHop(t)) < netCostMatrix(t, p)
            farthestPrevHop(t) = p;
        end;
        if netCostMatrix(p, farthestNextHop(p)) < netCostMatrix(p, t)
            farthestNextHop(p) = t;
        end;

        t = p;
    end;
end;

totalCost = distance(d);

%return;


function [F,totalCost,exitCode]=minCostFlow(AMat,CMat,b,maxIter)
%%MINCOSTFLOW Solve the minimum cost flow problem using a strong polynomial
%             time cycle cancelling algorithm that works with non-integer
%             costs. Minimum cost flow problems include transportation
%             problems.
%
%INPUTS: AMat An NXN matrix of costs in a directed graph such that
%             AMat(i,j) is the cost of an edge going from vertex i to
%             vertex j. If no vertex goes from node i to node j, then a
%             cost of 0 should be inserted.
%        CMat An NXN matrix of capacities of the edges in the graph.
%             CMat(i,j) is the capacity of an edge from node i to node j.
%             If no edge exists, then a capacity of zero should be used. It
%             is assumed that all capacities are finite.
%           b An NX1 vector of the supply provided by each node. It is
%             required that sum(b)=0.
%     maxIter An optional parameter specifying the maximum number of
%             iterations that should be performed by the algorithm. If
%             omitted, a maximum of 100+10N iterations is used.
%
%OUTPUTS: F The flow matrix. F(i,j) is the amount of flow going from node i
%           to node j. Note that F(i,j)=-F(j,i). If the algorithm could not
%           obtain an initial feasible solution, then an empty matrix is
%           returned. If the maximum number of iterations is exceeded, then
%           the algorithm will return a feasible solution that is not
%           optimal.
% totalCost The total cost of the flow. This is the quantity being
%           minimized. If the algorithm could not obtain an initial
%           feasible solution, then an empty matrix is returned.
%  exitCode A parameter indicating whether an error occurred or whether the
%           algorithm terminated successfully. Possible values are:
%           0 The algorithm was successful.
%           1 No feasible solution could be found.
%           2 The maximum number of iterations was reached.
%           3 Inputs are invalid. This means that either C contains NaN
%             values, sum(b)~=0 or AMat contains nonfinite values.
%
%The minimum cost flow problems seeks to find a flow F to minimize
%sum(sum(AMat.*F)) subject to the constraints:
%4) Total Vertex Flow: sum(F(i,:))=b(i) for all i.
%2) Capacity Constraint: 0<=F(u,v)<=CMat(u,v)
%3) Skew Symmetry: F(u,v)=-F(v,u)
%Whereas the maximum flow problem, which is solved in the function
%solveMaxFlowEdmondsKarp seeks to maximize the flow between two nodes the
%minimum cost flow problem seeks to minimize the total cost of a fixed
%amount of flow.
%
%The strong polynomial cycle cancelling algorithm of [1] is used. However,
%the epsilon-based convergence acceleration method described in [1] is not
%used. Rather, the algorithm is more similar to the simple cycle
%cancellation algorithm of Chapter 7.2 of [2], except the minimum mean
%cycle is always augmented rather than any negative cost cycle.
%
%The complexity of the non-epsilon accelerated algorithm in [1] is bounded
%as O(N^2*m^3*log(N)), where N is the number of vertices in the graph, and
%m is the number of edges. The true complexity depends on the efficiency of
%the function computeResidualCapacity, which is used to obtain the negative
%cycles.
%
%EXAMPLE: This is example 7.1 in Chapter 7.2 of [2].
% AMat=[0, 4, 1, 0;
%       0, 0, 2, 5;
%       0, 3, 0, 2;
%       0, 0, 0, 0];
% CMat=[0, 2, 2, 0;
%       0, 0, 1, 1;
%       0, 1, 0, 1;
%       0, 0, 0, 0];%Capacity matrix
% b=[2;0;0;-2];
% [F,totalCost,exitCode]=minCostFlow(AMat,CMat,b)
%One will get a flow matrix of
% F =[ 0     0     2     0;
%      0     0    -1     1;
%     -2     1     0     1;
%      0    -1    -1     0];
%having a total cost of 12.
%
%OTHER EXAMPLES: Transportation problems can be transformed into minimum
%cost flow problems and vice versa, as shown in Chapters 7.4 and 7.5 of
%[2]. Thus, the examples in the comments to the function
%solveTransportationProblem can be taken as additional examples of the
%minimum cost flow problem.
%
%REFERENCES:
%[1] A. V. Goldberg and R. E. Tarjan, "Finding minimum-cost circulations
%    by canceling negative cycles," Journal of the Association for
%    Computing Machinery, vol. 36, no. 4, pp. 873-886, Oct. 1989.
%[2] C. H. Papadimitriou and K. Steiglitz, Combinatorial Optimization:
%    Algorithms and Complexity. Englewood Cliffs, NJ: Prentice-Hall Inc.,
%    1982.
%
%July 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numVertex=length(b);

if(nargin<5||isempty(maxIter))
    maxIter=100+10*numVertex;
end

if(any(isnan(CMat(:)))||sum(b)~=0||any(~isfinite(AMat(:))))
    exitCode=3;%The problem is infeasible.
    F=[];
    totalCost=[];
    return;
end

%Step 1: Find a feasible flow.
%In this step, we must find a feasible flow F. This is a flow that
%satisfies constraints 1-3 but does not necessarily minimize the cost
%sum(sum(AMat.*F)). This can be done by ignoring costs and solving a
%maximum flow problem based only on the constraints. The maximum flow
%problem does not have demand constraints on all of the nodes, rather it
%only has them on a sink and source which are added.
%
%To enforce constraint 1, we have to create a network with an artificial
%sink and source. If b(i) is positive, then we need an arc from the source
%to that node with capacity b(i). If b(i) is negative, then we need an arc
%from the sink to that node with capacity -b(i). If the maximum flow
%problem solved on the modified graph saturates all of the edges coming out
%of the source, then the problem is feasible.

%Augment the capacity matrix.
CMatAugment=[CMat,zeros(numVertex,2);
             zeros(2,numVertex+2)];

source=numVertex+1;
sink=numVertex+2;
totalFlowIn=0;
for curVertex=1:numVertex
    if(b(curVertex)>0)
        CMatAugment(source,curVertex)=b(curVertex);
        totalFlowIn=totalFlowIn+b(curVertex);
    else
        CMatAugment(curVertex,sink)=-b(curVertex);
    end
end

[maxFlow,F]=solveMaxFlowEdmondsKarp(CMatAugment,source,sink);

%Check whether all of the source nodes have been saturated. This is true if
%the total flow in equals the maximum flow found. A comparison to an
%epsilon value is used as it is not clear whether it can be guaranteed
%within finite precision bounds that the two quantities will always be
%equal for feasible problems, though they seem to generally be equal for
%feasible problems.
if(abs(maxFlow-totalFlowIn)>eps(totalFlowIn))
    exitCode=1;%The problem is infeasible.
    F=[];
    totalCost=[];
    return;
end

%Get rid of the artificial source and sink nodes from the flow matrix.
F=F(1:numVertex,1:numVertex);

%To make the computation of the residual matrix simpler, we will transform
%the cost matrix. This simplifies the computation of costs in the residual
%flow network.
AMatOrig=AMat;
AMat=AMat-AMat';

%Compute the residual flow network, also known as the incremental flow
%network. Definition 7.3 from Chapter 7.2 of [2] explains what the
%incremental flow network is and how the costs are assigned. The residual
%capacity is updated during augmentation of the flow matrix F, so that it
%need not be recomputed each loop.

%Find the capacity of the residual flow network.
residualCapacity=CMat-F;
for curIter=1:maxIter
    %Step 2: Find the minimum mean cost cycle with respect to the costs on
    %        the residual flow network, also known as the incremental flow
    %        network.

    %The costs associated with the residual flow network are AMat(i,j) on
    %forward nodes and -AMat(i,j) on backward nodes. Hence the reason why
    %we replaced AMat with AMat-AMat'.
    costMat=Inf(numVertex,numVertex);
    costMat(residualCapacity~=0)=AMat(residualCapacity~=0);

    [minCycleMean,cycleVertices]=findMinMaxCycleMean(costMat,true);

    if(minCycleMean>=0)
        exitCode=0;
        FP=F;
        FP(F<0)=0;
        totalCost=sum(sum(FP.*AMatOrig));
        return;
    end

    %Step 3: The minimum mean cycle is negative. We must cancel the minimum
    %        mean cycle. That is, we augment the graph along the negative
    %        cycle. We adjust the flow around the cycle by as much
    %        as possible without violating capacity constraints, so that
    %        the negative cycle no longer exists.

    %Determine the minimum residual capacity of the cycle.
    numVertexInCycle=length(cycleVertices);
    minCapacity=Inf;
    startVertex=cycleVertices(1);
    for curEndVertex=2:numVertexInCycle
        endVertex=cycleVertices(curEndVertex);

        curCapacity=residualCapacity(startVertex,endVertex);
        minCapacity=min(curCapacity,minCapacity);

        startVertex=endVertex;
    end

    %Push the amount of flow around the negative cycle that will saturate
    %the minimum capacity arc on the minimum mean cycle. Note that the skew
    %symmetry constraint dictates how the flow is to be added (how
    %augmentation is performed).
    startVertex=cycleVertices(1);
    for curEndVertex=2:numVertexInCycle
        endVertex=cycleVertices(curEndVertex);

        %Update the flow
        F(startVertex,endVertex)=F(startVertex,endVertex)+minCapacity;
        F(endVertex,startVertex)=F(endVertex,startVertex)-minCapacity;

        %Update the residual flow network. The update is opposite that of
        %F, because the residual flow is CMat-F.
        residualCapacity(startVertex,endVertex)=residualCapacity(startVertex,endVertex)-minCapacity;
        residualCapacity(endVertex,startVertex)=residualCapacity(endVertex,startVertex)+minCapacity;

        startVertex=endVertex;
    end
end

%Find the cost of the flow.
FP=F;
FP(F<0)=0;
totalCost=sum(sum(FP.*AMatOrig));

%If we are here, the maximum number of iterations has elapsed. We will
%check whether convergence occurred on the final iteration, and it not,
%then we will return accordingly.
costMat=Inf(numVertex,numVertex);
costMat(residualCapacity~=0)=AMat(residualCapacity~=0);

minCycleMean=findMinMaxCycleMean(costMat,true);
if(minCycleMean>=0)
    exitCode=0;
    return;
end

%If we get here, then the algorithm terminated without cancelling all
%negative cycles. Thus, the algorithm did not converge.
exitCode=2;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function b = isParallel(v1, v2, varargin)
%ISPARALLEL Check parallelism of two vectors.
%
%   B = isParallel(V1, V2)
%   where V1 and V2 are two row vectors of length ND, ND being the
%   dimension, returns 1 if the vectors are parallel, and 0 otherwise.
%
%   Also works when V1 and V2 are two N-by-ND arrays with same number of
%   rows. In this case, return a N-by-1 array containing 1 at the positions
%   of parallel vectors.
%
%   Also works when one of V1 or V2 is N-by-1 and the other one is N-by-ND
%   array, in this case return N-by-1 results.
%
%   B = isParallel(V1, V2, ACCURACY)
%   specifies the accuracy for numerical computation. Default value is
%   1e-14.
%
%
%   Example
%   isParallel([1 2], [2 4])
%   ans =
%       1
%   isParallel([1 2], [1 3])
%   ans =
%       0
%
%   See also
%   vectors2d, isPerpendicular, lines2d
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2006-04-25
% Copyright 2006-2022 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas)

% default accuracy
acc = 1e-14;
if ~isempty(varargin)
    acc = abs(varargin{1});
end

% normalize vectors
v1 = normalizeVector(v1);
v2 = normalizeVector(v2);

% adapt size of inputs if needed
n1 = size(v1, 1);
n2 = size(v2, 1);
if n1 ~= n2
    if n1 == 1
        v1 = v1(ones(n2,1), :);
    elseif n2 == 1
        v2 = v2(ones(n1,1), :);
    end
end

% performs computation
if size(v1, 2) == 2
    % computation for plane vectors
    b = abs(v1(:, 1) .* v2(:, 2) - v1(:, 2) .* v2(:, 1)) < acc;
else
    % computation in greater dimensions
    b = vectorNorm(cross(v1, v2, 2)) < acc;
end


function [im_sort,OA]=pixel_order(im)
% Assign strict ordering to image pixels.
%
%   INPUT:
%       - IM is a monochromatic or multispectral image.
%
%   OUPUT:
%       - IM_SORT is an array that has the same dimensions as IM. Its
%       element entries correspond to the order of the grey level pixel in
%       that position. Note that the channels of multispectral images are
%       processed speparately.
%       - OA is a number in the range [0,1] and idicates the fraction of
%       unique filter response combinations (ie. order accuracy). If OA=1
%       the ordering is strict. For multichannel images OA is a vector
%       whose elements are the OAs of the corresponding image channels.
%
%   REFERENCES:
%       1. Coltuc D. and Bolon P., 1999, "Strict ordering on discrete images
%       and applications"
%       2. Coltuc D., Bolon P. and Chassery J-M., 2006, "Exact histogram
%       specification", IEEE Transcations on Image Processing
%       15(5):1143-1152
%
%   AUTHOR: Anton Semechko (asemechk@uoguelph.ca)
%   DATE:   Dec.2009

if nargin<1 || nargin>1
    err='Incorrect number of input arguments';
    error(err)
end

% Image dimensions
M=size(im,1);
N=size(im,2);
P=size(im,3);

% Filters
F2=(1/5)*[0 1 0; 1 1 1; 0 1 0];
F3=(1/9)*ones(3,3);
F4=(1/13)*ones(5,5); F4([1 2 4 5 6 10 16 20 21 22 24 25])=0;
F5=(1/21)*ones(5,5); F5([1,5,21,25])=0;
F6=(1/25)*ones(5,5);
F={F2 F3 F4 F5 F6};

% Convolve filters with the image (one channel at a time) and order
%--------------------------------------------------------------------------
im_sort=[];
OA=[];

for i=1:P
    FR=double(im(:,:,1));
    for j=1:5
        FR_j=imfilter(double(im(:,:,1)),F{j});
        FR=cat(3,FR,FR_j);
        clear FR_j
    end
    im(:,:,1)=[]; % free up some memory

    % Rearange the filter responses
    FR=reshape(FR,M*N,6);

    % Number of unique filter responses and ordering accuracy
    n=size(unique(FR,'rows'),1);
    OA=[OA,n/(M*N)];

    % Sort responses lexicographically
    [FR,idx_pos]=sortrows(FR);
    clear FR

    % idx_pos is pixel position sorted according to ascending pixel order
    % now sort ordered pixels according to pixel position (linear index)
    [idx_pos,idx_o]=sort(idx_pos,'ascend');
    clear idx_pos
    idx_o=reshape(idx_o,M,N);

    im_sort=cat(3,im_sort,idx_o);
    clear idx_o
end


function [xi,w]=fifthOrderPyramidCubPoints()
%%FIFTHORDERPYRAMIDCUBPOINTS Generate fifth-order cubature points for
%  integration over a 3-dimensional pyramid with a square base with the
%  peak vertex at (0,0,1) and the base vertices at (1,-1,-1), (-1,-1,-1),
%  (-1,1,-1), and (1,1,-1).
%
%INPUTS: None
%
%OUTPUTS: xi This is a 3XnumCubPoints set of points for the standard
%            square pyramid.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the standard square pyramid (8/3).
%
%This function implements the points given in [1] (15 points).
%
%EXAMPLE:
%We compare a 5th-order moment computed using these cubature points
%to one computed using monomialIntPyramid. The results are the same within
%typical finite precision limits.
% [xi,w]=fifthOrderPyramidCubPoints();
% alpha=[2;2;1];
% theMoment=findMomentFromSamp(alpha,xi,w);
% intVal=monomialIntPyramid(alpha);
% RelErr=(theMoment-intVal)/intVal
%
%REFERENCES:
%[1] F. D. Witherden and P. E. Vincent, "On the identification of symmetric
%    quadrature rules for finite element methods," Computer and Mathematics
%    with Applications, vol. 69, no. 10, pp. 1232-1241, May 2015.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

M=[                                     0,                                          0,   0.45971576156501338586164265377920811314,   0.18249431975770692138374895897213800931;
                                        0,                                          0,  -0.39919795837246198593385139590712322914,   0.45172563864726406056400285032640105704;
                                        0,                                          0,  -0.99999998701645569241460017355590234925,   0.15654542887619877154120304977336547704;
 0.70652603154632457420722562974792066862,                                          0,                                      -0.75,   0.20384344839498724639142514342645843799;
                                        0,   0.70652603154632457420722562974792066862,                                      -0.75,   0.20384344839498724639142514342645843799;
-0.70652603154632457420722562974792066862,                                          0,                                      -0.75,   0.20384344839498724639142514342645843799;
                                        0,  -0.70652603154632457420722562974792066862,                                      -0.75,   0.20384344839498724639142514342645843799;
 0.70511712277882760181079385797948261057,   0.70511712277882760181079385797948261057,  -0.87777618587595407108464357252416911085,   0.10578907087905457654220386143818487109;
 0.70511712277882760181079385797948261057,  -0.70511712277882760181079385797948261057,  -0.87777618587595407108464357252416911085,   0.10578907087905457654220386143818487109;
-0.70511712277882760181079385797948261057,   0.70511712277882760181079385797948261057,  -0.87777618587595407108464357252416911085,   0.10578907087905457654220386143818487109;
-0.70511712277882760181079385797948261057,  -0.70511712277882760181079385797948261057,  -0.87777618587595407108464357252416911085,   0.10578907087905457654220386143818487109;
 0.43288286410354097685000790909815143591,   0.43288286410354097685000790909815143591,  -0.15279732576055038842025517341026975071,   0.15934280057233240536079894703404722173;
 0.43288286410354097685000790909815143591,  -0.43288286410354097685000790909815143591,  -0.15279732576055038842025517341026975071,   0.15934280057233240536079894703404722173;
-0.43288286410354097685000790909815143591,   0.43288286410354097685000790909815143591,  -0.15279732576055038842025517341026975071,   0.15934280057233240536079894703404722173;
-0.43288286410354097685000790909815143591,  -0.43288286410354097685000790909815143591,  -0.15279732576055038842025517341026975071,   0.15934280057233240536079894703404722173];

w=M(:,4);
xi=M(:,1:3)';

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function c = acorr(x,maxlag)
%ACORR Estimate autocorrelation function of time series
%
%   C = ACORR(X,MAXLAG) returns normalized autocorrelation
%   sequences for each column of X using
%   C(:,i)=XCORR(X(:,i)-MEAN(X(:,i)),MAXLAG,'coeff'), but returns only
%   lags 1:MAXLAG. Default MAXLAG = M-1;
%
%   See also
%     XCORR
%
% Copyright (C) 2000 Aki Vehtari

% This software is distributed under the GNU General Public
% Licence (version 3 or later); please refer to the file
% Licence.txt, included with the software, for details.

if nargin < 1
  error('Not enough input arguments.');
end
if nargin < 2
  maxlag=length(x)-1;
end
[m,n]=size(x);
c=zeros(maxlag,n);
for i1=1:n
  ct=xcorr(x(:,i1)-mean(x(:,i1)),maxlag,'coeff');
  ct=ct(maxlag+2:end);
  c(:,i1)=ct;
end


function V=covnw(data,nlag,demean)
% Long-run covariance estimation using Newey-West (Bartlett) weights
%
% USAGE:
%   [V] = covnw(DATA)
%   [V] = covnw(DATA,NLAG,DEMEAN)
%
% INPUTS:
%   DATA   - T by K vector of dependent data
%   NLAG   - Non-negative integer containing the lag length to use.  If empty or not included,
%              NLAG=min(floor(1.2*T^(1/3)),T) is used
%   DEMEAN - Logical true or false (0 or 1) indicating whether the mean should be subtracted when
%              computing the covariance
%
% OUTPUTS:
%   V      - A K by K covariance matrix estimated using Newey-West (Bartlett) weights
%
% COMMENTS:
%
% EXAMPLES:
%   Simulate an AR(1)
%       y = armaxfilter_simulate(1000,0,1,.9);
%   Newey-West covariance with automatic BW selection
%       lrcov = covnw(y)
%   Newey-West covariance with 10 lags
%       lrcov = covnw(y, 10)
%   Newey-West covariance with 10 lags and no demeaning
%       lrcov = covnw(y, 10, 0)
%
% See also COVVAR

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T=size(data,1);
if nargin==1
    nlag=min(floor(1.2*T^(1/3)),T);
    demean=true;
elseif nargin==2
    demean=true;
end
if isempty(nlag)
    nlag=min(floor(1.2*T^(1/3)),T);
end
if isempty(demean)
    demean=true;
end
if ~ismember(demean,[0 1])
    error('DEMEAN must be either logical true or false.')
end
if floor(nlag)~=nlag || nlag<0
    error('NLAG must be a non-negative integer.')
end
if ndims(data)>2
    error('DATA must be a T by K matrix of data.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if demean
    data=data-repmat(mean(data),T,1);
end

% NW weights
w=(nlag+1-(0:nlag))./(nlag+1);
% Start the covariance
V=data'*data/T;
for i=1:nlag
    Gammai=(data((i+1):T,:)'*data(1:T-i,:))/T;
    GplusGprime=Gammai+Gammai';
    V=V+w(i+1)*GplusGprime;
end


%% Optimization of a simple (Rosenbrock) function, with no constraints
% The unconstrained solution is at [1,1]
rosen = @(x) (1-x(1)).^2 + 105*(x(2)-x(1).^2).^2;

% With no constraints, operation simply passes through
% directly to fminsearch. The solution should be [1 1]
xsol = fminsearchbnd(rosen,[3 3])

%% Full lower and upper bound constraints which will all be inactive
xsol = fminsearchbnd(rosen,[3 3],[-1 -1],[4 4])

%% Only lower bound constraints
xsol = fminsearchbnd(rosen,[3 3],[2 2])

%% Only upper bound constraints
xsol = fminsearchbnd(rosen,[-5 -5],[],[0 0])

%% Dual constraints
xsol = fminsearchbnd(rosen,[2.5 2.5],[2 2],[3 3])

%% Dual constraints, with an infeasible starting guess
xsol = fminsearchbnd(rosen,[0 0],[2 2],[3 3])

%% Mixed constraints
xsol = fminsearchbnd(rosen,[0 0],[2 -inf],[inf 3])

%% Provide your own fminsearch options
opts = optimset('fminsearch');
opts.Display = 'iter';
opts.TolX = 1.e-12;

n = [10,5];
H = randn(n);
H=H'*H;
Quadraticfun = @(x) x*H*x';

% Global minimizer is at [0 0 0 0 0].
% Set all lower bound constraints, all of which will
% be active in this test.
LB = [.5 .5 .5 .5 .5];
xsol = fminsearchbnd(Quadraticfun,[1 2 3 4 5],LB,[],opts)

%% Exactly fix one variable, constrain some others, and set a tolerance
opts = optimset('fminsearch');
opts.TolFun = 1.e-12;

LB = [-inf 2 1 -10];
UB = [ inf  inf 1  inf];
xsol = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB,opts)

%% All the standard outputs from fminsearch are still returned
[xsol,fval,exitflag,output] = fminsearchbnd(@(x) norm(x),[1 3 1 1],LB,UB)


function d=v_distchpf(pf1,pf2,mode)
%V_DISTCHPF calculates the cosh spectral distance between power spectra D=(PF1,PF2,MODE)
%
% Inputs: PF1,PF2     Power spectra to be compared. Each row represents a power spectrum: the first
%                     and last columns represent the DC and Nyquist terms respectively.
%                     PF1 and PF2 must have the same number of columns.
%
%         MODE        Character string selecting the following options:
%                         'x'  Calculate the full distance matrix from every row of PF1 to every row of PF2
%                         'd'  Calculate only the distance between corresponding rows of PF1 and PF2
%                              The default is 'd' if PF1 and PF2 have the same number of rows otherwise 'x'.
%
% Output: D           If MODE='d' then D is a column vector with the same number of rows as the shorter of PF1 and PF2.
%                     If MODE='x' then D is a matrix with the same number of rows as PF1 and the same number of columns as PF2'.
%
% The COSH spectral distance is the average over +ve and -ve frequency of
%
%                     cosh(log(p1/p2))-1   =   (p1-p2)^2/(2p1*p2)   =   (p1/p2 + p2/p1)/2 - 1
%
% The COSH distance is a symmetrical version of the Itakura-Saito distance: v_distchpf(x,y)=(v_distispf(x,y)+v_distispf(y,x))/2

% The Cosh distance can also be calculated directly from AR coefficients; providing np is large
% enough, the values of d0 and d1 in the following will be very similar:
%
%         np=255; d0=v_distchar(ar1,ar2); d1=v_distchpf(v_lpcar2pf(ar1,np),v_lpcar2pf(ar2,np))
%

% Ref: A.H.Gray Jr and J.D.Markel, "Distance measures for speech processing", IEEE ASSP-24(5): 380-391, Oct 1976
%      L. Rabiner abd B-H Juang, "Fundamentals of Speech Recognition", Section 4.5, Prentice-Hall 1993, ISBN 0-13-015157-2

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

[nf1,p2]=size(pf1);
p1=p2-1;
nf2=size(pf2,1);
if nargin<3 | isempty(mode) mode='0'; end
if any(mode=='d') | (mode~='x' & nf1==nf2)
   nx=min(nf1,nf2);
   r=pf1(1:nx,:)./pf2(1:nx,:);
   q=r+r.^(-1);
   d=(2*sum(q(:,2:p1),2)+q(:,1)+q(:,p2))/(4*p1)-1;
else
   r=permute(pf1(:,:,ones(1,nf2)),[1 3 2])./permute(pf2(:,:,ones(1,nf1)),[3 1 2]);
   q=r+r.^(-1);
   d=(2*sum(q(:,:,2:p1),3)+q(:,:,1)+q(:,:,p2))/(4*p1)-1;
end


function h = p37_h ( n, x )

%*****************************************************************************80
%
%% P37_H evaluates the Hessian for problem 37.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  h = zeros ( n, n );

  arg = - ( x(1) - pi )^2 - ( x(2) - pi )^2;
  dargdx1 = - 2.0 * ( x(1) - pi );
  dargdx2 = - 2.0 * ( x(2) - pi );

  factor = cos ( x(2) ) * ( sin ( x(1) ) - cos ( x(1) ) * dargdx1 );
  dfdx1 = cos ( x(2) ) * ( cos ( x(1) ) + sin ( x(1) ) * dargdx1 + 2.0 * cos ( x(1) ) );
  dfdx2 = - sin ( x(2) ) * ( sin ( x(1) ) - cos ( x(1) ) * dargdx1 );

  h(1,1) = ( dfdx1 + factor * dargdx1 ) * exp ( arg );
  h(1,2) = ( dfdx2 + factor * dargdx2 ) * exp ( arg );

  factor = cos ( x(1) ) * ( sin ( x(2) ) - cos ( x(2) ) * dargdx2 );
  dfdx1 = - sin ( x(1) ) * ( sin ( x(2) ) - cos ( x(2) ) * dargdx2 );
  dfdx2 = cos ( x(1) ) * ( cos ( x(2) ) + sin ( x(2) ) * dargdx2 ...
    + 2.0 * cos ( x(2) ) );

  h(2,1) = ( dfdx1 + factor * dargdx1 ) * exp ( arg );
  h(2,2) = ( dfdx2 + factor * dargdx2 ) * exp ( arg );

  return
end


function value = p19_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P19_F evaluates the integrand for problem 19.
%
%  Dimension:
%
%    DIM_NUM is arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integral Parameters:
%
%    Z defaults to (1/3,1/3,...,1/3).
%    You can reset Z by calling P19_R8VEC.
%
%  Integrand:
%
%    f(x) = product ( sqrt ( abs ( x(1:dim_num) - z(1:dim_num) ) ) )
%
%  Exact Integral:
%
%    With Z as given,
%
%      (2/3)**DIM_NUM * ( (2/3)**(3/2) + (1/3)**(3/2) )**DIM_NUM
%
%    or approximately 0.49**DIM_NUM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arnold Krommer, Christoph Ueberhuber,
%    Numerical Integration on Advanced Systems,
%    Springer, 1994,
%    ISBN: 3540584102,
%    LC: QA299.3.K76.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  z = [];
  z = p19_r8vec ( 'G', 'Z', dim_num, z );

  value(1:point_num) = 0.0;

  for point = 1 : point_num
    value(point) = prod ( sqrt ( abs ( x(1:dim_num,point) - z(1:dim_num)' ) ) );
  end

  p19_i4 ( 'I', '#', point_num );

  return
end


function y = contract(x,i,j)
%CONTRACT Contract tensor along two dimensions (array trace).
%
%   Y = CONTRACT(X,I,J) contracts the entries of X along dimensions I
%   and J. Contraction is a generalization of matrix trace. In other
%   words, the trace is performed along the two-dimensional slices
%   defined by dimensions I and J. It is possible to implement tensor
%   multiplication as an outer product followed by a contraction.
%
%   Examples
%   X = tensor(rand(4,3,2)); Y = tensor(rand(3,2,4));
%   Z1 = ttt(X,Y,1,3); %<-- Normal tensor multiplication
%   Z2 = contract(ttt(X,Y),1,6); %<-- Outer product + contract
%   norm(Z1-Z2) %<-- Should be zero
%
%   See also TENSOR, TENSOR/TTT.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

% Error checking
if x.size(i) ~= x.size(j)
    error('Must contract along equally sized dimensions');
end

% Error checking
if i == j
    error('Must contract along two different dimensions');
end

% Easy case - returns a scalar
if ndims(x) == 2
    y = trace(x.data);
    return;
end

% Remaining dimensions after trace
remdims = setdiff(1:ndims(x),[i j]);

% Size for y
newsize = x.size(remdims);

% Total size of remainder
m =  prod(newsize);

% Number of items to add for trace
n = x.size(i);

% Permute trace dimensions to the end
x = permute(x, [remdims i j]);

% Reshape data to be 3D
data = reshape(x.data, m, n, n);

% Add diagonal entries for each slice
newdata = zeros(m,1);
for i = 1:n
    newdata = newdata + data(:,i,i);
end

% Reshape result
if numel(newsize) > 1
    newdata = reshape(newdata,newsize);
end
y = tensor(newdata,newsize);


function value = p07_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P07_F evaluates the integrand for problem 07.
%
%  Dimension:
%
%    N arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integrand:
%
%    product ( pi / 2 ) * sin ( pi * x(1:dim_num) )
%
%  Exact Integral:
%
%    1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  value(1:point_num) = 0.0;

  for point = 1 : point_num
    value(point) = prod ( 0.5 * pi * sin ( pi * x(1:dim_num,point) ) );
  end

  p07_i4 ( 'I', '#', point_num );

  return
end


function [tSeries, trends] = detrendTSeries(tSeries,detrendOption,smoothFrames)
%
% [detrendedTSeries,  trends] = detrendTSeries(tSeries,detrendOption,[smoothFrames])
%
% detrendOption is one of the following:
%   0 no trend removal
%   1 highpass trend removal
%   2 quadratic removal
%   -1 linear trend removal
% default determined by calling 'detrendFlag' that uses blockedAnalysisParams.detrend
%
% smoothFrames only needed for detrendOption==1
%
% djh, 2/2001
% jw, 7/2012: For linear and quadratic detrending, set model to have a
%             maximum value of 1 to avoid rank deficient matrices when
%             calculating
%                   wgts = model\tSeries;

%disp('Detrending tSeries...');

% reshape into matrix if tSeries is 3D array
dims = size(tSeries);
nFrames = dims(1);
tSeries = reshape(tSeries, nFrames, []);

switch detrendOption
case 2
    % remove a quadratic function

    model = [(1:nFrames).*(1:nFrames);(1:nFrames);ones(1,nFrames)]';

    % Limit range of model to [0 1] to avoid the possibility of rank
    % deficiency in calculating model \ tSeries
    model = bsxfun(@rdivide, model, max(model));
    wgts = model\tSeries;

    trends = model*wgts;
    tSeries = tSeries - trends;

case -1
    % remove a linear function
    model = [(1:nFrames);ones(1,nFrames)]';
    model = bsxfun(@rdivide, model, max(model));

    wgts = model\tSeries;

    trends = model*wgts;
    tSeries = tSeries - trends;

case 1
    % Do high-pass baseline removal
    calcstep = 1e4;
    if size(tSeries,2) <= calcstep;
        [tSeries, trends] = removeBaseline2(tSeries, smoothFrames);
    else % solve the out of memory problem
        trends = NaN(size(tSeries));
        for ii = 1:calcstep:size(tSeries,2);
            curRange = ii:min(ii+calcstep-1,size(tSeries,2));
            [tSeries(:,curRange), trends(:,curRange)] = ...
                removeBaseline2(tSeries(:,curRange), smoothFrames);
        end
    end
otherwise
    % Do nothing
    trends = zeros(size(tSeries));

end

% reshape tSeries in case we changed it from 3D array to matrix
tSeries = reshape(tSeries, dims);
trends    = reshape(trends, dims);

return

%% DEBUG

% seed random stream
s = RandStream('mt19937ar','Seed',1);
RandStream.setGlobalStream(s);

% generate a time series
ts = single(smooth(randn(300,1), 5));

% detrend three ways
[~, fit1] = detrendTSeries(ts,-1); % linear
[~, fit2] = detrendTSeries(ts, 2); % quadratic
[~, fit3] = detrendTSeries(ts, 1, 20); % high pass

% plot
figure(101)
clf

plot(1:length(ts), ts, 'k-');
hold on
plot(1:length(ts), fit1, 'r-', 'LineWidth', 2);
plot(1:length(ts), fit2, 'g-', 'LineWidth', 2);
plot(1:length(ts), fit3, 'b-', 'LineWidth', 2);

legend({'linear', 'quadratic', 'highpass'}, 'Location', 'Best')

% Compare quadratic detrend using old vs new method

% old:
n = length(ts);
model = [(1:n).*(1:n);(1:n);ones(1,n)]';
wgts = model\ts;
trends = model*wgts;

% new (divide each column of model by its max)
n = length(ts);
model = [(1:n).*(1:n);(1:n);ones(1,n)]';
model = bsxfun(@rdivide, model, max(model));
wgts = model\ts;
fit2 = model*wgts;

figure(102);
plot(1:n, trends, 'r', 1:n, fit2, 'k')


function [Q, R] = gs_c(A)
%GS_C    Classical Gram-Schmidt QR factorization.
%        [Q, R] = GS_C(A) uses the classical Gram-Schmidt method to compute the
%        factorization A = Q*R for m-by-n A of full rank,
%        where Q is m-by-n with orthonormal columns and R is n-by-n.

%        Reference:
%        N. J. Higham, Accuracy and Stability of Numerical Algorithms,
%        Second edition, Society for Industrial and Applied Mathematics,
%        Philadelphia, PA, 2002; sec 19.8.

[m, n] = size(A);
Q = zeros(m,n);
R = zeros(n);

for j=1:n
    R(1:j-1,j) = Q(:,1:j-1)'*A(:,j);
    temp = A(:,j) - Q(:,1:j-1)*R(1:j-1,j);
    R(j,j) = norm(temp);
    Q(:,j) = temp/R(j,j);
end


% EX_LAPLACE_SQUARE: solve the Poisson problem in the unit square with a B-spline discretization.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_square.txt';

% Type of boundary conditions for each side of the domain
problem_data.nmnn_sides   = [];
problem_data.drchlt_sides = [1 2 3 4];

% Physical parameters
problem_data.c_diff  = @(x, y) ones(size(x));

% Source and boundary terms
problem_data.f = @(x, y) zeros (size (x));
problem_data.g = @test_square_g_nmnn;
problem_data.h = @(x, y, ind) exp (x) .* sin(y);

% Exact solution (optional)
problem_data.uex     = @(x, y) exp (x) .* sin (y);
problem_data.graduex = @(x, y) cat (1, ...
                       reshape (exp(x).*sin(y), [1, size(x)]), ...
                       reshape (exp(x).*cos(y), [1, size(x)]));

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.degree     = [3 3];       % Degree of the splines
method_data.regularity = [2 2];       % Regularity of the splines
method_data.nsub       = [9 9];       % Number of subdivisions
method_data.nquad      = [4 4];       % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER

[geometry, msh, space, u] = solve_laplace (problem_data, method_data);

% 4) POST-PROCESSING
% 4.1) EXPORT TO PARAVIEW

output_file = 'Square_BSP_Deg3_Reg2_Sub9';

vtk_pts = {linspace(0, 1, 20), linspace(0, 1, 20)};
fprintf ('The result is saved in the file %s \n \n', output_file);
sp_to_vtk (u, space, geometry, vtk_pts, output_file, 'u')

% 4.2) PLOT IN MATLAB. COMPARISON WITH THE EXACT SOLUTION

[eu, F] = sp_eval (u, space, geometry, vtk_pts);
[X, Y]  = deal (squeeze(F(1,:,:)), squeeze(F(2,:,:)));
subplot (1,2,1)
surf (X, Y, eu)
title ('Numerical solution'), axis tight
subplot (1,2,2)
surf (X, Y, problem_data.uex (X,Y))
title ('Exact solution'), axis tight

% Display errors of the computed solution in the L2 and H1 norm
[error_h1, error_l2] = ...
           sp_h1_error (space, msh, u, problem_data.uex, problem_data.graduex)

%!demo
%! ex_laplace_square

%!test
%! problem_data.geo_name = 'geo_square.txt';
%! problem_data.nmnn_sides   = [];
%! problem_data.drchlt_sides = [1 2 3 4];
%! problem_data.c_diff  = @(x, y) ones(size(x));
%! problem_data.f = @(x, y) zeros (size (x));
%! problem_data.g = @test_square_g_nmnn;
%! problem_data.h = @(x, y, ind) exp (x) .* sin(y);
%! problem_data.uex     = @(x, y) exp (x) .* sin (y);
%! problem_data.graduex = @(x, y) cat (1, ...
%!                       reshape (exp(x).*sin(y), [1, size(x)]), ...
%!                       reshape (exp(x).*cos(y), [1, size(x)]));
%! method_data.degree     = [3 3];       % Degree of the splines
%! method_data.regularity = [2 2];       % Regularity of the splines
%! method_data.nsub       = [9 9];       % Number of subdivisions
%! method_data.nquad      = [4 4];       % Points for the Gaussian quadrature rule
%! [geometry, msh, space, u] = solve_laplace (problem_data, method_data);
%! [error_h1, error_l2] = ...
%!           sp_h1_error (space, msh, u, problem_data.uex, problem_data.graduex);
%! assert (msh.nel, 81)
%! assert (space.ndof, 144)
%! assert (error_h1, 9.86428525677199e-06, 1e-14)
%! assert (error_l2, 1.68004134750130e-07, 1e-14)


function [q, M] = conversionMatrix2Quaternion(M, varargin)
% conversionMatrix2Quaternion - function to convert Rotation Matrix to Quaternion coeff
%
% Syntax:  q = conversionMatrix2Quaternion(M(3,3))
%          q = conversionMatrix2Quaternion(M(1,1),M(1,2),M(1,3),M(2,1),M(2,2),M(2,3),M(3,1),M(3,2),M(3,3))
%          q = conversionMatrix2Quaternion([theta, psi, phi])
%          q = conversionMatrix2Quaternion(theta, psi, phi)
%
% Inputs:
%          M - Rotation Matrix (3,3)
%     *theta - X rotation angle (deg)
%       *psi - Y rotation angle (deg)
%       *phi - Z rotation angle (deg)
%
% Outputs:
%     q - quaternion values [q0 qx qy qz]
%     M - Rotation Matrix used to compute q
%
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: none;

% Author: Marco Borges, Ph.D. Student, Computer/Biomedical Engineer
% UFMG, PPGEE, Neurodinamica Lab, Brazil
% email address: marcoafborges@gmail.com
% Website: http://www.cpdee.ufmg.br/
% Reference: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
% April 2013; v2; Last revision: 2013-04-18
% Changelog: v2 - add linear velocity extraction

%------------- BEGIN CODE --------------
% M = [m00 m01 m02;
%     m10 m11 m12;
%     m20 m21 m22];

if length(M) == 1 && nargin == 8
    M = [M, varargin{1}, varargin{2}; varargin{3}, varargin{4}, varargin{5}; varargin{6}, varargin{7}, varargin{8}];

elseif length(M) == 9
    M = [M(1), M(2), M(3); M(4), M(5), M(6); M(7), M(8), M(9)];

elseif length(M) == 3 % [theta (x rot), psi (y rot), phi (z rot)]
    theta = M(1); psi = M(2); phi = M(3);
    M = [cos(deg2rad(psi))*cos(deg2rad(phi)), cos(deg2rad(psi))*sin(deg2rad(phi)), -sin(deg2rad(psi));
         (-cos(deg2rad(theta))*sin(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (cos(deg2rad(theta))*cos(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), sin(deg2rad(theta))*cos(deg2rad(psi));
         (sin(deg2rad(theta))*sin(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (-sin(deg2rad(theta))*cos(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), cos(deg2rad(theta))*cos(deg2rad(psi))];

elseif length(M) == 1 && nargin == 3 % (theta (x rot), psi (y rot), phi (z rot))
    theta = M; psi = varargin{1}; phi = varargin{2};
    M = [cos(deg2rad(psi))*cos(deg2rad(phi)), cos(deg2rad(psi))*sin(deg2rad(phi)), -sin(deg2rad(psi));
         (-cos(deg2rad(theta))*sin(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (cos(deg2rad(theta))*cos(deg2rad(phi)))+(sin(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), sin(deg2rad(theta))*cos(deg2rad(psi));
         (sin(deg2rad(theta))*sin(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*cos(deg2rad(phi))), (-sin(deg2rad(theta))*cos(deg2rad(phi)))+(cos(deg2rad(theta))*sin(deg2rad(psi))*sin(deg2rad(phi))), cos(deg2rad(theta))*cos(deg2rad(psi))];
end

tr = M(1,1) + M(2,2) + M(3,3);

if (tr > 0)
    S = sqrt(tr+1.0) * 2; % S=4*qw
    qw = 0.25 * S;
    qx = (M(3,2) - M(2,3)) / S;
    qy = (M(1,3) - M(3,1)) / S;
    qz = (M(2,1) - M(1,2)) / S;
elseif ((M(1,1) > M(2,2)) && (M(1,1) > M(3,3)))
    S = sqrt(1.0 + M(1,1) - M(2,2) - M(3,3)) * 2; % S=4*qx
    qw = (M(3,2) - M(2,3)) / S;
    qx = 0.25 * S;
    qy = (M(1,2) + M(2,1)) / S;
    qz = (M(1,3) + M(3,1)) / S;
elseif (M(2,2) > M(3,3))
    S = sqrt(1.0 + M(2,2) - M(1,1) - M(3,3)) * 2; % S=4*qy
    qw = (M(1,3) - M(3,1)) / S;
    qx = (M(1,2) + M(2,1)) / S;
    qy = 0.25 * S;
    qz = (M(2,3) + M(3,2)) / S;
else
    S = sqrt(1.0 + M(3,3) - M(1,1) - M(2,2)) * 2; % S=4*qz
    qw = (M(2,1) - M(1,2)) / S;
    qx = (M(1,3) + M(3,1)) / S;
    qy = (M(2,3) + M(3,2)) / S;
    qz = 0.25 * S;
end

q = [qw qx qy qz];
%-------------- END CODE ---------------


function [y] = spm_phase_shuffle(x,n)
% phase-shuffling of a vector
% FORMAT [y] = spm_phase_shuffle(x,[n])
% x   - data matrix (time-series in columns)
% n   - optional window length for windowed shuffling
%__________________________________________________________________________
% Copyright (C) 2007-2015 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_phase_shuffle.m 6654 2015-12-22 12:55:36Z spm $

try

    % randomise phase - WFT
    %----------------------------------------------------------------------
    k     = 1:fix(n/2);
    for i = 1:size(x,2);
        C      = spm_wft(x(:,i),k,n);
        W      = abs(C).*exp(1i*angle(C(randperm(size(C,1)),:)));
        y(:,i) = spm_iwft(W,k,n)';
    end

catch

    % randomise phase - FT
    %----------------------------------------------------------------------
    n              = size(x,1);
    s              = fft(x);
    i              = 2:ceil(n/2);
    r              = rand(length(i),size(x,2))*2*pi - pi;
    p              = zeros(n,size(x,2));
    p(i,:)         =  r;
    p(n - i + 2,:) = -r;
    s              = abs(s).*exp(1i*p);
    y              = real(ifft(s));

end


function x = sindeg(degrees)
%sin of angle in degrees.
%JOD

rad = (degrees/180) * pi;

x = sin(rad);


function theta = polygon3dNormalAngle(points, ind)
%POLYGON3DNORMALANGLE Normal angle at a vertex of the 3D polygon
%
%   THETA = polygon3DNormalAngle(POLYGON, IND)
%   where POLYGON is a set of points, and IND is index of a point in
%   polygon. The function compute the angle of the normal cone localized at
%   this vertex.
%   If IND is a vector of indices, normal angle is computed for each vertex
%   specified by IND.
%
%   Example
%   % create an equilateral triangle in space
%   poly3d = [1 1 0;-1 0 1;0 -1 -1];
%   % compute each normal angle
%   theta = polygon3dNormalAngle(poly3d, 1:size(poly3d, 1));
%   % sum of normal angles must be equal to 2*PI for simple polygons
%   sum(theta)
%
%   IMPORTANT NOTE: works only for convex angles ! ! ! !
%
%   See also
%   polygons3d, faceNormalAngle
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2005-11-30
% Copyright 2005 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

% number of points
np = size(points, 1);

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

theta = zeros(nv, 1);

for i=1:nv
    p0 = points(ind(i), :);

    if ind(i)==1
        p1 = points(np, :);
    else
        p1 = points(ind(i)-1, :);
    end

    if ind(i)==np
        p2 = points(1, :);
    else
        p2 = points(ind(i)+1, :);
    end

    theta(i) = pi - anglePoints3d(p1, p0, p2);
end


function A=polyarea_signed(V)

% function A=polyarea_signed(V)
%-------------------------------------------------------------------------
%
%
%
% Background (https://demonstrations.wolfram.com/SignedAreaOfAPolygon/):
% The formula for the area of a simple polygon can be elegantly derived
% using Green's theorem and extended to moments of the region. S. F.
% Bockman, "Generalizing the Formula for Areas of Polygons to Moments,"
% Amer. Math. Monthly, 96(2), 1989 pp. 131-132.
%
%-------------------------------------------------------------------------
%%

E=[(1:size(V,1))' [(2:size(V,1))'; 1]]; %Edge array
X=V(:,1); %X coordinates
Y=V(:,2); %Y coordinates
XE=X(E);  %X coordinates of edge points
YE=Y(E); %Y coordinates of edge points
A=sum(0.5*((XE(:,1).*YE(:,2))-(XE(:,2).*YE(:,1)))); %Signed area

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function H = circlePlane3D( center, normal, radious, theintv, normalon, color, style )
%CIRCLEPLANE3D Summary of this function goes here
%--------------------------------------------------------------------------
%Generate a circle plane in 3D with the given center and radious
%The plane is defined by the normal vector
%theintv is the interval theta which allow you to control your polygon
%shape
% Example:,
%
%   circlePlane3D([0 0 0], [1 -1 2], 5, 0.2, 1, [0 0 1], '-');
%   circlePlane3D([3 3 -3],[0 1 1], 3, 0.1, 1, 'y', '-');
%
%   Cheng-Yuan Wu <ieda_wind@hotmail.com>
%   Version 1.00
%   Aug, 2012
%--------------------------------------------------------------------------
%generate circle polygon
t = 0:theintv:2*pi;
x = radious*cos(t);
y = radious*sin(t);
z = zeros(size(x));
%compute rotate theta and axis
zaxis = [0 0 1];
normal = normal/norm(normal);
ang = acos(dot(zaxis,normal));
axis = cross(zaxis, normal)/norm(cross(zaxis, normal));
% A skew symmetric representation of the normalized axis
axis_skewed = [ 0 -axis(3) axis(2) ; axis(3) 0 -axis(1) ; -axis(2) axis(1) 0];
% Rodrigues formula for the rotation matrix
R = eye(3) + sin(ang)*axis_skewed + (1-cos(ang))*axis_skewed*axis_skewed;
fx = R(1,1)*x + R(1,2)*y + R(1,3)*z;
fy = R(2,1)*x + R(2,2)*y + R(2,3)*z;
fz = R(3,1)*x + R(3,2)*y + R(3,3)*z;
%translate center
fx = fx+center(1);
fy = fy+center(2);
fz = fz+center(3);
H = fill3(fx, fy, fz, color);
if normalon == 1
    hold on;
    H = plot3([center(1) center(1)+normal(1)],[center(2) center(2)+normal(2)],[center(3) center(3)+normal(3)],style);
end

end


%MAIN_simpleHarmonicOscillator.m
%
% This script runs a simulation of a simple harmonic oscillator

% Use: EoM_Single_Pendulum to write the equations of motion

m = 1.0;  % (kg)  mass
k = 1.0;  % (N/m) spring constant

tSpan = [0,10]; %Simulation time interval

x0 = 0.5;  % (m) initial position
v0 = 0;  % (m/s) initial velocit
z0 = [x0;v0];

userFunc = @(t,z)simpleHarmonicOscillatorDynamics(t,z,m,k);

options = odeset(...
    'AbsTol',1e-8,...
    'RelTol',1e-8,...
    'Vectorized','on');

% Run the simulation!
sol = ode45(userFunc,tSpan,z0,options);

% Break apart solution for plotting
nPlot = 1000;
time = linspace(tSpan(1),tSpan(2),nPlot);
z = deval(sol,time); %Evaluate solution from ode45 at points in time
x = z(1,:);
v = z(2,:);

% Plotting!

figure(111);clf;

subplot(2,1,1);
plot(time,x,'k-','LineWidth',2);
xlabel('time (s)')
ylabel('position (m)');

subplot(2,1,2);
plot(time,v,'k-','LineWidth',2);
xlabel('time (s)')
ylabel('velocity (m/s)');


function x = r8mat_lt_solve ( n, a, b )

%*****************************************************************************80
%
%% R8MAT_LT_SOLVE solves a transposed lower triangular linear system.
%
%  Discussion:
%
%    Given the lower triangular matrix A, the linear system to be solved is:
%
%      A' * x = b
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of rows and columns of the matrix A.
%
%    Input, real A(N,N), the N by N lower triangular matrix.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Output, real X(N), the solution of the linear system.
%
  x(1:n) = 0.0;
%
%  Solve L'*x = b.
%
  for i = n : -1 : 1
    x(i) = ( b(i) - x(i+1:n) * a(i+1:n,i) ) / a(i,i);
  end

  return
end


function Trans = PUMA_TransMatrices(alpha, a, d, theta)
% This creates the Robot Manipulator Link(s) Transformation Matrix.
% The entries of the DH Table are passed in as the parameter.

%% Handle errors (if any) for the parameters passed into the function

% Check for the dimensions of the parameters passed
[~, c(1)] = size(alpha);
[~, c(2)] = size(a);
[~, c(3)] = size(d);
[~, c(4)] = size(theta);

if (c(1) ~= c(2) || c(1) ~= c(3) || c(1) ~= c(4))
    error('Invalid arguments. Size of all the arguments should be same')
    return
end

%% Generate the Transformation Matrices from the DH-parameters passed

% Find the Transformation matrix from the first row of DH-Table
% This will give us the position and orientation of the end of first link
% of the Robot Manipulator
Trans(:,:,1) = PUMA_Transformation(alpha(1), a(1), d(1), theta(1));

% Find the Transformation matrix from the rest of the rows of DH-Table
% This will give us the position and orientation of the rest of the links
% of the Robot Manipulator
for i = 2:c(1)
    Trans(:,:,i)= PUMA_Transformation(alpha(i), a(i), d(i), theta(i), Trans(:,:, i-1));
end

end


function mu = moebius ( n )

%*****************************************************************************80
%
%% MOEBIUS returns the value of MU(N), the Moebius function of N.
%
%  Definition:
%
%    MU(N) is defined as follows:
%
%      MU(N) = 1 if N = 1;
%              0 if N is divisible by the square of a prime;
%              (-1)^K, if N is the product of K distinct primes.
%
%  First values:
%
%     N  MU(N)
%
%     1    1
%     2   -1
%     3   -1
%     4    0
%     5   -1
%     6    1
%     7   -1
%     8    0
%     9    0
%    10    1
%    11   -1
%    12    0
%    13   -1
%    14    1
%    15    1
%    16    0
%    17   -1
%    18    0
%    19   -1
%    20    0
%
%    As special cases, MU(N) is -1 if N is a prime, and MU(N) is 0
%    if N is a square, cube, etc.
%
%    The Moebius function is related to Euler's totient function:
%
%      PHI(N) = Sum ( D divides N ) MU(D) * ( N / D ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the value to be analyzed.
%
%    Output, integer MU, the value of MU(N).
%    If N is less than or equal to 0, MU will be returned as -2.
%    If there was not enough internal space for factoring, MU
%    is returned as -3.
%
  if ( n <= 0 )
    mu = -2;
    return
  end

  if ( n == 1 )
    mu = 1;
    return
  end
%
%  Factor N.
%
  [ nfactor, factor, power, nleft ] = i4_factor ( n );

  if ( nleft ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MOEBIUS - Fatal error!\n' );
    fprintf ( 1, '  Not enough factorization space.\n' );
    mu = -3;
    return
  end

  mu = 1;

  for i = 1 : nfactor

    mu = - mu;

    if ( 1 < power(i) )
      mu = 0;
      return
    end

  end

  return
end


function d = amsr(im,sigma1,sigma2,sigma3)

if ~exist('sigma1','var'),sigma1 = 15;end
if ~exist('sigma2','var'),sigma2 = 80;end
if ~exist('sigma3','var'),sigma3 = 250;end

im = im2double(im); % double --> im2double
gausKernel1 = fspecial('gaussian',[sigma1 sigma1],5);
gausKernel2 = fspecial('gaussian',[sigma2 sigma2],20);
gausKernel3 = fspecial('gaussian',[sigma3 sigma3],50);
Y = 0.299.*im(:,:,1)+0.587.*im(:,:,2)+0.114.*im(:,:,3);

blur_im1 = (imfilter(Y,gausKernel1,'replicate'));
blur_im2 = (imfilter(Y,gausKernel2,'replicate'));
blur_im3 = (imfilter(Y,gausKernel3,'replicate'));

Y_ssr1 = log(Y) - log(blur_im1);
Y_ssr2 = log(Y) - log(blur_im2);
Y_ssr3 = log(Y) - log(blur_im3);

sr = sort(Y_ssr1(:));
p1 = sr(uint32(0.01*size(im,1)*size(im,2)));
p99 = sr(uint32(0.99*size(im,1)*size(im,2)));
lc1 = Y_ssr1 >= p1 & Y_ssr1 <= p99;
lc2 = Y_ssr1 < p1;
lc3 = Y_ssr1 > p99;
Y_ssr1(lc1) = 255 * (Y_ssr1(lc1)-p1)./(p99-p1);
Y_ssr1(lc2) = 0;
Y_ssr1(lc3) = 255;

sr = sort(Y_ssr2(:));
p1 = sr(uint32(0.01*size(im,1)*size(im,2)));
p99 = sr(uint32(0.99*size(im,1)*size(im,2)));
lc1 = Y_ssr2 >= p1 & Y_ssr2 <= p99;
lc2 = Y_ssr2 < p1;
lc3 = Y_ssr2 > p99;
Y_ssr2(lc1) = 255 * (Y_ssr2(lc1)-p1)./(p99-p1);
Y_ssr2(lc2) = 0;
Y_ssr2(lc3) = 255;

sr = sort(Y_ssr3(:));
p1 = sr(uint32(0.01*size(im,1)*size(im,2)));
p99 = sr(uint32(0.99*size(im,1)*size(im,2)));
lc1 = Y_ssr3 >= p1 & Y_ssr3 <= p99;
lc2 = Y_ssr3 < p1;
lc3 = Y_ssr3 > p99;
Y_ssr3(lc1) = 255 * (Y_ssr3(lc1)-p1)./(p99-p1);
Y_ssr3(lc2) = 0;
Y_ssr3(lc3) = 255;

% Y = 0.299.*im(:,:,1)+0.587.*im(:,:,2)+0.114.*im(:,:,3);
% Y_ssr1 = 0.299.*R_ssr1(:,:,1)+0.587.*R_ssr1(:,:,2)+0.114.*R_ssr1(:,:,3);
% Y_ssr2 = 0.299.*R_ssr2(:,:,1)+0.587.*R_ssr2(:,:,2)+0.114.*R_ssr2(:,:,3);
% Y_ssr3 = 0.299.*R_ssr3(:,:,1)+0.587.*R_ssr3(:,:,2)+0.114.*R_ssr3(:,:,3);
sigma = 32;
mu0 = 32;
mu1 = 96;
mu2 = 160;
mu3 = 224;
P0 = exp(-(Y-mu0).^2./(2*sigma^2));
P1 = exp(-(Y-mu1).^2./(2*sigma^2));
P2 = exp(-(Y-mu2).^2./(2*sigma^2));
P3 = max(P0,exp(-(Y-mu3).^2./(2*sigma^2)));
psum = P0+P1+P2+P3;
omega0 = P0./psum;
omega1 = P1./psum;
omega2 = P2./psum;
omega3 = P3./psum;
Y_amsr = omega0.*Y + omega1.*Y_ssr1 + omega2.*Y_ssr2 + omega3.*Y_ssr3;
ratio = Y_amsr./Y;

% modified: uint8 --> im2uint8
hsv = rgb2hsv(im2uint8(im));
v = hsv(:,:,3);%.*255;
v = 0.5.*(ratio.*(v+Y)+v-Y);
hsv(:,:,3) = v./255;
%d = uint8(hsv2rgb(hsv).*255);
d = double(hsv2rgb(hsv)); % modified
d = min(max(d,0),1); % add

% r = 0.5.*(ratio.*(im(:,:,1)+Y)+im(:,:,1)-Y);
% g = 0.5.*(ratio.*(im(:,:,2)+Y)+im(:,:,2)-Y);
% b = 0.5.*(ratio.*(im(:,:,3)+Y)+im(:,:,3)-Y);
% d = cat(3,r,g,b)./255;
end


% function vectorized_bspline_coeff
% ------
%
% Input
%  vi [n x m]
%  vs: [n x m]
%
%  see Kristin Branson's "A Practical Review of Uniform B-splines"
%
% Output
%   C [n x 1]: the coefficients
function C = vectorized_bspline_coeff(vi,vs)

    assert(isequal(size(vi),size(vs)));

    % Go through conditions
    C = zeros(size(vi));

    sel1 = vs >= vi & vs < vi+1;
    C(sel1) = (1/6)*(vs(sel1)-vi(sel1)).^3;

    sel2 = vs >= vi+1 & vs < vi+2;
    C(sel2) = (1/6)*(-3*(vs(sel2)-vi(sel2)-1).^3 + 3*(vs(sel2)-vi(sel2)-1).^2 + 3*(vs(sel2)-vi(sel2)-1)+1);

    sel3 = vs >= vi+2 & vs < vi+3;
    C(sel3) = (1/6)*(3*(vs(sel3)-vi(sel3)-2).^3 - 6*(vs(sel3)-vi(sel3)-2).^2 + 4);

    sel4 = vs >= vi+3 & vs < vi+4;
    C(sel4) = (1/6)*(1-(vs(sel4)-vi(sel4)-3)).^3;

end


function [ar,xi,kappa,ehat] = lpcana(x,M)
%  lpcana --> Linear prediction analysis.
%
%    [ar,xi,kappa,ehat] = lpcana(x,M)
%
%    The function performs autocorrelation based LP analysis on the
%    signal vector x using the Levinson-Durbin recursion. Thus, the
%    function finds the coefficients, ar=[1 -a(1) ... -a(M)], of an
%    M'th order forward linear predictor
%
%      xhat(n) = a(1)*x(n-1) + a(2)*x(n-2) + ... + a(M)*x(n-M)
%
%    such that the sum of the squares of the prediction errors
%
%      ehat(n) = x(n) - xhat(n)
%
%    is minimized. The reflection coefficients are returned in the
%    vector kappa, and the prediction error energies for the 0'th to
%    the M'th order solution are returned in the vector xi. Finally,
%    the residual signal, ehat, is obtained by applying the inverse
%    filter A(z) to the signal frame.

% Short-term autocorrelation.
[rx,eta] = xcorr(x,M,'biased');

% LP analysis based on Levinson-Durbin recursion.
[a,xi,kappa] = durbin(rx(M+1:2*M+1),M);
ar = [1; -a];

% Prediction error signal obtained by inverse filtering.
ehat = filter(ar,1,x);


function Q = M2Q(M)
% Convert from rotation matrix to quaternion form
% See: http://skal.planet-d.net/demo/matrixfaq.htm
%__________________________________________________________________________
% Copyright (C) 2005-2017 Wellcome Trust Centre for Neuroimaging

%
% $Id: M2Q.m 7147 2017-08-03 14:07:01Z spm $

d = diag(M(1:3,1:3));
t = sum(d) + 1;
if t>0.5
    s = sqrt(t)*2;
    Q = [(M(3,2)-M(2,3))/s (M(1,3)-M(3,1))/s (M(2,1)-M(1,2))/s 0.25*s]';
else
    t = find(d==max(d));
    t = t(1);
    switch(t)
    case 1
        s = 2*sqrt(1 + M(1,1) - M(2,2) - M(3,3));
        Q = [0.25*s (M(1,2)+M(2,1))/s (M(3,1)+M(1,3))/s (M(3,2)-M(2,3))/s]';
    case 2
        s = 2*sqrt(1 + M(2,2) - M(1,1) - M(3,3));
        Q = [(M(1,2)+M(2,1))/s 0.25*s (M(2,3)+M(3,2))/s (M(1,3)-M(3,1))/s ]';
    case 3
        s = 2*sqrt(1 + M(3,3) - M(1,1) - M(2,2));
        Q = [(M(3,1)+M(1,3))/s (M(2,3)+M(3,2))/s 0.25*s (M(2,1)-M(1,2))/s]';
    end
end
if Q(4)<0, Q = -Q; end % w must be +ve


function varargout = drawTorus(varargin)
%DRAWTORUS Draw a torus (3D ring).
%
%   drawTorus(TORUS)
%   Draws the torus on the current axis. TORUS is given by:
%   [XC YC ZY  R1 R2  THETA PHI]
%   where (XC YZ ZC) is the center of the torus, R1 is the main radius, R2
%   is the radius of the torus section, and (THETA PHI) is the angle of the
%   torus normal vector (both in degrees).
%
%   drawTorus(..., PNAME, PVALUE)
%   Specifies a set of parameter name-value pairs. Parameter names include
%   plitting options ('facecolor', 'linestyle'...), or options specific to
%   torus:
%   'nPhi'      number of meridians used to draw the torus (default is 60).
%   'nTheta'    number of parallels used to draw the torus (default is 60).
%
%
%   Example
%     % draw sample torus
%     figure;
%     drawTorus([50 50 50 30 10 30 45]);
%     axis equal; view([95 10]); light;
%
%   See also
%   drawEllipsoid, revolutionSurface, torusMesh
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2011-06-22, using Matlab 7.9.0.529 (R2009b)
% Copyright 2011-2022 INRA - Cepia Software Platform

%% Default values

% number of meridians
nPhi    = 60;

% number of parallels
nTheta  = 60;

%% Extract input arguments

% Check if axes handle is specified
if isAxisHandle(varargin{1})
    hAx = varargin{1};
    varargin(1) = [];
else
    hAx = gca;
end

torus = varargin{1};
varargin(1) = [];

center = torus(1:3);
r1 = torus(4);
r2 = torus(5);

normal = [0 0];
if size(torus, 2) >= 7
    normal = torus(6:7);
end

% default set of options for drawing meshes
options = {'FaceColor', 'g', 'linestyle', 'none'};

while length(varargin) > 1
    switch lower(varargin{1})
        case 'nphi'
            nPhi = varargin{2};

        case 'ntheta'
            nTheta = varargin{2};

        otherwise
            % assumes this is drawing option
            options = [options varargin(1:2)]; %#ok<AGROW>
    end

    varargin(1:2) = [];
end

%% Draw the torus

% create base torus
circle = circleToPolygon([r1 0 r2], nTheta);
[x, y, z] = revolutionSurface(circle, linspace(0, 2*pi, nPhi));

% transform torus
trans = localToGlobal3d([center normal]);
[x, y, z] = transformPoint3d(x, y, z, trans);

% draw the surface
hs = surf(hAx, x, y, z, options{:});

%% Process output arguments

if nargout > 0
    varargout = {hs};
end


function prob_test150 ( )

%*****************************************************************************80
%
%% TEST150 tests UNIFORM_MEAN, UNIFORM_SAMPLE, UNIFORM_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST150\n' );
  fprintf ( 1, '  For the Uniform PDF:\n' );
  fprintf ( 1, '  UNIFORM_MEAN computes mean;\n' );
  fprintf ( 1, '  UNIFORM_SAMPLE samples;\n' );
  fprintf ( 1, '  UNIFORM_VARIANCE computes variance.\n' );

  a = 1.0;
  b = 10.0;

  check = uniform_check ( a, b );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST150 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  mean = uniform_mean ( a, b );
  variance = uniform_variance ( a, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =     %14f\n', a );
  fprintf ( 1, '  PDF parameter B =     %14f\n', b );
  fprintf ( 1, '  PDF mean =            %14f\n', mean );
  fprintf ( 1, '  PDF variance =        %14f\n', variance );

  for i = 1 : nsample
    [ x(i), seed ] = uniform_sample ( a, b, seed );
  end

  mean = r8vec_mean ( nsample, x );
  variance = r8vec_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', nsample );
  fprintf ( 1, '  Sample mean =     %14f\n', mean );
  fprintf ( 1, '  Sample variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =  %14f\n', xmax );
  fprintf ( 1, '  Sample minimum =  %14f\n', xmin );

  return
end


function A = allcomb(varargin)

% ALLCOMB - All combinations
%    B = ALLCOMB(A1,A2,A3,...,AN) returns all combinations of the elements
%    in the arrays A1, A2, ..., and AN. B is P-by-N matrix is which P is the product
%    of the number of elements of the N inputs. This functionality is also
%    known as the Cartesian Product. The arguments can be numerical and/or
%    characters, or they can be cell arrays.
%
%    Examples:
%       allcomb([1 3 5],[-3 8],[0 1]) % numerical input:
%       % -> [ 1  -3   0
%       %      1  -3   1
%       %      1   8   0
%       %        ...
%       %      5  -3   1
%       %      5   8   1 ] ; % a 12-by-3 array
%
%       allcomb('abc','XY') % character arrays
%       % -> [ aX ; aY ; bX ; bY ; cX ; cY] % a 6-by-2 character array
%
%       allcomb('xy',[65 66]) % a combination
%       % -> ['xA' ; 'xB' ; 'yA' ; 'yB'] % a 4-by-2 character array
%
%       allcomb({'hello','Bye'},{'Joe', 10:12},{99999 []}) % all cell arrays
%       % -> {  'hello'  'Joe'        [99999]
%       %       'hello'  'Joe'             []
%       %       'hello'  [1x3 double] [99999]
%       %       'hello'  [1x3 double]      []
%       %       'Bye'    'Joe'        [99999]
%       %       'Bye'    'Joe'             []
%       %       'Bye'    [1x3 double] [99999]
%       %       'Bye'    [1x3 double]      [] } ; % a 8-by-3 cell array
%
%    ALLCOMB(..., 'matlab') causes the first column to change fastest which
%    is consistent with matlab indexing. Example:
%      allcomb(1:2,3:4,5:6,'matlab')
%      % -> [ 1 3 5 ; 1 4 5 ; 1 3 6 ; ... ; 2 4 6 ]
%
%    If one of the arguments is empty, ALLCOMB returns a 0-by-N empty array.
%
%    See also NCHOOSEK, PERMS, NDGRID
%         and NCHOOSE, COMBN, KTHCOMBN (Matlab Central FEX)

% Tested in Matlab R2015a
% version 4.1 (feb 2016)
% (c) Jos van der Geest
% email: samelinoa@gmail.com

% History
% 1.1 (feb 2006), removed minor bug when entering empty cell arrays;
%     added option to let the first input run fastest (suggestion by JD)
% 1.2 (jan 2010), using ii as an index on the left-hand for the multiple
%     output by NDGRID. Thanks to Jan Simon, for showing this little trick
% 2.0 (dec 2010). Bruno Luong convinced me that an empty input should
% return an empty output.
% 2.1 (feb 2011). A cell as input argument caused the check on the last
%      argument (specifying the order) to crash.
% 2.2 (jan 2012). removed a superfluous line of code (ischar(..))
% 3.0 (may 2012) removed check for doubles so character arrays are accepted
% 4.0 (feb 2014) added support for cell arrays
% 4.1 (feb 2016) fixed error for cell array input with last argument being
%     'matlab'. Thanks to Richard for pointing this out.

narginchk(1,Inf) ;

NC = nargin ;

% check if we should flip the order
if ischar(varargin{end}) && (strcmpi(varargin{end},'matlab') || strcmpi(varargin{end},'john')),
    % based on a suggestion by JD on the FEX
    NC = NC-1 ;
    ii = 1:NC ; % now first argument will change fastest
else
    % default: enter arguments backwards, so last one (AN) is changing fastest
    ii = NC:-1:1 ;
end

args = varargin(1:NC) ;
% check for empty inputs
if any(cellfun('isempty',args)),
    warning('ALLCOMB:EmptyInput','One of more empty inputs result in an empty output.') ;
    A = zeros(0,NC) ;
elseif NC > 1
    isCellInput = cellfun(@iscell,args) ;
    if any(isCellInput)
        if ~all(isCellInput)
            error('ALLCOMB:InvalidCellInput', ...
                'For cell input, all arguments should be cell arrays.') ;
        end
        % for cell input, we use to indices to get all combinations
        ix = cellfun(@(c) 1:numel(c), args,'un',0) ;

        % flip using ii if last column is changing fastest
        [ix{ii}] = ndgrid(ix{ii}) ;

        A = cell(numel(ix{1}),NC) ; % pre-allocate the output
        for k=1:NC,
            % combine
            A(:,k) = reshape(args{k}(ix{k}),[],1) ;
        end
    else
        % non-cell input, assuming all numerical values or strings
        % flip using ii if last column is changing fastest
        [A{ii}] = ndgrid(args{ii}) ;
        % concatenate
        A = reshape(cat(NC+1,A{:}),[],NC) ;
    end
elseif NC==1,
    A = args{1}(:) ; % nothing to combine

else % NC==0, there was only the 'matlab' flag argument
    A = zeros(0,0) ; % nothing
end


function [fNewLat1, fNewLat2, fNewLon1, fNewLon2] = rotate_xsection(fLat1, fLat2, fLon1, fLon2, fAngle)

    % Compute the center of the given cross-section
    fCenterLat = fLat1 - ((fLat1 - fLat2)/2);
    fCenterLon = fLon1 - ((fLon1 - fLon2)/2);

    % Move the center of the cross-section to the origin
    vPos1 = [fLon1-fCenterLon; fLat1-fCenterLat];
    vPos2 = [fLon2-fCenterLon; fLat2-fCenterLat];

    % Compute angle in radians
    fAngle = fAngle*pi/180;

    % Set up the rotation matrix
    mRotate = [cos(fAngle) -sin(fAngle); sin(fAngle) cos(fAngle)];

    % Rotate the cross-section vectors
    vNewPos1 = mRotate * vPos1;
    vNewPos2 = mRotate * vPos2;

    % Move them back to their previous position
    fNewLon1 = vNewPos1(1) + fCenterLon;
    fNewLat1 = vNewPos1(2) + fCenterLat;
    fNewLon2 = vNewPos2(1) + fCenterLon;
    fNewLat2 = vNewPos2(2) + fCenterLat;


function [ a_lu, pivot, info ] = r8ge_fa ( a, n )

%*****************************************************************************80
%
%% R8GE_FA factors a general matrix.
%
%  Discussion:
%
%    R8GE_FA is a simplified version of the LINPACK routine DGEFA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 November 2004
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real A(LDA,N), the matrix to be factored.
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Output, real A_LU(LDA,N), contains an upper
%    triangular matrix and the multipliers which were used to obtain
%    it.  The factorization can be written A = L * U, where L is a
%    product of permutation and unit lower triangular matrices and
%    U is upper triangular.
%
%    Output, integer PIVOT(N), a vector of pivot indices.
%
%    Output, integer INFO, singularity flag.
%    0, no singularity detected.
%    nonzero, the factorization failed on the INFO-th step.
%
  info = 0;
  a_lu(1:n,1:n) = a(1:n,1:n);

  for k = 1 : n-1
%
%  Find L, the index of the pivot row.
%
    l = k;
    for i = k+1 : n
      if ( abs ( a_lu(l,k) ) < abs ( a_lu(i,k) ) )
        l = i;
      end
    end

    pivot(k) = l;
%
%  If the pivot index is zero, the algorithm has failed.
%
    if ( a_lu(l,k) == 0.0E+00 )
      info = k;
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8GE_FA - Warning!\n' );
      fprintf ( 1, '  Zero pivot on step %d\n', info );
      return
    end
%
%  Interchange rows L and K if necessary.
%
    if ( l ~= k )
      [ a_lu(l,k), a_lu(k,k) ] = r8_swap ( a_lu(l,k), a_lu(k,k) );
    end
%
%  Normalize the values that lie below the pivot entry A(K,K).
%
    a_lu(k+1:n,k) = -a_lu(k+1:n,k) / a_lu(k,k);
%
%  Row elimination with column indexing.
%
    for j = k+1 : n

      if ( l ~= k )
        [ a_lu(l,j), a_lu(k,j) ] = r8_swap ( a_lu(l,j), a_lu(k,j) );
      end

      a_lu(k+1:n,j) = a_lu(k+1:n,j) + a_lu(k+1:n,k) * a_lu(k,j);

    end

  end

  pivot(n) = n;

  if ( a_lu(n,n) == 0.0E+00 )
    info = n;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8GE_FA - Warning!\n' );
    fprintf ( 1, '  Zero pivot on step %d\n', info );
    return
  end

  return
end


function [J, grad] = linearRegCostFunction(X, y, theta, lambda)
%LINEARREGCOSTFUNCTION Compute cost and gradient for regularized linear
%regression with multiple variables
%   [J, grad] = LINEARREGCOSTFUNCTION(X, y, theta, lambda) computes the
%   cost of using theta as the parameter for linear regression to fit the
%   data points in X and y. Returns the cost in J and the gradient in grad

% Initialize some useful values
m = length(y); % number of training examples

% You need to return the following variables correctly
J = 0;
grad = zeros(size(theta));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost and gradient of regularized linear
%               regression for a particular choice of theta.
%
%               You should set J to the cost and grad to the gradient.
%

% We can reuse ex1's computeCost() but it be messier and slower
h0 = X*theta;
J = (sum((h0 - y) .^ 2) + lambda*sum(theta(2:end) .^ 2))/(2*m);

grad = (1/m)*(X'*(h0-y)) + [0; (lambda/m)*theta(2:end)];

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

grad = grad(:);

end


% S = SKEW2(MTX,MEAN,VAR)
%
% Sample skew (third moment divided by variance^3/2) of a matrix.
%  MEAN (optional) and VAR (optional) make the computation faster.

function res = skew2(mtx, mn, v)

if (exist('mn') ~= 1)
  mn =  mean2(mtx);
end

if (exist('v') ~= 1)
  v =  var2(mtx,mn);
end

if (isreal(mtx))
  res = mean(mean((mtx-mn).^3)) / (v^(3/2));
else
  res = mean(mean(real(mtx-mn).^3)) / (real(v)^(3/2)) + ...
      i * mean(mean(imag(mtx-mn).^3)) / (imag(v)^(3/2));
end


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

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

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

% Randomly initialize the weights to small values
epsilon_init = 0.12;
W = rand(L_out, 1+L_in)*2*epsilon_init - epsilon_init;

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

end