function [] = main(file, k, iterations)
    %=======================================
    %           Initialising Data
    %=======================================
    % k = 3;
    % iterations = 20;
    % file = 'yeast_test.txt';
    data = load(file);
    rows = size(data, 1);
    cols = size(data, 2);
    data = data(1:rows, 1:cols-1);
    clusters = randi([1 k], rows, 1);
    clustered_data = [data clusters];
    mean_matrix = zeros(k, cols-1);

    %=======================================
    %           Calculating Mean
    %=======================================
    for i = 1:k
       index = clustered_data(:, end) == i;
       indexed_data = clustered_data(index, 1:end-1);
       mean_matrix(i, :) = mean(indexed_data);

    end

    %=======================================
    %           Computing Error
    %=======================================
    error = get_error(clustered_data, mean_matrix);
    fprintf('After initialization: error = %.4f \n', error);

    %=======================================
    %           Starting Iterations
    %=======================================
    for p = 1:iterations
       for q = 1:rows
          %=======================================
          % Deciding which Cluster data belongs
          %=======================================
          dist = get_euclidean(data(q, :), mean_matrix);
          [minimum_row, minimum_col] = min(dist);
          clusters(q) = minimum_col;
       end
       clustered_data = [data, clusters];

       %=======================================
       %          Calculating Mean
       %=======================================
       for i = 1:k
          index = clustered_data(:, end) == i;
          indexed_data = clustered_data(index, 1:end-1);
          mean_matrix(i, :) = mean(indexed_data);
       end
       %=======================================
       %           Computing Error
       %=======================================
       error = get_error(clustered_data, mean_matrix);
       fprintf('After iteration %d: error = %.4f \n', p, error);
    end
end

function [error] = get_error(data, mean_matrix)
    %=======================================
    %           Computing Error
    %=======================================
    error = 0;
    for j = 1: size(data, 1)
       c = data(j, end);
       dist = get_euclidean(data(j, 1:end-1), mean_matrix(c, 1:end));
       error = error + dist;
    end
end

function [distance_matrix] = get_euclidean(data, mean_matrix)
    %=======================================
    %           Calculating Euclidean
    %=======================================
    dist = data - mean_matrix;
    dist = dist.^2;
    dist = sum(dist, 2);
    distance_matrix = sqrt(dist);
end


function u = u_hat ( x )

%*****************************************************************************80
%
%% U_HAT evaluates the target function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2011
%
%  Author:
%
%    Jeff Borggaard, John Burkardt, Catalin Trenchea, Clayton Webster
%
%  Parameters:
%
%    Input, real X(*), the evaluation points.
%
%    Output, real U(*), the function values.
%
  u = x .* ( 1 - x.^2 );

  return
end


function mean = gamma_mean ( a, b, c )

%*****************************************************************************80
%
%% GAMMA_MEAN returns the mean of the Gamma PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < B,
%    0.0 < C.
%
%    Output, real MEAN, the mean of the PDF.
%
  mean = a + b * c;

  return
end


function Ip=perm2PermMatrix(p)
%%PERM2PERMMATRIX Given a permutation of the values 1:n, obtain the
%                 permutation matrix that corresponds to the permutation.
%
%INPUTS: p A 1Xn or nX1 permutation of the values 1:n.
%
%OUTPUTS: Ip A permutation matrix such that Ip*(1:n).'=p.
%
%The creation of a permutation matrix from a permutation basically involves
%rearranging the columns of an identity matrix.
%
%EXAMPLE:
% p=[3;1;4;5;2];
% Ip=perm2PermMatrix(p);
% all(Ip*(1:5).'==p)
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=length(p);
Ip=zeros(n,n);
for i=1:n
    Ip(i,p(i))=1;
end
end

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


function X = find_line_intersection(P1,V1,P2,V2)

if(length(P1) ~= 2)
    error('This function only works in 2D');
end

A = [V1(1),-V2(1) ; V1(2) -V2(2)];
B = P2-P1;
if(det(A) == 0)
    X = [];
    return;
end

K = inv(A)*B;

X = P1 + K(1)*V1;


function y = gam_lpdf(x,a,b)
%GAM_LPDF Log of Gamma probability density function (lpdf).
%
%   Y = GAM_LPDF(X,A,B) Returns the gamma lpdf with
%   shape A and inverse scale B, at the values in X.
%
%   The size of X is the common size of the input arguments. A
%   scalar input functions as a constant matrix of the same size as
%   the other inputs.
%
%   The parameterization is as in Gelman, Carlin, Stern, Dunson, Vehtari,
%   and Rubin (2013). Bayesian Data Analysis, third edition.
%
% Copyright (c) 1998-2004 Aki Vehtari

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

if nargin < 3,
  error('Requires three input arguments.');
end

%y = b.^a/gamma(a)*x^(a-1)*exp(-b*x);
y = a.*log(b)-gammaln(a)+(a-1).*log(x)-b.*x;


%% MULTIGRID OF FOR THE STOKES EQNS IN 2D
%
% This example is to show the convergence of multigrid methods for various
% finite element approximation of the Stokes equation on the unit square:
%
%       -div(mu*grad u) + grad p = f in \Omega,
%                        - div u = 0  in \Omega,
%                              u = g_D  on \Gamma.
%
% with the pure Dirichlet boundary condition.
%
% Reference
%
% M. Wang and L. Chen. Multigrid Methods for the Stokes equations
% using Distributive Gauss-Seidel Relaxations based on the Least Squares
% Commutator. Journal of Scientific Computing. 56(2): 409-431, 2013.

clear variables;
close all;

%% Setting
[node,elem] = squaremesh([0,1,0,1],0.125);
% [node,elem] = circlemesh(0,0,1,0.25);
% [node,elem] = uniformrefine(node,elem);
bdFlag = setboundary(node,elem,'Dirichlet');
mesh = struct('node',node,'elem',elem,'bdFlag',bdFlag);
figure;
showmesh(node,elem); pause(0.5);
% pde
pde = Stokesdata1;
%  pde = StokesZulehnerdata;
option.L0 = 0;
option.maxIt = 4;
option.printlevel = 1;
option.plotflag = 0;
option.rateflag = 0;

%% MG options
option.solver = 'asmg';
option.smoothingstep = 2;
option.smootherbarSp = 'SGS';

%% CR-P0 element
disp('CR-P0')
option.elemType = 'CRP0';
femStokes(mesh,pde,option);

%% P2-P0 element
disp('P2-P0')
option.elemType = 'P2P0';
femStokes(mesh,pde,option);

%% P2-P1 element
disp('P2-P1')
option.elemType = 'P2P1';
option.solver = 'asmg';
femStokes(mesh,pde,option);

%% isoP2-P0 element
disp('isoP2-P0')
option.elemType = 'isoP2P0';
femStokes(mesh,pde,option);

%% isoP2-P1 element
disp('isoP2-P1')
option.elemType = 'isoP2P1';
femStokes(mesh,pde,option);

%% P1b-P1 element
disp('P1b-P1')
option.elemType = 'P1bP1';
femStokes(mesh,pde,option);


function fI = faceinterpolate3(f,node,face,quadOrder)
%% FACEINTERPOLATE3 interpolate to face elements RT0.
%
% uI = faceinterpolate3(u,node,face) interpolates a given function u
% into the lowesr order RT0. The coefficient is given by the face integral
% int_f u*n ds.
%
% Example
%
%
%    [node,elem] = cubemesh([-1,1,-1,1,-1,1],1);
%    maxIt = 3;
%    pde = polynomialdata0;
%    err = zeros(maxIt,1);
%    N = zeros(maxIt,1);
%    for i =1:maxIt
%      [node,elem] = uniformrefine3(node,elem);
%      uI = faceinterpolate3(pde.u,node,elem);
%      err(i) = getL2error3RT0(node,elem,pde.u,uI);
%      N(i) = size(u,1);
%    end
%     figure;
%     showrate(N,err,2,'r-+','||u - u_I||');
%
% See also edgeinterpolate, edgeinterpolate1, edgeinterpolate2
%
% Created by Lin Zhong and Ming Wang at July, 2012.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%%
NF = size(face,1);
v12 = node(face(:,2),:) - node(face(:,1),:);
v13 = node(face(:,3),:) - node(face(:,1),:);
nVec = mycross(v12,v13);

%% assemble the right hand side
if ~exist('quadOrder','var'), quadOrder = 2; end
[lambda,weight] = quadpts(quadOrder);
nQuad = size(lambda,1);
bt = zeros(NF,3);
for p = 1:nQuad
    pxy = lambda(p,1)*node(face(:,1),:) ...
		+ lambda(p,2)*node(face(:,2),:) ...
		+ lambda(p,3)*node(face(:,3),:);
    bt = bt + weight(p)*f(pxy);
end
fI = dot(nVec,bt,2)/2;


% Compute empirical p-vals under the null hypothesis that observed samples
% come from a given surrogate distribution. P-values for Type I error in
% rejecting the null hypothesis are obtained by finding the proportion of
% samples in the distribution that
% (a) are larger than the observed sample (one-sided test)
% (b) are larger or smaller than the observed sample (two-sided test).
%
% This function is based on Arnaud Delorme's statcond:COMPUTE_PVALS
% function from EEGLAB
%
% Inputs:
%
%   distribution:   [d1 x d2 x ... x dM x N] matrix of surrogate samples.
%                   distribution(i,j,k,...,:) is a collection of N samples
%                   from a surrogate distribution.
%   observed:       [d1 x d2 x ... x dM] matrix of observations.
%   tail:           can be 'one' or 'both' indicating a one-tailed or
%                   two-tailed test
% Outputs:
%
%   pvals:          [d1 x d2 x ... x dM] matrix of p-values specifying
%                   probability of Type I error in rejecting the null
%                   hypothesis
%
% Author: Tim Mullen and Arnaud Delorme, SCCN/INC/UCSD

% Copyright: Tim Mullen and Arnaud Delorme
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function pvals = stat_surrogate_pvals(distribution,observed,tail)
numDims = ndims(distribution);
if iscolumn(distribution)
	numDims = 1;
end

n = size(distribution, numDims);
% once support for matlab <= R2016b is dropped:
% pvals = sum(distribution >= observed, numDims) / n;
pvals = sum(bsxfun(@ge, distribution, observed), numDims) / n;

if any(strcmpi(tail, {'right', 'one'}))
	% nothing to be done
	return;
end

p_left = 1 - pvals + sum(bsxfun(@eq, distribution, observed), numDims) / n;

if strcmpi(tail, 'both')
	pvals = 2 * min(pvals, p_left);
elseif strcmpi(tail, 'left')
	pvals = p_left;
else
	error('invalid value for tail: "%s", should be left, right, one or both', tail);
end

end


% Chapter 15 - Local and Global Bifurcations.
% Programs_15a - Determining the coefficients of the Lyapunov function
% for a Lienard system.
% Copyright Birkhauser 2013. Stephen Lynch.

% V3=[V30;V21;V12;V03], V4=[V40;V31;V22;V13;V04;eta4],
% V5=[V50;V41;V32;V23;V14;V05],V6=[V60;V51;V42;V33;V24;V15;V06;eta6]
% Symbolic Math toolbox required.

% When determining coefficients of V_{4m} set V_{2m,2m}=0.
% When determining coefficients of V_{4m+2} set V_{2m,2m+2}+V_{2m+2,2m}=0.

clear all

syms a1 a2 b2 a3 b3 a4 b4 a5 b5;
A=[3 0 -2 0;0 0 1 0;0 -1 0 0;0 2 0 -3];
B=[b2; 0; a2; 0];

V3=A\B

A=[0 -1 0 0  0 -1;0 3 0 -3 0 -2;0 0 0 1 0 -1;4 0 -2 0 0 0;0 0 2 0 -4 0; 0 0 1 0 0 0];
B=[a3; -2*a2*b2; 0; b3-2*a2^2;0;0];

V4=A\B

A=[5 0 -2 0 0 0;0 0 3 0 -4 0;0 0 0 0 1 0;0 -1 0 0 0 0;0 4 0 -3 0 0;0 0 0 2 0 -5];
B=[b4-10*a2^2*b2/3;0;0;a4-2*a2^3;-2*a2*b3;0];

V5=A\B

A=[6 0 -2 0 0 0 0 0;0 0 4 0 -4 0 0 0;0 0 0 0 2 0 -6 0;0 0 1 0 1 0 0 0;
   0 -1 0 0 0 0 0 -1;0 5 0 -3 0 0 0 -3;0 0 0 3 0 -5 0 -3;0 0 0 0 0 1 0 -1];
B=[b5-6*a2*a4-4*a2^2*b2^2/3+8*a2^4;16*a2^4/3+4*a2^2*b3/3-8*a2*a4/3;0;0;
   a5-8*a2^3*b2/3;-2*a2*b4+8*a2^3*b2+2*a2*b2*b3-4*a4*b2;
   16*a2^3*b2/3+4*a2*b2*b3/3-8*a4*b2/3;0];

V6=A\B

L0=-a1
eta4=V4(6,1);
[n,d]=numden(-3/8*a3+1/4*a2*b2);
L1=n
a3=2*a2*b2;
eta6=V6(8,1);
[n,d]=numden(-5/16*a5+1/8*a2*b4-5/24*a2*b2*b3+5/12*a4*b2);
L2=n

% End of Programs_15a.


function c=ref_rdgtii(f,g,a,M)
%REF_RDGTII  Reference Real DGT type II
%   Usage:  c=ref_rdgt(f,g,a,M);
%
%   Linear algebra version of the algorithm. Create big matrix
%   containing all the basis functions and multiply with the transpose.

L=size(f,1);

b=L/M;
N=L/a;

Mhalf=ceil(M/2);

F=zeros(L,M*N);

l=(0:L-1).';

for n=0:N-1

  % Do the unmodulated coefficient.
  F(:,M*n+1)=circshift(g,n*a+floor(a/2));

  for m=1:Mhalf-1
    F(:,M*n+2*m)=sqrt(2)*cos(2*pi*m*(l+.5)/M).*circshift(g,n*a+floor(a/2));

    F(:,M*n+2*m+1)=sqrt(2)*sin(2*pi*m*(l+.5)/M).*circshift(g,n*a+floor(a/2));

  end;

  if mod(M,2)==0
    F(:,M*(n+1))=cos(pi*l).*circshift(g,n*a+floor(a/2));
  end;

end;

% dot-transpose will work because F is real.
c=F.'*f;


function [ grid_weight, grid_point ] = sparse_grid_cfn ( dim_num, level_max, ...
  rule, point_num )

%*****************************************************************************80
%
%% SPARSE_GRID_CFN computes a sparse grid based on a CFN 1D rule.
%
%  Discussion:
%
%    The 1D quadrature rule is assumed to be Closed Fully Nested.
%
%    Closed Fully Nested rules include Clenshaw Curtis rules.
%
%    A Smolyak construction is used to create a multidimensional sparse grid.
%
%    The user specifies:
%    * the spatial dimension of the quadrature region,
%    * the level that defines the Smolyak grid.
%    * the quadrature rule.
%    * the number of points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, controls the size of the final
%    sparse grid.
%
%    Input, integer RULE, the index of the rule.
%    1, "CC", Clenshaw Curtis Closed Fully Nested rule.
%    2, "F1", Fejer 1 Open Fully Nested rule.
%    3, "F2", Fejer 2 Open Fully Nested rule.
%    4, "GP", Gauss Patterson Open Fully Nested rule.
%    5, "GL", Gauss Legendre Open Weakly Nested rule.
%    6, "GH", Gauss Hermite Open Weakly Nested rule.
%    7, "LG", Gauss Laguerre Open Non Nested rule.
%
%    Input, integer POINT_NUM, the number of points in the grid,
%    as determined by LEVELS_INDEX_SIZE.
%
%    Output, real GRID_WEIGHT(POINT_NUM), the weights.
%
%    Output, real GRID_POINT(DIM_NUM,POINT_NUM), the points.
%
  if ( rule ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SPARSE_GRID_CFN - Fatal error!\n' );
    fprintf ( 1, '  Illegal input rule index = %d\n', rule );
    error ( 'SPARSE_GRID_CFN - Fatal error!' );
  end
%
%  Determine the index vector, relative to the full product grid,
%  that identifies the points in the sparse grid.
%
  [ grid_index, grid_base ] = levels_index_cfn ( dim_num, level_max, point_num );
%
%  Compute the physical coordinates of the abscissas.
%
  if ( 0 == level_max )
    order_max = 1;
  else
    order_max = 2^level_max + 1;
  end

  grid_point = zeros ( dim_num, point_num );

  for point = 1 : point_num
    for dim = 1 : dim_num

      if ( rule == 1 )
        grid_point(dim,point) = ...
          cc_abscissa ( order_max, grid_index(dim,point) + 1 );
      end

    end
  end
%
%  Gather the weights.
%
  grid_weight = sparse_grid_weights_cfn ( dim_num, level_max, rule, ...
    point_num, grid_index );

  return
end


function fem = genFEM3D(mesh,femtype,bcind,option)

%% Usage: Form FEM global degrees of freedom on 3D tetrahedron mesh
%
% INPUTS:
% mesh --- a struct data contains rich mesh information.
% bcind --- [bc1,bc2,bc3,bc4,bc5,bc6] denotes the boundary condition of
%            bc1: x = xmin  bc2: x = xmax
%            bc3: y = ymin  bc4: y = ymax
%            bc5: z = zmin  bc6: z = zmax
%            if bc = 1: Dirichlet boundary condition
%               bc = 2: Neumann boundary condition
%               bc = 3: Robin boundary condition
% femtype --- the type of finite element methods
%             Conforming: P1, P2
%
% OUTPUTS:
% fem --- a struct data contains the following fields:
%         fem.p: (x,y,z) coordinate of each vertex w.r.t a global DoF
%         fem.t: indices of global DoF in each element
%         fem.bc: indices of global DoF on the boundary
%         fem.mapper: a vector to extract unknowns from global DoF.
%         fem.type: type of finite element methods
%         fem.ldof: number of local DoF on each element
%         fem.ng: number of Gaussian quadrature points on each element

% Last Modified: 08/07/2020 by Xu Zhang

%% 0. Inputs
if nargin == 2
    bcind = [1,1,1,1,1,1];  option.ng = 4;
end
if nargin == 3
    option.ng = 4;
end
if ~isfield(option,'ng')
    option.ng = 4;
end
ng = option.ng;

%% 1. Form fem p, t, and basis functions
if strcmp(femtype,'P1')
    fem = genP1FEM3D(mesh); option.ngauss = 4;
elseif strcmp(femtype,'P2')
    fem = genP2FEM3D(mesh); option.ngauss = 11;
end

%% 2. Form Gaussian Quadrature
p = mesh.p; t = mesh.t;
X1 = p(t(:,1),:); X2 = p(t(:,2),:); X3 = p(t(:,3),:); X4 = p(t(:,4),:);
gw = gaussWtetra(ng);
[gx,gy,gz] = gaussPtetra(X1,X2,X3,X4,ng);
A = tetraArea(X1,X2,X3,X4);
fem.gw = gw; fem.gx = gx; fem.gy = gy; fem.gz = gz; fem.area = A;

%% 3. Form bc mapper
[bc,mapper] = boundaryNode3D(mesh,fem.p,bcind);
fem.bc = bc; fem.mapper = mapper; fem.ng = option.ng; fem.bcind = bcind;


function gaussian_test ( )

%*****************************************************************************80
%
%% GAUSSIAN_TEST examines the gaussian correlation.
%
%  Discussion:
%
%    Note that I can't use the original value EIGEN_NUM = 20.  The
%    EIGS function fails to converge.  In fact, it still fails at 12.
%
%    This code is based substantially on a document by Toby Driscoll.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Toby Driscoll,
%    Mercer's theorem and the Karhunen-Loeve expansion,
%    Oxford University Mathematical Institute,
%    http://www2.maths.ox.ac.uk/chebfun/examples/stats/pdf/MercerKarhunenLoeve.pdf
%
  rmpath ( '/usr/local/matlab/toolbox/datafeed/datafeed' )
  addpath ( '../chebfun' )

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAUSSIAN_TEST:\n' );
  fprintf ( 1, '  Demonstrate Mercer''s theorem and the KL expansion\n' );
  fprintf ( 1, '  for the gaussian kernel.\n' );
%
%  Set the interval.
%
  a = 0.0;
  b = 10.0;
  fprintf ( 1, '  Using interval [%g,%g]\n', a, b );
  s_num = 21;
  s_vec = linspace ( a, b, s_num );
%
%  FRED is a function from the CHEBFUN library.
%
%  It constructs a "chebop" representing the Fredholm integral operator
%  with kernel K for functions in domain [A,B].
%
  F = fred ( @gaussian_correlation, domain ( [ a, b ] ) );
%
%  EIGS has been extended to be able to compute the eigenvalues Lambda
%  and eigenfunctions Psi of the Fredholm integral operator represented by F.
%
%  The "LM" switch requests that we return the eigenvalues of largest magnitude.
%
%  Each Psi is a CHEBFUN, that is, it takes a real number argument.
%
  eigen_num = 20;
  [ Psi, Lambda ] = eigs ( F, eigen_num, 'lm' );

  eigen_found = length ( Lambda );
  fprintf ( 1, '  Requested %d eigenmodes, computed %d\n', eigen_num, eigen_found );
  eigen_num = min ( eigen_num, eigen_found );
%
%  Print the eigenvalues.
%
  lambda_vec = diag ( Lambda );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   I       Lambda(I)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : eigen_num
    fprintf ( 1, '  %2d  %10g\n', i, lambda_vec(i) );
  end
%
%  Plot the eigenvalues.
%
  figure ( 1 )
  clf
  plot ( lambda_vec, 'Linewidth', 2 );
  hold on
  plot ( lambda_vec, 'b.', 'Markersize', 20 );
  title ( 'gaussian: Mercer eigenvalues' );
  xlabel ( '<--- N --->')
  grid on
  print -dpng 'gaussian_figure1.png'
%
%  Plot selected eigenfunctions.
%
  figure ( 2 )
  subplot ( 4, 1, 1 )
  plot ( Psi(:,1), 'Linewidth', 2 );
  title ( 'gaussian: Mercer eigenfunction PSI(1)' )
  grid on
  subplot ( 4, 1, 2 )
  plot ( Psi(:,2), 'Linewidth', 2 );
  title ( 'gaussian: Mercer eigenfunction PSI(2)' )
  grid on
  subplot ( 4, 1, 3 )
  plot ( Psi(:,5), 'Linewidth', 2 );
  title ( 'gaussian: Mercer eigenfunction PSI(5)' )
  grid on
  subplot ( 4, 1, 4 )
  plot ( Psi(:,10), 'Linewidth', 2 );
  title ( 'gaussian: Mercer eigenfunction PSI(10)' )
  grid on
  print -dpng 'gaussian_figure2.png'
%
%  Orthonormality check.
%
  ptp = Psi' * Psi;
  error_frobenius = r8mat_is_identity ( eigen_num, ptp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm of I - Psi'' * Psi = %g\n', error_frobenius );
%
%  K(S,S) should be exactly 1.
%  Because we are using a truncated representation of K, our estimate of K(S,S)
%  will be smaller than 1.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Truncated estimate of K(s,s) = 1 for S in the interval.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '        S        K(s,s) estimate\n' );
  fprintf ( 1, '\n' );
  for i = 1 : 21
    s = s_vec(i);
    ptlp = Psi(s,:) * Lambda * Psi(s,:)';
    fprintf ( 1, '  %10g  %14g\n', s, ptlp );
  end
%
%  Look at eigenvalue decay.
%
  x = diff ( log ( ( 1:eigen_num ) ) );
  y = diff ( log ( lambda_vec ) )';
  c = y ./ x;
  figure ( 3 );
  clf
  plot ( c, 'Linewidth', 2 );
  grid on
  hold on
  plot ( c, 'b.', 'Markersize', 20 );
  xlabel ( '<-- N -->' )
  title ( 'gaussian: Eigenvalue decay rate' );
  print -dpng 'gaussian_figure3.png'
%
%  Look at eigenvalue sum.
%
%  The trace of K(s,s) over [a,b] should be the integral of 1 over [a,b],
%  that is, b - a.
%
%  We compare the trace to the partial sums of the lambda's to see how much
%  of the variance of the process we have captured.
%
  trace_K = b - a;
  lambda_cum = cumsum ( lambda_vec ) / trace_K;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Index    Cumulative Eigenvalue sum\n' );
  fprintf ( 1, '\n' );
  for i = 1 : eigen_num
    fprintf ( 1, '  %5d  %10g\n', i, lambda_cum(i) );
  end
%
%  We decide to use the first 10 eigenfunctions.
%
  eigen_use = 10;
%
%  Find 400 realizations of the process by selecting, for each realization,
%  10 random parameters Z in the truncated KL expansion.
%
  Z = randn ( eigen_use, 400 );
  X = Psi(:,1:eigen_use) * ( sqrt ( Lambda(1:eigen_use,1:eigen_use) ) * Z );
%
%  Plot 40 of the realizations;
%  Plot their mean, computed from all 400.
%
  figure ( 4 )
  clf
  plot ( X(:,1:40) )
  mu = sum ( X, 2 ) / 400;
  hold on;
  plot ( mu, 'k', 'linewidth', 3 );
  title ( 'gaussian: 40 Random Realizations X(t,omega), and their Mean.' )
  print -dpng 'gaussian_figure4.png'
%
%  Estimate the covariance from the data.
%
  figure ( 5 )
  clf
  [ S, T ] = meshgrid ( s_vec, s_vec );
  C = cov ( X(s_vec,:)' );
  mesh ( S, T, C );
  hold on;
  D = gaussian_correlation ( S, T );
  plot3 ( S, T, D, 'k.', 'markersize', 10 )
  title ( 'gaussian: Covariance K(S,T) (dots), and Estimate from Realizations (mesh)' )
  print -dpng 'gaussian_figure5.png'
%
%  Using just 10 functions in the expansion,
%  reduce the correlation length,
%  and examine the sum of the lambda's.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use a fixed number of eigenfunctions = %d\n', eigen_use );
  fprintf ( 1, '  but vary the correlation length RHOBAR.\n' );
  fprintf ( 1, '  (We used RHOBAR = 1 above.)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The sum of the eigenvalues, divided by (B-A),\n' );
  fprintf ( 1, '  discloses the relative amount of the total variation\n' );
  fprintf ( 1, '  that is captured by the truncated expansion.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      RHOBAR      VARSUM\n' );
  fprintf ( 1, '\n' );
  rhobar = 4.0;
  for i = 1 : 10
    K = @ ( s, t ) exp ( - ( ( s - t ) / rhobar ).^2 );
    F = fred ( K, domain ( [ a, b ] ) );
    lambda_vec = eigs ( F, eigen_use, 'lm' );
    varsum = sum ( lambda_vec(1:eigen_use) ) / ( b - a );
    fprintf ( 1, '  %10g  %10g\n', rhobar, varsum );
    rhobar = rhobar / 2.0;
  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAUSSIAN_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  rmpath ( '../chebfun' )

  return
end


function [os,os1] = getosmatrix_bb(boxes,gts)
%% Given two sets of bounding boxes, N1 in the first one, and N2 in
%% the second one, compute a N1xN2 overlap score matrix where the
%% overlap score is the ratio of the intersection to union
%% Tomasz Malisiewicz (tomasz@cmu.edu)

if ~exist('gts','var')
  gts = boxes;
end

if numel(boxes) == 0 || numel(gts) == 0
  os = zeros(size(boxes,1),size(gts,1));

  return;
end

x1 = boxes(:,1);
y1 = boxes(:,2);
x2 = boxes(:,3);
y2 = boxes(:,4);

area = (x2-x1+1) .* (y2-y1+1);

xa1 = gts(:,1);
ya1 = gts(:,2);
xa2 = gts(:,3);
ya2 = gts(:,4);

area2 = (xa2-xa1+1) .* (ya2-ya1+1);

os = zeros(size(boxes,1),size(gts,1));
if size(boxes,1)*size(gts,1)==0
  return;
end

if nargout == 2
  os1 = os;
end

for i = 1:size(boxes,1)

  xx1 = max(boxes(i,1),gts(:,1));
  yy1 = max(boxes(i,2),gts(:,2));
  xx2 = min(boxes(i,3),gts(:,3));
  yy2 = min(boxes(i,4),gts(:,4));

  w = xx2-xx1+1;
  h = yy2-yy1+1;

  o = w.*h;
  o( (w<0) | (h<0) ) = 0;

  os(i,:) = o ./ (eps + (area(i) + area2 - o));

  if nargout == 2
    os1(i,:) = (o ./ (eps + area2));
    %os1(i,:) = o ./ area(i);
  end
end


s = [1 1 1 1 1]';
t = [2 3 4 5 6]';
G = graph(s',t');
I = full(incidence(G))

n = length(s);

ind = (1:n)'
A = full(sparse([s;t],[ind;ind],[ones(n,1)*-1;ones(n,1)]));

A

I

A - I

s = [1 1 1 1 1]';
t = [6 3 4 5 2]';
G = graph(s',t');
I = full(incidence(G))

ind = (1:n)'
A = full(sparse([s;t],[ind;ind],[ones(n,1)*-1;ones(n,1)]));

A

I

A - I

s = [1 1 1 1 1 6]';
n = length(s);
t = [6 3 4 5 2 1]';
G = graph(s',t');%switches orientation of duplicates
I = full(incidence(G))

ind = (1:n)'
A = full(sparse([s;t],[ind;ind],[ones(n,1)*-1;ones(n,1)]));

A

I

A - I

s = [1 1 1 1 1 6]';
n = length(s);
t = [6 3 4 5 2 1]';
G = digraph(s',t'); %does not switch orientation of duplicates
I = full(incidence(G))

ind = (1:n)'
A = full(sparse([s;t],[ind;ind],[ones(n,1)*-1;ones(n,1)]));

A

I

A - I


function I=gausslaguerr(f2,a,b,n)

%I=gausslaguerr(f,a,b,n)
%Aproximates integral using Gauss-Laguerre method

%Laguerre polynomial
p=polelague(n);
%Roots
x=roots(p(n+1,:));

G=feval(f2,x);		%Function evaluation on the nodes

%Coeficients
for i=1:n
   C(i)=((factorial(n).^2).*x(i))./(polyval(p(n+1,:),(x(i))).^2);
end

I=dot(C,G);


function f = p31_f ( n, x )

%*****************************************************************************80
%
%% P31_F evaluates the objective function for problem 31.
%
%  Discussion:
%
%    The minimal function value is -10.15320.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 January 2001
%
%  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 N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  m = 5;
  a = [ 4.0, 4.0, 4.0, 4.0; ...
        1.0, 1.0, 1.0, 1.0; ...
        8.0, 8.0, 8.0, 8.0; ...
        6.0, 6.0, 6.0, 6.0; ...
        3.0, 7.0, 3.0, 7.0 ]';

  c = [ 0.1, 0.2, 0.2, 0.4, 0.6 ]';

  f = 0.0;
  for j = 1 : m
    f = f - 1.0 / ( c(j) + sum ( ( x(1:n) - a(1:n,j) ).^2 ) );
  end

  return
end


function w = wf_fft(w);

% WAVEFORM = WF_FFT(WAVEFORM)  calculate frequency spectrum of a waveform.
% The results of the fast fourier transform is added as new fields
% in the waveform:
%   FFT_FREQ is a vector of frequencies with N samples
%   FFT_AMP is a vector of spetral amplitudes
%   FFT_PHASE is a vector of phases
%   FFT_DOM is the scalar frequency of the maximum amplitude
%           peak (or dominant frequency. This could change)

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$

admin.deprecated(mfilename,'wf_fft.compute');

% CHECK ARGUMENTS
if ~strcmpi(class(w),'waveform')
    error('First input must be a waveform object');
end

% STEP THROUGH WAVFORMS ADDING NEW FIELDS
[N,M] = size(w);
for i = 1:N*M
    Fn = get(w(i),'NYQ');
    x = get(w(i),'DATA');
    NFFT=2.^(ceil(log(length(x))/log(2)));  % Next highest power of 2
    FFTX=fft(x,NFFT);                       % Take fft, padding with zeros.
    NumUniquePts = ceil((NFFT+1)/2);
    FFTX=FFTX(1:NumUniquePts);              % throw out neg frequencies
    MX=abs(FFTX);                           % Take magnitude of X
    MX=MX*2;                                % Multiply by 2
    MX=MX/length(x);
    PX=phase(FFTX);                           % Take magnitude of X
    f=(0:NumUniquePts-1)*2/NFFT;
    f=f*Fn;
    w(i) = addfield(w(i),'FFT_FREQ',f');
    w(i) = addfield(w(i),'FFT_AMP',MX);
    w(i) = addfield(w(i),'FFT_PHASE',PX);
    a = find(MX == max(MX));
    w(i) = addfield(w(i),'FFT_DOM',f(a));
end;


function val = straight_line_integral_2(xs,ys,zs,data3d,x1,y1,z1,x2,y2,z2)
%
%   Deshan Yang, PhD
%	Department of radiation oncology
%	Washington University in Saint Louis
%   01/16/2011, Saint Louis, MO, USA
%
%   val = straight_line_integral_2(xs,ys,zs,data3d,x1,y1,z1,x2,y2,z2)
%
%   Perform straight line integration on the 3D data from point 1 to point
%   2
%
%   Deshan Yang, PhD
%   01/16/2011, Saint Louis, MO, USA
%
%   This function assumes that the point positions are given in the same
%   coordinate system as the 3D data. Any coordination transformation or
%   scaling should be done priorly.
%
%   Both data and points should be in single, or double
%
dim = [size(data3d) 1];
% if numel(xs)~=length(xs) || length(xs) ~= dim(2) || ...
% 		numel(ys)~=length(ys) || length(ys) ~= dim(1) || ...
% 		numel(zs)~=length(zs) || length(zs) ~= dim(3)
% 	fprintf('Incorrect image data: length does not match to the image dimension.\n');
% 	return;
% end

% Make sure the image and the voxel coordinate be positively increasing. If not, then flip it
[xs,ys,zs,data3d] = make_sure_positive(xs,ys,zs,data3d);
psrc = [x1,y1,z1];
pcdet = [x2,y2,z2];

corner_points = [...
	xs(1) ys(1) zs(1);...
	xs(1) ys(1) zs(end);...
	xs(1) ys(end) zs(1);...
	xs(1) ys(end) zs(end);...
	xs(end) ys(1) zs(1);...
	xs(end) ys(1) zs(end);...
	xs(end) ys(end) zs(1);...
	xs(end) ys(end) zs(end)...
	];

corner_points_t = zeros(8,1);
for k =1:8
	p = corner_points(k,:);
	[q,corner_points_t(k)] = project_1_point_to_a_line(psrc,pcdet,p);
end

t_min = min(corner_points_t);
t_max = max(corner_points_t);

dist = norm(psrc-pcdet);

N = ceil(dist*(t_max-t_min));	% N planes
dt = (t_max-t_min)/N;
ts = t_min:dt:t_max;

xst = psrc(1)+(pcdet(1)-psrc(1))*ts;
yst = psrc(2)+(pcdet(2)-psrc(2))*ts;
zst = psrc(3)+(pcdet(3)-psrc(3))*ts;

val = sum(interp3(xs,ys,zs,data3d,xst,yst,zst,'linear',0))*dt;

% % Transform the point coordinate into the image voxel index
% x1 = (x1 - xs(1))/dx+0.5;
% x2 = (x2 - xs(1))/dx+0.5;
% y1 = (y1 - ys(1))/dy+0.5;
% y2 = (y2 - ys(1))/dy+0.5;
% z1 = (z1 - zs(1))/dz+0.5;
% z2 = (z2 - zs(1))/dz+0.5;
%
%
% nx = ceil(min(x1,x2)):floor(max(x1,x2));
% tx = (nx-x1)/(x2-x1);
%
% ny = ceil(min(y1,y2)):floor(max(y1,y2));
% ty = (ny-y1)/(y2-y1);
%
% nz = ceil(min(z1,z2)):floor(max(z1,z2));
% tz = (nz-z1)/(z2-z1);
%
%
%
% ts = sort([tx ty tz]);    % It may be slow here
% dist = sqrt((x2-x1).^2+(y2-y1).^2+(z2-z1).^2);
%
% nx = (x2-x1)*ts+x1;
% ny = (y2-y1)*ts+y1;
% nz = (z2-z1)*ts+z1;
%
% idxes = find(nx>=0 & nx<=dim(2) & ny>=0 & ny<=dim(1) & nz>=0 & nz<=dim(3));
% nx = nx(idxes);
% ny = ny(idxes);
% nz = nz(idxes);
% N_1 = length(nx)-1;
%
% xidx = max(ceil((nx(1:N_1)+nx(2:end))/2),1);
% yidx = max(ceil((ny(1:N_1)+ny(2:end))/2),1);
% zidx = max(ceil((nz(1:N_1)+nz(2:end))/2),1);
%
% if ~isempty(xidx)
%     if x2~=x1
%         lengths = diff(nx)/(x2-x1);
%     else
%         lengths = diff(ny)/(y2-y1);
%     end
%
%     val = sum(data3d(sub2ind(dim,yidx,xidx,zidx)).*lengths)*dist;
% 	val = val*dx*dy*dz;	% Convert the length back to the original image coordinate length
% else
%     val = 0;
% end
%
%
%
%


function T = tn_vector()

    T.expectation = @TN_vector_expectation;
    function [exp] = TN_vector_expectation(mus, taus)

        sigmas = 1.0 ./ sqrt(taus);
        x = - mus ./ sigmas;
        %lambdax = norm.pdf(x)/(0.5*erfc(x/sqrt(2)));
        lambdax = pdf('Normal',x,0,1) ./ (0.5 * erfc(x/sqrt(2)));
        exp = mus + sigmas .* lambdax;

        for i = 1 : length(exp)
            v = exp(i);
            mu = mus(i);
            tau = taus(i);
            sigma = sigmas(i);

            if mu < -30 * sigma
                v = 1/(abs(mu)*tau);
            else
                % do nothing;
            end

            if isnan(v)
                v = 0;
            end

            exp(i) = v;
        end

        exp(isinf(exp)) = 0;

    end

    T.variance = @TN_vector_variance;
    function [var] = TN_vector_variance(mus, taus)

        sigmas = 1.0 ./ sqrt(taus);
        x = - mus ./ sigmas;
        %lambdax = norm.pdf(x)/(0.5*erfc(x/math.sqrt(2)));
        lambdax = pdf('Normal',x,0,1) ./ (0.5 * erfc(x/sqrt(2)));
        deltax = lambdax .* (lambdax-x);
        var = sigmas.^2 .* ( 1 - deltax );

        for i = 1 : length(var)
            v = var(i);
            mu = mus(i);
            tau = taus(i);
            sigma = sigmas(i);

            if mu < -30 * sigma
                v = (1/(abs(mu)*tau)).^2;
            else
                % do nothing;
            end

            if isnan(v)
                v = 0;
            end

            var(i) = v;
        end

        var(isinf(var)) = 0;
    end
end


function [e,E_q] = q2e(q)

% Q2E  Quaternion to Euler angles conversion.
%   Q2E(Q) returns an Euler angles vector [roll;pitch;yaw] corresponding to
%   the orientation quaternion Q.
%
%   [E,Jq] = Q2E(Q) returns also the Jacobian matrix.
%
%   See also QUATERNION, EULERANGLES, R2Q, E2Q, Q2V.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.
%   Revised 2018 Joan Sola - handle full gimbal range (2pi, pi, 2pi)

a  = q(1); % a: real part
b  = q(2);
c  = q(3);
d  = q(4);

r32 =  2*c*d + 2*a*b;
r33 =  a^2   - b^2   - c^2 + d^2;
r31 =  2*b*d - 2*a*c;
r21 =  2*b*c + 2*a*d;
r11 =  a^2   + b^2   - c^2 - d^2;

n   = sqrt(r11^2+r21^2);

e   = [ atan2( r32,r33)
        atan2(-r31, n)
        atan2( r21,r11) ];

if nargout >1

    % partials of R wrt q
    dr11dq  = [  2*a,  2*b, -2*c, -2*d];
    dr21dq  = [  2*d,  2*c,  2*b,  2*a];
    dr31dq  = [ -2*c,  2*d, -2*a,  2*b];
    dr32dq  = [  2*b,  2*a,  2*d,  2*c];
    dr33dq  = [  2*a, -2*b, -2*c,  2*d];

    % partials of n wrt R elements
    dndr11 = r11/n;
    dndr21 = r21/n;

    % partials of e wrt n
    de2dn   = r31/(n^2 + r31^2);

    % partials of e wrt R elements
    de1dr32 =  r33/(r33^2 + r32^2);
    de1dr33 = -r32/(r33^2 + r32^2);
    de2dr31 = -n /(n^2 + r31^2);
    de3dr11 = -r21/(r11^2 + r21^2);
    de3dr21 =  r11/(r11^2 + r21^2);

    % chain rule: rows of Jacobian
    de1dq  = de1dr33*dr33dq + de1dr32*dr32dq;
    de2dq  = de2dr31*dr31dq + de2dn*(dndr11*dr11dq+dndr21*dr21dq);
    de3dq  = de3dr11*dr11dq + de3dr21*dr21dq;

    % assemble Jacobian
    E_q    = [de1dq;de2dq;de3dq];
end

return

%% jac

syms a b c d real
q=[a;b;c;d];
[e,E_q] = q2e(q);
simplify(E_q-jacobian(e,q))

%% test
for i = 1 : 1000
    e = [2/3;1/3;2/3] .* randn(3,1);
    eo = q2e(e2q(e));
    if any(e-eo > 1e-10) % Should never enter this IF
        e
        eo
        disp('###########')
    end
end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [ r, w ] = oned_gauss ( rule )

%*****************************************************************************80
%
%% ONED_GAUSS sets up a Gauss quadrature rule in [-1,+1].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2011
%
%  Author:
%
%    Jeff Borggaard
%
%  Parameters:
%
%    Input, integer RULE, the number of Gauss points desired.
%
%    Output, real R(RULE), Gauss points located between (-1,1).
%
%    Output, real W(RULE), Gauss weights corresponding to r.
%
  r = zeros(rule,1);
  w = zeros(rule,1);

  if ( rule == 1 )
    r(1) = 0;
    w(1) = 2;
  elseif ( rule == 2 )
    r(1) =-1.0 / sqrt(3.0);
    r(2) =-r(1);
    w(1) = 1.0;
    w(2) = 1.0;
  elseif ( rule == 3 )
    r(1) =-sqrt(3.0/5.0);
    r(2) = 0.0;
    r(3) =-r(1);
    w(1) = 5.0 / 9.0;
    w(2) = 8.0 / 9.0;
    w(3) = w(1);
  elseif ( rule == 4 )
    r(1) =-sqrt((3.0+2.0*sqrt(6.0/5.0))/7.0);
    r(2) =-sqrt((3.0-2.0*sqrt(6.0/5.0))/7.0);
    r(3) =-r(2);
    r(4) =-r(1);
    w(1) = 0.5 - 1.0 / ( 6.0 * sqrt(6.0/5.0) );
    w(2) = 0.5 + 1.0 / ( 6.0 * sqrt(6.0/5.0) );
    w(3) = w(2);
    w(4) = w(1);
  elseif ( rule == 5 )
    r(1) =-sqrt(5.0+4.0*sqrt(5.0/14.0)) / 3.0;
    r(2) =-sqrt(5.0-4.0*sqrt(5.0/14.0)) / 3.0;
    r(3) = 0.0;
    r(4) =-r(2);
    r(5) =-r(1);
    w(1) = 161.0/450.0-13.0/(180.*sqrt(5.0/14.0));
    w(2) = 161.0/450.0+13.0/(180.*sqrt(5.0/14.0));
    w(3) = 128.0/225.0;
    w(4) = w(2);
    w(5) = w(1);
  elseif ( rule == 6 )
    r(1) = -0.2386191861;
    r(2) = -0.6612093865;
    r(3) = -0.9324695142;
    r(4) = - r(1);
    r(5) = - r(2);
    r(6) = - r(3);
    w(1) = .4679139346;
    w(2) = .3607615730;
    w(3) = .1713244924;
    w(4) = w(1);
    w(5) = w(2);
    w(6) = w(3);
  else
    error('Quadrature rule not supported')
    keyboard
  end

  return
end


function rank = bal_seq_rank ( n, t )

%*****************************************************************************80
%
%% BAL_SEQ_RANK ranks a balanced sequence.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of 0's (and 1's) in the sequence.
%    N must be positive.
%
%    Input, integer T(2*N), a balanced sequence.
%
%    Output, integer RANK, the rank of the balanced sequence.
%

%
%  Check.
%
  ierror = bal_seq_check ( n, t );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BAL_SEQ_RANK - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal.\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'BAL_SEQ_RANK - Fatal error!' );
  end

  y = 0;
  rank = 0;

  for x = 1 : 2 * n - 1

    if ( t(x) == 0 )
      y = y + 1;
    else
      mxy = mountain ( n, x, y + 1 );
      rank = rank + mxy;
      y = y - 1;
    end

  end

  return
end


function v = mono_value ( d, nx, f, x )

%*****************************************************************************80
%
%% MONO_VALUE evaluates a monomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 December 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer D, the spatial dimension.
%
%    Input, integer NX, the number of evaluation points.
%
%    Input, integer F(D), the exponents of the monomial.
%
%    Input, real X(D,NX), the coordinates of the evaluation points.
%
%    Output, real V(NX), the value of the monomial at X.
%
  v = ones ( nx, 1 );

  for i = 1 : d
    v(1:nx,1) = v(1:nx,1) .* x(i,1:nx)' .^ f(i);
  end

  return
end


function out = SY_StatAv(y,whatType,n)
% SY_StatAv     Simple mean-stationarity metric, StatAv.
%
% The StatAv measure divides the time series into non-overlapping subsegments,
% calculates the mean in each of these segments and returns the standard deviation
% of this set of means.
%
% Empirically mean-stationary data would display StatAv approaching to zero.
%
% cf. "Heart rate control in normal and aborted-SIDS infants", S. M. Pincus et al.
% Am J. Physiol. Regul. Integr. Comp. Physiol. 264(3) R638 (1993)
%
%---INPUTS:
%
% y, the input time series
%
% whatType, the type of StatAv to perform:
%           (i) 'seg': divide the time series into n segments
%           (ii) 'len': divide the time series into segments of length n
%
% n, either the number of subsegments ('seg') or their length ('len')

% Might be nicer to use the 'buffer' function for this...?
% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
% Check Inputs
% ------------------------------------------------------------------------------
if nargin < 2 || isempty(whatType)
    whatType = 'seg'; % divide into n segments by default
end

if nargin < 3 || isempty(n)
    n = 5; % use 5 segments
end

N = length(y); % Time-series length

% ------------------------------------------------------------------------------
% Compute the means in local time-series segments
% ------------------------------------------------------------------------------

switch whatType
case 'seg'
    % divide time series into n segments
    M = zeros(n,1);
    p = floor(N/n);% lose the last N mod n data points

    for j = 1:n
        M(j) = mean(y(p*(j-1)+1:p*j));
    end
case 'len'
    if N > 2*n
        pn = floor(N/n);
        M = zeros(pn,1);
        for j = 1:pn
            M(j) = mean(y((j-1)*n+1:j*n));
        end
    else
        fprintf(1,'This time series (N = %u) is too short for StatAv(%s,''%u'')\n',N,whatType,n);
        out = NaN; return
    end
otherwise
    error('Error evaluating StatAv of type ''%s'', please select either ''seg'' or ''len''',whatType)
end

% ------------------------------------------------------------------------------
% Compute the StatAv statistic

s = std(y); % should be 1 (for a z-scored time-series input)
sdav = std(M);
out = sdav/s;

end


%DEMEV2	Demonstrate Bayesian classification for the MLP.
%
%	Description
%	A synthetic two class two-dimensional dataset X is sampled  from a
%	mixture of four Gaussians.  Each class is associated with two of the
%	Gaussians so that the optimal decision boundary is non-linear. A 2-
%	layer network with logistic outputs is trained by minimizing the
%	cross-entropy error function with isotroipc Gaussian regularizer (one
%	hyperparameter for each of the four standard weight groups), using
%	the scaled conjugate gradient optimizer. The hyperparameter vectors
%	ALPHA and BETA are re-estimated using the function EVIDENCE. A graph
%	is plotted of the optimal, regularised, and unregularised decision
%	boundaries.  A further plot of the moderated versus unmoderated
%	contours is generated.
%
%	See also
%	EVIDENCE, MLP, SCG, DEMARD, DEMMLP2
%

%	Copyright (c) Ian T Nabney (1996-2001)

clc;

disp('This program demonstrates the use of the evidence procedure on')
disp('a two-class problem.  It also shows the improved generalisation')
disp('performance that can be achieved with moderated outputs; that is')
disp('predictions where an approximate integration over the true')
disp('posterior distribution is carried out.')
disp(' ')
disp('First we generate a synthetic dataset with two-dimensional input')
disp('sampled from a mixture of four Gaussians.  Each class is')
disp('associated with two of the Gaussians so that the optimal decision')
disp('boundary is non-linear.')
disp(' ')
disp('Press any key to see a plot of the data.')
pause;

% Generate the matrix of inputs x and targets t.

rand('state', 423);
randn('state', 423);

ClassSymbol1 = 'r.';
ClassSymbol2 = 'y.';
PointSize = 12;
titleSize = 10;

fh1 = figure;
set(fh1, 'Name', 'True Data Distribution');
whitebg(fh1, 'k');

%
% Generate the data
%
n=200;

% Set up mixture model: 2d data with four centres
% Class 1 is first two centres, class 2 from the other two
mix = gmm(2, 4, 'full');
mix.priors = [0.25 0.25 0.25 0.25];
mix.centres = [0 -0.1; 1.5 0; 1 1; 1 -1];
mix.covars(:,:,1) = [0.625 -0.2165; -0.2165 0.875];
mix.covars(:,:,2) = [0.25 0; 0 0.25];
mix.covars(:,:,3) = [0.2241 -0.1368; -0.1368 0.9759];
mix.covars(:,:,4) = [0.2375 0.1516; 0.1516 0.4125];

[data, label] = gmmsamp(mix, n);

%
% Calculate some useful axis limits
%
x0 = min(data(:,1));
x1 = max(data(:,1));
y0 = min(data(:,2));
y1 = max(data(:,2));
dx = x1-x0;
dy = y1-y0;
expand = 5/100;			% Add on 5 percent each way
x0 = x0 - dx*expand;
x1 = x1 + dx*expand;
y0 = y0 - dy*expand;
y1 = y1 + dy*expand;
resolution = 100;
step = dx/resolution;
xrange = [x0:step:x1];
yrange = [y0:step:y1];
%
% Generate the grid
%
[X Y]=meshgrid([x0:step:x1],[y0:step:y1]);
%
% Calculate the class conditional densities, the unconditional densities and
% the posterior probabilities
%
px_j = gmmactiv(mix, [X(:) Y(:)]);
px = reshape(px_j*(mix.priors)',size(X));
post = gmmpost(mix, [X(:) Y(:)]);
p1_x = reshape(post(:, 1) + post(:, 2), size(X));
p2_x = reshape(post(:, 3) + post(:, 4), size(X));

plot(data((label<=2),1),data(label<=2,2),ClassSymbol1, 'MarkerSize', ...
PointSize)
hold on
axis([x0 x1 y0 y1])
plot(data((label>2),1),data(label>2,2),ClassSymbol2, 'MarkerSize', ...
    PointSize)

% Convert targets to 0-1 encoding
target=[label<=2];
disp(' ')
disp('Press any key to continue')
pause; clc;

disp('Next we create a two-layer MLP network with 6 hidden units and')
disp('one logistic output.  We use a separate inverse variance')
disp('hyperparameter for each group of weights (inputs, input bias,')
disp('outputs, output bias) and the weights are optimised with the')
disp('scaled conjugate gradient algorithm.  After each 100 iterations')
disp('the hyperparameters are re-estimated twice.  There are eight')
disp('cycles of the whole algorithm.')
disp(' ')
disp('Press any key to train the network and determine the hyperparameters.')
pause;

% Set up network parameters.
nin = 2;		% Number of inputs.
nhidden = 6;		% Number of hidden units.
nout = 1;		% Number of outputs.
alpha = 0.01;		% Initial prior hyperparameter.
aw1 = 0.01;
ab1 = 0.01;
aw2 = 0.01;
ab2 = 0.01;

% Create and initialize network weight vector.
prior = mlpprior(nin, nhidden, nout, aw1, ab1, aw2, ab2);
net = mlp(nin, nhidden, nout, 'logistic', prior);

% Set up vector of options for the optimiser.
nouter = 8;			% Number of outer loops.
ninner = 2;			% Number of innter loops.
options = foptions;		% Default options vector.
options(1) = 1;			% This provides display of error values.
options(2) = 1.0e-5;		% Absolute precision for weights.
options(3) = 1.0e-5;		% Precision for objective function.
options(14) = 100;		% Number of training cycles in inner loop.

% Train using scaled conjugate gradients, re-estimating alpha and beta.
for k = 1:nouter
  net = netopt(net, options, data, target, 'scg');
  [net, gamma] = evidence(net, data, target, ninner);
  fprintf(1, '\nRe-estimation cycle %d:\n', k);
  disp(['  alpha = ', num2str(net.alpha')]);
  fprintf(1, '  gamma =  %8.5f\n\n', gamma);
  disp(' ')
  disp('Press any key to continue.')
  pause;
end

disp(' ')
disp('Network training and hyperparameter re-estimation are now complete.')
disp('Notice that the final error value is close to the number of data')
disp(['points (', num2str(n), ') divided by two.'])
disp('Also, the hyperparameter values differ, which suggests that a single')
disp('hyperparameter would not be so effective.')
disp(' ')
disp('First we train an MLP without Bayesian regularisation on the')
disp('same dataset using 400 iterations of scaled conjugate gradient')
disp(' ')
disp('Press any key to train the network by maximum likelihood.')
pause;
% Train standard network
net2 = mlp(nin, nhidden, nout, 'logistic');
options(14) = 400;
net2 = netopt(net2, options, data, target, 'scg');
y2g = mlpfwd(net2, [X(:), Y(:)]);
y2g = reshape(y2g(:, 1), size(X));

disp(' ')
disp('We can now plot the function represented by the trained networks.')
disp('We show the decision boundaries (output = 0.5) and the optimal')
disp('decision boundary given by applying Bayes'' theorem to the true')
disp('data model.')
disp(' ')
disp('Press any key to add the boundaries to the plot.')
pause;

% Evaluate predictions.
[yg, ymodg] = mlpevfwd(net, data, target, [X(:) Y(:)]);
yg = reshape(yg(:,1),size(X));
ymodg = reshape(ymodg(:,1),size(X));

% Bayesian decision boundary
[cB, hB] = contour(xrange,yrange,p1_x,[0.5 0.5],'b-');
[cNb, hNb] = contour(xrange,yrange,yg,[0.5 0.5],'r-');
[cN, hN] = contour(xrange,yrange,y2g,[0.5 0.5],'g-');
set(hB, 'LineWidth', 2);
set(hNb, 'LineWidth', 2);
set(hN, 'LineWidth', 2);
Chandles = [hB(1) hNb(1) hN(1)];
legend(Chandles, 'Bayes', ...
  'Reg. Network', 'Network', 3);

disp(' ')
disp('Note how the regularised network predictions are closer to the')
disp('optimal decision boundary, while the unregularised network is')
disp('overtrained.')

disp(' ')
disp('We will now compare moderated and unmoderated outputs for the');
disp('regularised network by showing the contour plot of the posterior')
disp('probability estimates.')
disp(' ')
disp('The first plot shows the regularised (moderated) predictions')
disp('and the second shows the standard predictions from the same network.')
disp('These agree at the level 0.5.')
disp('Press any key to continue')
pause
levels = 0:0.1:1;
fh4 = figure;
set(fh4, 'Name', 'Moderated outputs');
hold on
plot(data((label<=2),1),data(label<=2,2),'r.', 'MarkerSize', PointSize)
plot(data((label>2),1),data(label>2,2),'y.', 'MarkerSize', PointSize)

[cNby, hNby] = contour(xrange, yrange, ymodg, levels, 'k-');
set(hNby, 'LineWidth', 1);

fh5 = figure;
set(fh5, 'Name', 'Unmoderated outputs');
hold on
plot(data((label<=2),1),data(label<=2,2),'r.', 'MarkerSize', PointSize)
plot(data((label>2),1),data(label>2,2),'y.', 'MarkerSize', PointSize)

[cNbm, hNbm] = contour(xrange, yrange, yg, levels, 'k-');
set(hNbm, 'LineWidth', 1);

disp(' ')
disp('Note how the moderated contours are more widely spaced.  This shows')
disp('that there is a larger region where the outputs are close to 0.5')
disp('and a smaller region where the outputs are close to 0 or 1.')
disp(' ')
disp('Press any key to exit')
pause
close(fh1);
close(fh4);
close(fh5);


function colors = RainbowColors_ss(numcolors)
% Generates RGB Triplets based on the idealized colors below:
% 0. red    [1 0 0]
% 1. orange [1 .5 0]
% 2. yellow [1 1 0]
% 2.5. green  [0 1 0]
% 3. cyan   [0 1 1]
% 4. blue   [0 0 1]
% 5. purple [.5 0 .5]
%
% INPUT
% -numcolors: number of colors you want out
% OUTPUT
% - colors a 3 column matrix signifying Red Green and Blue values with each
% column, number of rows = numcolors
%
% Brendon Watson 2015

cidxs = linspace(0,1,numcolors);
r = redfunc(cidxs);
g = greenfunc(cidxs);
b = bluefunc(cidxs);

colors = [r g b];

1;

function y = redfunc(x)
for a = 1:length(x)
    if x(a)<2/5
        y(a) = 1;
    elseif x(a)>=2/5 && x(a)<2.5/5
        y(a) = 1-5*(x(a)-2/5);
    elseif x(a)>=2.5/5 && x(a)<4/5
        y(a) = 0;
    elseif x(a)>=4/5
        y(a) = 0+.5*5*(x(a)-4/5);
    end
end
y = y';

function y = greenfunc(x)
for a = 1:length(x)
    if x(a)<2/5
        y(a) = .5*5*(x(a));
    elseif x(a)>=2/5 && x(a)<3/5
        y(a) = 1;
    elseif x(a)>=3/5 && x(a)<4/5
        y(a) = 1-5*(x(a)-3/5);
    elseif x(a)>=4/5
        y(a) = 0;
    end
end
y = y';

function y = bluefunc(x)
for a = 1:length(x)
    if x(a)<3/5
        y(a) = 0;
    elseif x(a)>=2.5/5 && x(a)<3/5
        y(a) = 5*(x(a)-2.5/5);
    elseif x(a)>=3/5 && x(a)<4/5
        y(a) = 1;
    elseif x(a)>=4/5
        y(a) = 1-.5*5*(x(a)-4/5);
    end
end
y = y';


function CRB = crb_general_sto_1d(design, wavelength, doas, P, noise_var, snapshot_count)
%CRB_GENERAL_STO_1D CRB for general 1D arrays based on the stochastic
%(unconditional) model, in radians.
%Inputs:
%   design - Array design.
%   wavelength - Wavelength.
%   doas - DOA vector in radians.
%   P - Source covariance matrix. If all sources are uncorrelated and
%       shares the same power, you can just pass in a scalar. If all
%       sources are uncorrelated but have different powers, you can just
%       pass in a vector.
%   noise_var - Noise power.
%   snapshot_count - (Optional) number of snapshots. Default is one.
%Reference:
% [1] P. Stoica and A. Nehorai, "Performance study of conditional and
%     unconditional direction-of-arrival estimation," IEEE Transactions on
%     Acoustics, Speech and Signal Processing, vol. 38, no. 10,
%     pp. 1783-1795, Oct. 1990.
if design.dim ~= 1
    error('1D array expected.');
end
if nargin <= 5
    snapshot_count = 1;
end
m = design.element_count;
k = length(doas);
P = unify_source_power_matrix(P, k);
[A, D] = steering_matrix(design, wavelength, doas);
R = A*P*A' + noise_var * eye(m);
H = D'*(eye(m) - A/(A'*A)*A')*D;
CRB = real(H .* ((P*A'/R*A*P).'));
CRB = eye(k) / CRB * (noise_var / snapshot_count / 2);
end


function particles = correction_step(particles, z)

% Weight the particles according to the current map of the particle
% and the landmark observations z.
% z: struct array containing the landmark observations.
% Each observation z(j) has an id z(j).id, a range z(j).range, and a bearing z(j).bearing
% The vector observedLandmarks indicates which landmarks have been observed
% at some point by the robot.

% Number of particles
numParticles = length(particles);

% Number of measurements in this time step
m = size(z, 2);

% TODO: Construct the sensor noise matrix Q_t (2 x 2)
Q_t = 0.01*eye(2);
% process each particle
for i = 1:numParticles
  robot = particles(i).pose;
  % process each measurement
  for j = 1:m
    % Get the id of the landmark corresponding to the j-th observation
    % particles(i).landmarks(l) is the EKF for this landmark
    l = z(j).id;

    % The (2x2) EKF of the landmark is given by
    % its mean particles(i).landmarks(l).mu
    % and by its covariance particles(i).landmarks(l).sigma

    % If the landmark is observed for the first time:
    if (particles(i).landmarks(l).observed == false)

      % TODO: Initialize its position based on the measurement and the current robot pose:

      particles(i).landmarks(l).mu = [robot(1)+z(j).range*cos(z(j).bearing + robot(3)); robot(2) + z(j).range*sin(z(j).bearing + robot(3))];

      % get the Jacobian with respect to the landmark position
      [h, H] = measurement_model(particles(i), z(j));

      % TODO: initialize the EKF for this landmark

      particles(i).landmarks(l).sigma = H\Q_t*inv(H)';

      % Indicate that this landmark has been observed
      particles(i).landmarks(l).observed = true;

    else

      % get the expected measurement
      [expectedZ, H] = measurement_model(particles(i), z(j));

      % TODO: compute the measurement covariance

	Q = H*particles(i).landmarks(l).sigma*H' + Q_t;

      % TODO: calculate the Kalman gain

	K = particles(i).landmarks(l).sigma*H'/Q;

      % TODO: compute the error between the z and expectedZ (remember to normalize the angle)

	z_diff = [z(j).range;z(j).bearing] - expectedZ;
	z_diff(2) = normalize_angle(z_diff(2));

      % TODO: update the mean and covariance of the EKF for this landmark

	particles(i).landmarks(l).mu = particles(i).landmarks(l).mu + K*z_diff;
	particles(i).landmarks(l).sigma = (eye(2) - K*H)*particles(i).landmarks(l).sigma;

      % TODO: compute the likelihood of this observation, multiply with the former weight
      %       to account for observing several features in one time step
	particles(i).weight = particles(i).weight*1/sqrt(det(2*pi*Q))*exp(-1/2*z_diff'/Q*z_diff);
    end

  end % measurement loop
end % particle loop

end


function [ c, seed ] = c8vec_uniform_01 ( n, seed )

%*****************************************************************************80
%
%% C8VEC_UNIFORM_01 returns a unit pseudorandom C8VEC.
%
%  Discussion:
%
%    The angles should be uniformly distributed between 0 and 2 * PI,
%    the square roots of the radius uniformly distributed between 0 and 1.
%
%    This results in a uniform distribution of values in the unit circle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Second Edition,
%    Springer, 1987,
%    ISBN: 0387964673,
%    LC: QA76.9.C65.B73.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, December 1986, pages 362-376.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley, 1998,
%    ISBN: 0471134031,
%    LC: T57.62.H37.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, Number 2, 1969, pages 136-143.
%
%  Parameters:
%
%    Input, integer N, the number of values to compute.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, complex C(N), the pseudorandom complex vector.
%
%    Output, integer SEED, a seed for the random number generator.
%
  c = zeros ( n, 1 );

  i4_huge = 2147483647;

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'C8VEC_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'C8VEC_UNIFORM_01 - Fatal error!' );
  end

  for j = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + i4_huge;
    end

    r = sqrt ( seed * 4.656612875E-10 );

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + i4_huge;
    end

    theta = 2.0 * pi * seed * 4.656612875E-10;

    c(j) = r * ( cos ( theta ) + sin ( theta ) * i );

  end

  return
end


function [points1, points2] = get_uniform_points_boundary(ellipse1, ellipse2, nr_points)
% GET_UNIFORM_POINTS_BOUNDARY gives the sets of points that are
% equidistantly chosen on the boudary of two ellipses, so that future OSPA
% could be calculated
%
% Input:
%        ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2]
%        ellipse2, 1x5, parameterization of the other ellispe [m1 m2 alpha l1 l2]
%        nr_points, nr of points that are uniformly chosen on the boundary
%                   to calculate OPSA distance
% Output:
%       points1, 2xnr_points, points that are chosen on the ellipse1
%       poitns2, 2xnr_points, points that are chosen on the ellispe2
% Written by Shishan Yang

theta = (0:2*pi/nr_points:2*pi-2*pi/nr_points);

alpha1 = ellipse1(3);
center1 = ellipse1(1:2)';
l1 = ellipse1(4:5);
gt_rotation_mat = [cos(alpha1) -sin(alpha1); sin(alpha1) cos(alpha1)];
points1(1,:) = l1(1)*cos(theta);
points1(2,:) = l1(2)*sin(theta);

alpha2 = ellipse2(3);
center2 = ellipse2(1:2)';
l2 = ellipse2(4:5);
est_rotation_mat = [cos(alpha2) -sin(alpha2); sin(alpha2) cos(alpha2)];
points2(1,:) = l2(1)*cos(theta);
points2(2,:) = l2(2)*sin(theta);

points1 = gt_rotation_mat*points1 + repmat(center1,1,size(points1,2));
points2 = est_rotation_mat*points2 + repmat(center2,1,size(points2,2));
end


function value = r8_aid ( x )

%*****************************************************************************80
%
%% R8_AID evaluates the derivative of the Airy function Ai of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the derivative of the Airy function
%    Ai of X.
%
  persistent aifcs
  persistent aigcs
  persistent naif
  persistent naig
  persistent x2sml
  persistent x3sml

  if ( isempty ( naif ) )
    aifcs = [ ...
       0.105274612265314088088970057325134114, ...
       0.011836136281529978442889292583980840, ...
       0.000123281041732256643051689242469164, ...
       0.000000622612256381399016825658693579, ...
       0.000000001852988878441452950548140821, ...
       0.000000000003633288725904357915995625, ...
       0.000000000000005046217040440664768330, ...
       0.000000000000000005223816555471480985, ...
       0.000000000000000000004185745090748989, ...
       0.000000000000000000000002672887324883, ...
       0.000000000000000000000000001392128006, ...
       0.000000000000000000000000000000602653, ...
       0.000000000000000000000000000000000220 ]';
    aigcs = [ ...
       0.0212338781509186668523122276848937, ...
       0.0863159303352144067524942809461604, ...
       0.0017975947203832313578033963225230, ...
       0.0000142654998755506932526620687495, ...
       0.0000000594379952836832010488787064, ...
       0.0000000001524033664794478945214786, ...
       0.0000000000002645876603490435305100, ...
       0.0000000000000003315624296815020591, ...
       0.0000000000000000003139789757594792, ...
       0.0000000000000000000002325767379040, ...
       0.0000000000000000000000001384384231, ...
       0.0000000000000000000000000000676629, ...
       0.0000000000000000000000000000000276 ]';
    eta = 0.1 * r8_mach ( 3 );
    naif = r8_inits ( aifcs, 13, eta );
    naig = r8_inits ( aigcs, 13, eta );
    x3sml = r8_mach ( 3 )^0.3334;
    x2sml = sqrt ( r8_mach ( 3 ) );
  end

  if ( x < - 1.0 )
    [ xn, phi ] = r8_admp ( x );
    value = xn * cos ( phi );
  elseif ( abs ( x ) <= x2sml )
    x2 = 0.0;
    x3 = 0.0;
    value = ( x2 * ( 0.125 + r8_csevl ( x3, aifcs, naif ) ) ...
      - r8_csevl ( x3, aigcs, naig ) ) - 0.25;
  elseif ( abs ( x ) <= x3sml )
    x2 = x * x;
    x3 = 0.0;
    value = ( x2 * ( 0.125 + r8_csevl ( x3, aifcs, naif ) ) ...
      - r8_csevl ( x3, aigcs, naig ) ) - 0.25;
  elseif ( x <= 1.0 )
    x2 = x * x;
    x3 = x * x * x;
    value = ( x2 * ( 0.125 + r8_csevl ( x3, aifcs, naif ) ) ...
      - r8_csevl ( x3, aigcs, naig ) ) - 0.25;
  else
    value = r8_aide ( x ) ...
      * exp ( - 2.0 * x * sqrt ( x ) / 3.0 );
  end

  return
end


function pols = kjacopols ( x, a, b, n )

%*****************************************************************************80
%
%% KJACOPOLS evaluates Jacobi polynomials.
%
%  Discussion:
%
%    This routine evaluates Jacobi polynomials.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    26 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by 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:
%
%    Input, real X, the evaluation point.
%
%    Input, real A, B, the parameter values.
%
%    Input, integer N, the highest degree to be evaluated.
%
%    Output, real POLS(N+1), the polynomial values.
%
  pols = zeros(n+1,1);

  pkp1 = 1.0;
  pols(1) = pkp1;

  if ( n == 0 )
    return
  end

  pk = pkp1;
  pkp1 = ( a / 2.0 - b / 2.0 ) ...
    + ( 1.0 + a / 2.0 + b / 2.0 ) * x;
  pols(2) = pkp1;

  if ( n == 1 )
    return
  end

  for k = 2 : n

    pkm1 = pk;
    pk = pkp1;

    alpha = ( 2.0 * k + a + b - 1.0 ) ...
      * ( a * a - b * b + ( 2.0 * k + a + b - 2.0 ) ...
      * ( 2.0 * k + a + b ) * x );

    beta = 2.0 * ( k + a - 1.0 ) * ( k + b - 1.0 ) ...
      * ( 2.0 * k + a + b );

    pkp1 = ( alpha * pk - beta * pkm1 ) ...
      / ( 2.0 * k * ( k + a + b ) ...
      * ( 2.0 * k + a + b - 2.0 ) );

    pols(k+1) = pkp1;

  end

  return
end


function [ t, wts ] = h_quadrature_rule ( nt )

%*****************************************************************************80
%
%% H_QUADRATURE_RULE: quadrature for H(i,x).
%
%  Discussion:
%
%    H(i,x) is the physicist's Hermite polynomial of degree I.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NT, the order of the rule.
%
%    Output, real T(NT,1), WTS(NT,1), the points and weights of the rule.
%
  aj = zeros ( nt, 1 );
  bj = sqrt ( ( 1 : nt )' / 2.0 );
  wts = zeros ( nt, 1 );
  wts(1,1) = sqrt ( sqrt ( pi ) );

  [ t, wts ] = imtqlx ( nt, aj, bj, wts );

  wts(1:nt,1) = wts(1:nt,1).^2;

  return
end


function v = dquat2pos(dq)

% DQUAT2POS  Transforms a double quaternion point position representation
%            into a vector representation.
%
%     V = DQUAT2POS(DQ) transforms the double quaternion representation of
%        a point position (in the euclidean space) into a vector v, which
%        represents the point coordinates. DQ is either a vector of size 8
%        or an array of size 8*N (each column represents a point position
%        dual quaternion) where N is the number of point locations. V is a
%        vector of size 3 or an array of size 3*N depending on the input
%        format.
%
% See also POS2DQUAT, DQUAT2VEL, DQUAT2LINE, DQUAT2LINEVEL

sdq = size(dq);
if sdq == [1 8], dq = dq'; sdq = size(dq); end

% wrong format
if sdq(1) ~= 8
    error('DualQuaternion:dquat2pos:wrongsize',...
        '%d rows in the DQ array. It should be 8.',sdq(1));
end

% check point position dual quaternion
tol = 1e-6;
[maxval1,imax1] = max(abs(dq(1,:)-1));
[maxval2,imax2] = max(max(abs(dq(2:5,:))));
imax = union(imax1,imax2);
if maxval1 > tol || maxval2 > tol
    warning('DualQuaternion:dquat2pos:wrongFormat',...
        'At least one dual quaternion is not a point position dual quaternion (tol = %.1e).\n Indices of max values: %d',...
        tol,imax);
end

% extraction of the point position coordinates
v = dq(6:8,:);


function [gamma]=wildual(g,M,L)
%WILDUAL  Wilson dual window
%   Usage:  gamma=wildual(g,M);
%           gamma=wildual(g,M,L);
%
%   Input parameters:
%         g     : Gabor window.
%         M     : Number of modulations.
%         L     : Length of window. (optional)
%   Output parameters:
%         gamma : Canonical dual window.
%
%   `wildual(g,M)` returns the dual window of the Wilson or WMDCT basis with
%   window *g*, parameter *M* and length equal to the length of the window *g*.
%
%   The window *g* may be a vector of numerical values, a text string or a
%   cell array. See the help of |wilwin| for more details.
%
%   If the length of *g* is equal to $2\cdot M$ then the input window is
%   assumed to be an FIR window. In this case, the dual window also has
%   length of $2\cdot M$. Otherwise the smallest possible transform length is
%   chosen as the window length.
%
%   `wildual(g,M,L)` does the same, but now *L* is used as the length of the
%   Wilson basis.
%
%   The input window *g* must be real and whole-point even. If *g* is not
%   whole-point even, then reconstruction using the dual window will not be
%   perfect. For a random window *g*, the window closest to *g* that satisfies
%   these restrictions can be found by ::
%
%     g_wpe = real(peven(g));
%
%   All windows in the toolbox satisfies these restrictions unless
%   clearly stated otherwise.
%
%   See also:  dwilt, wilwin, wmdct, wilorth, isevenfunction

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: TEST_DWILT
%   REFERENCE: OK

complainif_argnonotinrange(nargin,2,3,mfilename);

if nargin==2
    L=[];
end;

%% ------ step 2: Verify a, M and L
if isempty(L)
    if isnumeric(g)
        % Use the window length
        Ls=length(g);
    else
        % Use the smallest possible length
        Ls=1;
    end;

    % ----- step 2b : Verify M and get L from the window length ----------
    L=dwiltlength(Ls,M);

else

    % ----- step 2a : Verify M and get L

    Luser=dwiltlength(L,M);
    if Luser~=L
        error(['%s: Incorrect transform length L=%i specified. Next valid length ' ...
               'is L=%i. See the help of DWILTLENGTH for the requirements.'],...
              upper(mfilename),L,Luser);
    end;

end;

%% ----- step 3 : Determine the window

[g,info]=wilwin(g,M,L,upper(mfilename));

if L<info.gl
  error('%s: Window is too long.',upper(mfilename));
end;

%% ----- call gabdual ----------------
a=M;

g=fir2long(g,L);
gamma=2*comp_gabdual_long(g,a,2*M);


function result = torus_6s2 ( func, r1, r2, result )

%*****************************************************************************80
%
%% TORUS_6S2 approximates an integral inside a torus in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that:
%
%      ( SQRT ( X**2 + Y**2 ) - R1 )**2 + Z**2 <= R2**2.
%
%  Discussion:
%
%    An 84 point 6-th degree formula is used, Stroud number TOR3-S2:6-1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function of three variables which is to be integrated,
%    of the form:
%      function value = func ( x, y, z )
%
%    Input, real R1, R2, the two radii that define the torus.
%
%    Output, real RESULT, the approximate integral of the function.
%
  norder = 2;
  s = [ 0.322914992E+00, 0.644171310E+00 ];
  weight = [ 0.387077796E+00, 0.165609800E+00 ];

  w = 1.0E+00 / ( 7.0E+00 * r1 * pi );

  quad = 0.0E+00;

  for n = 1 : 7

    u = 0.5E+00 * sqrt ( 3.0E+00 ) * r2;
    cth = cos ( 2.0E+00 * pi * n / 7.0E+00 );
    sth = sin ( 2.0E+00 * pi * n / 7.0E+00 );

    for i = 1 : 2

      u = -u;

      x = ( r1 + u ) * cth;
      y = ( r1 + u ) * sth;
      z = 0.0E+00;
      quad = quad + 0.232710567E+00 * w * ( r1 + u ) * feval ( func, x, y, z );

      x = r1 * cth;
      y = r1 * sth;
      z = u;
      quad = quad + 0.232710567E+00 * w * r1 * feval ( func, x, y, z );

    end

    for k = 1 : norder

      u = s(k) * r2;
      v = u;

      for i = 1 : 2

        u = -u;

        for j = 1 : 2

          v = -v;

          x = ( r1 + u ) * cth;
          y = ( r1 + u ) * sth;
          z = v;
          quad = quad + weight(k) * w * ( r1 + u ) * feval ( func, x, y, z );

        end
      end
    end
  end

  volume = torus_volume_3d ( r1, r2 );
  result = quad * volume;

  return
end


function u = nvecs(X,n,r,opts)
%NVECS Compute the leading mode-n vectors for a tensor.
%
%   U = NVECS(X,n,r) computes the r leading eigenvalues of Xn*Xn'
%   (where Xn is the mode-n matricization of X), which provides
%   information about the mode-n fibers. In two-dimensions, the r
%   leading mode-1 vectors are the same as the r left singular vectors
%   and the r leading mode-2 vectors are the same as the r right
%   singular vectors.
%
%   U = NVECS(X,n,r,OPTS) specifies options:
%   OPTS.eigsopts: options passed to the EIGS routine [struct('disp',0)]
%   OPTS.flipsign: make each column's largest element positive [true]
%
%   See also TENSOR, TENMAT, EIGS.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

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

if ~exist('opts','var')
    opts = struct;
end

if isfield(opts,'eigsopts')
    eigsopts = opts.eigsopts;
else
    eigsopts.disp = 0;
end

Xn = double(tenmat(X,n));
Y = Xn*Xn';
warning off;
[u,d] = eigs(Y, r, 'LM', eigsopts);
warning on;
if isfield(opts,'flipsign')
    flipsign = opts.flipsign;
else
    flipsign = true;
end

if flipsign
    % Make the largest magnitude element be positive
    [val,loc] = max(abs(u));
    for i = 1:r
        if u(loc(i),i) < 0
            u(:,i) = u(:,i) * -1;
        end
    end
end


function	[out,dout]=rodrigues(in)

% RODRIGUES	Transform rotation matrix into rotation vector and viceversa.
%
%		Sintax:  [OUT]=RODRIGUES(IN)
% 		If IN is a 3x3 rotation matrix then OUT is the
%		corresponding 3x1 rotation vector
% 		if IN is a rotation 3-vector then OUT is the
%		corresponding 3x3 rotation matrix
%

%%
%%		Copyright (c) March 1993 -- Pietro Perona
%%		California Institute of Technology
%%

%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995.
%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !!

%% BUG when norm(om)=pi fixed -- April 6th, 1997;
%% Jean-Yves Bouguet

%% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003;
%% Jean-Yves Bouguet

% BUG FOR THE CASE norm(om)=pi fixed by Mike Burl on Feb 27, 2007

[m,n] = size(in);
%bigeps = 10e+4*eps;
bigeps = 10e+20*eps;

if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector
    theta = norm(in);
    if theta < eps
        R = eye(3);

        %if nargout > 1,

        dRdin = [0 0 0;
            0 0 1;
            0 -1 0;
            0 0 -1;
            0 0 0;
            1 0 0;
            0 1 0;
            -1 0 0;
            0 0 0];

        %end;

    else
        if n==length(in)  in=in'; end; 	%% make it a column vec. if necess.

        %m3 = [in,theta]

        dm3din = [eye(3);in'/theta];

        omega = in/theta;

        %m2 = [omega;theta]

        dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1];

        alpha = cos(theta);
        beta = sin(theta);
        gamma = 1-cos(theta);
        omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]];
        A = omega*omega';

        %m1 = [alpha;beta;gamma;omegav;A];

        dm1dm2 = zeros(21,4);
        dm1dm2(1,4) = -sin(theta);
        dm1dm2(2,4) = cos(theta);
        dm1dm2(3,4) = sin(theta);
        dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0;
            0 0 -1 0 0 0 1 0 0;
            0 1 0 -1 0 0 0 0 0]';

        w1 = omega(1);
        w2 = omega(2);
        w3 = omega(3);

        dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0];
        dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0];
        dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3];

        R = eye(3)*alpha + omegav*beta + A*gamma;

        dRdm1 = zeros(9,21);

        dRdm1([1 5 9],1) = ones(3,1);
        dRdm1(:,2) = omegav(:);
        dRdm1(:,4:12) = beta*eye(9);
        dRdm1(:,3) = A(:);
        dRdm1(:,13:21) = gamma*eye(9);

        dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din;

    end;
    out = R;
    dout = dRdin;

    %% it is prob. a rot matr.
elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)...
        & (abs(det(in)-1) < bigeps))
    R = in;

    % project the rotation matrix to SO(3);
    [U,S,V] = svd(R);
    R = U*V';

    tr = (trace(R)-1)/2;
    dtrdR = [1 0 0 0 1 0 0 0 1]/2;
    theta = real(acos(tr));

    if sin(theta) >= 1e-4,

        dthetadtr = -1/sqrt(1-tr^2);

        dthetadR = dthetadtr * dtrdR;
        % var1 = [vth;theta];
        vth = 1/(2*sin(theta));
        dvthdtheta = -vth*cos(theta)/sin(theta);
        dvar1dtheta = [dvthdtheta;1];

        dvar1dR =  dvar1dtheta * dthetadR;

        om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]';

        dom1dR = [0 0 0 0 0 1 0 -1 0;
            0 0 -1 0 0 0 1 0 0;
            0 1 0 -1 0 0 0 0 0];

        % var = [om1;vth;theta];
        dvardR = [dom1dR;dvar1dR];

        % var2 = [om;theta];
        om = vth*om1;
        domdvar = [vth*eye(3) om1 zeros(3,1)];
        dthetadvar = [0 0 0 0 1];
        dvar2dvar = [domdvar;dthetadvar];

        out = om*theta;
        domegadvar2 = [theta*eye(3) om];

        dout = domegadvar2 * dvar2dvar * dvardR;

    else
        if tr > 0; 			% case norm(om)=0;

            out = [0 0 0]';

            dout = [0 0 0 0 0 1/2 0 -1/2 0;
                0 0 -1/2 0 0 0 1/2 0 0;
                0 1/2 0 -1/2 0 0 0 0 0];
        else

            % case norm(om)=pi;
            if(0)

                %% fixed April 6th by Bouguet -- not working in all cases!
                out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]);
                %keyboard;

            else

                % Solution by Mike Burl on Feb 27, 2007
                % This is a better way to determine the signs of the
                % entries of the rotation vector using a hash table on all
                % the combinations of signs of a pairs of products (in the
                % rotation matrix)

                % Define hashvec and Smat
                hashvec = [0; -1; -3; -9; 9; 3; 1; 13; 5; -7; -11];
                Smat = [1,1,1; 1,0,-1; 0,1,-1; 1,-1,0; 1,1,0; 0,1,1; 1,0,1; 1,1,1; 1,1,-1;
                    1,-1,-1; 1,-1,1];

                M = (R+eye(3,3))/2;
                uabs = sqrt(M(1,1));
                vabs = sqrt(M(2,2));
                wabs = sqrt(M(3,3));

                mvec = [M(1,2), M(2,3), M(1,3)];
                syn  = ((mvec > 1e-4) - (mvec < -1e-4)); % robust sign() function
                hash = syn * [9; 3; 1];
                idx = find(hash == hashvec);
                svec = Smat(idx,:)';

                if ( isempty(svec) )
                    out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]);
                else
                    out = theta * [uabs; vabs; wabs] .* svec;
                end

            end;

            if nargout > 1,
                fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n');
                dout = NaN*ones(3,9);
            end;
        end;
    end;

else
    error('Neither a rotation matrix nor a rotation vector were provided');
end;

return;

%% test of the Jacobians:

%%%% TEST OF dRdom:
om = randn(3,1);
dom = randn(3,1)/1000000;

[R1,dR1] = rodrigues(om);
R2 = rodrigues(om+dom);

R2a = R1 + reshape(dR1 * dom,3,3);

gain = norm(R2 - R1)/norm(R2 - R2a)

%%% TEST OF dOmdR:
om = randn(3,1);
R = rodrigues(om);
dom = randn(3,1)/10000;
dR = rodrigues(om+dom) - R;

[omc,domdR] = rodrigues(R);
[om2] = rodrigues(R+dR);

om_app = omc + domdR*dR(:);

gain = norm(om2 - omc)/norm(om2 - om_app)

%%% OTHER BUG: (FIXED NOW!!!)

omu = randn(3,1);
omu = omu/norm(omu)
om = pi*omu;
[R,dR]= rodrigues(om);
[om2] = rodrigues(R);
[om om2]

%%% NORMAL OPERATION

om = randn(3,1);
[R,dR]= rodrigues(om);
[om2] = rodrigues(R);
[om om2]

return

% Test: norm(om) = pi

u = randn(3,1);
u = u / sqrt(sum(u.^2));
om = pi*u;
R = rodrigues(om);

R2 = rodrigues(rodrigues(R));

norm(R - R2)


function [r, v] = orb2eci(mu, oev)

% convert classical orbital elements to eci state vector

% input

%  mu     = gravitational constant (km**3/sec**2)
%  oev(1) = semimajor axis (kilometers)
%  oev(2) = orbital eccentricity (non-dimensional)
%           (0 <= eccentricity < 1)
%  oev(3) = orbital inclination (radians)
%           (0 <= inclination <= pi)
%  oev(4) = argument of perigee (radians)
%           (0 <= argument of perigee <= 2 pi)
%  oev(5) = right ascension of ascending node (radians)
%           (0 <= raan <= 2 pi)
%  oev(6) = true anomaly (radians)
%           (0 <= true anomaly <= 2 pi)

% output

%  r = eci position vector (kilometers)
%  v = eci velocity vector (kilometers/second)

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

r = zeros(3, 1);
v = zeros(3, 1);

% unload orbital elements array

sma = oev(1);
ecc = oev(2);
inc = oev(3);
argper = oev(4);
raan = oev(5);
tanom = oev(6);

slr = sma * (1 - ecc * ecc);

rm = slr / (1 + ecc * cos(tanom));

arglat = argper + tanom;

sarglat = sin(arglat);
carglat = cos(arglat);

c4 = sqrt(mu / slr);
c5 = ecc * cos(argper) + carglat;
c6 = ecc * sin(argper) + sarglat;

sinc = sin(inc);
cinc = cos(inc);

sraan = sin(raan);
craan = cos(raan);

% position vector

r(1) = rm * (craan * carglat - sraan * cinc * sarglat);
r(2) = rm * (sraan * carglat + cinc * sarglat * craan);
r(3) = rm * sinc * sarglat;

% velocity vector

v(1) = -c4 * (craan * c6 + sraan * cinc * c5);
v(2) = -c4 * (sraan * c6 - craan * cinc * c5);
v(3) = c4 * c5 * sinc;


function Stall_margin = retreating_stall_envelope(mu_act,Cts_act)
% Datum Rotor Coefficients (mu verus Cw/s)      High Speed Rotor Coefficients (mu verus Cw/s)
% a:	-7.6568	Squared term                    a:	-7.4029	Squared term
% b:	0.2992	Linear term                     b:	1.2392	Linear term
% c:	0.5357	Constant                        c:	0.4827	Constant

highspeed = 0; %0: datum; 1: high speed

% Datum Rotor Coefficients (mu verus Cw/s)
Cdat=[-7.6568   ;
0.2992;
0.5357];

% High Speed Rotor Coefficients (mu verus Cw/s)
Chs=[-7.4029	;
1.2392	;
0.4827	];

C = highspeed*Chs+(1-highspeed)*Cdat;
a = C(1); b = C(2); c = C(3);

Cts_act=Cts_act*2; %because of the way to find Ct in the place where this data comes from (with a 1/2 in Ct calc)

mu_all = a*Cts_act.^2+Cts_act*b+c;

Cts_all = (-b-sqrt(b^2-4*a*(c-mu_act)))/(2*a);

Stall_margin = (mu_all-mu_act)/abs(mu_all);
if isreal(Cts_all)
    Ct_margin = (Cts_all-Cts_act)/abs(Cts_all);
    Stall_margin = (Stall_margin+Ct_margin)/2;
else
    Stall_margin = -abs(Stall_margin);
end


function kf = polynomial_correlation(xf, yf, a, b)
%POLYNOMIAL_CORRELATION Polynomial Kernel at all shifts, i.e. kernel correlation.
%   Evaluates a polynomial kernel with constant A and exponent B, for all
%   relative shifts between input images XF and YF, which must both be MxN.
%   They must also be periodic (ie., pre-processed with a cosine window).
%   The result is an MxN map of responses.
%
%   Inputs and output are all in the Fourier domain.
%
%   Joao F. Henriques, 2014
%   http://www.isr.uc.pt/~henriques/

	%cross-correlation term in Fourier domain
	xyf = xf .* conj(yf);
	xy = sum(real(ifft2(xyf)), 3);  %to spatial domain

	%calculate polynomial response for all positions, then go back to the
	%Fourier domain
	kf = fft2((xy / numel(xf) + a) .^ b);

end


function d = DistanceToSE3(mat)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes mat: A 4x4 matrix.
% Returns the Frobenius norm to describe the distance of mat from the SE(3)
% manifold.
% Compute the determinant of matR, the top 3x3 submatrix of mat. If
% det(matR) <= 0, return a large number. If det(matR) > 0, replace the top
% 3x3 submatrix of mat with matR' * matR, and set the first three entries
% of the fourth column of mat to zero. Then return norm(mat - I).
% Example Inputs:
%
% clear; clc;
% mat = [1.0, 0.0,   0.0,  1.2;
%        0.0, 0.1, -0.95,  1.5;
%        0.0, 1.0,   0.1, -0.9;
%        0.0, 0.0,   0.1, 0.98];
% d = DistanceToSE3(mat)
%
% Output:
% d =
%     0.1349

[R, p] = TransToRp(mat);
if det(R) > 0
	d = norm([R' * R, [0; 0; 0]; mat(4, :)] - eye(4), 'fro');
else
    d = 1e+9;
end
end


function  Metric_table  = MFSK_DEMOD(Data_in,M,E_s,SNR_dB)
%*** MFSK DEMOD ***
%function  Metric_table  = MFSK_DEMOD(Data_in,M,E_s,SNR_dB)
% Create a (M x n) matrix containing the metrics of the M-FSK demodulator,
% simulating an AWGN channel
% Data_in is random values [0 .. M-1], representing the data that must be
% transmitted
% M is the number of frequencies used (M-FSK)
% E_s is the transmitted signal energy
% SNR_dB is the signal-to-noise ratio, measured in dBs

    n = length(Data_in);

    %Calculate the mu and sigma_2 values from the SNR

    SNR = Convert_SNR(SNR_dB);
    N_o = E_s / SNR;        % Noise Power on channel in dB (Proakis - MFSK)

    mu = 0;
    sigma_2 = (1/2)*N_o;
    sigma = sqrt(sigma_2);

    %******************************
    %*** Create Decision matrix ***
    %******************************

    Metric_table = [];
    T_Metrics =[];

    %Create 2 x M metric outputs for timeslot i (M equals number of frequencies in M-FSK)
    for num = 1:n
        Signal = [];
        for i = 0:M-1
            if (i == Data_in(num))
                %It is the symbol that was transmitted

                %Determine a value for phi
                phi = 2 * pi * rand;

                % Compute the COS Component:
                Signal(i+1,1) = sqrt(E_s) * cos(phi) + normrnd(mu,sigma);

                % Compute the SIN Component
                Signal(i+1,2) = sqrt(E_s) * sin(phi) + normrnd(mu,sigma);

            else
                Signal(i+1,1) = normrnd(mu,sigma);
                Signal(i+1,2) = normrnd(mu,sigma);
            end
        end

        % Compute the Envelope metrics for each symbol
        Metrics = [];  % Vector containing the computed envelope metrics

        for i = 1:M
            Metrics(i) = Decision_metric(Signal(i,1),Signal(i,2));
        end

        Metric_table = [Metric_table transpose(Metrics)];
    end


% Demo of use of TVDIP - total variation denoising algorithm
clear all;

% Generate test signal: piecewise smooth with noise corruption
y = [1.5*ones(10000,1); -0.7*ones(2000,1); 1.3*ones(100,1); -0.3*ones(5000,1)];
N = length(y);
y = y + 0.5*randn(N,1);

% Find the value of lambda greater than which the TVD solution is just the
% mean
lmax = tvdiplmax(y);

% Perform TV denoising for lambda across a range of values up to a small
% fraction of the maximum found above
lratio = [1e-4 1e-3 1e-2 1e-1];
L = length(lratio);
[x, E, status] = tvdip(y,lmax*lratio,1,1e-3,100);

% Plots - display original test signal y, and TVD results
close all;
figure;
for l = 1:L
    subplot(L,1,l);
    hold on;
    plot(y,'-','Color',0.8*[1 1 1]);
    plot(x(:,l),'k-');
    axis tight;
    title(sprintf('\\lambda=%5.2e, \\lambda/\\lambda_{max}=%5.2e',lmax*lratio(l),lratio(l)));
end
xlabel('n');
legend({'Input y_n','TVD x_n'});


function p=v_sone2phon(s)
%V_PHON2SONE convert SONE loudness values to PHONs p=(s)
%Inputs:    s is a matrix of sone values
%
%Outputs:   p is a matrix, the same size as s, of phon values
%
% The phon scale measures perceived loudness in dB; at 1 kHz it is identical to dB SPL
% relative to 20e-6 Pa sound pressure. The sone scale is proportional to apparent loudness
% and, by definition, equals 1 at 40 phon. The form of the loudness curve is taken from [1].
% The hearing threshold at 1 kHz for 18 to 25 year olds with normal hearing is taken from [2].
%
% Refs: [1]	J. Lochner and J. Burger. Form of the loudness function in the presence of masking noise.
%           The Journal of the Acoustical Society of America, 33: 1705, 1961.
%       [2]	ISO/TC43. Acoustics, Normal equal-loudness-level contours.
%           Standard ISO 226:2003, Aug. 2003.

%      Copyright (C) Mike Brookes 2012-2013
%      Version: $Id: v_sone2phon.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b d
if isempty(a)
    b=1/(log(10)*0.1*0.27); % 0.27 is the exponent from [1] and [2]
    d=exp(2.4/b); % 2.4 dB is teh hearing threshold from [2]
    a=exp(40/b)-d; % scale factor to make p=40 give s=1
end
if nargout>0

    p=b*log(a*s+d);
else
    if nargin<1 || isempty(s)
        pp=linspace(5,90,100)'; % phon values
        ss=v_phon2sone(pp);
    else
        ss=s;
    end
    semilogx(ss,v_sone2phon(ss));
    v_axisenlarge(-1);
    v_xticksi;
    ylabel('phon = dB SPL @ 1 kHz');
    xlabel('sone');
end