function [H,info] = freqwavelet(name,L, varargin)
%FREQWAVELET  Wavelet in the freq. domain
%   Usage: H=freqwavelet(name,L)
%          H=freqwavelet(name,L,scale)
%          [H,info]=freqwavelet(...)
%
%   Input parameters:
%         name  : Name of the wavelet
%         L     : System length
%   Output parameters:
%         H     : Frequency domain wavelet
%         info  : Struct with additional outputs
%
%   `freqwavelet(name,L)` returns peak-normalized "mother" frequency-domain
%   wavelet `name` for system length *L*. The basic scale is selected such
%   that the peak is positioned at the frequency 0.1 relative to the Nyquist
%   rate (fs/2).
%
%   The supported wavelets that can be used in place of `name` (the actual
%   equations can be found at the end of this help):
%
%     'cauchy'    Cauchy wavelet with alpha=100. Custom order=(alpha-1)/2
%                 (with alpha>1) can be set by `{'cauchy',alpha}`. The
%                 higher the order, the narrower and more symmetric the
%                 wavelet. A numericaly stable formula is used in order
%                 to allow support even for very high `alpha` e.g.
%                 10^6.
%
%     'morse'     Morse wavelet with alpha=100 and gamma=3. Both parameters
%                 can be set directly by `{'morse',alpha,gamma}`. `alpha`
%                 has the same role as for 'cauchy', `gamma` is the
%                 'skewness' parameter. 'morse' becomes 'cauchy' for
%                 gamma=1.
%
%     'morlet'    Morlet wavelet with sigma=4. The parameter `sigma`
%                 is the center frequency to standard deviation ratio of
%                 the main generating Gaussian distribution. Note that the
%                 true peak and standard deviation of the Morlet wavelet
%                 differ, in particular for low values of `sigma` (<5).
%                 This is a consequence of the correction factor. The
%                 parameter can be set directly by `{'morlet',sigma}`.
%
%    'fbsp'       Frequency B-spline wavelet of order m=3, center frequency
%                 to bandwidth ratio fb = 2. The parameters can be set
%                 by `{'fbsp',m,fb}`. Note that `m` must be integer and
%                 greater than or equal to 1, and `fb` must be greater than
%                 or equal to 2.
%
%    'analyticsp' Positive frequency part of cosine-modulated B-spline
%                 wavelet of  order order=3, with center frequency to main
%                 lobe width ratio fb = 1. The parameters can be set by
%                 `{'analyticsp',order,fb}`. Note that `order` and `fb`
%                 must be integer and greater than or equal to 1.
%
%    'cplxsp'     Complex-modulated B-spline of order order=3, with center
%                 frequency to main lobe width ratio fb = 1. The parameters
%                 can be set by `{'cplxsp',order,fb}`. Note that `order`
%                 and `fb` must be integer and greater than or equal to 1.
%
%   `freqwavelet(name,L,scale)` returns a "dilated" version of the wavelet.
%   The nonzero scalar `scale` controls both the bandwidth and the center
%   frequency. Values greater than 1 make the wavelet wider (and narrower
%   in the frequency domain), values lower than 1 make the wavelet narrower.
%   The center frequency is moved to`0.1/scale`. The center frequency is
%   limited to the range of "positive" frequencies ]0,1] (up to Nyquist rate).
%   If `scale` is a vector, the output is a `L x numel(scale)` matrix with
%   the individual wavelets as columns.
%
%   The following additional flags and key-value parameters are available:
%
%     'scale'           Wavelet scale (relative to basic scale)
%
%     'waveletParams'   a vector containing the respective wavelet parameters
%                       [alpha, beta, gamma] for cauchy and morse wavelets
%                       [sigma] for morlet wavelets
%                       [order, fb] for splines
%
%     'basefc',fc      Normalized center frequency of the mother wavelet
%                      (scale=1). The default is 0.1.
%
%     'bwthr',bwthr    The height at which the bandwidth is computed.
%                      The default value is 10^(-3/10) (~0.5).
%
%     'efsuppthr',thr  The threshold determining the effective support of
%                      the wavelet. The default value is 10^(-5).
%
%     'scal',s         Scale the filter by the constant *s*. This can be
%                      useful to equalize channels in a filter bank.
%
%     'delay',d        Set the delay of the filter. Can be either a scalar or
%                      a vector of the same length as *scale*. Default value is zero.
%
%   The admissible range of scales can be adjusted to handle different
%   scenarios:
%
%     'positive'       Enables the construction of wavelets at postive
%                      center frequencies ]0,1]. If `basefc=0.1`, this
%                      corresponds to scales larger than or equal to 0.1.
%                      This is the default.
%
%     'negative'       Enables the construction of wavelets at negative
%                      center frequencies [-1,0[. If `basefc=0.1`, this
%                      corresponds to scales smaller than or equal to -0.1.
%
%     'analytic'       Enables the construction of wavelets with center
%                      frequencies in ]0,2] for analysis of analytic signals.
%                      [! This feature is currently experimental and may not
%                      always work as intended. !]
%
%   The format of the output is controlled by the following flags:
%   'full' (default),'econ','asfreqfilter':
%
%     'full'           The output is a `L x numel(scale)` matrix.
%
%     'econ'           The output is a `numel(scale)` cell array with
%                      individual freq. domain wavelets truncated to the
%                      length of the effective support given by parameter 'efsuppthr'.
%                      Does not run stably for system lengths > 2000
%
%     'asfreqfilter'   As 'econ', but the elements of the cell-array are
%                      filter structs with fields .H and .foff as in
%                      |blfilter| to be used in |filterbank| and related.
%
%   `[H,info]=freqwavelet(...)` additionally returns a struct with the
%   following fields:
%
%     .fc             Normalized center frequency.
%
%     .foff           Index of the first sample above the effective
%                     support threshold (minus one).
%                     It can be directly used to convert the 'econ'
%                     output to 'full' by `circshift(postpad(H,L),foff)`.
%
%     .fsupp          Length of the effective support (with values above
%                     efsuppthr).
%
%     .basefc         Center frequency of the implied mother wavelet.
%
%     .scale          The scale used.
%
%     .dilation       The actual dilation used in the formula.
%
%     .bw             Relative bandwidth at -3 dB (half of the height).
%
%     .tfr            Time-frequency ratio of a Gaussian with the same
%                     bandwidth as the wavelet.
%
%     .aprecise       Exact natural subsampling factors (not rounded).
%
%     .a_natural      Fractional natural subsampling factors in the
%                     format acceptable by |filterbank| and related.
%
%     .cauchyAlpha    Alpha value of closest Cauchy wavelet [NOTE: Not
%                     implemented for non-Morse wavelets.]
%
%   Additionally, the function accepts flags to normalize the output.
%   Please see the help of |setnorm|. By default, no normaliazation is
%   applied.
%
%   Wavelet definitions
%   -------------------
%
%   `C` is a normalization constant.
%
%   Cauchy wavelet
%
%       H = C \xi^{\frac{\alpha-1}{2}} exp( -2\pi\xi )
%
%       .. math:: H = C \xi^{\frac{\alpha-1}{2}} exp( -2\pi\xi )
%
%   Morse wavelet
%
%       H = C \xi^{\frac{\alpha-1}{2\gamma}} exp( -2\pi\xi^{\gamma} )
%
%       .. math:: H = C \xi^{\frac{\alpha-1}{2\gamma}} exp( -2\pi\xi^{\gamma} )
%
%   Morlet wavelet
%
%       H = C \xi^{\frac{\alpha-1}{2\gamma}} exp( -2\pi\xi^{\gamma} )
%
%       .. math:: H = C \xi^{\frac{\alpha-1}{2\gamma}} exp( -2\pi\xi^{\gamma} )
%
%   Frequency bandlimited spline wavelet
%
%       H = C B (\xi - m \frac{\xi}{4})
%
%       .. math:: H = C B (\xi - m \frac{\xi}{4})
%
%   Analytic spline wavelet
%
%       H = C exp(-j \omega x) A(-exp(j \omega)) H(exp(-j \omega)
%
%       .. math:: H = C exp(-j \omega x) A(-exp(j \omega)) H(exp(-j \omega))
%
%   Complex spline wavelet
%
%       H = C exp(-j \omega x + \xi )
%
%       .. math:: H = C exp(-j \omega x + \xi )
%
%
%   References: rioul92 unalsc94
%
%   See also: setnorm, filterbank, blfilter

complainif_notenoughargs(nargin,2,upper(mfilename));

%set default input parameters
if ~isscalar(L)
    error('%s: L must be a scalar',upper(mfilename));
end

if ~iscell(name), name = {name}; end

freqwavelettypes = getfield(arg_freqwavelet(),'flags','wavelettype');

if ~ischar(name{1}) || ~any(strcmpi(name{1},freqwavelettypes))
  error('%s: First input argument must be the name of a supported window.',...
        upper(mfilename));
end

definput.import={'setnorm', 'freqwavelet'};
definput.importdefaults={'null'};
definput.keyvals.scale = 1;
definput.keyvals.scal = [];
definput.keyvals.basefc = 0.1;
definput.keyvals.delay=0;
definput.keyvals.fc = [];
definput.keyvals.bwthr = 10^(-3/10);
definput.keyvals.efsuppthr = 10^(-5);
definput.flags.freqrange = {'positive','negative','analytic'};
definput.flags.outformat = {'full','econ','asfreqfilter'};
definput.keyvals.fs = 2;
definput.keyvals.alphaStep = definput.keyvals.fs/L;

if ~iscell(name)
    name = {name};
end

[flags,kv,scale]=ltfatarghelper({'scale'},definput,varargin,'freqwavelet');

%check L
if ~isscalar(L)
    error('%s: L must be a scalar',upper(mfilename));
end

%check basefc
if ~isscalar(kv.basefc)
    error('%s: basefc must be a positive scalar',upper(mfilename));
end

scale = scale(:).'; % Scale should always be a row vector
if ~isnumeric(scale), error('%s: scale must be numeric',upper(mfilename)); end

if isempty(kv.scal)
    kv.scal = scale;
elseif ~isnumeric(kv.scal)
    error('%s: scal must be numeric',upper(mfilename));
elseif numel(kv.scal) ~= numel(scale)
    error('%s: scal must have exactly as many entries as scale',upper(mfilename));
end

if kv.alphaStep > 0.02
    error('%s: wavelet sampling too small. increase system length.',upper(mfilename));
end

%if L/scale > 10
%   error('%s: scale too large.',upper(mfilename));
%end

% Check range of scales
if flags.do_positive && (any(scale <= 0) || any(kv.basefc./scale > 1))
    error('%s: Frequency range flag is set to positive. scale must be positive and not smaller than basefc.', upper(mfilename));
end
if flags.do_negative && (any(scale >= 0) || any(kv.basefc./scale < -1))
    error('%s: Frequency range flag is set to negative. scale must be negative and not larger than -basefc.', upper(mfilename));
end
if flags.do_analytic && (any(scale <= 0) || any(kv.basefc./scale > 2))
    error('%s: Frequency range flag is set to analytic. scale must be positive and not smaller than 2*basefc.', upper(mfilename));
end

% Check delay vector
if numel(kv.delay) == 1
    kv.delay = repmat(kv.delay, 1, numel(scale));
end
if numel(kv.delay) ~= numel(scale)
    error('%s: delay must have exactly as many entries as scale',upper(mfilename));
end
% Check other parameters
if kv.efsuppthr < 0, error('%s: efsuppthr must be nonnegative',upper(mfilename)); end
if kv.bwthr < 0, error('%s: bwthr must be nonnegative',upper(mfilename)); end
if kv.bwthr < kv.efsuppthr, error('%s: efsuppthr must be lower than bwthr.',upper(mfilename)); end

M = numel(scale);

if flags.do_full
    H = zeros(L,M);
else
    H = cell(1,M);
end

if flags.do_negative
    wltype = 'negative';
else
    wltype = 'positive';
end

%% generate the wavelet prototype
[fun, fsupp_, peakpos, cauchyAlpha] = helper_waveletgeneratorfunc(name, wltype);

%% calculate the support as f(scale)
fsupp = zeros(5,M);
fsupp(5,:) = kv.fs;
basedil = peakpos/(kv.basefc);

if kv.efsuppthr > 0
    fsupp(1,:) = max(0,(fsupp_(1)/basedil)./scale);
    fsupp(5,:) = min(kv.fs,(fsupp_(5)/basedil)./scale);
end
fsupp(2,:) = max(0,(fsupp_(2)/basedil)./scale);
fsupp(3,:) = (fsupp_(3)/basedil)./scale;
fsupp(4,:) = min(kv.fs,(fsupp_(4)/basedil)./scale);

if flags.do_negative
    fsupp = flip(fsupp);
end

fsuppL = fsuppL_inner(fsupp,kv.fs,L,1:5);

%% calculate H
if flags.do_full
    if ~flags.do_negative
        y = ((0:L-1)').*basedil*kv.alphaStep*scale;
    else
        y = ([0;(L-1:-1:1)']).*basedil*kv.alphaStep*scale;
    end
    H = abs(kv.scal).*setnorm(fun(y), flags.norm);
elseif flags.do_econ
    for ii = 1:numel(scale)
        y = ((fsuppL(1,ii):fsuppL(end,ii)-1)').*basedil*kv.alphaStep*abs(scale(ii));
        H{ii} = abs(kv.scal(ii)).*setnorm(fun(y), flags.norm);%TODO: check output format: should be cell
    end
elseif flags.do_asfreqfilter
    for m = 1:numel(scale)
        y = @(L) ((fsuppL_inner(fsupp(:,m),kv.fs,L,1):fsuppL_inner(fsupp(:,m),kv.fs,L,5)-1)').*basedil*abs(scale(m))*kv.fs/L;
        H{m} = struct('H',@(L) abs(kv.scal(m))*setnorm(fun(y(L)),flags.norm),'foff',@(L)fsuppL_inner(fsupp(:,m),kv.fs,L,1),'realonly',0, 'delay', kv.delay(m));
   end
end

%% write info struct
info.basefc = kv.basefc;
info.fc = fsupp(3,:);
info.scale = scale;%';
info.dilation = basedil.*scale;%';
info.fsupp = fsuppL(end,:) - fsuppL(1,:) + ones(1,numel(scale));
if info.fsupp <= 0, info.fsupp = 0; end
info.bw  = (fsupp(4,:) - fsupp(2,:));
bwinsamples = info.bw./kv.alphaStep;
info.aprecise = L./bwinsamples;
info.a_natural(:,2) = ceil(bwinsamples);
info.a_natural = info.a_natural';
info.tfr = (cauchyAlpha - 1)./(pi*info.fc.^2*L);
info.cauchyAlpha = cauchyAlpha;
info.foff = fsuppL(1,:);

%if M==1 && iscell(H)
%    H = H{1};
%end

end

function fsuppL = fsuppL_inner(fsupp,fs,L,idx)
    fsuppL_all = [ ceil(fsupp(1:2,:)/fs*L); round(fsupp(3,:)/fs*L); floor(fsupp(4:5,:)/fs*L) ];
    fsuppL = fsuppL_all(idx,:);
end


% runpca() -  perform principal component analysis (PCA) using singular value
%             decomposition (SVD) using Matlab svd() or svds()
%                        >> inv(eigvec)*data = pc;
% Usage:
%    >> [pc,eigvec,sv] = runpca(data);
%    >> [pc,eigvec,sv] = runpca(data,num,norm)
%
% Inputs:
%   data   - input data matrix (rows are variables, columns observations)
%   num    - number of principal comps to return  {def|0|[] -> rows in data}
%   norm   - 1/0 = do/don't normalize the eigvec's to be equivariant
%                                                {def|0 -> no normalization}
% Outputs:
%   pc     - the principal components, i.e.        >> inv(eigvec)*data = pc;
%   eigvec - the inverse weight matrix (=eigenvectors). >> data = eigvec*pc;
%   sv     - the singular values (=eigenvalues)
%
% Author: Colin Humphries, CNL / Salk Institute, 1997
%
% See also: runica()

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

% 01/31/00 renamed runpca() and improved usage message -sm
% 01-25-02 reformated help & license, added links -ad

function [pc,A,sv]= runpca2(data,K,dosym)
[chans,frames] = size(data);

if nargin < 3 || K < chans
    dosym = 1;
end
if nargin < 2
    K = chans;
end
if nargin < 1
  help runpca
  return
end

% remove the mean
for i = 1:chans
  data(i,:) = data(i,:) - mean(data(i,:));
end

% do svd
[U,S,V] = svd(data*data'/frames); % U and V should be the same since data*data' is symmetric

sv = sqrt(diag(S));
if dosym == 1
    pc = pinv(diag(sv(1:K))) * V(:,1:K)' * data;
    A = U(:,1:K) * diag(sv(1:K));
else
    pc = U * pinv(diag(sv)) * V' * data;
    A = U * diag(sv) * V';
end


function varargout = euclideanMST(points)
%EUCLIDEANMST Build euclidean minimal spanning tree of a set of points.
%
%   EDGES = euclideanMST(POINTS)
%   POINTS is a [NxP] array, N being the number of points and P being the
%   dimension.
%   Result EDGES is a [Mx2] array, containing indices of each vertex for
%   each edges.
%
%   [EDGES DISTS] = euclideanMST(POINTS)
%   Also returns the lengths of edges computed by MST algorithm.
%
%   Algorithm first computes Delaunay triangulation of the set of points,
%   then computes euclidean length of each edge of triangulation, and
%   finally uses prim algorithm to simplify the graph.
%
%   Example
%     % choose random points in the plane and display their Euclidean MST
%     pts = rand(50, 2)*100;
%     edges = euclideanMST(pts);
%     drawGraph(pts, edges)
%
%   See also
%   prim_mst, distancePoints, delaunayn
%

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2007-07-27, using Matlab 7.4.0.287 (R2007a)
% Copyright 2007-2022 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas

% dimension
D   = size(points, 2);
Df  = factorial(D);

% compute all couples of vertices in unit triangle, tetrahedron, or n-dim
% simplex
subs = zeros(Df, 2);
k = 1;
for i = 1:D
    for j = i+1:D+1
        subs(k, 1) = i;
        subs(k, 2) = j;
        k = k + 1;
    end
end

% compute delaunay triangulation in D dimensions
tri = delaunayn(points);
Nt  = size(tri, 1);

% compute all possible edges
edges = zeros(Nt*Df, 2);
for t = 1:Nt
    for i = 1:Df
        edges((t-1)*Df+i, 1) = tri(t, subs(i, 1));
        edges((t-1)*Df+i, 2) = tri(t, subs(i, 2));
    end
end

% simplify edges
edges = unique(sort(edges, 2), 'rows');

% compute euclidean length of each edge
val = zeros(size(edges, 1), 1);
for i = 1:size(edges,1)
    val(i) = distancePoints(points(edges(i,1), :), points(edges(i,2), :));
end

% compute MST of created graph
[edges2, vals2] = prim_mst(edges, val);

% process output arguments
if nargout == 1
    varargout{1} = edges2;
elseif nargout==2
    varargout{1} = edges2;
    varargout{2} = vals2;
end


function [x]=grandn(a,b)

% function [x]=grandn(a,b)
% random number generator from the Gamma (a,b) distribution

% part 1: obtain a random number from Gamma(a,1)

if a>=1
x=gammadrawover1(a);
else
xtilde=gammadrawover1(a+1);
u=rand;
x=xtilde*u^(1/a);
end

% part 2: transform into a draw from G(a,b)
x=x*b;

% auxiliary nested function to draw a Gamma random number when argument 'a' is greater than or equal to 1
function x=gammadrawover1(a)
d=a-1/3;
c=1/((9*d)^0.5);
check=0;
   while check==0
   z=randn;
   u=rand;
   v=(1+c*z)^3;
      if (v>0 && log(u)<0.5*z^2+d-d*v+d*log(v))
      x=d*v;
      check=1;
      else
      check=0;
      end
   end
end

% declare end of general function
end


function [ intersect, p ] = plane_imp_line_par_int_3d ( a, b, c, d, x0, y0, z0, ...
  f, g, h )

%*****************************************************************************80
%
%% PLANE_IMP_LINE_PAR_INT_3D: intersection ( implicit plane, parametric line ) in 3D.
%
%  Discussion:
%
%    The implicit form of a plane in 3D is:
%
%      A * X + B * Y + C * Z + D = 0
%
%    The parametric form of a line in 3D is:
%
%      X = X0 + F * T
%      Y = Y0 + G * T
%      Z = Z0 + H * T
%
%    We normalize by choosing F*F+G*G+H*H=1 and 0 <= F.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Adrian Bowyer and John Woodwark,
%    A Programmer's Geometry,
%    Butterworths, 1983, page 111.
%
%  Parameters:
%
%    Input, real A, B, C, D, the implicit plane parameters.
%
%    Input, real X0, Y0, Z0, F, G, H, parameters that define the
%    parametric line.
%
%    Output, logical INTERSECT, is TRUE if the line and the plane
%    intersect.
%
%    Output, real P(3), is a point of intersection of the line
%    and the plane, if INTERSECT is TRUE.
%
  dim_num = 3;
  tol = 0.00001;
%
%  Check.
%
  norm1 = sqrt ( a * a + b * b + c * c );

  if ( norm1 == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PLANE_IMP_LINE_PAR_INT_3D - Fatal error!\n' );
    fprintf ( 1, '  The plane normal vector is null.\n' );
    error ( 'PLANE_IMP_LINE_PAR_INT_3D - Fatal error!' )
  end

  norm2 = sqrt ( f * f + g * g + h * h );

  if ( norm2 == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PLANE_IMP_LINE_PAR_INT_3D - Fatal error!\n' );
    fprintf ( 1, '  The line direction vector is null.\n' );
    error ( 'PLANE_IMP_LINE_PAR_INT_3D - Fatal error!' )
  end

  denom = a * f + b * g + c * h;
%
%  The line and the plane may be parallel.
%
  if ( abs ( denom ) < tol * norm1 * norm2 )

    if ( a * x0 + b * y0 + c * z0 + d == 0.0 )
      intersect = 1;
      p(1) = x0;
      p(2) = y0;
      p(3) = z0;
    else
      intersect = 0;
      p(1:dim_num) = 0.0;
    end
%
%  If they are not parallel, they must intersect.
%
  else

    intersect = 1;
    t = - ( a * x0 + b * y0 + c * z0 + d ) / denom;
    p(1) = x0 + t * f;
    p(2) = y0 + t * g;
    p(3) = z0 + t * h;

  end

  return
end


function fun = makelinefun(x1,y1,x2,y2,o)
%ref http://stackoverflow.com/questions/13209373/matlab-straight-line-between-2-points-with-n-points-between
if nargin < 5
    o = 1;
end
if o == 2,
    fun  = @(N) deal(linspace(x1,x2,N), linspace(y1,y2,N));
else
	fun  = @(N) [linspace(x1,x2,N) ; linspace(y1,y2,N)];
end

%
% USAGE:
%
% f = makelinefun(0,0,6,9);
% xy = f(4)
%
%   xy =
%       0     2     4     6
%       0     3     6     9
%
%       f = makelinefun(0,0,6,9);
% [x,y] = f(4)
%
%   x =
%       0     2     4     6
%   y =
%       0     3     6     9
%


function d = phasecorr(A,B)
%PHASECORR Compute phase correlation matrix
%
%   D = PHASECORR(A, B) computes the phase correlation matrix from input
%   2-D images A and B. PHASECORR returns the 'full' correlation matrix
%   similar to normxcorr2.

%   Copyright 2013 The MathWorks, Inc.

size_A  = size(A);
size_B  = size(B);

% Let fft2 zero pad time domain signals such that we form the 'full'
% correlation matrix analogous to the result produced by normxcorr2.
outSize = size_A + size_B - 1;

% Form 2-D spectra of moving and fixed.
% Window each signal prior to taking FFT to help reduce ringing effects in
% frequency domain due to finite image.
A = fft2(A,outSize(1),outSize(2));
B = fft2(B,outSize(1),outSize(2));

% Form phase correlation matrix, d
% Use 'symmetric' option as performance optimization. We expect that the
% input moving images A and B are real valued, so d should always be real
% valued.
ABConj = A .* conj(B);
d = ifft2(ABConj ./ abs(eps+ABConj),'symmetric');


function p = chebyshev(n,var)
%CHEBYSHEV    Chebyshev polynomial (exactly representable up to degree n<=81)
%
%   p = chebyshev(n,var)
%
%Computed by recursion
%  p(0,x) = 1
%  p(1,x) = x
%  p(n,x) = 2*x*p(n-1,x) - p(n-2,x)  for n>1
%
%Parameter var for dependent variable optional (default 'x')
%

% written  07/30/02     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

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

  if nargin==1
    var = 'x';
  else
    if ~ischar(var)
      error('variable must be string')
    end
  end

  if n==0
    p = polynom(1,var);
  elseif n==1
    p = polynom([1 0],var);
  else
    t1 = [1 zeros(1,n)];
    t2 = [0 1 zeros(1,n-1)];
    for i=3:n+1
      t3 = [0 2*t2(1:n)] - t1;
      t1 = t2;
      t2 = t3;
    end
    p = polynom(fliplr(t3),var);
  end

  if rndold
    setround(rndold)
  end


function pass = test_roots01( pref )
% Check that the marching squares and Bezoutian agree with each other.
% Uncomment tests if harder tests should be executed.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e3 * pref.cheb2Prefs.chebfun2eps;
j = 1;

%%
f = chebfun2(@(x,y) 144*(x.^4+y.^4)-225*(x.^2+y.^2) + 350*x.^2.*y.^2+81);
g = chebfun2(@(x,y) y-x.^6);
r1 = roots([f;g],'ms');
r2 = roots([f;g],'resultant');
pass(j) = ( norm(sort(r1(:,1))-sort(r2(:,1))) < tol ); j = j + 1;
pass(j) = ( norm(sort(r1(:,2))-sort(r2(:,2))) < tol ); j = j + 1;

%%
f = chebfun2(@(x,y)(y.^2-x.^3).*((y-0.7).^2-(x-0.3).^3).*((y+0.2).^2-(x+0.8).^3).*((y+0.2).^2-(x-0.8).^3));
g = chebfun2(@(x,y)((y+.4).^3-(x-.4).^2).*((y+.3).^3-(x-.3).^2).*((y-.5).^3-(x+.6).^2).*((y+0.3).^3-(2*x-0.8).^3));
r2 = roots([f;g],'resultant');
pass(j) = ~( length(r2) - 13 );  j = j + 1;
pass(j) = ( norm(f(r2(:,1),r2(:,2))) < 1e3*tol ); j = j + 1;
pass(j) = ( norm(g(r2(:,1),r2(:,2))) < 1e3*tol ); j = j + 1;

%%
f = chebfun2(@(x,y)y.^2-x.^3);
g = chebfun2(@(x,y)(y+.1).^3-(x-.1).^2);
r1 = roots([f;g],'ms');
r2 = roots([f;g],'resultant');
pass(j) = ( norm(sort(r1(:,1))-sort(r2(:,1))) < tol ); j = j + 1;
pass(j) = ( norm(sort(r1(:,2))-sort(r2(:,2))) < tol ); j = j + 1;

%%
p = chebfun2(@(x,y) x - y + .5);
q = chebfun2(@(x,y) x + y );
r = roots([p; q]);
pass(j) = norm( r - [-.25 .25] ) < tol; j = j + 1;

p = chebfun2(@(x, y) y + x/2 + 1/10);
q = chebfun2(@(x, y) y - 2.1*x + 2);
r = roots([p ;  q], 'resultant');
pass(j) = norm(r - [0.730769230769231, -0.465384615384615]) < tol; j = j + 1;

end


function pdf = gompertz_pdf ( x, a, b )

%*****************************************************************************80
%
%% GOMPERTZ_PDF evaluates the Gompertz PDF.
%
%  Discussion:
%
%    PDF(X)(A,B) = B * A**X / exp ( B * ( A**X - 1 ) / log ( A ) )
%
%    for
%
%      0.0 <= X
%      1.0 <  A
%      0.0 <  B
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Johnson, Kotz, and Balakrishnan,
%    Continuous Univariate Distributions, Volume 2, second edition,
%    Wiley, 1994, pages 25-26.
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%
%    Input, real A, B, the parameters of the PDF.
%    1 < A, 0 < B.
%
%    Output, real PDF, the value of the PDF.
%
  if ( x < 0.0 )

    pdf = 0.0;

  elseif ( 1.0 < a )

    pdf = exp ( log ( b ) + x * log ( a ) - ( b / log ( a ) ) * ( a^x - 1.0 ) );

  end

  return
end


function ASK_FSK_PSK(msglen)
%msglen= number of bits to be transmitted
%take msglen=10000, or 20000 for more accuracy
%If you have any problem or feedback please contact me @
%%===============================================
% NIKESH BAJAJ
% Asst. Prof., Lovely Professional University, India
% Almameter: Aligarh Muslim University, India
% +919915522564, bajaj.nikkey@gmail.com
%%===============================================
n=msglen;
b=randint(1,n);
f1=1;f2=2;
t=0:1/30:1-1/30;
%ASK
sa1=sin(2*pi*f1*t);
E1=sum(sa1.^2);
sa1=sa1/sqrt(E1); %unit energy
sa0=0*sin(2*pi*f1*t);
%FSK
sf0=sin(2*pi*f1*t);
E=sum(sf0.^2);
sf0=sf0/sqrt(E);
sf1=sin(2*pi*f2*t);
E=sum(sf1.^2);
sf1=sf1/sqrt(E);
%PSK
sp0=-sin(2*pi*f1*t)/sqrt(E1);
sp1=sin(2*pi*f1*t)/sqrt(E1);

%MODULATION
ask=[];psk=[];fsk=[];
for i=1:n
    if b(i)==1
        ask=[ask sa1];
        psk=[psk sp1];
        fsk=[fsk sf1];
    else
        ask=[ask sa0];
        psk=[psk sp0];
        fsk=[fsk sf0];
    end
end
figure(1)
subplot(411)
stairs(0:10,[b(1:10) b(10)],'linewidth',1.5)
axis([0 10 -0.5 1.5])
title('Message Bits');grid on
subplot(412)
tb=0:1/30:10-1/30;
plot(tb, ask(1:10*30),'b','linewidth',1.5)
title('ASK Modulation');grid on
subplot(413)
plot(tb, fsk(1:10*30),'r','linewidth',1.5)
title('FSK Modulation');grid on
subplot(414)
plot(tb, psk(1:10*30),'k','linewidth',1.5)
title('PSK Modulation');grid on
xlabel('Time');ylabel('Amplitude')
%AWGN
for snr=0:20
    askn=awgn(ask,snr);
    pskn=awgn(psk,snr);
    fskn=awgn(fsk,snr);

    %DETECTION
    A=[];F=[];P=[];
    for i=1:n
        %ASK Detection
        if sum(sa1.*askn(1+30*(i-1):30*i))>0.5
            A=[A 1];
        else
            A=[A 0];
        end
        %FSK Detection
        if sum(sf1.*fskn(1+30*(i-1):30*i))>0.5
            F=[F 1];
        else
            F=[F 0];
        end
        %PSK Detection
        if sum(sp1.*pskn(1+30*(i-1):30*i))>0
            P=[P 1];
        else
            P=[P 0];
        end
    end

    %BER
    errA=0;errF=0; errP=0;
    for i=1:n
        if A(i)==b(i)
            errA=errA;
        else
            errA=errA+1;
        end
        if F(i)==b(i)
            errF=errF;
        else
            errF=errF+1;
        end
        if P(i)==b(i)
            errP=errP;
        else
            errP=errP+1;
        end
    end
    BER_A(snr+1)=errA/n;
    BER_F(snr+1)=errF/n;
    BER_P(snr+1)=errP/n;
end

figure(2)
subplot(411)
stairs(0:10,[b(1:10) b(10)],'linewidth',1.5)
axis([0 10 -0.5 1.5]);grid on
title('Received signal after AWGN Channel')
subplot(412)
tb=0:1/30:10-1/30;
plot(tb, askn(1:10*30),'b','linewidth',1.5)
title('Received ASK signal');grid on
subplot(413)
plot(tb, fskn(1:10*30),'r','linewidth',1.5)
title('Received FSK signal');grid on
subplot(414)
plot(tb, pskn(1:10*30),'k','linewidth',1.5)
title('Received PSK signal');grid on
figure(3)
semilogy(0:20,BER_A, 'b','linewidth',2)
title('BER Vs SNR')
grid on;
hold on
semilogy(0:20,BER_F,'r','linewidth',2)
semilogy(0:20,BER_P, 'k','linewidth',2)
xlabel('Eo/No(dB)')
ylabel('BER')
hold off
legend('ASK','FSK','PSK');


% Optimal doping profile optimization with current gain constraint.
% Boyd, Kim, Vandenberghe, and Hassibi, "A tutorial on geometric programming"
% Joshi, Boyd, and Dutton, "Optimal doping profiles via geometric programming"
% Written for CVX by Almir Mutapcic 02/08/06
% (a figure is generated)
%
% Determines the optimal doping profile that minimizes base transit
% time subject to a lower bound constraint on the current gain (beta).
% This problem can be posed as a GP:
%
%   minimize   tau_B
%       s.t.   Nmin <= v <= Nmax
%              y_(i+1) + v_i^const1 <= y_i
%              w_(i+1) + v_i^const2 <= w_i, etc...
%              beta => beta_min
%
% where variables are v_i, y_i, and w_i.

% problem size
M = 20;

% problem constants
g1 = 0.42;
g2 = 0.69;
Nmax = 5*10^18;
Nmin = 5*10^16;
Nref = 10^17;
Dn0 = 20.72;
ni0 = 1.4*(10^10);
WB = 10^(-5);
C =  WB^2/((M^2)*(Nref^g1)*Dn0);

% minimum current gain values
beta_min_GE = [1 1.4 1.8 2.2 2.6 3.0 3.4 3.43]*(1e-11);

% exponent powers
pwi = g2 -1;
pwj = 1+g1-g2;

v_array = [];
for k = 1:length(beta_min_GE)
    fprintf( 'beta_min_GE = %g: ', beta_min_GE(k) );
    cvx_begin gp quiet
        % optimization variables
        variables v(M) y(M) w(M)

        % objective function is the base transmit time
        tau_B = C*w(1);

        minimize( tau_B )
        subject to
        % fixed problem constraints
        Nmin <= v <= Nmax;

        for i=1:M-1
            y(i+1) + v(i)^pwj <= y(i);
            w(i+1) + y(i)*v(i)^pwi <= w(i);
        end

        % equalities
        y(M) == v(M)^pwj;
        w(M) == y(M)*v(M)^pwi;

        % changing constraint
        (WB*beta_min_GE(k)/(M*Nref^(g1-g2)*Dn0))*y(1) <= 1;
    cvx_end
    fprintf( '%s\n', cvx_status );
    % keep the optimal solution
    v_array = [v_array v];
end

% plot the basic optimal doping profile
figure, clf
nbw = 0:1/M:1-1/M;
for k = 1:length(beta_min_GE)
  semilogy(nbw,v_array(:,k),'LineWidth',2); hold on;
end
axis([0 1 1e16 1e19]);
xlabel('base');
ylabel('doping');
text(0,Nmin,'Nmin ', 'HorizontalAlignment','right');
text(0,Nmax,'Nmax ', 'HorizontalAlignment','right');
hold off;


% CXCORR Circular Cross Correlation function estimates.
% CXCORR(a,b), where a and b represent samples taken over time interval T
% which is assumed to be a common period of two corresponded periodic signals.
% a and b are supposed to be length M row vectors, either real or complex.
%
% [x,c]=CXCORR(a,b) returns the length M-1 circular cross correlation sequence c
% with corresponded lags x.
%
% The circular cross correlation is:
%         c(k) = sum[a(n)*conj(b(n+k))]/[norm(a)*norm(b)];
% where vector b is shifted CIRCULARLY by k samples.
%
% The function doesn't check the format of input vectors a and b!
%
% For circular covariance between a and b look for CXCOV(a,b) in
% http://www.mathworks.com/matlabcentral/fileexchange/loadAuthor.do?objectType=author&objectId=1093734
%
% Reference:
% A. V. Oppenheim, R. W. Schafer and J. R. Buck, Discrete-Time Signal Processing,
% Upper Saddler River, NJ : Prentice Hall, 1999.
%
% Author: G. Levin, Apr. 26, 2004.

function [x,c]=CXCORR(a,b)
na=norm(a);
nb=norm(b);
a=a/na; %normalization
b=b/nb;
for k=1:length(b)
    c(k)=a*b';
    b=[b(end),b(1:end-1)]; %circular shift
end
x=[0:length(b)-1]; %lags


% CONVERT ROTATION ANGLES INTO A QUATERNION
function [q] = GetQuaternionFromEulers(eulers)
% THis function converts euler angle rotations into a
% quaternion via its components. Associated block:
% "Rotation Angles to Quaternions".
eulers = 0.5*eulers;
% Build vector of trigonometric arguements
trigArgs = [sin(eulers);cos(eulers)];
% Assemble Quaternion components
q = zeros(4,1);
q(1) = trigArgs(4)*trigArgs(5)*trigArgs(6) + trigArgs(1)*trigArgs(2)*trigArgs(3);
q(2) = trigArgs(4)*trigArgs(5)*trigArgs(3) - trigArgs(1)*trigArgs(2)*trigArgs(6);
q(3) = trigArgs(4)*trigArgs(2)*trigArgs(6) + trigArgs(1)*trigArgs(5)*trigArgs(3);
q(4) = trigArgs(1)*trigArgs(5)*trigArgs(6) - trigArgs(4)*trigArgs(2)*trigArgs(3);
% Renormalise
q = unit(q);
end


clear; close all; clc;

% A = [2 2;2 7];
% A =[1,3;4,2]/4;
A=[2,1;1,2]; % shear
% angle = pi/2; A = [cos(angle) -sin(angle); sin(angle) cos(angle)]; %rotation
% A = [0, 1; 1, 0]; % permutation
% A = [1,0;0,0]; % projection
% vector = [1,2]'; A = vector*vector'; % projection on a vector

%% animation with DOTS
n_steps = 100;
[x,y] = ndgrid(-1:0.15:1);
xy_min = min(min(A*[x(:), y(:)]'))*1.5;
xy_max = max(max(A*[x(:), y(:)]'))*1.5;

dot_colors = jet(length(x(:)));

figure(2);
set(gcf,'color','w')
set(gcf,'position',[250 120 960 540])
scatter(x(:), y(:),30,dot_colors,'filled')
grid on;
xlim([xy_min, xy_max]); ylim([xy_min, xy_max]);
% pause;

step_mtx = (A-eye(2))/n_steps;

for i_steps = 1:n_steps

    new_xy = (eye(2)+step_mtx*i_steps)*[x(:), y(:)]';
    scatter(new_xy(1,:), new_xy(2,:),30,dot_colors,'filled')

    grid on;
    xlim([xy_min, xy_max]); ylim([xy_min, xy_max]);
    axis square

    pause(0.01);
end

%%
v = VideoWriter('permutation_transform.mp4','MPEG-4');
v.Quality = 100;

open(v);
writeVideo(v,F);
close(v);


function [tfr,t,f] = tfrppage(x,t,N,h,trace);
%TFRPPAGE Pseudo Page time-frequency distribution.
%	[TFR,T,F]=TFRPPAGE(X,T,N,H,TRACE) computes the Pseudo Page
%	distribution of a discrete-time signal X, or the
%	cross Pseudo Page representation between two signals.
%
%	X     : signal if auto-PPage, or [X1,X2] if cross-PPage.
%	T     : time instant(s)          (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%	H     : frequency smoothing window, H(0) being forced to 1.
%	                                 (default : Hamming(N/4)).
%	TRACE : if nonzero, the progression of the algorithm is shown
%	                                 (default : 0).
%	TFR   : time-frequency representation. When called without
%	        output arguments, TFRPPAGE runs TFRQVIEW.
%	F     : vector of normalized frequencies.
%
%	Example :
%	 sig=fmlin(128,0.1,0.4); tfrppage(sig);
%
%	See also all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

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

[xrow,xcol] = size(x);
if (nargin < 1),
 error('At least 1 parameter is required');
elseif nargin<=2,
 N=xrow;
end;

hlength=floor(N/4);
if (rem(hlength,2)==0),
 hlength=hlength+1;
end;

if (nargin == 1),
 t=1:xrow; h = tftb_window(hlength); trace=0;
elseif (nargin == 2 | nargin == 3),
 h = tftb_window(hlength); trace = 0;
elseif (nargin == 4),
 trace = 0;
end;

if (N<0),
 error('N must be greater than zero');
end;
[trow,tcol] = size(t);
if (xcol==0)|(xcol>2),
 error('X must have one or two columns');
elseif (trow~=1),
 error('T must only have one row');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

[hrow,hcol]=size(h); Lh=(hrow-1)/2; h=h/h(Lh+1);
if (hcol~=1)|(rem(hrow,2)==0),
 error('H must be a smoothing window with odd length');
end;

tfr= zeros (N,tcol) ;
if trace, disp('Pseudo Page distribution'); end;
for icol=1:tcol,
 ti= t(icol); tau=0:min([N-1,Lh,ti-1]);
 indices= rem(N+tau,N)+1;
 if trace, disprog(icol,tcol,10); end;
 tfr(indices,icol)=h(Lh+1+tau).*x(ti,1).*conj(x(ti-tau,xcol));
end;
if trace, fprintf('\n'); end;
tfr= real(fft(tfr));

if (nargout==0),
 tfrqview(tfr,x,t,'tfrppage',h);
elseif (nargout==3),
 if rem(N,2)==0,
  f=[0:N/2-1 -N/2:-1]'/N;
 else
  f=[0:(N-1)/2 -(N-1)/2:-1]'/N;
 end;
end;


function value = r8_choose ( n, k )

%*****************************************************************************80
%
%% R8_CHOOSE computes the binomial coefficient C(N,K).
%
%  Discussion:
%
%    The value is calculated in such a way as to avoid overflow and
%    roundoff.  The calculation is done in integer arithmetic.
%
%    The formula used is:
%
%      C(N,K) = N! / ( K! * (N-K)! )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    ML Wolfson, HV Wright,
%    Algorithm 160:
%    Combinatorial of M Things Taken N at a Time,
%    Communications of the ACM,
%    Volume 6, Number 4, April 1963, page 161.
%
%  Parameters:
%
%    Input, integer N, K, are the values of N and K.
%
%    Output, real VALUE, the number of combinations of N
%    things taken K at a time.
%
  mn = min ( k, n - k );

  if ( mn < 0 )

    value = 0;

  elseif ( mn == 0 )

    value = 1;

  else

    mx = max ( k, n - k );
    value = mx + 1;

    for i = 2 : mn
      value = ( value * ( mx + i ) ) / i;
    end

  end

  return
end


function p = predict(Theta1, Theta2, X)
%PREDICT Predict the label of an input given a trained neural network
%   p = PREDICT(Theta1, Theta2, X) outputs the predicted label of X given the
%   trained weights of a neural network (Theta1, Theta2)

% Useful values
m = size(X, 1);
num_labels = size(Theta2, 1);

% You need to return the following variables correctly
p = zeros(size(X, 1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned neural network. You should set p to a
%               vector containing labels between 1 to num_labels.
%
% Hint: The max function might come in useful. In particular, the max
%       function can also return the index of the max element, for more
%       information see 'help max'. If your examples are in rows, then, you
%       can use max(A, [], 2) to obtain the max for each row.
%
X = [ones(m, 1) X];
XX = sigmoid(X*Theta1');
pp = sigmoid([ones(size(XX, 1), 1) XX] * Theta2');
[a, p] = max(pp, [], 2);

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

end


function p = predict(Theta1, Theta2, X)
%PREDICT Predict the label of an input given a trained neural network
%   p = PREDICT(Theta1, Theta2, X) outputs the predicted label of X given the
%   trained weights of a neural network (Theta1, Theta2)

% Useful values
m = size(X, 1);
num_labels = size(Theta2, 1);

% You need to return the following variables correctly
p = zeros(size(X, 1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned neural network. You should set p to a
%               vector containing labels between 1 to num_labels.
%
% Hint: The max function might come in useful. In particular, the max
%       function can also return the index of the max element, for more
%       information see 'help max'. If your examples are in rows, then, you
%       can use max(A, [], 2) to obtain the max for each row.
%

X = [ones(m, 1) X];

for j=1:m,

    %first layer propagation
    z2 = sigmoid(X(j,:) * Theta1');

    %bias to hidden layer
    z2 = [1 z2];

    %hidden layer propagation and getting max (candidate)
    [trash,p(j)] = max(sigmoid(z2 * Theta2'));
end;

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

end


function y = loggausspdf2(X, sigma)
% log pdf of Gaussian with zero mena
% Based on code written by Mo Chen (mochen@ie.cuhk.edu.hk). March 2009.
d = size(X,1);

[R,p]= chol(sigma);
if p ~= 0
    error('ERROR: sigma is not SPD.');
end
q = sum((R'\X).^2,1);  % quadratic term (M distance)
c = d*log(2*pi)+2*sum(log(diag(R)));   % normalization constant
y = -(c+q)/2;


function [path P] = path_from_pred(pred,d,varargin)
% PATH_FROM_PRED Convert a predecessor array into a path to a vertex.
%
% path = path_from_pred(pred,d) returns the list of vertices on the path
% from a source vertex to a vertex d.  The predecessor array is a row
% vector such that
%   pred(i) = 0 if vertex i is a source vertex u,
%   pred(i) = 0 if vertex i has no predecessor associated with it, or
%   pred(i) = j where j preceeds vertex i on the shortest path from u to i.
% The vertex d is a number from 1 to n, where n is the length of the
% predecessor array.
%
% The returned path is a 1 by k+1 array where the path from the source to d
% has k edges path lists the order of visting the vertices.  In the case
% that the vertex is the source or unreachable from the source, the path
% just has the single starting vertex.  This reflects one potential problem
% with this function, the source is not uniquely encoded in the predecessor
% array and the source and unreachable vertices are treated equally.
%
% [path P] = path_from_pred(pred,d) also returns the adjacency matrix of the
% directed graph corresponding to the path.  Building this matrix takes
% additional time.
%
% Example:
%    load('graphs/bfs_example.mat');
%    [d dt pred] = bfs(A,1,struct('target', 3));
%    path = path_from_pred(pred,3); % sequence of vertices to vertex 3
%
% See also BFS, DFS, SHORTEST_PATHS

% David Gleich
% Copyright, Stanford University, 2007-2008

%% History
%  2007-04-17: Initial coding
%%

% TODO handle the case of a matrix of predecessors from all_shortest_paths

path = path_from_pred_mex(pred,d);

if nargout == 2
    n = length(pred);
    P = sparse(path(1:end-1), path(2:end), 1, n, n);
end


function g = sigmoidGradient(z)
%SIGMOIDGRADIENT returns the gradient of the sigmoid function
%evaluated at z
%   g = SIGMOIDGRADIENT(z) computes the gradient of the sigmoid function
%   evaluated at z. This should work regardless if z is a matrix or a
%   vector. In particular, if z is a vector or matrix, you should return
%   the gradient for each element.

g = zeros(size(z));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the gradient of the sigmoid function evaluated at
%               each value of z (z can be a matrix, vector or scalar).

g = sigmoid(z) .* (1 - sigmoid(z));

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

end


function result = polygon_1_2d ( n, v )

%*****************************************************************************80
%
%% POLYGON_1_2D integrates the function 1 over a polygon in 2D.
%
%  Discussion
%
%    The polygon is bounded by the points (X(1:N), Y(1:N)).
%
%    INTEGRAL = 0.5 * SUM ( 1 <= I <= N )
%      ( X(I) + X(I-1) ) * ( Y(I) - Y(I-1) )
%
%    where X(0) and Y(0) should be replaced by X(N) and Y(N).
%
%    Note that the integral of 1 over a polygon is the area of the polygon.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    S F Bockman,
%    Generalizing the Formula for Areas of Polygons to Moments,
%    American Mathematical Society Monthly,
%    1989, pages 131-132.
%
%  Parameters:
%
%    Input, integer N, the number of vertices of the polygon.
%    N should be at least 3 for a nonzero result.
%
%    Input, real V(2,N), the coordinates of the vertices
%    of the polygon.  These vertices should be given in counter-clockwise order.
%
%    Output, real RESULT, the value of the integral.
%
  result = 0.0;

  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGON_1_2D - Warning!\n' );
    fprintf ( 1, '  The number of vertices must be at least 3.\n' );
    fprintf ( 1, '  The input value of N = %d\n', n );
    error ( 'POLYGON_1_2D - Fatal error!' );
  end

  for i = 1 : n

    if ( i == 1 )
      im1 = n;
    else
      im1 = i - 1;
    end

    result = result + 0.5 * ( v(1,i) + v(1,im1) ) * ( v(2,i) - v(2,im1) );

  end

  return
end


function [V,F]=platonic_solid(varargin)

% function [V,F]=platonic_solid(n,r)
% ------------------------------------------------------------------------
% PLATONIC_SOLID Creates the PATCH data, the vertices (V) and faces (F) for
% a given platonic solid (according to "n" see below) with radius (r)
%
% n=1 -> Tetrahedron
% n=2 -> Cube
% n=3 -> Octahedron
% n=4 -> Icosahedron
% n=5 -> Dodecahedron
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 12/08/2014 Updated for GIBBON
%------------------------------------------------------------------------
%% Parse input

switch nargin
    case 1
        n=varargin{1};
        r=1;
    case 2
        n=varargin{1};
        r=varargin{2};
end

if isempty(r)
    r=1;
end

%%
switch n
    case 1 % Tetrahedron
        X=[-0.5;0.5;0;0;];
        Y=[-sqrt(3)/6;  -sqrt(3)/6; sqrt(3)/3; 0];
        Z=[-0.25.*sqrt(2/3); -0.25.*sqrt(2/3); -0.25.*sqrt(2/3);  0.75.*sqrt(2/3)];
        X=X([1 2 4 3]);
        Y=Y([1 2 4 3]);
        Z=Z([1 2 4 3]);
        F=[3,2,1;1,2,4;2,3,4;4,3,1;];
        F=fliplr(F);
    case 2 % Cube
        X=[-1;  1; 1; -1; -1;  1; 1; -1;];
        Y=[-1; -1; 1;  1; -1; -1; 1;  1;];
        Z=[-1; -1;-1; -1;  1;  1; 1;  1;];
        F=[4,3,2,1;
            1,2,6,5;
            2,3,7,6;
            3,4,8,7;
            4,1,5,8;
            5,6,7,8;];
    case 3 % Octahedron
        X=[-1;  1; 1; -1;  0;   0;];
        Y=[-1; -1; 1;  1;  0;   0;];
        Z=[ 0;   0; 0;  0; -1;  1;];
        F=[5,2,1;5,3,2;5,4,3;5,1,4;1,2,6;2,3,6;3,4,6;4,1,6;];
    case 4 % Icosahedron
        phi=(1+sqrt(5))/2;
        X=[0;0;0;0;-1;-1;1;1;-phi;phi;phi;-phi;];
        Y=[-1;-1;1;1;-phi;phi;phi;-phi;0;0;0;0;];
        Z=[-phi;phi;phi;-phi;0;0;0;0;-1;-1;1;1;];
        F=[9,4,1;1,5,9;1,8,5;10,8,1;4,10,1;5,2,12;12,2,3;12,3,6;12,6,9;12,9,5;10,7,11;8,10,11;2,8,11;3,2,11;7,3,11;2,5,8;10,4,7;7,6,3;6,7,4;6,4,9;];
    case 5 % Dodecahedron
        phi=(1+sqrt(5))/2;
        X=[1;(1/phi);-phi;phi;-1;0;-phi;1;-1;-1;1;(1/phi);-1;0;0;-(1/phi);phi;-(1/phi);1;0;];
        Y=[1;0;-(1/phi);(1/phi);1;-phi;(1/phi);-1;1;-1;-1;0;-1;-phi;phi;0;-(1/phi);0;1;phi;];
        Z=[1;phi;0;0;-1;-(1/phi);0;1;1;1;-1;-phi;-1;(1/phi);-(1/phi);phi;0;-phi;-1;(1/phi);];
        F=[20,9,16,2,1;2,16,10,14,8;16,9,7,3,10;7,9,20,15,5;18,13,3,7,5;3,13,6,14,10;6,13,18,12,11;6,11,17,8,14;11,12,19,4,17;1,2,8,17,4;1,4,19,15,20;12,18,5,15,19;];
    otherwise
        warning('False input for n')
end

%Altering radius
[THETA,PHI,~]=cart2sph(X,Y,Z);
[X,Y,Z]=sph2cart(THETA,PHI,r.*ones(size(X)));
V=[X(:) Y(:) Z(:)];

%%
% _*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 krn = spm_smoothkern(fwhm,x,t)
% Generate a Gaussian smoothing kernel
% FORMAT krn = spm_smoothkern(fwhm,x,t)
% fwhm - full width at half maximum
% x    - position
% t    - either 0 (nearest neighbour) or 1 (linear).
%        [Default: 1]
%
% krn  - value of kernel at position x
%__________________________________________________________________________
%
% For smoothing images, one should really convolve a Gaussian with a sinc
% function. For smoothing histograms, the kernel should be a Gaussian
% convolved with the histogram basis function used. This function returns
% a Gaussian convolved with a triangular (1st degree B-spline) basis
% function (by default). A Gaussian convolved with a hat function (0th
% degree B-spline) can also be returned.
%__________________________________________________________________________
% Copyright (C) 2005-2011 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_smoothkern.m 4419 2011-08-03 18:42:35Z guillaume $

if nargin<3, t = 1; end

% Variance from FWHM
s = (fwhm/sqrt(8*log(2)))^2+eps;

% The simple way to do it. Not good for small FWHM
% krn = (1/sqrt(2*pi*s))*exp(-(x.^2)/(2*s));

if t==0
    % Gaussian convolved with 0th degree B-spline
    % int(exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t= -0.5..0.5)
    w1  = 1/sqrt(2*s);
    krn = 0.5*(erf(w1*(x+0.5))-erf(w1*(x-0.5)));
    krn(krn<0) = 0;

elseif t==1
    % Gaussian convolved with 1st degree B-spline
    %  int((1-t)*exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t= 0..1)
    % +int((t+1)*exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t=-1..0)
    w1  =  0.5*sqrt(2/s);
    w2  = -0.5/s;
    w3  = sqrt(s/2/pi);
    krn = 0.5*(erf(w1*(x+1)).*(x+1) + erf(w1*(x-1)).*(x-1) - 2*erf(w1*x   ).* x)...
          +w3*(exp(w2*(x+1).^2)     + exp(w2*(x-1).^2)     - 2*exp(w2*x.^2));
    krn(krn<0) = 0;

else
    error('Only defined for nearest neighbour and linear interpolation.');
    % If anyone knows a nice formula for a sinc function convolved with a
    % a Gaussian, then that could be quite useful.
end


function [node,face,centroids]=latticegrid(varargin)
%
% [node,face,centroids]=latticegrid(xrange,yrange,zrange,...)
%
% generate a 3D lattice
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%   xrange, yrange, zrange ...: 1D vectors specifying the range of each
%         dimension of the lattice
%
% output:
%   node: the vertices of the 3D lattice
%   face: the list of cell faces of the lattice, including both internal
%         and external facets. By default, face is in the form of a cell
%         array, with each row representing a face. One can use
%         cell2mat(face) to convert it to an array
%   centroids: the centroids of each lattice cell
%
% example:
%    % generate a 3D lattice
%    [node,face,c0]=latticegrid([1 2 4],1:3,1:4);
%    plotmesh(node,face)
%
%    % mesh the 3D lattice based on the face info
%    [no,el]=surf2mesh(node,face,[],[],1,0.01,c0);
%    figure; plotmesh(no,el)
%
%    % mesh a 2-layer structure using a simple lattice
%    [node,face,c0]=latticegrid([0 10],[0 5],[0 3.5 4]);
%    c0(:,4)=[0.01;0.001];
%    [no,el]=surf2mesh(node,face,[],[],1,[],c0);
%    figure; plotmesh(no,el)
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

n=length(varargin);
p=cell(n,1);
[p{:}]=ndgrid(varargin{:});
node=zeros(length(p{1}(:)),n);
for i=1:n
   node(:,i)=p{i}(:);
end
if(nargout==1)
    return;
end

dim=size(p{1});

dd=[dim(1) dim(1)*dim(2)];
onecube=[0 dd(1) dd(1)+1 1; ...
         0 1 dd(2)+1 dd(2); ...
         0 dd(2) dd(2)+dd(1) dd(1)];
onecube=[onecube;onecube+repmat([dd(2);dd(1);1],1,4)];

len=prod(dim(1:3)-1);
face=repmat(onecube,len,1);
[xx,yy,zz]=ndgrid(1:dim(1)-1,1:dim(2)-1,1:dim(3)-1);
idx=sub2ind(dim,xx(:),yy(:),zz(:))';
orig=repmat(idx,size(onecube,1),1);

for i=1:size(onecube,2)
    face(:,i)=face(:,i)+orig(:);
end
face=unique(face,'rows');
face=mat2cell(face,ones(size(face,1),1));

if(nargout>=3)
    diffvec=cellfun(@diff,varargin,'UniformOutput',false);
    [xx,yy,zz]=ndgrid(diffvec{:});
    centroids=node(idx,:)+[xx(:) yy(:) zz(:)]*0.5;
end


function variance = logistic_variance ( a, b )

%*****************************************************************************80
%
%% LOGISTIC_VARIANCE returns the variance of the Logistic PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < B.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = ( pi * b )^2 / 3.0;

  return
end


function [ lambda, x ] = jacobi_iterate ( n, a )

%*****************************************************************************80
%
%% JACOBI_ITERATE applies the Jacobi eigenvalue iteration to a symmetric matrix.
%
%  Discussion:
%
%    I had to modify the code, in order to avoid cases where the
%    off-diagonal element was not exactly zero, but very very close.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real A(N,N), a symmetric matrix.
%    On output, the matrix has been overwritten by an approximately
%    diagonal matrix, with the eigenvalues on the diagonal.
%
%    Output, real LAMBDA(N), the computed eigenvalues.
%
%    Output, real X(N,N), the computed eigenvector matrix.
%
  eps = 0.00001;
  maxit = 100;

  error_frobenius = r8mat_is_symmetric ( n, n, a );

  if ( eps < error_frobenius )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'JACOBI_ITERATE - Fatal error!\n' );
    fprintf ( 1, '  The input matrix is not symmetric.\n' );
    error ( 'JACOBI_ITERATE - Fatal error!' );
  end

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

  norm_fro = r8mat_norm_fro ( n, n, b );

  x = identity ( n, n );

  for it = 1 : maxit

    for i = 1 : n
      for j = 1 : i - 1

        if ( eps * norm_fro < abs ( b(i,j) ) + abs ( b(j,i) ) )

          u = ( b(j,j) - b(i,i) ) / ( b(i,j) + b(j,i) );

          t = r8_sign ( u ) / ( abs ( u ) + sqrt ( u * u + 1.0 ) );
          c = 1.0 / sqrt ( t * t + 1.0 );
          s = t * c;
%
%  A -> A * Q.
%
          for k = 1 : n
            t1 = b(i,k);
            t2 = b(j,k);
            b(i,k) = t1 * c - t2 * s;
            b(j,k) = t1 * s + t2 * c;
          end
%
%  A -> Q' * A
%
          for k = 1 : n
            t1 = b(k,i);
            t2 = b(k,j);
            b(k,i) = c * t1 - s * t2;
            b(k,j) = s * t1 + c * t2;
          end
%
%  X -> Q' * X
%
          for k = 1 : n
            t1 = x(k,i);
            t2 = x(k,j);
            x(k,i) = c * t1 - s * t2;
            x(k,j) = s * t1 + c * t2;
          end

        end

      end
    end
%
%  Test the size of the off-diagonal elements.
%
    sum2 = 0.0;
    for i = 1 : n
      sum2 = sum2 + sum ( abs ( b(i,1:i-1) ) );
    end

    if ( sum2 <= eps * ( norm_fro + 1.0 ) )
      break
    end

  end

  lambda = diag ( b );

  return
end


function [EC,ec,degij] = edge_nei_overlap_bu(CIJ)
% EDGE_NEI_OVERLAP_BU        Overlap amongst neighbors of two adjacent nodes
%
%   [EC,ec,degij] = edge_nei_bu(CIJ);
%
%   This function determines the neighbors of two nodes that are linked by
%   an edge, and then computes their overlap.  Connection matrix must be
%   binary and directed.  Entries of 'EC' that are 'inf' indicate that no
%   edge is present.  Entries of 'EC' that are 0 denote "local bridges", i.e.
%   edges that link completely non-overlapping neighborhoods.  Low values
%   of EC indicate edges that are "weak ties".
%
%   If CIJ is weighted, the weights are ignored.
%
%   Inputs:     CIJ,    undirected (binary/weighted) connection matrix
%
%   Outputs:    EC,     edge neighborhood overlap matrix
%               ec,     edge neighborhood overlap per edge, in vector format
%               degij,  degrees of node pairs connected by each edge
%
%   Reference: Easley and Kleinberg (2010) Networks, Crowds, and Markets.
%              Cambridge University Press, Chapter 3.
%
%   Olaf Sporns, Indiana University, 2012

[ik,jk,ck] = find(CIJ);
lel = length(ck);
N = size(CIJ,1);

[deg] = degrees_und(CIJ);

ec = zeros(1,lel);
degij = zeros(2,lel);
for e=1:lel
    neiik = setdiff(union(find(CIJ(ik(e),:)),find(CIJ(:,ik(e))')),[ik(e) jk(e)]);
    neijk = setdiff(union(find(CIJ(jk(e),:)),find(CIJ(:,jk(e))')),[ik(e) jk(e)]);
    ec(e) = length(intersect(neiik,neijk))/length(union(neiik,neijk));
    degij(:,e) = [deg(ik(e)) deg(jk(e))];
end;

ff = find(CIJ);
EC = 1./zeros(N);
EC(ff) = ec;                        %#ok<FNDSB>


function quadmom_test08 ( )

%*****************************************************************************80
%
%% QUADMOM_TEST08 integrates sin(x) against a lower truncated normal weight.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub, John Welsch,
%    Calculation of Gaussian Quadrature Rules,
%    Mathematics of Computation,
%    Volume 23, Number 106, April 1969, pages 221-230.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUADMOM_TEST08:\n' );
  fprintf ( 1, '  Integrate sin(x) against a lower truncated normal weight.\n' );

  mu = 0.0;
  sigma = 1.0;
  a = -3.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  MU = %g\n', mu );
  fprintf ( 1, '  SIGMA = %g\n', sigma );
  fprintf ( 1, '  A = %g\n', a );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   N    Estimate\n' );
  fprintf ( 1, '\n' );
%
%  N is the order of the quadrature rule.
%
  for n = 1 : 9
%
%  Compute the M = 2 * N + 1 moments.
%
    m = 2 * n + 1;

    moment = moments_truncated_normal_a ( m, mu, sigma, a );
%
%  Compute the N points and weights by the method of moments.
%
    [ x, w ] = moment_method ( n, moment );
%
%  Evaluate the quadrature rule when f(x) = sin(x).
%
    q = w' * sin ( x );

    fprintf ( 1, '  %2d  %14.6g\n', n, q );

  end

  return
end


function p = legendre(n,var)
%LEGENDRE     Legendre polynomial
%
%   p = legendre(n,var)
%
%Computed by recursion
%  p(0,x) = 1
%  p(1,x) = x
%  p(n,x) = (2*n-1)/n * x * p(n-1,x) - (n-1)/n * p(n-2,x)    for n>1
%
%Parameter var for dependent variable optional (default 'x')
%

% written  07/30/02     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

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

  if nargin==1
    var = 'x';
  else
    if ~ischar(var)
      error('variable must be string')
    end
  end

  if n==0
    p = polynom(1,var);
  elseif n==1
    p = polynom([1 0],var);
  else
    t1 = [1 zeros(1,n)];
    t2 = [0 1 zeros(1,n-1)];
    for i=3:n+1
      t3 = (2*i-3)/(i-1)*[0 t2(1:n)] - (i-2)/(i-1)*t1;
      t1 = t2;
      t2 = t3;
    end
    p = polynom(fliplr(t3),var);
  end

  if rndold
    setround(rndold)
  end


function [A, B] = zeroKronApprox( K, m, n )
%
%       [A, B] = zeroKronApprox( K, m, n );
%
% computes a kronecker sum approximation to the blurring matrix K
% that arises from the input PSF under zero boundary conditions.
% This approximation is done a-la Kamm-Nagy (see paper for details).
%
%  Input:
%         K - psfMatrix object
%         m - size of matrix A (assumed square)
%         n - size of matrix B (assumed square)
%
%  Output:
%      Matrices A and B such that K \aprrox A \otimes B.
%

%  J. Nagy  2/11/02

%  Modifications:  This used to be zeroSepMatrix, which did not compute
%                  A and B.
%  5/25/02, J. Nagy
%           This now computes A and B.
%
%  11/17/02, J. Nagy
%            This was designed for image processing problems, where
%            it is common to use lexicographical (row) ordering.
%            But @kronMatrix functions where designed using vec (column)
%            ordering.  This inconsistency has been fixed.

P1 = K.psf;
P2 = P1.image;
PSF = P2{1};
c1 = P1.center;
center = c1{1};

[mp, np] = size(PSF);

if ( mp ~= np )
  error('For now, we expect PSF to be square')
end

%
% Compute weighted PSF.
%
for i = 1:center(1)
  Aweights(i,1) = sqrt( i+mp-center(1) );
end;
for i = center(1)+1:mp
  Aweights(i,1) = sqrt( mp+center(1)-i);
end;
for i = 1:center(2)
  Bweights(i,1) = sqrt( i+np-center(2) );
end;
for i = center(2)+1:np
  Bweights(i,1) = sqrt( np+center(2)-i );
end;

Phat = (Aweights*Bweights').*PSF;
%
% Compute SVD of weighted PSF, which is then used to construct
% the separable approximation.
%
[U,S,V] = svd( Phat );

%
% check to make sure first column looks like
% a Gaussian, and is not inverted.
%
minU = abs(min(min(U(:,1))));
maxU = max(max(abs(U(:,1))));
if minU == maxU
  U = -U;
  V = -V;
end

%
% Construct approximation.
%
a = ( U(:,1) * sqrt(S(1,1)) ) ./ Aweights;
b = ( V(:,1) * sqrt(S(1,1)) ) ./ Bweights;

%
%  This construction corresponds to lexicographical ordering,
%  but kronMatrix does everything corresponding to vec ordering.
%  Thus, these two do not work together.
%%
%%A = build_toep(a, center(1), n);
%%B = build_toep(b, center(2), m);

%  To fix this, we just need to switch A and B.  Then kronMatrix
%  can continue to be used corresponding to a vec ordering.
%
A = build_toep(b, center(2), m);
B = build_toep(a, center(1), n);


function [ v1, v2, v3, v4 ] = tetrahedron_ref ( )

%*****************************************************************************80
%
%% TETRAHEDRON_REF returns the vertices of the reference tetrahedron.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 July 2014
%
%  Author:
%
%    John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Output, real V1(3), V2(3), V3(3), V4(3), the vertices.
%
  v1(1) = - 1.0;
  v1(2) = - 1.0 / sqrt ( 3.0 );
  v1(3) = - 1.0 / sqrt ( 6.0 );

  v2(1) =   0.0;
  v2(2) =   2.0 / sqrt ( 3.0 );
  v2(3) = - 1.0 / sqrt ( 6.0 );

  v3(1) =   1.0;
  v3(2) = - 1.0 / sqrt ( 3.0 );
  v3(3) = - 1.0 / sqrt ( 6.0 );

  v4(1) =   0.0;
  v4(2) =   0.0;
  v4(3) =   3.0 / sqrt ( 6.0 );

  return
end


function pdf = beta_pdf ( x, a, b )

%*****************************************************************************80
%
%% BETA_PDF evaluates the Beta PDF.
%
%  Discussion:
%
%    PDF(X)(A,B) = X**(A-1) * (1-X)**(B-1) / BETA(A,B).
%
%    A = B = 1 yields the Uniform distribution on [0,1].
%    A = B = 1/2 yields the Arcsin distribution.
%        B = 1 yields the power function distribution.
%    A = B -> Infinity tends to the Normal distribution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%    0.0 <= X <= 1.0.
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < A,
%    0.0 < B.
%
%    Output, real PDF, the value of the PDF.
%
  if ( x < 0.0 | 1.0 < x )
    pdf = 0.0;
  else
    pdf = x^( a - 1.0 ) * ( 1.0 - x )^( b - 1.0 ) / beta ( a, b );
  end

  return
end


function [y,dy] = softbndloss(x,slb,sub,TolCon)
%SOFTBNDLOSS Loss function for soft bounds for function minimization.

% Penalization relative scale
if nargin < 4 || isempty(TolCon); TolCon = 1e-3; end

compute_grad = nargout > 1;     % Compute gradient only if requested

ell = (sub - slb).*TolCon;

y = 0;
dy = zeros(size(x));

idx = x < slb;
if any(idx)
    y = y + 0.5*sum(((slb(idx) - x(idx))./ell(idx)).^2);
    if compute_grad
        dy(idx) = (x(idx) - slb(idx))./ell(idx).^2;
    end
end

idx = x > sub;
if any(idx)
    y = y + 0.5*sum(((x(idx) - sub(idx))./ell(idx)).^2);
    if compute_grad
        dy(idx) = (x(idx) - sub(idx))./ell(idx).^2;
    end
end

end


%% A script to generate copy'able functions for quad-curl problem
% used in "Error analysis of a decoupled finite element method for quad-curl problems"
% https://arxiv.org/abs/2102.03396
%
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

clear; clc;
syms x y z
% U = [sin(y), -sin(z), sin(x)];
% U = [0, 0, (sin(x))^2*(sin(y))^2*sin(z)];
U = [0, 0, (sin(x)).^3.*(sin(y)).^3.*(sin(z)).^2];
% U = [0, 0, x^2*(1-x)^2*y^2*(1-y)^2*z^2*(1-z)^2];

X = [x y z];

%%
curlu = curl(U,X);
curlcurlu = curl(curlu,X);
tricurlu = curl(curlcurlu,X);
quadcurlu = curl(tricurlu,X);
pentacurlu = curl(quadcurlu,X);
hexacurlu = curl(pentacurlu,X);

vectorsStr = {'curlu', 'curlcurlu', 'tricurlu', ...
              'quadcurlu', 'pentacurlu', 'hexacurlu'};

%%
for idx = 1:length(vectorsStr)
    vector = eval(vectorsStr{idx});
    fprintf("function s = %s(p)\n",vectorsStr{idx})
    fprintf("x = p(:,1); y = p(:,2); z = p(:,3);\n")
    for j = 1:3
        vStr = char(vector(j));
        vStr = strrep(vStr, '*', '.*');
        vStr = strrep(vStr, '^', '.^');
        fprintf("s(:,%d) = %s;\n", j, vStr)
    end
    fprintf('end\n\n')
end


function [costs,paths] = dijkstra(AorV,xyCorE,SID,FID,iswaitbar)
%DIJKSTRA Calculate Minimum Costs and Paths using Dijkstra's Algorithm
%
%   Inputs:
%     [AorV] Either A or V where
%         A   is a NxN adjacency matrix, where A(I,J) is nonzero (=1)
%               if and only if an edge connects point I to point J
%               NOTE: Works for both symmetric and asymmetric A
%         V   is a Nx2 (or Nx3) matrix of x,y,(z) coordinates
%     [xyCorE] Either xy or C or E (or E3) where
%         xy  is a Nx2 (or Nx3) matrix of x,y,(z) coordinates (equivalent to V)
%               NOTE: only valid with A as the first input
%         C   is a NxN cost (perhaps distance) matrix, where C(I,J) contains
%               the value of the cost to move from point I to point J
%               NOTE: only valid with A as the first input
%         E   is a Px2 matrix containing a list of edge connections
%               NOTE: only valid with V as the first input
%         E3  is a Px3 matrix containing a list of edge connections in the
%               first two columns and edge weights in the third column
%               NOTE: only valid with V as the first input
%     [SID] (optional) 1xL vector of starting points
%         if unspecified, the algorithm will calculate the minimal path from
%         all N points to the finish point(s) (automatically sets SID = 1:N)
%     [FID] (optional) 1xM vector of finish points
%         if unspecified, the algorithm will calculate the minimal path from
%         the starting point(s) to all N points (automatically sets FID = 1:N)
%     [iswaitbar] (optional) a scalar logical that initializes a waitbar if nonzero
%
%   Outputs:
%     [costs] is an LxM matrix of minimum cost values for the minimal paths
%     [paths] is an LxM cell array containing the shortest path arrays
%
%   Revision Notes:
%     (4/29/09) Previously, this code ignored edges that have a cost of zero,
%     potentially producing an incorrect result when such a condition exists.
%     I have solved this issue by using NaNs in the table rather than a
%     sparse matrix of zeros. However, storing all of the NaNs requires more
%     memory than a sparse matrix. This may be an issue for massive data
%     sets, but only if there are one or more 0-cost edges, because a sparse
%     matrix is still used if all of the costs are positive.
%
%   Note:
%     If the inputs are [A,xy] or [V,E], the cost is assumed to be (and is
%       calculated as) the point-to-point Euclidean distance
%     If the inputs are [A,C] or [V,E3], the cost is obtained from either
%       the C matrix or from the edge weights in the 3rd column of E3
%
%   Example:
%       % Calculate the (all pairs) shortest distances and paths using [A,xy] inputs
%       n = 7; A = zeros(n); xy = 10*rand(n,2)
%       tri = delaunay(xy(:,1),xy(:,2));
%       I = tri(:); J = tri(:,[2 3 1]); J = J(:);
%       IJ = I + n*(J-1); A(IJ) = 1
%       [costs,paths] = dijkstra(A,xy)
%
%   Example:
%       % Calculate the (all pairs) shortest distances and paths using [A,C] inputs
%       n = 7; A = zeros(n); xy = 10*rand(n,2)
%       tri = delaunay(xy(:,1),xy(:,2));
%       I = tri(:); J = tri(:,[2 3 1]); J = J(:);
%       IJ = I + n*(J-1); A(IJ) = 1
%       a = (1:n); b = a(ones(n,1),:);
%       C = round(reshape(sqrt(sum((xy(b,:) - xy(b',:)).^2,2)),n,n))
%       [costs,paths] = dijkstra(A,C)
%
%   Example:
%       % Calculate the (all pairs) shortest distances and paths using [V,E] inputs
%       n = 7; V = 10*rand(n,2)
%       I = delaunay(V(:,1),V(:,2));
%       J = I(:,[2 3 1]); E = [I(:) J(:)]
%       [costs,paths] = dijkstra(V,E)
%
%   Example:
%       % Calculate the (all pairs) shortest distances and paths using [V,E3] inputs
%       n = 7; V = 10*rand(n,2)
%       I = delaunay(V(:,1),V(:,2));
%       J = I(:,[2 3 1]);
%       D = sqrt(sum((V(I(:),:) - V(J(:),:)).^2,2));
%       E3 = [I(:) J(:) D]
%       [costs,paths] = dijkstra(V,E3)
%
%   Example:
%       % Calculate the shortest distances and paths from the 3rd point to all the rest
%       n = 7; V = 10*rand(n,2)
%       I = delaunay(V(:,1),V(:,2));
%       J = I(:,[2 3 1]); E = [I(:) J(:)]
%       [costs,paths] = dijkstra(V,E,3)
%
%   Example:
%       % Calculate the shortest distances and paths from all points to the 2nd
%       n = 7; A = zeros(n); xy = 10*rand(n,2)
%       tri = delaunay(xy(:,1),xy(:,2));
%       I = tri(:); J = tri(:,[2 3 1]); J = J(:);
%       IJ = I + n*(J-1); A(IJ) = 1
%       [costs,paths] = dijkstra(A,xy,1:n,2)
%
%   Example:
%       % Calculate the shortest distance and path from points [1 3 4] to [2 3 5 7]
%       n = 7; V = 10*rand(n,2)
%       I = delaunay(V(:,1),V(:,2));
%       J = I(:,[2 3 1]); E = [I(:) J(:)]
%       [costs,paths] = dijkstra(V,E,[1 3 4],[2 3 5 7])
%
%   Example:
%       % Calculate the shortest distance and path between two points
%       n = 1000; A = zeros(n); xy = 10*rand(n,2);
%       tri = delaunay(xy(:,1),xy(:,2));
%       I = tri(:); J = tri(:,[2 3 1]); J = J(:);
%       D = sqrt(sum((xy(I,:)-xy(J,:)).^2,2));
%       I(D > 0.75,:) = []; J(D > 0.75,:) = [];
%       IJ = I + n*(J-1); A(IJ) = 1;
%       [cost,path] = dijkstra(A,xy,1,n)
%       gplot(A,xy,'k.:'); hold on;
%       plot(xy(path,1),xy(path,2),'ro-','LineWidth',2); hold off
%       title(sprintf('Distance from 1 to 1000 = %1.3f',cost))
%
% Web Resources:
%   Dijkstra's Algorithm
%   Graphs
%   Adjacency Matrix
%
% See also: gplot, gplotd, gplotdc, distmat, ve2axy, axy2ve
%
% Author: Joseph Kirk
% Email: jdkirk630@gmail.com
% Release: 1.1
% Date: 4/29/09

% Process Inputs
error(nargchk(2,5,nargin));
all_positive = 1;
[n,nc] = size(AorV);
[m,mc] = size(xyCorE);
[E,cost] = processInputs(AorV,xyCorE);
if nargin < 5
    iswaitbar = 0;
end
if nargin < 4
    FID = (1:n);
end
if nargin < 3
    SID = (1:n);
end
if max(SID) > n || min(SID) < 1
    eval(['help ' mfilename]);
    error('Invalid [SID] input. See help notes above.');
end
if max(FID) > n || min(FID) < 1
    eval(['help ' mfilename]);
    error('Invalid [FID] input. See help notes above.');
end

isreversed = 0;
if length(FID) < length(SID)
    E = E(:,[2 1]);
    cost = cost';
    tmp = SID;
    SID = FID;
    FID = tmp;
    isreversed = 1;
end

L = length(SID);
M = length(FID);
costs = zeros(L,M);
paths = num2cell(nan(L,M));

% Find the Minimum Costs and Paths using Dijkstra's Algorithm
if iswaitbar, wbh = waitbar(0,'Please Wait ... '); end
for k = 1:L
    % Initializations
    if all_positive, TBL = sparse(1,n); else TBL = NaN(1,n); end
    min_cost = Inf(1,n);
    settled = zeros(1,n);
    path = num2cell(nan(1,n));
    I = SID(k);
    min_cost(I) = 0;
    TBL(I) = 0;
    settled(I) = 1;
    path(I) = {I};

    while any(~settled(FID))
        % Update the Table
        TAB = TBL;
        if all_positive, TBL(I) = 0; else TBL(I) = NaN; end
        nids = find(E(:,1) == I);
        % Calculate the Costs to the Neighbor Points and Record Paths
        for kk = 1:length(nids)
            J = E(nids(kk),2);
            if ~settled(J)
                c = cost(I,J);
                if all_positive, empty = ~TAB(J); else empty = isnan(TAB(J)); end
                if empty || (TAB(J) > (TAB(I) + c))
                    TBL(J) = TAB(I) + c;
                    if isreversed
                        path{J} = [J path{I}];
                    else
                        path{J} = [path{I} J];
                    end
                else
                    TBL(J) = TAB(J);
                end
            end
        end

        if all_positive, K = find(TBL); else K = find(~isnan(TBL)); end
        % Find the Minimum Value in the Table
        N = find(TBL(K) == min(TBL(K)));
        if isempty(N)
            break
        else
            % Settle the Minimum Value
            I = K(N(1));
            min_cost(I) = TBL(I);
            settled(I) = 1;
        end
    end
    % Store Costs and Paths
    costs(k,:) = min_cost(FID);
    paths(k,:) = path(FID);
    if iswaitbar, waitbar(k/L,wbh); end
end
if iswaitbar, close(wbh); end

if isreversed
    costs = costs';
    paths = paths';
end

if L == 1 && M == 1
    paths = paths{1};
end

% -------------------------------------------------------------------
    function [E,C] = processInputs(AorV,xyCorE)
        C = sparse(n,n);
        if n == nc
            if m == n
                if m == mc % Inputs: A,cost
                    A = AorV;
                    A = A - diag(diag(A));
                    C = xyCorE;
                    all_positive = all(C(logical(A)) > 0);
                    E = a2e(A);
                else % Inputs: A,xy
                    A = AorV;
                    A = A - diag(diag(A));
                    xy = xyCorE;
                    E = a2e(A);
                    D = ve2d(xy,E);
                    all_positive = all(D > 0);
                    for row = 1:length(D)
                        C(E(row,1),E(row,2)) = D(row);
                    end
                end
            else
                eval(['help ' mfilename]);
                error('Invalid [A,xy] or [A,cost] inputs. See help notes above.');
            end
        else
            if mc == 2 % Inputs: V,E
                V = AorV;
                E = xyCorE;
                D = ve2d(V,E);
                all_positive = all(D > 0);
                for row = 1:m
                    C(E(row,1),E(row,2)) = D(row);
                end
            elseif mc == 3 % Inputs: V,E3
                E3 = xyCorE;
                all_positive = all(E3 > 0);
                E = E3(:,1:2);
                for row = 1:m
                    C(E3(row,1),E3(row,2)) = E3(row,3);
                end
            else
                eval(['help ' mfilename]);
                error('Invalid [V,E] inputs. See help notes above.');
            end
        end
    end

    % Convert Adjacency Matrix to Edge List
    function E = a2e(A)
        [I,J] = find(A);
        E = [I J];
    end

    % Compute Euclidean Distance for Edges
    function D = ve2d(V,E)
        VI = V(E(:,1),:);
        VJ = V(E(:,2),:);
        D = sqrt(sum((VI - VJ).^2,2));
    end
end


%
function a = fitellipse(X,Y)

% FITELLIPSE  Least-squares fit of ellipse to 2D points.
%        A = FITELLIPSE(X,Y) returns the parameters of the best-fit
%        ellipse to 2D points (X,Y).
%        The returned vector A contains the center, radii, and orientation
%        of the ellipse, stored as (Cx, Cy, Rx, Ry, theta_radians)
%
% Authors: Andrew Fitzgibbon, Maurizio Pilu, Bob Fisher
% Reference: "Direct Least Squares Fitting of Ellipses", IEEE T-PAMI, 1999
%
%  @Article{Fitzgibbon99,
%   author = "Fitzgibbon, A.~W.and Pilu, M. and Fisher, R.~B.",
%   title = "Direct least-squares fitting of ellipses",
%   journal = pami,
%   year = 1999,
%   volume = 21,
%   number = 5,
%   month = may,
%   pages = "476--480"
%  }
%
% This is a more bulletproof version than that in the paper, incorporating
% scaling to reduce roundoff error, correction of behaviour when the input
% data are on a perfect hyperbola, and returns the geometric parameters
% of the ellipse, rather than the coefficients of the quadratic form.
%
%  Example:  Run fitellipse without any arguments to get a demo
if nargin == 0
  % Create an ellipse
  t = linspace(0,2);

  Rx = 300;
  Ry = 200;
  Cx = 250;
  Cy = 150;
  Rotation = .4; % Radians

  NoiseLevel = .5; % Will add Gaussian noise of this std.dev. to points

  x = Rx * cos(t);
  y = Ry * sin(t);
  nx = x*cos(Rotation)-y*sin(Rotation) + Cx + randn(size(t))*NoiseLevel;
  ny = x*sin(Rotation)+y*cos(Rotation) + Cy + randn(size(t))*NoiseLevel;

  % Clear figure
  clf
  % Draw it
  plot(nx,ny,'o');
  % Show the window
  figure(gcf)
  % Fit it
  params = fitellipse(nx,ny);
  % Note it may return (Rotation - pi/2) and swapped radii, this is fine.
  Given = round([Cx Cy Rx Ry Rotation*180]);
  Returned = round(params.*[1 1 1 1 180]);

  % Draw the returned ellipse
  t = linspace(0,pi*2);
  x = params(3) * cos(t);
  y = params(4) * sin(t);
  nx = x*cos(params(5))-y*sin(params(5)) + params(1);
  ny = x*sin(params(5))+y*cos(params(5)) + params(2);
  hold on
  plot(nx,ny,'r-')

  return
end

% normalize data
mx = mean(X);
my = mean(Y);
sx = (max(X)-min(X))/2;
sy = (max(Y)-min(Y))/2;

x = (X-mx)/sx;
y = (Y-my)/sy;

% Force to column vectors
x = x(:);
y = y(:);

% Build design matrix
D = [ x.*x  x.*y  y.*y  x  y  ones(size(x)) ];

% Build scatter matrix
S = D'*D;

% Build 6x6 constraint matrix
C(6,6) = 0; C(1,3) = -2; C(2,2) = 1; C(3,1) = -2;

% Solve eigensystem
if 0
  % Old way, numerically unstable if not implemented in matlab
  [gevec, geval] = eig(S,C);

  % Find the negative eigenvalue
  I = find(real(diag(geval)) < 1e-8 & ~isinf(diag(geval)));

  % Extract eigenvector corresponding to negative eigenvalue
  A = real(gevec(:,I));
else
  % New way, numerically stabler in C [gevec, geval] = eig(S,C);

  % Break into blocks
  tmpA = S(1:3,1:3);
  tmpB = S(1:3,4:6);
  tmpC = S(4:6,4:6);
  tmpD = C(1:3,1:3);
  tmpE = inv(tmpC)*tmpB';

  % Return if tmpE contains nan
  if sum(sum(isnan(tmpE))) ~= 0
      a = [];
      return;
  end
  [evec_x, eval_x] = eig(inv(tmpD) * (tmpA - tmpB*tmpE));

  % Find the positive (as det(tmpD) < 0) eigenvalue
  I = find(real(diag(eval_x)) < 1e-8 & ~isinf(diag(eval_x)));

  % Extract eigenvector corresponding to negative eigenvalue
  A = real(evec_x(:,I));

  % Recover the bottom half...
  evec_y = -tmpE * A;
  A = [A; evec_y];
end

% unnormalize
par = [
  A(1)*sy*sy,   ...
      A(2)*sx*sy,   ...
      A(3)*sx*sx,   ...
      -2*A(1)*sy*sy*mx - A(2)*sx*sy*my + A(4)*sx*sy*sy,   ...
      -A(2)*sx*sy*mx - 2*A(3)*sx*sx*my + A(5)*sx*sx*sy,   ...
      A(1)*sy*sy*mx*mx + A(2)*sx*sy*mx*my + A(3)*sx*sx*my*my   ...
      - A(4)*sx*sy*sy*mx - A(5)*sx*sx*sy*my   ...
      + A(6)*sx*sx*sy*sy   ...
      ]';

% Convert to geometric radii, and centers

thetarad = 0.5*atan2(par(2),par(1) - par(3));
cost = cos(thetarad);
sint = sin(thetarad);
sin_squared = sint.*sint;
cos_squared = cost.*cost;
cos_sin = sint .* cost;

Ao = par(6);
Au =   par(4) .* cost + par(5) .* sint;
Av = - par(4) .* sint + par(5) .* cost;
Auu = par(1) .* cos_squared + par(3) .* sin_squared + par(2) .* cos_sin;
Avv = par(1) .* sin_squared + par(3) .* cos_squared - par(2) .* cos_sin;

% ROTATED = [Ao Au Av Auu Avv]

tuCentre = - Au./(2.*Auu);
tvCentre = - Av./(2.*Avv);
wCentre = Ao - Auu.*tuCentre.*tuCentre - Avv.*tvCentre.*tvCentre;

uCentre = tuCentre .* cost - tvCentre .* sint;
vCentre = tuCentre .* sint + tvCentre .* cost;

Ru = -wCentre./Auu;
Rv = -wCentre./Avv;

Ru = sqrt(abs(Ru)).*sign(Ru);
Rv = sqrt(abs(Rv)).*sign(Rv);

a = [uCentre, vCentre, Ru, Rv, thetarad];


function f = bohach1 ( m, x )

%*****************************************************************************80
%
%% BOHACH1 evaluates the Bohachevsky function #1.
%
%  Discussion:
%
%    The minimizer is
%
%      X* = [ 0.0, 0.0 ]
%      F(X*) = 0.0
%
%    Suggested starting point:
%
%      X = [ 0.5, 1.0 ];
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, integer M, the number of variables.
%
%    Input, real X(M), the argument of the function.
%
%    Output, real F, the value of the function at X.
%
  f =       x(1) * x(1) - 0.3 * cos ( 3.0 * pi * x(1) ) ...
    + 2.0 * x(2) * x(2) - 0.4 * cos ( 4.0 * pi * x(2) ) ...
    + 0.7;

  return
end


function b = Theil_Sen_Regress(x,y)

[N c]=size(x);

Comb = combnk(1:N,2);
deltay=diff(y(Comb),1,2);
deltax=diff(x(Comb),1,2);

theil=diff(y(Comb),1,2)./diff(x(Comb),1,2);
b=median(theil);

%% Example code below !!

% % %  x = (-15:25)';
% % % y =2*x + normrnd(0,3,41,1);
% % % y([38 40 41]) = 0;
% % % y([1 3])=20;
% % %
% % % bls = regress(y,[ones(41,1) x]);
% % % [N c]=size(x);
% % % Comb = combnk(1:N,2);
% % % deltay=diff(y(Comb),1,2);
% % % deltax=diff(x(Comb),1,2);
% % % theil=diff(y(Comb),1,2)./diff(x(Comb),1,2);
% % % b=median(theil);
% % %
% % % scatter(x,y,'filled'); grid on; hold on
% % % plot(x,bls(1)+bls(2)*x,'r','LineWidth',2);
% % % plot(x,b*x,'g','LineWidth',3);
% % % plot(x,2*x,'k','LineWidth',2);
% % % legend('Data','Ordinary Least Squares','Theil-Senn Estimator','Real slope')


function p_sign = perm_sign ( n, p )

%*****************************************************************************80
%
%% PERM_SIGN returns the sign of a permutation.
%
%  Discussion:
%
%    A permutation can always be replaced by a sequence of pairwise
%    transpositions.  A given permutation can be represented by
%    many different such transposition sequences, but the number of
%    such transpositions will always be odd or always be even.
%    If the number of transpositions is even or odd, the permutation is
%    said to be even or odd.
%
%  Example:
%
%    Input:
%
%      N = 9
%      P = 2, 3, 9, 6, 7, 8, 5, 4, 1
%
%    Output:
%
%      P_SIGN = +1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2007
%
%  Author:
%
%    Original FORTRAN77 version by Albert Nijenhuis, Herbert Wilf.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Albert Nijenhuis, Herbert Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the number of objects permuted.
%
%    Input, integer P(N), a permutation, in standard index form.
%
%    Output, integer P_SIGN, the "sign" of the permutation.
%    +1, the permutation is even,
%    -1, the permutation is odd.
%
  ierror = perm_check ( n, p );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PERM_SIGN - Fatal error!\n' );
    fprintf ( 1, '  The input array does not represent\n' );
    fprintf ( 1, '  a proper permutation.  In particular, the\n' );
    fprintf ( 1, '  array is missing the value %d\n', ierror );
    error ( 'PERM_SIGN - Fatal error!' );
  end
%
%  Make a temporary copy of the permutation.
%
  q(1:n) = p(1:n);
%
%  Start with P_SIGN indicating an even permutation.
%  Restore each element of the permutation to its correct position,
%  updating P_SIGN as you go.
%
  p_sign = 1;

  for i = 1 : n - 1

    j = i4vec_index ( n, q, i );

    if ( j ~= i )
      temp = q(i);
      q(i) = q(j);
      q(j) = temp;
      p_sign = - p_sign;
    end

  end

  return
end


function [fx,dF_dX,dF_dTheta] = f_Henon(Xt,Theta,ut,inF)
% Henon chaotic map evolution function

x       = Xt;
a       = Theta(1);
b       = Theta(2);

fx      = zeros(2,1);
fx(1)   = x(2) + 1 -a.*x(1).^2;
fx(2)   = b.*x(1);

J       = zeros(2,2);
J(1,:)  = [-2.*a*x(1),1];
J(2,:)  = [b,0];
dF_dX   = J';

dF_dTheta = zeros(2,2);
dF_dTheta(1,:)      = [-x(1).^2,0];
dF_dTheta(2,:)      = [0,x(1)];
dF_dTheta = dF_dTheta';


function a = lietzke_inverse ( n )

%*****************************************************************************80
%
%% LIETZKE_INVERSE returns the inverse of the LIETZKE matrix.
%
%  Example:
%
%    N = 5
%
%   0.5833   -0.5000         0         0    0.0833
%  -0.5000    1.0000   -0.5000         0         0
%        0   -0.5000    1.0000   -0.5000         0
%        0         0   -0.5000    1.0000   -0.5000
%   0.0833         0         0   -0.5000    0.5833
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of rows and columns
%    of the matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  a(1,1) = ( n + 2 ) / ( 2 * n + 2 );
  for i = 2 : n - 1
    a(i,i) = 1.0;
  end
  a(n,n) = ( n + 2 ) / ( 2 * n + 2 );

  if ( n == 2 )

    for i = 1 : n - 1
      a(i,i+1) = - 1.0 / 3.0;
    end

    for i = 2 : n
      a(i,i-1) = - 1.0 / 3.0;
    end

  else

    for i = 1 : n - 1
      a(i,i+1) = - 0.5;
    end

    for i = 2 : n
      a(i,i-1) = - 0.5;
    end

  end

  a(1,n) = 1.0 / ( 2 * n + 2 );
  a(n,1) = 1.0 / ( 2 * n + 2 );

  return
end


function u = dirichlet_condition ( node_num, node_xy, time )

%*****************************************************************************80
%
%% DIRICHLET_CONDITION sets the value of a Dirichlet boundary condition.
%
%  Discussion:
%
%    For efficiency, this routine may be called once, with all the
%    nodes input.  Of course, only those nodes that lie on the boundary
%    will need a meaningful value of U to be assigned.  Other nodes
%    may be given a dummy value for U.
%
%    This routine is set for the unit square.
%
%    We assume that the equation to be solved is
%
%      dUdT - Laplacian U + K * U = F
%
%    with
%
%      K(X,Y,T) = 0
%      F(X,Y,T) = (2*pi*pi-1)*sin(pi*x)*sin(pi*y)*exp(-t).
%
%    The exact solution is:
%
%      U = sin(pi*x) * sin(pi*y) * exp(-t)
%
%  Modified:
%
%    07 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the coordinates of the points.
%
%    Input, real TIME, the current time.
%
%    Output, real U(NODE_NUM), the value of the solution at the
%    the points.
%
  u(1:node_num) = sin ( pi * node_xy(1,1:node_num) ) ...
               .* sin ( pi * node_xy(2,1:node_num) ) * exp ( - time );

  return
end


% Filename: givepd.m
% Author: Shashank G. Sawant
% email: sgsawant@gmail.com
% Description: Given 2 sinusoidal signals of the
% same frequency, the function gives the "phase difference" between the
% 2 given signals
% The phase difference is in RADIANS!!!!!!
% The output is limited to pd={-pi,pi}radians
% Time Stamp: 2010 October 19 2043hrs

% pd - output phase difference (in radians)
% v - first sinusoidal signal
% i - second sinusoidal signal
% Note: v and i should have the same frequency
function pd=givepd(v,i)
L=length(v);
if(L~=length(i))
    error('The length of the 2 sinusoidal input vectors is not same!!!');
end

% The following block calculates the FFT
NFFT = 2^nextpow2(L);
V = fft(v,NFFT)/L;                  %Fourier Transform of signal v

% The following block calculates the phase of the most significant
% frequency component
[value,index]=max(2*abs(V(1:NFFT/2+1)));
pV = angle(V(index));

% The following block calculates the FFT
NFFT = 2^nextpow2(L);
I = fft(i,NFFT)/L;                  %Fourier Transform of signal i

% The following block calculates the phase of the most significant
% frequency component
[value,index]=max(2*abs(I(1:NFFT/2+1)));
pI = angle(I(index));

% The following is the phase difference between the 2 signals
pd=pV-pI;

% The code below limits the output to pd={-pi,pi}radians
while 1
    if pd>pi
        pd=pd-2*pi;
    elseif pd<-pi
        pd=pd+2*pi;
    else
        break;
    end
end

return;
end


function y = tanh(x)
exp_term = exp(x);
exp_term2 = exp(-x);
y = (exp_term - exp_term2) ./ (exp_term + exp_term2);

end


clear;
clc;
% This program solves the economic emission dispatch dispatch with Bmn coefficients by
% quadratic programming and equal incremental cost critetion
% the elddata matrix should have 5 columns of fuel cost coefficients and plant  limits.
% 1.a ($/MW^2) 2. b $/MW 3. c ($) 4.lower lomit(MW) 5.Upper limit(MW)
%no of rows denote the no of plants(n)
elddata=[0.15247	38.53973	756.79886	10	125
0.10587	46.15916	451.32513	10	150
0.02803	40.3965	1049.9977	35	225
0.03546	38.30553	1243.5311	35	210
0.02111	36.32782	1658.5596	130	325
0.01799	38.27041	1356.6592	125	315
];
% the emidata matrix should have 3 columns of fuel cost coefficients and plant  limits.
% 1.a (Kg/MW^2) 2. b Kg/MW 3. c (Kg)

emidata=[0.00419	0.32767	13.85932
0.00419	0.32767	13.85932
0.00683	-0.54551	40.2669
0.00683	-0.54551	40.2669
0.00461	-0.51116	42.89553
0.00461	-0.51116	42.8955];
% h1 1nd h2 are  is the weightage factor for economy and emission
h1=1; h2=44.788;
% Demand (MW)
Pd=700;
% Loss coefficients it should be squarematrix of size nXn where n is the no
% of plants
B=1e-4*[1.4	.17	.15	.19	.26	.22
.17	.6	.13	.16	.15	.2
.15	.13	.65	.17	.24	.19
.19	.16	.17	.71	.3	.25
.26	.15	.24	.3	.69	.32
.22	.2	.19	.25	.32	.85];
[P Fcost Emi Pl]=emield(elddata,emidata,h1,h2,B,Pd)


function sphere = createSphere(varargin)
%CREATESPHERE Create a sphere containing 4 points
%
%   s = createSphere(p1, p2, p3, p4);
%   return in s the sphere common to the 4 pointsp1, p2, p3 and p4.
%
%   Ref: P. Bourke
%   http://astronomy.swin.edu.au/~pbourke/geometry/spherefrom4/
%
%   See also
%   spheres, circles3d
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 22/03/2005.
%

if length(varargin)==4
    pts = [varargin{1};varargin{2};varargin{3};varargin{4}];
elseif length(varargin)==1
    pts = varargin{1};
else
    error('wrong number of arguments in createSphere');
end

m1 = det([pts ones(4,1)]);
s2 = sum(pts.*pts, 2);
m2 = det([s2 pts(:,2) pts(:,3) ones(4,1)]);
m3 = det([pts(:,1) s2 pts(:,3) ones(4,1)]);
m4 = det([pts(:,1) pts(:,2) s2 ones(4,1)]);

m5 = det([s2 pts]);

x0 = m2*.5/m1;
y0 = m3*.5/m1;
z0 = m4*.5/m1;
r  = sqrt(x0*x0 + y0*y0 + z0*z0 - m5/m1);

sphere = [x0 y0 z0 r];


function [ x, seed ] = latin_center ( dim_num, point_num, seed )

%*****************************************************************************80
%
%% LATIN_CENTER returns center points in a Latin square.
%
%  Discussion:
%
%    In each spatial dimension, there will be exactly one
%    point with the coordinate value
%
%      ( 1, 3, 5, ..., 2*point_num-1 ) / ( 2 * point_num )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer dim_num, the spatial dimension.
%
%    Input, integer point_num, the number of points.
%
%    Input, integer SEED, a seed for UNIFORM, the random number generator.
%
%    Output, real x(dim_num,point_num), the points.
%
%    Output, integer SEED, the updated random number seed.
%
  base = 1;

  for i = 1: dim_num

    [ perm, seed ] = perm_uniform ( point_num, base, seed );

    for j = 1: point_num
      x(i,j) = ( 2 * perm(j) - 1 ) / ( 2 * point_num );
    end

  end

  return
end


function a = biw ( n )

%*****************************************************************************80
%
%% BIW returns the BIW matrix.
%
%  Discussion:
%
%    BIW is a bidiagonal matrix of Wilkinson.   Originally, this matrix
%    was considered for N = 100.
%
%  Formula:
%
%    if ( I == J )
%      A(I,J) = 0.5 + I / ( 10 * N )
%    else if ( J == I+1 )
%      A(I,J) = -1.0
%    else
%      A(I,J) = 0
%
%  Example:
%
%    N = 5
%
%    0.52 -1.00  0.00  0.00  0.00
%    0.00  0.54 -1.00  0.00  0.00
%    0.00  0.00  0.56 -1.00  0.00
%    0.00  0.00  0.00  0.58 -1.00
%    0.00  0.00  0.00  0.00  0.60
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    a(i,i) = 0.5 + i / ( 10 * n );
  end

  for i = 1 : n - 1
    a(i,i+1) = - 1.0;
  end

  return
end


function [t, u] = swept_sine(f,cyc,res,plt)
% generate a swept sine curve
% function [t, u] = swept_sine(f,cyc,res,plt)
%
% inputs 4 - 3 optional
% f      frequency interval ([low high] (Hz)       class real
% cyc    number of cycles to cover (total)         class real      - optional
% res    number of points per cycle (resolution)   class integer   - optional
% plt    plot function (0 / 1)                     class integer   - optional
%
% outputs 2
% t      time increment vector (non-linear)        class real
% u      steering input content                    class real
%
% michael arant - Aug 7, 2009
%
% ex
%	[t u] = swept_sine([.1 5],25,25,1,0)

if nargin < 1; help swept_sine; error('Need inputs'); end
if ~exist('cyc','var'); cyc = ceil((f(2) / f(1)) / 5); end
if ~exist('res','var'); res = 50; end
if ~exist('plt','var'); plt = 0; end

% define sine cycles
a = linspace(0,pi*cyc*2,res*cyc)';
% generate sine
u = sin(a);

% time step vector (time between points)
ti = linspace(1/(res * f(1)),1/(res * f(2)),res*cyc)';
% time vector (point location in time) - scale by number of points and cycles
t = (cumsum(ti) - ti(1)) * cyc / res;

% proof of linear time reduction
% diff(diff(t(1:res:end)))

% debug plot
if plt
	figure; set(gcf,'color','w'); plot(u); grid on;
	title('Steering Input'); ylabel('Amplitude (deg)'); xlabel('Increment')
	figure; set(gcf,'color','w'); plot(t,u); grid on;
	title('Steering Input'); ylabel('Amplitude (deg)'); xlabel('Time (s)')
end


function a = lehmer ( n )

%*****************************************************************************80
%
%% LEHMER returns the Lehmer matrix.
%
%  Discussion:
%
%    The Lehmer matrix is a symmetric positive definite N by N
%    matrix with
%
%      A(i,j) = min ( i, j ) / max ( i, j )
%
%    A is totally nonnegative.  The inverse of A is tridiagonal,
%    and explicit formulas are known for its entries.
%
%    The condition number of A satisfies the bounds:
%
%      N <= condition ( A ) <= 4*N*N.
%
%  Reference:
%
%    M. Newman and J. Todd,
%    The evaluation of matrix inversion programs,
%    Journal of the Society for Industrial and Applied Mathematics,
%    Volume 6, 1958, pages 466-476.
%
%    Solutions to problem E710 (proposed by D H Lehmer):
%    The inverse of a matrix,
%    American Mathematical Monthly,
%    Volume 53, 1946, pages 534-535.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = ones ( n, 1 ) * ( 1:n );
  a = a ./ a';
  a = tril ( a ) + tril ( a, -1 )';

  return
end


function fx1 = p16_fx1 ( x )

%*****************************************************************************80
%
%% P16_FX1 evaluates the derivative of the function for problem 16.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the abscissa.
%
%    Output, real FX1, the first derivative of the function at X.
%
  e = 0.8;

  fx1 = ( pi / 180.0 ) - e * pi * cos ( pi * x / 180.0  ) / 180.0;

  return
end