function [V]=ellipseCoord(A,t)

% function [V]=ellipseCoord(A,t)
% ------------------------------------------------------------------------
% Calculates ellipse coordiantes for the angles in t based on the vector A
% which defines the centre coordinates, the radii and the angle
% respectively.
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
% 2013/24/09
%------------------------------------------------------------------------

%%
x0=A(1);
y0=A(2);
x=A(3).*cos(t);
y=A(4).*sin(t);
V=[x(:) y(:) zeros(size(x(:)))];
[R,~]=euler2DCM([0 0 -A(5)]);
V=(R*V')';
V(:,1)=V(:,1)+x0;
V(:,2)=V(:,2)+y0;

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


%% RATE OF CONVERGENCE OF CUBIC FINITE ELEMENT METHOD
%
% This example is to show the rate of convergence of cubic finite element
% approximation of the Poisson equation on the regular polygons:
%
% $$- \Delta u = f \; \hbox{in } Omega$$
%
% for the Dirichlet boundary condition.

clear variables
%% Set up problem
% PDE and Boundary condition.
pde = simpledata; % f = 1, g_D = 0
% FEM
option.elemType = 'P3';

%% Options
option.L0 = 0;
option.maxIt = 4;
option.printlevel = 1;
option.plotflag = 1;

%% Case 1: Triangle
[node,elem] = regpolygon(3,0.5);
bdFlag = setboundary(node,elem,'Dirichlet');
mesh = struct('node',node,'elem',elem,'bdFlag',bdFlag);
showmesh(node,elem);
femPoisson(mesh,pde,option);

%% Case 2: Square
[node,elem] = squaremesh([0,1,0,1],0.25);
bdFlag = setboundary(node,elem,'Dirichlet');
mesh = struct('node',node,'elem',elem,'bdFlag',bdFlag);
showmesh(node,elem);
femPoisson(mesh,pde,option);

%% Case 3: Pentagon
[node,elem] = regpolygon(5,0.5);
bdFlag = setboundary(node,elem,'Dirichlet');
mesh = struct('node',node,'elem',elem,'bdFlag',bdFlag);
showmesh(node,elem);
femPoisson(mesh,pde,option);

%% Case 4: Hexagon
[node,elem] = regpolygon(6,0.5);
showmesh(node,elem);
bdFlag = setboundary(node,elem,'Dirichlet');
mesh = struct('node',node,'elem',elem,'bdFlag',bdFlag);
showmesh(node,elem);
femPoisson(mesh,pde,option);


% X = NPAIRSK(N, K)
%
% Number of pair combinations for N objects to K pairs.
%
% For instance,
%
%   npairsk(2,1) = 1
%
% because two elements can be divided into a pair in one way.
%
%   npairsk(4,2) = 3
%
% because four elements (A,B,C,D) can be divided into two pairs in three
% ways: [(A,B);(C,D)], [(A,C);(B,D)], [(A,D);(B,C)].
%
%   npairsk(3,1) = 3
%
% because [(A,B)], [(A,C)], [(B,C)].

% Last modified 2011-01-28
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function x = npairsk(n, k)

x = factorial(n) ./ (factorial(2*k) .* factorial(n-2*k)) .* ngroupsk(k,2);


function [L,La,Lb] = points2plucker(A,B)

% POINTS2PLUCKER  Plucker line from two homogeneous points
%   L = POINTS2PLUCKER(A,B) is the Plucker line that passes over two points
%   A and B. The points are specified by their homogeneous coordinates
%   [x;y;z;w].
%
%   The result is a Plucker line expressed as a 6-vector. This vector can
%   be decomosed as L = [a;b], where the 3-vectors a and b admit the
%   following interpretation:
%
%   *   a - is a vector normal to the plane that contains the line and the
%       origin of coordinates.
%
%   *   b - is a vector director of the line, which lies on the plane
%       above.
%
%   *   a and b are orthogonal, i.e. dot(a,b)=0.
%
%   *   the distance from the line to the origin is given by
%       norm(a)/norm(b).
%
%   [L,La,Lb] = ... returns the Jacobians wrt A and B.
%
%   See also PLANES2PLUCKER.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

L = [cross(A(1:3),B(1:3));A(4)*B(1:3)-B(4)*A(1:3)];

if nargout > 1

    [a1,a2,a3,a4] = split(A);
    [b1,b2,b3,b4] = split(B);

    La = [...
        [   0,  b3, -b2,   0]
        [ -b3,   0,  b1,   0]
        [  b2, -b1,   0,   0]
        [ -b4,   0,   0,  b1]
        [   0, -b4,   0,  b2]
        [   0,   0, -b4,  b3]];

    Lb = [...
        [   0, -a3,  a2,   0]
        [  a3,   0, -a1,   0]
        [ -a2,  a1,   0,   0]
        [  a4,   0,   0, -a1]
        [   0,  a4,   0, -a2]
        [   0,   0,  a4, -a3]];
end

return

%% jac

syms a1 a2 a3 a4 b1 b2 b3 b4 p1 p2 p3 p4 q1 q2 q3 q4 real
A=[a1;a2;a3;a4];
B=[b1;b2;b3;b4];
L=points2plucker(A,B)

La = jacobian(L,A)
Lb = jacobian(L,B)

% ========== 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 x= cauchyinv(p, varargin)

% USAGE:       x= cauchyinv(p, a, b)
%
% Inverse of the Cauchy cumulative distribution function (cdf), x= a + b*tan(pi*(p-0.5)).
%
% ARGUMENTS:
% p (0<=p<=1) might be of any dimension.
% a (default value: 0.0) must be scalars or size(p).
% b (b>0, default value: 1.0) must be scalars or size(p).
%
% EXAMPLE:
% p= 0:0.01:1;
% plot(cauchyinv(p), p);
%
% SEE ALSO:    cauchycdf, cauchyfit, cauchypdf, cauchyrnd.
%
% Copyright (C) Peder Axensten <peder at axensten dot se>
%
% HISTORY:
% Version 1.0, 2006-07-10.
% Version 1.1, 2006-07-26.
% - Added cauchyfit to the cauchy package.
% Version 1.2, 2006-07-31:
% - cauchyinv(0, ...) returned a large negative number but should be -Inf.
% - Size comparison in argument check didn't work.
% - Various other improvements to check list.

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

	% Default values
	a=	0.0;
	b=	1.0;

	% Check the arguments
	if(nargin >= 2)
		a=	varargin{1};
		if(nargin == 3)
			b=			varargin{2};
			b(b <= 0)=	NaN;	% Make NaN of out of range values.
		end
	end
	if((nargin < 1) || (nargin > 3))
		error('At least one argument, at most three!');
	end

	p(p < 0 | 1 < p)=	NaN;

	% Calculate
	x=			a + b.*tan(pi*(p-0.5));

	% Extreme values.
	if(numel(p) == 1), 	p= repmat(p, size(x));		end
	x(p == 0)=	-Inf;
	x(p == 1)=	Inf;
end


function P = agm_pi ( d )

%*****************************************************************************80
%
%% AGM_PI  Arithmetic-geometric mean for pi.
%
%  Discussion:
%
%    The Brent-Salamin algorithm is used to compute PI to D decimal digits.
%
%  Licensing:
%
%    Copyright (c) 2011, The MathWorks, Inc.
%    All rights reserved.
%
%    Redistribution and use in source and binary forms, with or without
%    modification, are permitted provided that the following conditions are
%    met:
%
%        * Redistributions of source code must retain the above copyright
%          notice, this list of conditions and the following disclaimer.
%        * 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
%        * Neither the name of the The MathWorks, Inc. nor the names
%          of its contributors may be used to endorse or promote products derived
%          from this software without specific prior written permission.
%
%    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 OWNER 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.
%
%  Modified:
%
%    27 February 2012
%
%  Author:
%
%    Cleve Moler
%
%  Reference:
%
%    Cleve Moler,
%    Cleve's Corner, "Computing Pi",
%    http://www.mathworks.com/company/newsletters/news_notes/2011/
%
%  Parameters:
%
%    Input, integer D, the number of decimal digits desired.
%
%    Output, symbolic P, the value of pi to D digits.
%
  digits ( d )

  a = vpa(1,d);
  b = 1/sqrt(vpa(2,d));
  s = 1/vpa(4,d);
  p = 1;
  n = ceil(log2(d));

  for k = 1:n
    c = (a+b)/2;
    b = sqrt(a*b);
    s = s - p*(c-a)^2;
    p = 2*p;
    a = c;
  end

  P = a^2 / s;

  return
end


function p = predict(theta, X)
%PREDICT Predict whether the label is 0 or 1 using learned logistic
%regression parameters theta
%   p = PREDICT(theta, X) computes the predictions for X using a
%   threshold at 0.5 (i.e., if sigmoid(theta'*x) >= 0.5, predict 1)

m = size(X, 1); % Number of training examples

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

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters.
%               You should set p to a vector of 0's and 1's
%

temp = sigmoid(X * theta);
p = (temp >= 0.5)

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

end


function fx = p03_fun ( n, x )

%*****************************************************************************80
%
%% P03_FUN evaluates the integrand for problem 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gwynne Evans,
%    Practical Numerical Integration,
%    Wiley, 1993,
%    ISBN: 047193898X,
%    LC: QA299.3E93.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(2,N), the evaluation points.
%
%    Output, real FX(N,1), the integrand values.
%
  fx(1:n,1) = 1.0 ./ sqrt ( 2.0 - x(1,1:n) - x(2,1:n) );

  return
end


%% Example 2.7: Hyperbolic Conservation Laws in 2D
%
% In this example we will solve the 2D scalar conservation law
%
% $$ u_t + f(u)_x + g(u)_y = 0, \qquad u(x,y,0) = u_0(x,y) $$
%
% by dimensional splitting. To this end, we use two substeps,
%
% $$ S_x(t): \quad v_t + f(v)_x = 0, \qquad v(x,0)=v_0(x)$$
%
% $$ S_y(t): \quad w_t + g(w)_y = 0, \qquad w(y,0)=w_0(y)$$
%
% and construct the approximate solution from the formula
%
% $$u(x,t)\approx [S_y(\Delta t)\circ S_x(\Delta t) ]^n u_0(x)$$
%
% The 1-D hyperbolic steps will be solved using the Lax-Friedrichs scheme

%% Initial setup
N = 256;
h = 2*pi/N;
x =-pi+(0:N)*h;  x=0.5*(x(1:end-1)+x(2:end));
y = x;
[X,Y] = ndgrid(x,y);
u0 = exp( -4*sin(X/2).^2 - 4*sin(Y/2).^2 );
h=path; path(h,'../Example2_5');

%% Flux functions
% As a concrete example, we choose the Buckley-Leverett flux in the
% x-direction and a convex flux in the y-direction
%
% $$f(u) = u^2 / (u^2 + (1-u)^2), \qquad g(u) = u(1-u)$$
%
% This system can be seen as a simplified model of two-phase flow in a
% periodic porous medium under the influence of convective and gravity
% forces (convective in the x-direction and gravity in the y-direction).
% The two fluxes are shown in the plot below
u = linspace(0,1,101);
plot(u, fflux(u), u, gflux(u)), axis tight, legend('f(u)','g(u)',2)

%% Evolution of the solution
T = 1.5;
nsplit = 50;
u = dimsplit('fflux','gflux', u0, x, y, T, nsplit);
I = 1:10:nsplit+1;
t = linspace(0,T,nsplit+1);
c = linspace(0,1,11);
for i=1:6
   subplot(2,3,i),
   contourf(x,y,u(:,:,I(i))',c), caxis([0 1]) %, colorbar('horiz')
   %surf(x,y,u(:,:,I(i))'), shading interp, view(3)
   title(['Time: t=' num2str(t(I(i)))]);
end
%%
% To understand the evolution of the initial blob, we discuss the movement
% in the two spatial directions separately. In the x-direction, the
% nonconvex Buckley-Leverett flux will turn the right-hand side of the
% symmetric blob into a leading shock, followed by a rarefaction. Likewise,
% the left-hand side of the blob turns into a shock followed by a
% rarefaction wave. In the y-direction, the convex flux gives a shock along
% the upper part of the blob and a rarefaction wave along the lower part.

%% Compare different time steps
% Having established the dynamics of the problem, we can investigate the
% performance of our numerical method for different choices of the
% splitting step. To this end, we increase time interval to [0,8].
T = 8;
c = linspace(0,.25,11);
for n=1:4,
	nsplit=4*2.^(n-1);
	u=dimsplit('fflux','gflux',u0,x,y,T,nsplit);
	subplot(2,2,n)
	contourf(x,y,u(:,:,nsplit+1)',c), caxis([0 .25]);
	xlabel('x'), ylabel('y','Rotation',0),  colorbar
	title([num2str(nsplit) ' steps']);
end;
path(h);
%%
% It is quite amazing how accurate one can capture the dynamics of this
% flow with only four splitting steps. Increasing the number of steps,
% improves small-scale features of the solution, but does not change the
% qualitative behavior of the approximate solution.


function value = composite_abscissa ( order, i )

%*****************************************************************************80
%
%% COMPOSITE_ABSCISSA returns the I-th abscissa of a composite rule.
%
%  Discussion:
%
%    Our convention is that the abscissas are numbered from left to
%    right, that the interval is [-1,1], and the the abscissas are
%    evenly spaced, and that, except for the order 1 rule, the
%    endpoints are included.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ORDER, the order of the rule.
%
%    Input, integer I, the index of the desired abscissa.
%    1 <= I <= ORDER.
%
%    Output, real VALUE, the value of the I-th abscissa in the
%    rule of order ORDER.
%
  a = -1.0;
  b = +1.0;

  if ( order < 1 )
    value = - Inf;
  elseif ( i < 1 | order < i )
    value = - Inf;
  elseif ( order == 1 )
    value = 0.5 * ( a + b );
  else
    value = ( ( order - i     ) * a   ...
            + (         i - 1 ) * b ) ...
            / ( order     - 1 );
  end

  return
end


function Z = projectData(X, U, K)
%PROJECTDATA Computes the reduced data representation when projecting only
%on to the top k eigenvectors
%   Z = projectData(X, U, K) computes the projection of
%   the normalized inputs X into the reduced dimensional space spanned by
%   the first K columns of U. It returns the projected examples in Z.
%

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

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the projection of the data using only the top K
%               eigenvectors in U (first K columns).
%               For the i-th example X(i,:), the projection on to the k-th
%               eigenvector is given as follows:
%                    x = X(i, :)';
%                    projection_k = x' * U(:, k);
%

Z = X * U(:, 1:K);

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

end


function node_xy = grid_nodes_01 ( x_num, y_num )

%*****************************************************************************80
%
%% GRID_NODES_01 returns an equally spaced rectangular grid of nodes in the unit square.
%
%  Example:
%
%    X_NUM = 5
%    Y_NUM = 3
%
%    NODE_XY =
%    ( 0, 0.25, 0.5, 0.75, 1, 0,   0.25, 0.5, 0.75, 1,   0, 0.25, 0.5, 0.75, 1;
%      0, 0,    0,   0,    0, 0.5, 0.5,  0.5, 0.5,  0.5, 1, 1.0,  1.0, 1.0,  1 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X_NUM, Y_NUM, the number of nodes in the X and Y directions.
%
%    Output, real NODE_XY(2,X_NUM*Y_NUM), the coordinates of the nodes.
%
  node_num = x_num * y_num;

  node_xy(1:2,1:node_num) = 0.0;

  if ( x_num == 1 )
    node_xy(1,1:node_num) = 0.5;
  else
    for i = 1 : x_num
      node_xy(1,i:x_num:i+(y_num-1)*x_num) = ( i - 1 ) / ( x_num - 1 );
    end
  end

  if ( y_num == 1 )
    node_xy(2,1:node_num) = 0.5;
  else
    for j = 1 : y_num
      node_xy(2,1+(j-1)*x_num:j*x_num) = ( j - 1 ) / ( y_num - 1 );
    end
  end

  return
end


function gt = gtom(adj,numSteps)
%GTOM       Generalized topological overlap measure
%
%   gt = gtom(adj,numSteps);
%
%   The m-th step generalized topological overlap measure (GTOM) quantifies
%   the extent to which a pair of nodes have similar m-th step neighbors.
%   Mth-step neighbors are nodes that are reachable by a path of at most
%   length m.
%
%   This function computes the the M x M generalized topological overlap
%   measure (GTOM) matrix for number of steps, numSteps.
%
%   Inputs:       adj,    adjacency matrix (binary,undirected)
%            numSteps,    number of steps
%
%   Outputs:       gt,    GTOM matrix
%
%   NOTE: When numSteps is equal to 1, GTOM is identical to the topological
%   overlap measure (TOM) from reference [2]. In that case the 'gt' matrix
%   records, for each pair of nodes, the fraction of neighbors the two
%   nodes share in common, where "neighbors" are one step removed. As
%   'numSteps' is increased, neighbors that are furter out are considered.
%   Elements of 'gt' are bounded between 0 and 1.  The 'gt' matrix can be
%   converted from a similarity to a distance matrix by taking 1-gt.
%
%   References: [1] Yip & Horvath (2007) BMC Bioinformatics 2007, 8:22
%               [2] Ravasz et al (2002) Science 297 (5586), 1551.
%
%   J Goni, University of Navarra and Indiana University, 2009/2011

%#ok<*ASGLU>

%initial state for bm matrix;
bm = adj;
bmAux = bm;
numNodes = size(adj,1);

if (numSteps > numNodes)
    disp('warning, reached maximum value for numSteps. numSteps reduced to adj-size')
    numSteps = numNodes;
end

if (numSteps == 0)
    %GTOM0
    gt = adj;
else

    for steps = 2:numSteps
        for i = 1:numNodes

            %neighbours of node i
            [neighRow,neighColumn] = find(bm(i,:)==1);

            %neighbours of neighbours of node i
            [neighNeighRow,neighNeighColumn] = find(bm(neighColumn,:)==1);
            newNeigh = setdiff(unique(neighNeighColumn),i);

            %neighbours of neighbours of node i become considered node i neighbours
            bmAux(i,newNeigh) = 1;

            %keep symmetry of matrix
            bmAux(newNeigh,i) = 1;
        end
        %bm is updated with new step all at once
        bm = bmAux;

    end

    clear bmAux newNeigh;

    %numerators of GTOM formula
    numeratorMatrix = bm*bm + adj + speye(numNodes,numNodes);

    %vector containing degree of each node
    bmSum=sum(bm);
    clear bm;

    denominatorMatrix = -adj + min(repmat(bmSum,numNodes,1),repmat(bmSum',1,numNodes)) + 1;
    gt = numeratorMatrix ./ denominatorMatrix;
end


%  Figure 6.60      Feedback Control of Dynamic Systems, 5e
%                   Franklin, Powell, Emami
%

clear all
close all

num=10;
den=conv([1 0],[1/2.5 1]);
den=conv(den,[1/6 1]);
w=logspace(-1,2,100);
[mag,phas]=bode(num,den,w);
[OLgm,OLpm,OLwcg,OLwcp]=margin(mag,phas,w)
%Lead compensator
numl=10*[1 2];
denl=[1 20];
num1=conv(num,numl);
den1=conv(den,denl);
[magcl,phascl]=bode(num1,den1,w);
[D1gm,D1pm,D1wcg,D1wcp]=margin(magcl,phascl,w)
numll=10*conv(numl,[1 4]);
denll=conv(denl,[1 40]);
num2=conv(num,numll);
den2=conv(den,denll);
[magcll,phascll]=bode(num2,den2,w);
[D2gm,D2pm,D2wcg,D2wcp]=margin(magcll,phascll,w)
subplot(2,1,1)
loglog(w,mag,'-',w,magcl,'--',w,magcll,'-.',w,ones(100,1),'-');
axis([.1 100 .01 10])
grid;
ylabel('Magnitude');
title('Fig. 6.60 Bode plot for Example 6.16.   (a) magnitude');
subplot(2,1,2)
semilogx(w,phas,'-',w,phascl,'--',w,phascll,'-.',w,-180*ones(100,1),'-');
axis([.1 100 -250 -50])
grid;
xlabel('\omega (rad/sec)');
ylabel('phase (deg)');
title('Fig. 6.60 (b) phase.');


function Q = randrot(n, N)
% Generates uniformly random rotation matrices.
%
% function Q = randrot(n, N)
%
% Q is an n-by-n-by-N array such that each slice Q(:, :, i) is a random
% orthogonal matrix of size n of determinant +1 (i.e., a matrix in SO(n)),
% sampled from the Haar measure (uniform distribution).
%
% By default, N = 1.
%
% Complexity: N times O(n^3).
% Theory in Diaconis and Shahshahani 1987 for the uniformity on O(n);
% With details in Mezzadri 2007,
% "How to generate random matrices from the classical compact groups."
%
% To ensure matrices in SO(n), we permute the two first columns when
% the determinant is -1.
%
% See also: randskew qr_unique randunitary

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Sept. 25, 2012.
% Contributors:
% Change log:
%   June 18, 2019 (NB)
%       Now generating all initial random matrices in one shot (which
%       should be more efficient) and calling qr_unique.

    if nargin < 2
        N = 1;
    end

    if n == 1
        Q = ones(1, 1, N);
        return;
    end

    % Generated as such, Q is uniformly distributed over O(n): the group
    % of orthogonal matrices; see Mezzadri 2007.
    Q = qr_unique(randn(n, n, N));

    for k = 1 : N

        % If a slice of Q is in O(n) but not in SO(n), we permute its two
        % first columns to negate its determinant. This ensures the new
        % slice is in SO(n), uniformly distributed.
        if det(Q(:, :, k)) < 0
            Q(:, [1 2], k) = Q(:, [2 1], k);
        end

    end

end


% Copyright (C) 1993-2014, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

%%begin

% Frequently we want to define a smooth sequence of positions (or poses) from
% one point to another.  First consider the 1-dimensional case.
%
% We define the start and end position

p0 = -1;
p1 = 2;

% and a smooth path from p0 to p1 in 50 time steps is given by

p = tpoly(p0, p1, 50);
about p
% which we see has 50 rows.  We can plot this

plot(p)

% and see that it does indeed move smoothly from p0 to p1 and that the initial
% and final derivative (and second derivative) is zero.

% We can also get the velocity and acceleration

[p,pd,pdd] = tpoly(p0, p1, 50);
subplot(3,1,1); plot(p); xlabel('Time'); ylabel('p');
subplot(3,1,2); plot(pd); xlabel('Time'); ylabel('pd');
subplot(3,1,3); plot(pdd); xlabel('Time'); ylabel('pdd');

% This path is a 5th order polynomial and it suffers from the disadvantage that
% the velocity is mostly below the maximum possible value.  An alternative is

[p,pd,pdd] = lspb(p0, p1, 50);
subplot(3,1,1); plot(p); xlabel('Time'); ylabel('p');
subplot(3,1,2); plot(pd); xlabel('Time'); ylabel('pd');
subplot(3,1,3); plot(pdd); xlabel('Time'); ylabel('pdd');
% which we see has a trapezoidal velocity profile.

% Frequently the start and end values are vectors, not scalars, perhaps a 3D
% position or Euler angles.  In this case we apply the scalar trajectory function
% to a vector with

p = mtraj(@tpoly, [0 1 2], [2 1 0], 50);
about p
% and p again has one row per time step, and one column per vector dimension

clf; plot(p)

%---
% Finally, we may wish to interpolate poses.  We will define a start and end pose

T0 = transl(0.4, 0.2, 0) * trotx(pi);
T1 = transl(-0.4, -0.2, 0.3) * troty(pi/2) * trotz(-pi/2);

% and a smooth sequence between them in 50 steps is

T = ctraj(T0, T1, 50);
about T
% which is a 4x4x50 matrix.  The first pose is

T(:,:,1)

% and the 10th pose is

T(:,:,10)

% We can plot the motion of this coordinate frame by

clf; tranimate(T)


function [ x, fx ] = local_min ( a, b, epsi, t, f, x )

%*****************************************************************************80
%
%% LOCAL_MIN seeks a local minimum of a function F(X) in an interval [A,B].
%
%  Discussion:
%
%    The method used is a combination of golden section search and
%    successive parabolic interpolation.  Convergence is never much slower
%    than that for a Fibonacci search.  If F has a continuous second
%    derivative which is positive at the minimum (which is not at A or
%    B), then convergence is superlinear, and usually of the order of
%    about 1.324....
%
%    The values EPSI and T define a tolerance TOL = EPSI * abs ( X ) + T.
%    F is never evaluated at two points closer than TOL.
%
%    If F is a unimodal function and the computed values of F are always
%    unimodal when separated by at least SQEPS * abs ( X ) + (T/3), then
%    LOCAL_MIN approximates the abscissa of the global minimum of F on the
%    interval [A,B] with an error less than 3*SQEPS*abs(LOCAL_MIN)+T.
%
%    If F is not unimodal, then LOCAL_MIN may approximate a local, but
%    perhaps non-global, minimum to the same accuracy.
%
%    Thanks to Jonathan Eggleston for pointing out a correction to the
%    golden section step, 01 July 2013.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2008
%
%  Author:
%
%    Original FORTRAN77 version by Richard Brent.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Richard Brent,
%    Algorithms for Minimization Without Derivatives,
%    Dover, 2002,
%    ISBN: 0-486-41998-3,
%    LC: QA402.5.B74.
%
%  Parameters:
%
%    Input, real A, B, the endpoints of the interval.
%
%    Input, real EPSI, a positive relative error tolerance.
%    EPSI should be no smaller than twice the relative machine precision,
%    and preferably not much less than the square root of the relative
%    machine precision.
%
%    Input, real T, a positive absolute error tolerance.
%
%    Input, function value = F ( x ), the name of a user-supplied
%    function whose local minimum is being sought.
%
%    Output, real X, the estimated value of an abscissa
%    for which F attains a local minimum value in [A,B].
%
%    Output, real FX, the value F(X).
%

%
%  C is the square of the inverse of the golden ratio.
%
  c = 0.5 * ( 3.0 - sqrt ( 5.0 ) );

  sa = a;
  sb = b;
  x = sa + c * ( b - a );
  w = x;
  v = w;
  e = 0.0;
  fx = f ( x );
  fw = fx;
  fv = fw;

  while ( 1 )

    m = 0.5 * ( sa + sb );
    tol = epsi * abs ( x ) + t;
    t2 = 2.0 * tol;
%
%  Check the stopping criterion.
%
    if ( abs ( x - m ) <= t2 - 0.5 * ( sb - sa ) )
      break
    end
%
%  Fit a parabola.
%
    r = 0.0;
    q = r;
    p = q;

    if ( tol < abs ( e ) )

      r = ( x - w ) * ( fx - fv );
      q = ( x - v ) * ( fx - fw );
      p = ( x - v ) * q - ( x - w ) * r;
      q = 2.0 * ( q - r );

      if ( 0.0 < q )
        p = - p;
      end

      q = abs ( q );

      r = e;
      e = d;

    end

    if ( abs ( p ) < abs ( 0.5 * q * r ) & ...
         q * ( sa - x ) < p & ...
         p < q * ( sb - x ) )
%
%  Take the parabolic interpolation step.
%
      d = p / q;
      u = x + d;
%
%  F must not be evaluated too close to A or B.
%
      if ( ( u - sa ) < t2 | ( sb - u ) < t2 )

        if ( x < m )
          d = tol;
        else
          d = - tol;
        end

      end
%
%  A golden-section step.
%
    else

      if ( x < m )
        e = sb - x;
      else
        e = sa - x;
      end

      d = c * e;

    end
%
%  F must not be evaluated too close to X.
%
    if ( tol <= abs ( d ) )
      u = x + d;
    elseif ( 0.0 < d )
      u = x + tol;
    else
      u = x - tol;
    end

    fu = f ( u );
%
%  Update A, B, V, W, and X.
%
    if ( fu <= fx )

      if ( u < x )
        sb = x;
      else
        sa = x;
      end

      v = w;
      fv = fw;
      w = x;
      fw = fx;
      x = u;
      fx = fu;

    else

      if ( u < x )
        sa = u;
      else
        sb = u;
      end

      if ( fu <= fw | w == x )
        v = w;
        fv = fw;
        w = u;
        fw = fu;
      elseif ( fu <= fv | v == x | v == w )
        v = u;
        fv = fu;
      end

    end

  end

  return
end


function S = AccSign(p)
%ACCSIGN      Computes sign(sum(p))
%
%   S = AccSign(p)
%
%On return, S is the sign of sum(p), also in the presence
%  of underflow. Input vector p may be single or double precision.
%
%Implements Algorithm 8.2 from
%  S.M. Rump, T. Ogita, S. Oishi: Accurate Floating-point Summation II:
%    Sign, K-fold Faithful and Rounding to Nearest, Siam J. Sci. Comput.,
%    31(2):1269-1302, 2008.
%Requires (4m+2)n flops for m executions of repeat-until loop in Transform.
%
%Reference implementation! Slow due to interpretation!
%

% written  03/03/07     S.M. Rump
% modified 05/09/09     S.M. Rump  rounding to nearest, complex input
%

  if ~isreal(p)
    error('AccSign for real input only')
  end

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

  if isa(p,'double')
    nmax = 2^52;            % nmax = 450,359,962,737,0496
  else
    nmax = 2^23;            % nmax = 8,388,608
  end
  if length(p)>nmax
    error(['maximum length of input vector for AccSign ' int2str(nmax) '.'])
  end

  kPhi = 1;
  [tau1,tau2,p] = Transform(p,0,kPhi);
  S = sign(tau1);

  if rndold
    setround(rndold)
  end


function [yi, ypi, yppi] = cubiconv(x, y, xi)

% CUBICONV Cubic convolution interpolation
%    CUBICONV(X,Y,XI) interpolates to find YI, the value of
%    the underlying function Y at the points in the uniform array
%    XI, using cubic convolution interpolation.  X and Y must be
%    vectors of length N.
%
%    [YI,YPI,YPPI] = CUBICONV() also returns the interpolated
%    quartic derivative and cubic second derivative of the underlying
%    function Y at points XI.

% Joe Henning - Fall 2011

% Cubic Convolution Interpolation for Digital Image Processing
% Robert G. Keys
% IEEE Transactions on Acoustics, Speech, and Signal Processing, Vol. ASSP-29, No. 6
% December, 1981

n = length(x);
%h = (x(n) - x(1))/n;

for i = 1:length(xi)
   if (xi(i) < x(1) || xi(i) > x(n))
      fprintf('??? Bad x input to cubiconv ==> x(1) <= x <= x(n)\n');
      yi(i) = NaN;
      continue;
   end

   % Find the right place in the table by means of a bisection.
   klo = 1;
   khi = n;
   while (khi-klo > 1)
      k = fix((khi+klo)/2.0);
      if (x(k) > xi(i))
         khi = k;
      else
         klo = k;
      end
   end

   h = x(khi) - x(klo);
   if (h == 0.0)
      fprintf('??? Bad x input to cubiconv ==> x values must be distinct\n');
      yi(i) = NaN;
      ypi(i) = NaN;
      yppi(i) = NaN;
      continue;
   end

   km1 = klo - 1;
   kp1 = khi + 1;
   if (km1 < 1)
      a = 3*y(klo) - 3*y(khi) + y(khi+1);
   else
      a = y(km1);
   end
   if (kp1 > n)
      d = 3*y(khi) - 3*y(klo) + y(klo-1);
   else
      d = y(kp1);
   end
   b = y(klo);
   c = y(khi);

   % Evaluate cubic polynomial
   t = (xi(i) - x(klo))/h;
   t2 = t*t;
   t3 = t2*t;
   h2 = h*h;
   c00 = (-t3 + 2*t2 - t)/2.0;
   c10 = (3*t3 - 5*t2 + 2)/2.0;
   c20 = (-3*t3 + 4*t2 + t)/2.0;
   c30 = (t3 - t2)/2.0;

   yi(i) = a*c00 + b*c10 + c*c20 + d*c30;

   % Differentiate to find the second-order interpolant
   c00 = (-3*t2 + 4*t - 1)/2.0;
   c10 = (9*t2 - 10*t)/2.0;
   c20 = (-9*t2 + 8*t + 1)/2.0;
   c30 = (3*t2 - 2*t)/2.0;

   ypi(i) = a*c00/h + b*c10/h + c*c20/h + d*c30/h;

   % Differentiate to find the first-order interpolant
   c00 = (-6*t + 4)/2.0;
   c10 = (18*t - 10)/2.0;
   c20 = (-18*t + 8)/2.0;
   c30 = (6*t - 2)/2.0;

   yppi(i) = a*c00/h2 + b*c10/h2 + c*c20/h2 + d*c30/h2;
end


function [cart] = Polar2Cart(polar)
 %% Cnverts polar coordinats [alpha;r] to cartesian [x;y]

    alpha = polar(1,:);
    r = polar(2,:);
    x = zeros(1,size(polar,2));
    y = zeros(1,size(polar,2));

    for i = 1:size(polar,2)
    x(i) = r(i)*cos(alpha(i));
    y(i) = r(i)*sin(alpha(i));
    end

    cart = [x;y];
end


%% 1D Heat Transfer FDM
% by Manuel Diaz
clc; clear; close all;

%% Parameters
alpha = 0.25;       % difusion speed
iter = 400;         % iteration time steps

%% Domain Grid
L = 30;    % Length
n = 30;    % Nodes
dx = L/n; dt = 0.5;
x  = 1:dx:L;
t  = zeros(1,n);    % temperature of the nodes in our Domain

%% Wall Temperatura at x = 0
twall = 1;

%% Initial Condition
t_0 = zeros(1,n);   % @time = 0
t_0(1) = twall;     %Dirichlet BC
t_0(n) = t_0(n-1);   %Neumann BC

%% Main Loop
t_next = zeros(1,n); %Next time step
t = t_0;			 %Load I.C.
for k = 1:iter;
    for i = 2:n-1;
        t_next(i) = t(i) + (dt/dx^2)*alpha*(t(i+1)-2*t(i)+t(i-1));
    end
    % BC
    t_next(1) = twall;
	t_next(n) = t_next(n-1);
	% Update info
	t = t_next;
end

%% Make pretty figures
plot(x,t,'.'); xlabel 'x cell'; ylabel 'Temperature'; title '1D Heat Equation using FDM';


% Compute the center frequency of the mel filterbank
% Inputs:
%   start_freq:     the lowerest linear frequency included in the calculation
%   linear_samp:    sampling frequency
%   N_mel:          number of mel banks used
% Output:
%   mel_center_freq:    the center linear frequency of each mel bank
%   bin_upper_edge:

function [mel_center_freq, bin_upper_edge] = mel_center_FE(start_freq, linear_samp, N_mel)

% get the mel version of start frequency and linear sampling rate
start_mel = linear2mel(start_freq);
samp_mel = linear2mel(linear_samp/2);

for i=1:N_mel
    tmp_mel = (i-1)*(samp_mel-start_mel) / (N_mel+1);
    mel_center_freq(i) = mel2linear(tmp_mel+start_mel);
    tmp_mel = (i+1)*(samp_mel-start_mel) / (N_mel+1);
    bin_upper_edge(i) =  mel2linear(tmp_mel+start_mel);
end


function [alpha,beta]=moments23TermRecur(mu)
%%MOMENTS23TERMRECUR Given 2*N+1 moments of a scalar weighting function
%          from 0 to 2*N, the kth moment is
%          mu(k+1)=integral_a^b w(x)*x^k dx where w(x) is a weighting
%          function and a and b need not be finite, obtain the three term
%          recusion for orthogonal polynomials that share the same moments.
%          The three term recursion coefficients cna be used with the
%          orthoPolyZerosFromRecur to obtain quadrature points and weights
%          when using the same weighting function. The three-term recursion
%          has the form:
%          x*p_{k-1}(x)=beta_{k-1}*p_{k-2}(x)+alpha_k*p_{k-1}+beta_kp_k(x)
%          for k=1,...,N starting with p_{-1}(x)=0. This can be useful for
%          obtaining cubature points for integration over continuous
%          probability distributions with easy-to-express noncentral
%          moments (e.g. from the moment generting function of the
%          distribution.
%
%INPUTS: mu A length (2*N+1) array holding the moments from orders 0 to 2*N
%           of the desired weighting function (which could be, but needn't
%           be, a continuous probability distribution).
%
%OUTPUTS: alpha A length N vector of coefficients.
%          beta A length N-1 vector of coefficients.S
%
%The algorithm is taken from Section 4 of [1]. To use these with the
%orthoPolyZerosFromRecur function, one must pass alpha, beta, and mu(1).
%See the example below.
%
%EXAMPLE:
%In this example, we get the values for a three-term recursion of the
%standard normal distribution. We then plus the values into
%orthoPolyZerosFromRecur and get cubature points for integration voer the
%distribution. We then show that these points are identical tot he ones
%obtained from the quadraturePoints1D function (differences are within
%finite precision limits).
% meanVal=0;
% variance=1;
% N=5;
% mu=zeros(2*N+1,1);
% for k=0:2*N
%     mu(k+1)=GaussianD.momentGenFun(meanVal,variance,k);
% end
% [alpha,beta]=moments23TermRecur(mu);
% [xi,w]=orthoPolyZerosFromRecur(N,alpha,beta,[],mu(1));
% [xiGauss,wGauss]=quadraturePoints1D(N,0);
% max(abs((xi-xiGauss)))
% max(abs(w-wGauss))
%
%REFERENCES:
%[1] G. H. Golub and J. H. Welsh, "Calculation of Gauss quadrature rules,"
%    Mathematics of Computation, vol. 23, pp. 221-230, 1969.
%
%January 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=(length(mu)-1)/2;

if(fix(N)~=N)
    error('This function requires an odd number of moments.')
end

%The Gram Matrix in Section 4.
M=zeros(N+1,N+1);
for i1=1:(N+1)
    for i2=1:(N+1)
        M(i1,i2)=mu(i1+i2-1);
    end
end

%Using cholSemiDef instead of chol, it is more robust to semidefinite
%matrices.
R=cholSemiDef(M,'upper');

%Equation 4.3 in [1] for both alpha and beta.
alpha=zeros(N,1);
alpha(1)=R(1,2)/R(1,1);
for i=2:N
    alpha(i)=R(i,i+1)/R(i,i)-R(i-1,i)/R(i-1,i-1);
end

beta=zeros(N-1,1);
for i=1:(N-1)
    beta(i)=R(i+1,i+1)/R(i,i);
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 [dist, pos] = distancePointLine(point, line)
%DISTANCEPOINTLINE Minimum distance between a point and a line
%
%   D = distancePointLine(POINT, LINE)
%   Return the euclidean distance between line LINE and point POINT.
%
%   LINE has the form: [x0 y0 dx dy], and POINT is [x y].
%
%   If LINE is N-by-4 array, result is N-by-1 array computes for each line.
%
%   If POINT is N-by-2, then result is computed for each point.
%
%   If both POINT and LINE are array, result is computed for each couple of
%   point and line, and is returned in a NP-by-NL array, where NP is the
%   number of points, and NL is the number of lines.
%
%
%   See also:
%   lines2d, points2d, distancePoints, distancePointEdge
%

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2005-06-24
% Copyright 2016 INRA - BIA-BIBS.

%   HISTORY:
%   2012-10-24 rewrite using bsxfun

% direction vector of each line (row vectors)
vx = line(:, 3)';
vy = line(:, 4)';

% squared norm of direction vectors, with a check of validity
delta = (vx .* vx + vy .* vy);
invalidEdges = delta < eps;
delta(invalidEdges) = 1;

% difference of coordinates between point and line origins
% (NP-by-NE arrays)
dx  = bsxfun(@minus, point(:, 1), line(:, 1)');
dy  = bsxfun(@minus, point(:, 2), line(:, 2)');

% compute position of points projected on the line, by using normalised dot
% product
% (result is a NP-by-NL array)
pos = bsxfun(@rdivide, bsxfun(@times, dx, vx) + bsxfun(@times, dy, vy), delta);

% ensure degenerated lines are correclty processed (consider the line
% origin as closest point)
pos(:, invalidEdges) = 0;

% compute distance between point and its projection on the line
dist = hypot(bsxfun(@times, pos, vx) - dx, bsxfun(@times, pos, vy) - dy);

% if size(line, 1)==1 && size(point, 1)>1
%     line = repmat(line, [size(point, 1) 1]);
% end
%
% if size(point, 1)==1 && size(line, 1)>1
%     point = repmat(point, [size(line, 1) 1]);
% end
%
% dx = line(:, 3);
% dy = line(:, 4);
%
% % compute position of points projected on line
% tp = ((point(:, 2) - line(:, 2)).*dy + (point(:, 1) - line(:, 1)).*dx) ./ (dx.*dx+dy.*dy);
% p0 = line(:, 1:2) + [tp tp].*[dx dy];
%
%
% % compute distances between points and their projections
% dx = point - p0;
% dist  = sqrt(sum(dx.*dx, 2));


function quad_error = simplex_unit_monomial_quadrature ( dim_num, expon, ...
  point_num, x, w )

%*****************************************************************************80
%
%% SIMPLEX_UNIT_MONOMIAL_QUADRATURE: quadrature of monomials in a unit simplex.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer EXPON(DIM_NUM), the exponents.
%
%    Input, integer POINT_NUM, the number of points in the rule.
%
%    Input, real X(DIM_NUM,POINT_NUM), the quadrature points.
%
%    Input, real W(POINT_NUM), the quadrature weights.
%
%    Output, real QUAD_ERROR, the quadrature error.
%

%
%  Get the exact value of the integral of the unscaled monomial.
%
  scale = simplex_unit_monomial_int ( dim_num, expon );
%
%  Evaluate the monomial at the quadrature points.
%
  value = monomial_value ( dim_num, point_num, x, expon );
%
%  Compute the weighted sum and divide by the exact value.
%
  volume = simplex_unit_volume ( dim_num );
  quad = volume * ( w * value' ) / scale;
%
%  Error:
%
  exact = 1.0;
  quad_error = abs ( quad - exact );

  return
end


function point = projPointOnLine3d(point, line)
%PROJPOINTONLINE3D Project a 3D point orthogonally onto a 3D line.
%
%   PT2 = projPointOnLine3d(PT, LINE).
%   Computes the (orthogonal) projection of 3D point PT onto the 3D line
%   LINE.
%
%   Function works also for multiple points and lines. In this case, it
%   returns multiple points.
%   Point PT1 is a N-by-3 array, and LINE is a N-by-6 array.
%   Result PT2 is a N-by-3 array, containing coordinates of orthogonal
%   projections of PT1 onto lines LINE.
%
%
%   See also:
%   projPointOnLine, distancePointLine3d
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 2012-08-23.
%

%   HISTORY

% direction vector of the line
vx = line(:, 4);
vy = line(:, 5);
vz = line(:, 6);

% difference of point with line origin
dx = point(:,1) - line(:,1);
dy = point(:,2) - line(:,2);
dz = point(:,3) - line(:,3);

% Position of projection on line, using dot product
delta = vx .* vx + vy .* vy + vz .* vz;
tp = (dx .* vx + dy .* vy + dz .* vz) ./ delta;

% convert position on line to cartesian coordinates
point = [line(:,1) + tp .* vx, line(:,2) + tp .* vy, line(:,3) + tp .* vz];


function diceC = ellipseDice(E1,E2,varargin)
% Compute the percentage overlap of a pair of ellipses
%
% Syntax
%   diceC = ellipseDice(E1,E2,varargin)
%
% Input
%   E1, E2 - Two structs that contain the ellipse parameters
%       .center
%       .sigma
%       .theta
%
% Optional key/value pairs
%   spatial samples - vector of the sample points in deg (default -10:0.1:10);
%   show -  Create an image showing the overlap
%
% Outputs
%   diceC - Dice coefficient = 2*areaOfE1E2Intersection / eArea1 + eArea2 ;
%
% Author, Wandell January 17, 2020
%
% See also
%   ellipseInterior, ellipsePoints, ...

% Examples:
%{
   E1.center = [0,0]; E1.sigma = [3,1]; E1.theta = pi;
   E2.center = [-1,0]; E2.sigma = [3,1]; E2.theta = pi/2;
   diceC = ellipseDice(E1,E2,'show',true);
%}
%{
   E1.center = [0,0]; E1.sigma = [1,1]; E1.theta = pi;
   E2.center = [-1,0]; E2.sigma = [3,3]; E2.theta = pi/2;
   diceC = ellipseDice(E1,E2,'show',true);
%}
%{
   E1.center = [0,0]; E1.sigma = [1,1]; E1.theta = pi;
   E2.center = [0,0]; E2.sigma = [1,1]; E2.theta = pi/2;
   diceC = ellipseDice(E1,E2,'show',true);
%}

%% Input parameters

% Remove spaces and force lower case
varargin = mrvParamFormat(varargin);

p = inputParser;

% Validation function
vFunc = @(x)(isstruct(x) && isfield(x,'center') && isfield(x,'sigma') && isfield(x,'theta'));
p.addRequired('E1',vFunc);
p.addRequired('E2',vFunc);

p.addParameter('spatialsamples',(-10:0.05:10),@isvector);
p.addParameter('show',false,@islogical);

p.parse(E1,E2,varargin{:});

samples = p.Results.spatialsamples;
show    = p.Results.show;

%% Compute

% 1 inside, 0 outside.
[img1, nSamples] = ellipseInterior('center',E1.center, ...
    'sigma',E1.sigma, ...
    'theta',E1.theta',...
    'spatial samples',samples);
img2   = ellipseInterior('center',E2.center,...
    'sigma',E2.sigma,...
    'theta',E2.theta,...
    'spatial samples',samples);

%%
overlapArea = sum(img1(:) .* img2(:));
diceC = 2*overlapArea/ (sum(img1(:)) + sum(img2(:)));

if show
    img = img1 + img2;
    mrvNewGraphWin; colormap([0.2 0.3 0.4; 0.6 0.6 0.6; 1 1 1]);
    image(samples,samples,img + 1); axis image;
    xlabel('Deg'); ylabel('Deg'); grid on
end

end


function fx = p02_fun ( n, x )

%*****************************************************************************80
%
%% P02_FUN evaluates the integrand for problem 2.
%
%  Discussion:
%
%    The integrand is discontinuous at X = 0.3.
%
%  Interval:
%
%    0 <= x <= 1
%
%  Integrand:
%
%    if ( x < 0.3 )
%      f(x) = 0
%    else
%      f(x) = 1
%
%  Antiderivative:
%
%    if ( x < 0.3 )
%      g(x) = 0
%    else
%      g(x) = X - 0.3
%
%  Exact Integral:
%
%    0.7
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    David Kahaner,
%    Comparison of Numerical Quadrature Formulas,
%    in Mathematical Software, edited by John R Rice,
%    Academic Press, 1971.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the integrand values.
%
  fx = ( 0.3 <= x );

  return
end


function y = entr( x )

%ENTR   Scalar entropy.
%   ENTR(X) returns an array of the same size as X with the unnormalized
%   entropy function applied to each element:
%                { -X.*LOG(X) if X > 0,
%      ENTR(X) = { 0          if X == 0,
%                { -Inf       otherwise.
%   If X is a vector representing a discrete probability distribution, then
%   SUM(ENTR(X)) returns its entropy.
%
%   Disciplined convex programming information:
%       ENTR(X) is concave and nonmonotonic in X. Thus when used in CVX
%       expressions, X must be real and affine. Its use will effectively
%       constrain X to be nonnegative: there is no need to add an
%       additional X >= 0 to your model in order to enforce this.

narginchk(1,1);
cvx_expert_check( 'entr', x );
y = -rel_entr( x, 1 );

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


function p = equivalentPerimeter(grains)
% returns the equivalent perimeter of grain-polygon
%
% Description
% The equivalent perimeter of grain-polygon is defined as
%
% $$ p = 2 \pi ER $$,
%
% where $ER$ is the <grain2d.equivalentRadius.html equivalent radius> of a
% grain
%
% Input
%  grains - @grain2d
%
% Output
%  p - equivalent perimeter in measurement units
%
% See also
% grain2d/deltaarea grain2d/paris grain2d/equivalentRadius

p = 2*pi*equivalentRadius(grains);


function n2 = dist2(x, c)
%DIST2	Calculates squared distance between two sets of points.
%
%	Description
%	D = DIST2(X, C) takes two matrices of vectors and calculates the
%	squared Euclidean distance between them.  Both matrices must be of
%	the same column dimension.  If X has M rows and N columns, and C has
%	L rows and N columns, then the result has M rows and L columns.  The
%	I, Jth entry is the  squared distance from the Ith row of X to the
%	Jth row of C.
%
%	See also
%	GMMACTIV, KMEANS, RBFFWD
%

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

if nargin<2
    c = x;
end
[ndata, dimx] = size(x);
[ncentres, dimc] = size(c);
if dimx ~= dimc
	error('Data dimension does not match dimension of centres')
end

n2 = (ones(ncentres, 1) * sum((x.^2)', 1))' + ...
  ones(ndata, 1) * sum((c.^2)',1) - ...
  2.*(x*(c'));

% Rounding errors occasionally cause negative entries in n2
if any(any(n2<0))
  n2(n2<0) = 0;
end


% Polynomial discrimination
% Section 8.6.2, Boyd & Vandenberghe "Convex Optimization"
% Original by Lieven Vandenberghe
% Adapted for CVX by Joelle Skaf - 10/23/05
% (a figure is generated)
%
% The goal is to find the polynomial of degree 4 on R^n that separates
% two sets of points {x_1,...,x_N} and {y_1,...,y_N}. We are trying to find
% the coefficients of an order-4-polynomial P(x) that would satisfy:
%           minimize    t
%               s.t.    P(x_i) <= t  for i = 1,...,N
%                       P(y_i) >= t   for i = 1,...,M

% Data generation
rand('state',0);
N = 100;
M = 120;

% The points X lie within a circle of radius 0.9, with a wedge of points
% near [1.1,0] removed. The points Y lie outside a circle of radius 1.1,
% with a wedge of points near [1.1,0] added. The wedges are precisely what
% makes the separation difficult and interesting.
X = 2 * rand(2,N) - 1;
X = X * diag(0.9*rand(1,N)./sqrt(sum(X.^2)));
Y = 2 * rand(2,M) - 1;
Y = Y * diag((1.1+rand(1,M))./sqrt(sum(Y.^2)));
d = sqrt(sum((X-[1.1;0]*ones(1,N)).^2));
Y = [ Y, X(:,d<0.9) ];
X = X(:,d>1);
N = size(X,2);
M = size(Y,2);

% Construct Vandermonde-style monomial matrices
p1   = [0,0,1,0,1,2,0,1,2,3,0,1,2,3,4]';
p2   = [0,1,1,2,2,2,3,3,3,3,4,4,4,4,4]'-p1;
np   = length(p1);
op   = ones(np,1);
monX = X(op,:) .^ p1(:,ones(1,N)) .* X(2*op,:) .^ p2(:,ones(1,N));
monY = Y(op,:) .^ p1(:,ones(1,M)) .* Y(2*op,:) .^ p2(:,ones(1,M));

% Solution via CVX
fprintf(1,'Finding the optimal polynomial of order 4 that separates the 2 classes...');

cvx_begin
    variables a(np) t(1)
    minimize ( t )
    a'*monX <= t;
    a'*monY >= -t;
    % For normalization purposes only
    norm(a) <= 1;
cvx_end

fprintf(1,'Done! \n');

% Displaying results
nopts = 2000;
angles = linspace(0,2*pi,nopts);
cont = zeros(2,nopts);
for i=1:nopts
   v = [cos(angles(i)); sin(angles(i))];
   l = 0;  u = 1;
   while ( u - l > 1e-3 )
      s = (u+l)/2;
      x = s * v;
      if a' * ( x(op,:) .^ p1 .* x(2*op) .^ p2 ) > 0,
          u = s;
      else
          l = s;
      end
   end;
   s = (u+l)/2;
   cont(:,i) = s*v;
end;

graph = plot(X(1,:),X(2,:),'o', Y(1,:), Y(2,:),'o', cont(1,:), cont(2,:), '-');
set(graph(2),'MarkerFaceColor',[0 0.5 0]);
title('Optimal order-4 polynomial that separates the 2 classes')
% print -deps min-deg-discr.eps

%%%% Dual infeasible ?????


function len = polygonLength(poly, varargin)
%POLYGONLENGTH Perimeter of a polygon.
%
%   L = polygonLength(POLYGON);
%   Computes the boundary length of a polygon. POLYGON is given by a N-by-2
%   array of vertices.
%
%   Example
%     % Perimeter of a circle approximation
%     poly = circleToPolygon([0 0 1], 200);
%     polygonLength(poly)
%     ans =
%         6.2829
%
%   See also
%   polygons2d, polygonCentroid, polygonArea, drawPolygon, polylineLength
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2005-05-11
% Copyright 2005-2022 INRA - TPV URPOI - BIA IMASTE

% If first argument is a cell array, this is a multi-polygon, and we simply
% add the lengths of individual polygons
if iscell(poly)
    len = 0;
    for i = 1:length(poly)
        len = len + polygonLength(poly{i});
    end
    return;
end

% case of a polygon given as two coordinate arrays
if nargin == 2
    poly = [poly varargin{1}];
end

% check there are enough points
if size(poly, 1) < 2
    len = 0;
    return;
end

% compute length
if size(poly, 2) == 2
    % polygon in dimension 2 (classical case)
    dp = diff(poly([1:end 1], :), 1, 1);
    len = sum(hypot(dp(:, 1), dp(:, 2)));
else
    % polygon of larger dimension
    len = sum(sqrt(sum(diff(poly([2:end 1], :), 1, 1).^2, 2)));
end


function qr = plane_normal_xyz_to_qr ( pp, normal, pq, pr, n, xyz )

%*****************************************************************************80
%
%% PLANE_NORMAL_XYZ_TO_QR: XYZ to QR coordinates for a normal form plane.
%
%  Discussion:
%
%    The normal form of a plane in 3D is:
%
%      PP is a point on the plane,
%      NORMAL is a normal vector to the plane.
%
%    Two vectors PQ and PR can be computed with the properties that
%    * NORMAL, PQ and PR are pairwise orthogonal;
%    * PQ and PR have unit length;
%    * every point P in the plane has a "QR" representation
%      as P = PP + q * PQ + r * PR.
%
%    This function is given the XYZ coordinates of a set of points on the
%    plane, and returns the QR coordinates.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PP(3), a point on the plane.
%
%    Input, real NORMAL(3), a normal vector N to the plane.  The
%    vector must not have zero length, but it is not necessary for N
%    to have unit length.
%
%    Input, real PQ(3), a vector of unit length,
%    perpendicular to the vector N and the vector PR.
%
%    Input, real PR(3), a vector of unit length,
%    perpendicular to the vector N and the vector PQ.
%
%    Input, integer N, the number of points on the plane.
%
%    Input, real XYZ(3,N), the XYZ coordinates of the points.
%
%    Output, real QR(2,N), the QR coordinates of the points.
%
  qr = [ pq'; pr' ] * ( xyz(1:3,1:n) - repmat ( pp, 1, n ) );

  return
end


function C = ttimes(A,B)
% TTIMES Tropical multiplication (max-plus algebra)
%
% C = TTIMES(A,B) Computes the tropical multiplication of the matrices A
% and B, AB(i,j) = max(A(i,:) + B(:,j)');
%
% See also tplus

n = size(A,1);
m = size(B,2);
C = reshape(max(kron(ones(m,1),A)+kron(B',ones(n,1)),[],2),n,m);


% Weighted analytic center of a set of linear inequalities
% Joëlle Skaf - 04/29/08
%
% The weighted analytic center of a set of linear inequalities:
%           a_i^Tx <= b_i   i=1,...,m,
% is the solution of the unconstrained minimization problem
%           minimize    -sum_{i=1}^m w_i*log(b_i-a_i^Tx),
% where w_i>0

% Input data
randn('state', 0);
rand('state', 0);
n = 10;
m = 50;
tmp = randn(n,1);
A = randn(m,n);
b = A*tmp + 2*rand(m,1);
w = rand(m,1);

% Analytic center
cvx_begin
    variable x(n)
    minimize -sum(w.*log(b-A*x))
cvx_end

disp('The weighted analytic center of the set of linear inequalities is: ');
disp(x);


function lambda = gk316_eigenvalues ( n )

%*****************************************************************************80
%
%% GK316_EIGENVALUES returns the eigenvalues of GK316.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  if ( n == 1 )

    lambda(1,1) = 1.0;

  else

    lambda(1:n-2,1) = 1.0;

    a = 1.0;
    b = - ( n + 1 );
    c = - ( n * ( n + 1 ) * ( 2 * n - 5 ) ) / 6.0;

    lambda(n-1,1) = ( - b + sqrt ( b * b - 4.0 * a * c ) ) / ( 2.0 * a );
    lambda(n,1) =   ( - b - sqrt ( b * b - 4.0 * a * c ) ) / ( 2.0 * a );

  end

  return
end


function [a,t,tLength,k]=BSplineInterpIntMultiDim(t,tLength,a,k,intDims)
%%BSPLINEINTERPINTMULTIDIM Given a hypermatrix of multivariate b-spline
%            interpolation weights a and a set of knots t,compute the
%            modified weights and knots to interpolate the integral of the
%            a given point using b-spline interpolation. If all knots at
%            the ith boundary are repeated k(i) times, then an integral
%            over the ith dimension will start from the beginning of the
%            interpolation region. Otherwise, this can be considered an
%            indefinite integral with a particular additive constant and
%            differencing and be used to get a definite integral.
%
%INPUTS: t The maxNumKnotsXnumDims set of knots fo r the interpolation
%          function. These are actually the values in each coordinate
%          dimension given in ascending order. The full knots are implied
%          [t1,t2,etc.]=ndgrid(t(1:tLengths(1),1),t(1:tLengths(2),2),...)
%          and the dimensions can be put together into the full set of
%          numDimsXtotalNumPoints points as tTotal=[t1(:).';t2(:).';...].
%          The first and last k(curDim)-1 knots in each dimension are
%          outside of the ends of the region with data or mark the ends of
%          the region with data. The number of rows is the maximum number
%          needed for all of the dimensions. tLength says how many items
%          in each column are actually used. These values must be real.
%  tLength A numDimX1 vector where tLength(i) says the number of elements
%          in t(:,i).
%        a A hypermatrix with numDim+1 indices containing the set of
%          coefficients for the b-splines covering all of the interpolation
%          intervals for all of the sets of interpolation values. If there
%          is only one set, the final dimension is unitary meaning that
%          there are effectively only numDim dimensions. The derivatives
%          will be applied to all sets present. These values can be real or
%          complex.
%        k A numDimsX1 or 1XnumDims set of the order of the b-splines in
%          each dimension. The value k-1 is the polynomial order of the
%          approximation being performed. If the order is the same in all
%          dimensions, then a single scalar can be passed.
%  intDims This is a numDimsX1 or a 1XnumDims boolean vector indicating
%          over which dimensions the integrals should be taken.
%
%OUTPUTS: a The modified coefficient matrix. This is one larger in each
%           dimensions where an integral was taken.
%         t The modified knots.
%   tLength The modified vector indicating how many knows are present.
%         k The modified set of orders.
%
%Equation 22 in Chapter X of [1] has 1D integration. Chapter XVII describes
%the use of tensor product splines for more than one dimension. The
%generalization to more than one dimension comes from just evaluating the
%integrals across each dimension one at a time, if they are taken.
%
%EXAMPLE 1:
% f=@(x,y,z)(x.^4-2*x.^2+x).*(y.^4-2*y.^2+y).*(z.^4-3*z.^2+z);
% %The definite integral of f with respect to x and z starting at -1.5 is
% fIntXZ=@(x,y,z)((-891+240*x.^2-320*x.^3+96*x.^5).*y.*(1-2*y+y.^3).*(-477+80*z.^2-160*z.^3+32*z.^5))/76800;
%
% numDims=3;
% numPointsX=10;
% numPointsY=11;
% numPointsZ=9;
% tauLengths=[numPointsX;numPointsY;numPointsZ];
% tau=zeros(max(tauLengths),numDims);
% tau(1:tauLengths(1),1)=linspace(-1.5,1.5,numPointsX);
% tau(1:tauLengths(2),2)=linspace(-1.5,1.5,numPointsY);
% tau(1:tauLengths(3),3)=linspace(-1.5,1.5,numPointsZ);
% %Note that using meshgrid would have put the elements in the wong order.
% %ndgrid must be used.
% [tau1,tau2,tau3]=ndgrid(tau(1:tauLengths(1),1),tau(1:tauLengths(2),2),tau(1:tauLengths(3),3));
% y=f(tau1,tau2,tau3);
%
% k=[5;5;5];
% [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,y(:),k);
% intDims=[1;0;1];
% [a,t,tLength,k]=BSplineInterpIntMultiDim(t,tLength,a,k,intDims);
%
% numPointsY=21;
% numPointsZ=19;
% pointsX=linspace(-1.5,1.5,numPointsX);
% pointsY=linspace(-1.5,1.5,numPointsY);
% pointsZ=linspace(-1.5,1.5,numPointsZ);
% [X,Y,Z]=ndgrid(pointsX,pointsY,pointsZ);
% x=[X(:).';Y(:).';Z(:).'];
%
% zIntTrue=fIntXZ(X,Y,Z);
% z=BSplineInterpValMultiDim(x,t,tLength,a,k);
% max(abs(z(:)-zIntTrue(:)))
%
%EXAMPLE 2:
%This is similar to example 1, except two sets of values are fitted,
%integrated, and interpolated at once.
% f1=@(x,y,z)(x.^4-2*x.^2+x).*(y.^4-2*y.^2+y).*(z.^4-3*z.^2+z);
% f2=@(x,y,z)(x.^3-2*x.^2+x).*(y.^4-2*y.^2+y).*(2*z.^3-2*z.^2+1);
% %The definite integral of f with respect to x and z starting at -1.5 is
% fIntXZ1=@(x,y,z)((-891+240*x.^2-320*x.^3+96*x.^5).*y.*(1-2*y+y.^3).*(-477+80*z.^2-160*z.^3+32*z.^5))/76800;
% fIntXZ2=@(x,y,z)(((-891+16*x.^2.*(6+x.*(-8+3*x))).*y.*(1-2*y+y.^3).*(-315+16*z.*(6+z.^2.*(-4+3*z))))/18432);
%
% numDims=3;
% numPointsX=10;
% numPointsY=11;
% numPointsZ=9;
% tauLengths=[numPointsX;numPointsY;numPointsZ];
% tau=zeros(max(tauLengths),numDims);
% tau(1:tauLengths(1),1)=linspace(-1.5,1.5,numPointsX);
% tau(1:tauLengths(2),2)=linspace(-1.5,1.5,numPointsY);
% tau(1:tauLengths(3),3)=linspace(-1.5,1.5,numPointsZ);
% %Note that using meshgrid would have put the elements in the wong order.
% %ndgrid must be used.
% [tau1,tau2,tau3]=ndgrid(tau(1:tauLengths(1),1),tau(1:tauLengths(2),2),tau(1:tauLengths(3),3));
% y1=f1(tau1,tau2,tau3);
% y2=f2(tau1,tau2,tau3);
% y=[y1(:),y2(:)];
%
% k=[5;5;5];
% [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,y,k);
% intDims=[1;0;1];
% [a,t,tLength,k]=BSplineInterpIntMultiDim(t,tLength,a,k,intDims);
%
% numPointsY=21;
% numPointsZ=19;
% pointsX=linspace(-1.5,1.5,numPointsX);
% pointsY=linspace(-1.5,1.5,numPointsY);
% pointsZ=linspace(-1.5,1.5,numPointsZ);
% [X,Y,Z]=ndgrid(pointsX,pointsY,pointsZ);
% x=[X(:).';Y(:).';Z(:).'];
%
% zIntTrue1=fIntXZ1(X,Y,Z);
% zIntTrue2=fIntXZ2(X,Y,Z);
% z=BSplineInterpValMultiDim(x,t,tLength,a,k);
% max(abs(z(:,1)-zIntTrue1(:)))
% max(abs(z(:,2)-zIntTrue2(:)))
%
%EXAMPLE 3:
%This is an example of a bivariate complex function whose integral over the
%first dimension is interpolated.numPoints=80;
% xMin=0;
% xMax=3;
% yMin=0;
% yMax=3;
% x=linspace(xMin,xMax,numPoints);
% y=linspace(yMin,yMax,numPoints);
% [X,Y]=ndgrid(x,y);
% f=@(x,y)(sin(5*y)+1j*cos(10*x));
% %The integral from 0 in the x dimension.
% fIntx=@(x,y)((1/10)*1j*sin(10*x)+x.*sin(5*y));
%
% fxy=f(X(:),Y(:));
% k=[5;5];
% tau=[x(:),y(:)];
% tauLengths=[numPoints,numPoints];
% [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,fxy,k);
% intDims=[1;0];
% [a,t,tLength,k]=BSplineInterpIntMultiDim(t,tLength,a,k,intDims);
%
% %Interpolate the derivative
% numPoints=100;
% x=linspace(xMin,xMax,numPoints);
% y=linspace(xMin,xMax,numPoints);
% [X,Y]=ndgrid(x,y);
%
% fxy=fIntx(X(:),Y(:));
% pts=[X(:).';Y(:).'];
% fxyInterp=reshape(BSplineInterpValMultiDim(pts,t,tLength,a,k),[numPoints,numPoints]);
% fxy=reshape(fxy,[numPoints,numPoints]);
%
% figure(1)
% clf
% hold on
% surface(X,Y,real(fxy),'EdgeColor','none')
% title('Real Integral Values')
% colorbar()
%
% figure(2)
% clf
% hold on
% surface(X,Y,imag(fxy),'EdgeColor','none')
% title('Imaginary Integral Values')
% colorbar()
%
% figure(3)
% clf
% hold on
% surface(X,Y,abs(real(fxyInterp)-real(fxy)),'EdgeColor','none')
% title('Real Interpolation Error')
% colorbar()
%
% figure(4)
% clf
% hold on
% surface(X,Y,abs(imag(fxyInterp)-imag(fxy)),'EdgeColor','none')
% title('Imaginary Interpolation Error')
% colorbar()
%
%REFERENCES:
%[1] C. de Boor, A Practical Guide to Splines. New York: Springer-Verlag,
%    1978.
%
%April 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDims=length(tLength);
numA=size(a);

if(length(numA)==numDims)
    numSets=1;
    numA=[numA,1];
else
    numSets=numA(end);
end

tNew=zeros(max(tLength)+2,numDims);
for curDim=1:numDims
    if(intDims(curDim)==0)
        tNew(1:tLength(curDim),curDim)=t(1:tLength(curDim),curDim);
        continue;
    end

    temp=zeros(1,numDims);
    temp(curDim)=1;
    aNew=zeros(numA+[temp,0]);

    %Next, we go through all tuples of values for the dimensions other than
    %this one. In each instance, we must take the integral of the a terms
    %that are present. These indices select all of the dimensions that are
    %not the current one across which derivatives are being taken.
    for curSet=1:numSets
        idxList=[1:(curDim-1),(curDim+1):numDims];

        maxVals=numA(idxList)-1;
        numACur=numA(curDim);
        curTuple=getNextTuple(numDims-1);
        idxCell=cell(1,numDims+1);
        idxCell{numDims+1}=curSet;
        tCur=t(1:tLength(curDim),curDim);
        kCur=k(curDim);

        outputIdx=1:(numACur+1);
        while(~isempty(curTuple))
            idxCell{curDim}=1:numA(curDim);%Mark the free dimension.

            %We have to select the elements in a to work on based on the
            %current tuple.
            for curIdx=1:(numDims-1)
                idxCell{idxList(curIdx)}=curTuple(curIdx)+1;
            end

            aCur=reshape(a(idxCell{:}),[numACur,1]);

            [~,aCur]=BSplineInterpInt(tCur,aCur,kCur);
            idxCell{curDim}=outputIdx;
            aNew(idxCell{:})=aCur;

            curTuple=getNextTuple(curTuple,maxVals);
        end
    end
    numA(curDim)=numA(curDim)+1;
    a=aNew;
    k(curDim)=k(curDim)+1;
    tNew(1:(tLength(curDim)+2),curDim)=[t(1,curDim);t(1:tLength(curDim),curDim);t(tLength(curDim),curDim)];
    tLength(curDim)=tLength(curDim)+2;
end
a=aNew;
t=tNew;

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 value = beta_inc ( a, b, x )

%*****************************************************************************80
%
%% BETA_INC returns the value of the incomplete Beta function.
%
%  Discussion:
%
%    This calculation requires an iteration.  In some cases, the iteration
%    may not converge rapidly, or may become inaccurate.
%
%    BETA_INC(A,B,X)
%
%      =   Integral ( 0 <= T <= X ) T^(A-1) (1-T)^(B-1) dT
%        / Integral ( 0 <= T <= 1 ) T^(A-1) (1-T)^(B-1) dT
%
%      =   Integral ( 0 <= T <= X ) T^(A-1) (1-T)^(B-1) dT
%        / BETA(A,B)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 September 2004
%
%  Author:
%
%    Original FORTRAN77 version by Majumder, Bhattacharjee.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Majumder and Bhattacharjee,
%    Algorithm AS63,
%    Applied Statistics,
%    1973, volume 22, number 3.
%
%  Parameters:
%
%    Input, A, B, the parameters of the function.
%    0.0D+00 < A,
%    0.0D+00 < B.
%
%    Input, real X, the argument of the function.
%    Normally, 0.0D+00 <= X <= 1.0.
%
%    Output, BETA_INC, the value of the function.
%
  it_max = 1000;
  tol = 1.0E-07;

  if ( a <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BETA_INC - Fatal error!\n' );
    fprintf ( 1, '  A <= 0.\n' );
    error ( 'BETA_INC - Fatal error!' );
  end

  if ( b <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BETA_INC - Fatal error!\n' );
    fprintf ( 1, '  B <= 0.\n' );
    error ( 'BETA_INC - Fatal error!' );
  end

  if ( x <= 0.0 )
    value = 0.0;
    return
  elseif ( 1.0 <= x )
    value = 1.0;
    return
  end
%
%  Change tail if necessary and determine S.
%
  psq = a + b;

  if ( a < ( a + b ) * x )
    xx = 1.0 - x;
    cx = x;
    pp = b;
    qq = a;
    indx = 1;
  else
    xx = x;
    cx = 1.0 - x;
    pp = a;
    qq = b;
    indx = 0;
  end

  term = 1.0;
  i = 1;
  value = 1.0;

  ns = floor ( qq + cx * ( a + b ) );
%
%  Use Soper's reduction formulas.
%
  rx = xx / cx;

  temp = qq - i;
  if ( ns == 0 )
    rx = xx;
  end

  it = 0;

  while ( 1 )

    it = it + 1;

    if ( it_max < it )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'BETA_INC - Fatal error!\n' );
      fprintf ( 1, '  Maximum number of iterations exceeded!\n' );
      fprintf ( 1, '  IT_MAX = %d\n', it_max );
      error ( 'BETA_INC - Fatal error!' );
    end

    term = term * temp * rx / ( pp + i );
    value = value + term;
    temp = abs ( term );

    if ( temp <= tol & temp <= tol * value )
      break
    end

    i = i + 1;
    ns = ns - 1;

    if ( 0 <= ns )
      temp = qq - i;
      if ( ns == 0 )
        rx = xx;
      end
    else
      temp = psq;
      psq = psq + 1.0;
    end

  end
%
%  Finish calculation.
%
  value = value * exp ( pp * log ( xx )  + ( qq - 1.0 ) * log ( cx ) ) ...
    / ( beta ( a, b ) * pp );

  if ( indx )
    value = 1.0 - value;
  end

  return
end


%% Pendulum example
%  This example applies energy-preserving piDMD to reconstruct the trajectory of
%  a double pendulum from noisy measurements.
addpath('../src')
rng(1); % Set random seed

% Define parameters of problem
l1=1; l2=1.5; % Lengths of rods
m1=1 ; m2=1.5; g=9.81; % Masses and gravity
params = [l1, l2, m1, m2, g]; % Concatenate parameters

% Construct linearised energy inner product
W(1,1) = (m1/2+m2/2)*g*l1;
W(2,2) = m2/2*g*l2;
W(3,3) = (m1/2+m2/2)*l1^2;
W(4,3) = m2/2*l1*l2;
W(3,4) = W(4,3);
W(4,4) = m2/2*l2^2;
C = chol(W); % Calculate inner product

% Set number of samples and span
tend = 30; nt = 1000;
tspan= linspace(0,tend,nt);

% Set initial conditions
theta1= 0.4; theta1_prime=0;
theta2= 0.7; theta2_prime=0;
y0=[theta1 theta1_prime theta2 theta2_prime];

% Solve ODE
[t,y]=ode45(@(t,y) pendulum(t,y,params), tspan,y0);

% Extract data
th1 = y(:,1); th2 = y(:,3); th1dt = y(:,2); th2dt = y(:,4);
x = [th1'; th2'; th1dt'; th2dt'];
xn = x + 1e-1*std(x,[],2).*randn(size(x)); % Add noise
data = C*xn; % Rescale measurements into energy norm
nTrain = nt-1;
X = data(:,1:nTrain); Y = data(:,2:nTrain+1);

% Train models
[piA,piVals] = piDMD(X,Y,'orthogonal');
[exA,exVals] = piDMD(X,Y,'exact');

% Perform reconstructions
piRec = zeros(4,nt); piRec(:,1) = data(:,1);
exRec = zeros(4,nt); exRec(:,1) = data(:,1);
for j = 2:nt
piRec(:,j) = piA(piRec(:,j-1));
exRec(:,j) = exA(exRec(:,j-1));
end

% Rescale reconstructions back into physical norm
fpiRec = C\piRec; fexRec = C\exRec;

%% Plot results
figure(1); LW = 'LineWidth'; IN = 'Interpreter'; LT = 'Latex'; FS = 'FontSize';
subplot(3,1,1)
c1 = .8*[1 1 1]; c2 = .8*[1 1 1];
plot(tspan,xn(1,:),LW,2,'Color',c1)
hold on
plot(tspan,xn(2,:),LW,2,'Color',c2)
hold off; trajPlot('measurements'); xticklabels([])
subplot(3,1,2)
plot(tspan,x(1,:),LW,3,'Color', c1)
hold on
plot(tspan,fpiRec(1,:),'b--',LW,2)
plot(tspan,fexRec(1,:),'r--',LW,2)
ylabel('$\theta_1$',IN,LT)
hold off; trajPlot('$\theta_1$'); xticklabels([])
subplot(3,1,3)
l1=plot(t,x(2,:),LW,3,'Color', c2);
hold on
l2=plot(t,fpiRec(2,:),'b--',LW,2);
l3=plot(t,fexRec(2,:),'r--',LW,2);
hold off; xlabel('time',FS,20,IN,LT)
trajPlot('$\theta_2$')
legend([l1,l2,l3],{'truth','piDMD','exact DMD'},IN,LT)
function yp = pendulum(~, y, params)

l1=params(1);  l2=params(2);
m1=params(3);  m2=params(4);
g=params(5);

a = (m1+m2)*l1 ;
b = m2*l2*cos(y(1)-y(3)) ;
c = m2*l1*cos(y(1)-y(3)) ;
d = m2*l2 ;
e = -m2*l2*y(4)* y(4)*sin(y(1)-y(3))-g*(m1+m2)*sin(y(1)) ;
f = m2*l1*y(2)*y(2)*sin(y(1)-y(3))-m2*g*sin(y(3)) ;
yp=zeros(4,1);
yp(1) = y(2);
yp(3)= y(4) ;
yp(2)= (e*d-b*f)/(a*d-c*b) ;
yp(4)= (a*f-c*e)/(a*d-c*b) ;
end

function trajPlot(j) % Nice plot of trajectories
yticks([-pi/4,0,pi/4]); yticklabels([{'$-\pi/4$'},{'0'},{'$\pi/4$'}])
set(gca,'TickLabelInterpreter','Latex','FontSize',20);grid on
ylim([-1,1])
ylabel(j,'Interpreter','latex','FontSize',20)
end


%
% This code calculates the interevent distances and the correlation integral
% of a given earthquake distribution.
% Francesco Pacchiani 3/2000
%
%
% Calculation of the 3D distances between all possible pairs
% (combination of n epicenters taken 2 at a time) of earthquakes of
% the given dataset.
%
%
% Variables
%
N = size(E,1);				% N= # of events in the catalogue; E= Earthquake catalogue
pairdist = []; 			% pairdist= Vector of interevent distances
j = nchoosek(N,2);			% j= # of interevent distances calculated
pairdist = zeros(j,1);
k = 0;
%E.Latitude= (max(Da(:,2))+min(Da(:,2)))/2;
%
%
% Calculation of the interevent distances in 2D plus the depths differences.
%
%
for i = 1:(N-1)

    lon1 = repmat(E(i,1), [(N-i),1]);
    lat1 = repmat(E(i,2), [(N-i),1]);

    lon2 = E((i+1):end, 1);
    lat2 = E((i+1):end, 2);

    pairdist(k+1:k + size(lon1, 1)) = distance(lat1,lon1,lat2,lon2);

    k = k + size(lon1,1);

end

clear i j k;
%
%
% Conversion of the interevent distances from degrees to kilometers and calculates
% the interevent distances in three dimensions.
%
%
if dtokm == 1
    pairdist = pairdist.*111;
end
%
%
% Calculation of the correlation integral using as input the
% pair distances computed above.
%
%
% Variables
%
d = 2;						%d = the dimension of the embedding volume.
rmax = max(pairdist);
rmin = min(pairdist);

if rmin == 0
    rmin = 0.01;
end

lrmin = log10(rmin);
lrmax = log10(max(pairdist));

%u = (log10(rmin):0.15:log10(rmax))';
%
% Defining the distance vector r in order that on the
% log-log graph all the points plot at equal distances from one another.
%
r = (logspace(lrmin, lrmax, 50))';
%r = zeros(size(u,1),1);
%r = 10.^u;
%
%
corint = [];						% corint= Vector of ?cumulative? correlation integral values for increasing interevent radius
corint = zeros(size(r,1),1);
k = 1;

for i = 1:size(r,1)

    j = [];
    j = pairdist < r(i);
    corint (k,1) = (2/(N*(N-1)))*sum(j);
    k = k + 1;

end

clear i j k;
%
%
% Plotting of the correlation integral in function of the interevent
% distance r.
%
%
dofdnofig;


function [label, model, llh] = mixGaussEm(X, init)
% Perform EM algorithm for fitting the Gaussian mixture model.
% Input:
%   X: d x n data matrix
%   init: k (1 x 1) number of components or label (1 x n, 1<=label(i)<=k) or model structure
% Output:
%   label: 1 x n cluster label
%   model: trained model structure
%   llh: loglikelihood
% Written by Mo Chen (sth4nth@gmail.com).
%% init
fprintf('EM for Gaussian mixture: running ... \n');
tol = 1e-6;
maxiter = 500;
llh = -inf(1,maxiter);
R = initialization(X,init);
for iter = 2:maxiter
    [~,label(1,:)] = max(R,[],2);
    R = R(:,unique(label));   % remove empty clusters
    model = maximization(X,R);
    [R, llh(iter)] = expectation(X,model);
    if abs(llh(iter)-llh(iter-1)) < tol*abs(llh(iter)); break; end;
end
llh = llh(2:iter);

function R = initialization(X, init)
n = size(X,2);
if isstruct(init)  % init with a model
    R  = expectation(X,init);
elseif numel(init) == 1  % random init k
    k = init;
    label = ceil(k*rand(1,n));
    R = full(sparse(1:n,label,1,n,k,n));
elseif all(size(init)==[1,n])  % init with labels
    label = init;
    k = max(label);
    R = full(sparse(1:n,label,1,n,k,n));
else
    error('ERROR: init is not valid.');
end

function [R, llh] = expectation(X, model)
mu = model.mu;
Sigma = model.Sigma;
w = model.w;

n = size(X,2);
k = size(mu,2);
R = zeros(n,k);
for i = 1:k
    R(:,i) = loggausspdf(X,mu(:,i),Sigma(:,:,i));
end
R = bsxfun(@plus,R,log(w));
T = logsumexp(R,2);
llh = sum(T)/n; % loglikelihood
R = exp(bsxfun(@minus,R,T));

function model = maximization(X, R)
[d,n] = size(X);
k = size(R,2);
nk = sum(R,1);
w = nk/n;
mu = bsxfun(@times, X*R, 1./nk);

Sigma = zeros(d,d,k);
r = sqrt(R);
for i = 1:k
    Xo = bsxfun(@minus,X,mu(:,i));
    Xo = bsxfun(@times,Xo,r(:,i)');
    Sigma(:,:,i) = Xo*Xo'/nk(i)+eye(d)*(1e-6);
end

model.mu = mu;
model.Sigma = Sigma;
model.w = w;

function y = loggausspdf(X, mu, Sigma)
d = size(X,1);
X = bsxfun(@minus,X,mu);
[U,p]= chol(Sigma);
if p ~= 0
    error('ERROR: Sigma is not PD.');
end
Q = U'\X;
q = dot(Q,Q,1);  % quadratic term (M distance)
c = d*log(2*pi)+2*sum(log(diag(U)));   % normalization constant
y = -(c+q)/2;


function value = r8_cotd ( degrees )

%*****************************************************************************80
%
%% R8_COTD returns the cotangent of an angle given in degrees.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real DEGREES, the angle in degrees.
%
%    Output, real VALUE, the cotangent of the angle.
%
  radians = pi * ( degrees / 180.0 );

  value = cos ( radians ) / sin ( radians );

  return
end


function [h mu ul ll] = circ_mtest(alpha, dir, xi, w, d)
%
% [pval, z] = circ_mtest(alpha, dir, w, d)
%   One-Sample test for the mean angle.
%   H0: the population has mean dir.
%   HA: the population has not mean dir.
%
%   Note: This is the equvivalent to a one-sample t-test with specified
%         mean direction.
%
%   Input:
%     alpha	sample of angles in radians
%     dir   assumed mean direction
%     [xi   alpha level of the test]
%     [w		number of incidences in case of binned angle data]
%     [d    spacing of bin centers for binned data, if supplied
%           correction factor is used to correct for bias in
%           estimation of r, in radians (!)]
%
%   Output:
%     h     0 if H0 can not be rejected, 1 otherwise
%     mu    mean
%     ul    upper (1-xi) confidence level
%     ll    lower (1-xi) confidence level
%
% PHB 7/6/2008
%
% References:
%   Biostatistical Analysis, J. H. Zar
%
% Circular Statistics Toolbox for Matlab

% By Philipp Berens, 2009
% berens@tuebingen.mpg.de - www.kyb.mpg.de/~berens/circStat.html

if size(alpha,2) > size(alpha,1)
	alpha = alpha';
end

if nargin<3
  xi = 0.05;
end

if nargin<4
  % if no specific weighting has been specified
  % assume no binning has taken place
	w = ones(size(alpha));
else
  if size(w,2) > size(w,1)
    w = w';
  end
  if length(alpha)~=length(w)
    error('Input dimensions do not match.')
  end
end

if nargin<5
  % per default do not apply correct for binned data
  d = 0;
end

% compute ingredients
mu = circ_mean(alpha,w);
t = circ_confmean(alpha,xi,w,d);
ul = mu + t;
ll = mu - t;

% compute test via confidence limits (example 27.3)
h = abs(circ_dist2(dir,mu)) > t;


function [z,pv1,pv2,pv3]=compare_bino_prob(X,Y)
% [z,pv1,pv2,pv3]=compare_bino_prob(X,Y)
% Compares probabilities of two binomial samples X, Y
% Let X and Y be sets of 0 or 1, results from a Bernoully experiments
% with probability p1 for set X and p2 for set Y.
% Test the null hypothesis
%       H_0: p1 = p2
%    vs alternatives
%       H_1: p1 > p2
%       H_2: p1 < p2
%       H_3: p1 != p2
%
% Input:
%       X - row of 0 or 1
%       Y - row of 0 or 1
%
% Output:
%       z   - normal statistics N(0,1) for H_0
%       pv1 - p-value for H_1
%       pv2 - p-value for H_2
%       pv3 - p-value for H_3

% Dimiar Atanasov (2008)
% datanasov@nbu.bg

if size(X,1) > 1 || size(Y,1) > 1
    error('Sets should be rows');
end;

n_x1 = size( find(X == 1), 1 );
n_x0 = size( find(X == 0), 1 );

n_y1 = size( find(Y == 1), 2);
n_y0 = size( find(Y == 0), 2);

n_x = n_x1 + n_x0;
n_y = n_y1 + n_y0;

n_1 = n_x1 + n_y1;
n_0 = n_x0 + n_y0;

n = n_0 + n_1;

h_x = n_x1 / n_x;
h_y = n_y1 / n_y;

h = (n_x1 + n_y1)/(n_x + n_y);

if (n_1^2  / n < 5) || (n_0^2 / n < 5) || ( n_1*n_0 / n < 5)
    disp('Missing asymptotic behaviour!!!');
end;

s = h*(1-h)*(1/n_x + 1/n_y);

z = (h_x - h_y) / sqrt(s);

pv1 = 1 - normcdf(z);
pv2 = normcdf(z);
pv3 = (1 - normcdf( abs(z) ))/2;


function coeff = eq_dist_coeff(dim,N,varargin)
%EQ_DIST_COEFF Coefficient of minimum distance of an EQ point set
%
%Syntax
% coeff = eq_dist_coeff(dim,N,options);
%
%Description
% COEFF = EQ_DIST_COEFF(dim,N) does the following:
% 1) uses the recursive zonal equal area sphere partitioning algorithm to
%    partition the unit sphere S^dim into N regions,
% 2) finds the EQ point set, the set of center points of each region,
% 3) finds the minimum Euclidean distance between points of the EQ point set,
% 4) sets COEFF to be the coefficient in the expression for the lower bound on
%    the minimum distance of a minimum energy point set:
%
%    DIST >= COEFF N^(-1/dim).
%
% The argument dim must be a positive integer.
% The argument N must be a positive integer or an array of positive integers.
% The result COEFF will be an array of the same size as N.
%
% COEFF = EQ_DIST_COEFF(dim,N,'offset','extra'), for dim == 2 or dim == 3, uses
% experimental extra rotation offsets to try to maximize the minimum distance.
% For dim > 3, extra offsets are not used.
%
%Notes
% The expression for the lower bound on minimum distance of a minimum r^(-s)
% energy point set on S^dim was given by [RakSZ95] for s == 0 and dim = 2,
% [Dahl78] for s == dim-1, [KuiSS04 Theorem 8] for dim-1 <= s < dim and
% [KuiS98 (1.12) p. 525] for s > dim.
%
% Ideally eq_dist_coeff(dim,N) should tend to area_of_sphere(dim)^(1/dim) as
% N goes to infinity.
%
%Examples
% > coeff=eq_dist_coeff(2,10)
%  coeff =
%      3.3250
%
% > coeff=eq_dist_coeff(3,1:6)
%  coeff =
%      2.0000    2.5198    2.0396    2.2449    2.4183    2.5698
%
%See also
% PARTITION_OPTIONS, EQ_MIN_DIST

% Copyright 2004-2005 Paul Leopardi for the University of New South Wales.
% $Revision 1.10 $ $Date 2005-06-01 $
% Documentation files renamed
% $Revision 1.00 $ $Date 2005-02-12 $
%
% For licensing, see COPYING.
% For references, see AUTHORS.
% For revision history, see CHANGELOG.

%
% Check number of arguments
%
error(nargchk(2,4,nargin));
%
% dim is the number of dimensions
% N is the number of regions
%
dist = eq_min_dist(dim,N,varargin{:});
coeff =  dist .* N.^(1/dim);
%
% end function


function u = ref_to_koorn ( r )

%*****************************************************************************80
%
%% REF_TO_KOORN maps points from the reference to Koornwinder's triangle.
%
%  Discussion:
%
%    The reference triangle has vertices:
%
%      ( -1, -1/sqrt(3) )
%      ( +1, -1/sqrt(3) )
%      (  0, +2/sqrt(3) )
%
%    Koornwinder's triangle has vertices:
%
%      ( -1, -1 )
%      ( +1, -1 )
%      ( -1, +1 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R(2), the coordinates of a point in the
%    reference triangle.
%
%    Output, real U(2), the coordinates of the point in
%    the Koornwinder triangle.
%
  a10 = -1.0 / 3.0;
  a11 =  1.0;
  a12 = -1.0 / sqrt ( 3.0 );

  a20 = - 1.0 / 3.0;
  a21 =   0.0;
  a22 =   2.0 * sqrt ( 3.0 ) / 3.0;

  u(1) = a10 + r(1) + a12 * r(2);
  u(2) = a20 +        a22 * r(2);

  return
end


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

%  L. Perrone, 4/28/02

%  Modifications:
%  5/25/02, J. Nagy
%           Cosmetic changes to incorporate into RestoreTools
%
%
%  11/17/02, J. Nagy
%            This was designed for image processing problems, where
%            it is common to use lexicographical (row) ordering.
%            But @kronMatrix functions where designed using vec (column)
%            ordering.  This inconsistency has been fixed.

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

[mp, np] = size(PSF);

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

%
% Compute weighted PSF.
%
c = zeros(mp,1);
c(1) = mp;
c(2:2:end) = 1;
R = chol( toeplitz(c) );

Phat = R*PSF*R';

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

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

%
% Construct approximation.
%
a = R \ ( U(:,1) * sqrt(S(1,1)) );
b = R \ ( V(:,1) * sqrt(S(1,1)) );

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

A = build_toep(b, center(2), m) + buildHank(b, center(2), m);
B = build_toep(a, center(1), n) + buildHank(a, center(1), n);


function h=plotgauss2d(mu, Sigma, plot_cross)
% PLOTGAUSS2D Plot a 2D Gaussian as an ellipse with optional cross hairs
% h=plotgauss2(mu, Sigma)
%
% h=plotgauss2(mu, Sigma, 1) also plots the major and minor axes
%
% Example
% clf; S=[2 1; 1 2]; plotgauss2d([0;0], S, 1); axis equal

h = plotcov2(mu, Sigma);
return;

%%%%%%%%%%%%%%%%%%%%%%%%
function old

if nargin < 3, plot_cross = 0; end
[V,D]=eig(Sigma);
lam1 = D(1,1);
lam2 = D(2,2);
v1 = V(:,1);
v2 = V(:,2);
%assert(approxeq(v1' * v2, 0))
if v1(1)==0
  theta = 0; % horizontal
else
  theta = atan(v1(2)/v1(1));
end
a = sqrt(lam1);
b = sqrt(lam2);
h=plot_ellipse(mu(1), mu(2), theta, a,b);

if plot_cross
  mu = mu(:);
  held = ishold;
  hold on
  minor1 = mu-a*v1; minor2 = mu+a*v1;
  hminor = line([minor1(1) minor2(1)], [minor1(2) minor2(2)]);

  major1 = mu-b*v2; major2 = mu+b*v2;
  hmajor = line([major1(1) major2(1)], [major1(2) major2(2)]);
  %set(hmajor,'color','r')
  if ~held
    hold off
  end
end


function determ = skew_circulant_determinant ( n, x )

%*****************************************************************************80
%
%% SKEW_CIRCULANT_DETERMINANT returns the determinant of the SKEW_CIRCULANT matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real X(N), the values in the first row of A.
%
%    Output, real DETERM, the determinant.
%
  determ = 1.0;

  j_hi = floor ( ( n + 1 ) / 2 );

  for j = 1 : j_hi

    lambda = 0.0;

    for k = 1 : n
      angle = ( 2 * j - 1 ) * ( k - 1 ) * pi / n;
      lambda = lambda + x(k) * complex ( cos ( angle ), sin ( angle ) );
    end

    if ( 2 * j <= n )
      determ = determ * ( abs ( lambda ) )^2;
    else
      determ = determ * real ( lambda );
    end

  end

  return
end


% Gives the spectrum of a derivative effect (i.e. a zero at zero frequency)
%
% Input
%  dftlen : the number of bin in spectrum (full DFT length).
%  fs     : [Hz] The sampling frequency
%
% Output
%  S      : The spectrum of the derivative effect
%
% Copyright (c) 2011 University of Crete - Computer Science Department
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function S = spec_derivative(dftlen, fs)

    % The half-spectrum of a zero at frequency zero is:
    hS = fs*(2i*pi/dftlen)*(0:dftlen/2).';

    if mod(dftlen,2)==1
        % If DFT length is odd
        S = [hS; hS(end:-1:2)];
    else
        % If DFT length is even
        hS(end) = 0;
        S = hspec2spec(hS);
    end

return


%KMEANS  Finds centers of clusters and groups input samples around the clusters
%
%     labels = cv.kmeans(data, K)
%     [labels, centers, compactness] = cv.kmeans(...)
%     [...] = cv.kmeans(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __data__ Data for clustering. An array of D-dimensional points with
%   floating-point coordinates is needed. Examples of this matrix can be: NxD
%   numeric matrix (one row per sample), or Nx1xD/1xNxD array (with dimensions
%   across slices).
% * __K__ Number of clusters to split the set by.
%
% ## Output
% * __labels__ Integer array that stores the cluster indices for every sample.
% * __centers__ Output matrix of the cluster centers, one row per each cluster
%   center.
% * __compactness__ Measure of compactness. See below.
%
% ## Options
% * __Criteria__ The algorithm termination criteria, that is, the maximum
%   number of iterations and/or the desired accuracy. The accuracy is
%   specified as `criteria.epsilon`. As soon as each of the cluster centers
%   moves by less than `criteria.epsilon` on some iteration, the algorithm
%   stops. default
%   `struct('type','Count+EPS', 'maxCount',100, 'epsilon',eps('float'))`
% * __Attempts__ The number of times the algorithm is executed using different
%   initial labelings. The algorithm returns the labels that yield the best
%   compactness (see the last function parameter). default 10.
% * __Initialization__ Method to initialize seeds. One of the followings:
%   * __Random__ Select random initial centers in each attempt. (default)
%   * __PP__ Use kmeans++ center initialization by Arthur and Vassilvitskii
%     [Arthur2007].
% * __InitialLabels__ Integer array that stores the initial cluster indices
%   for every sample. During the first (and possibly the only) attempt, kmeans
%   uses the user-supplied labels instead of computing them from the initial
%   centers. For the second and further attempts, it uses the random or
%   semi-random centers. Use one of the `Initialization` methods to specify
%   the exact method. Not set by default.
%
% The function cv.kmeans implements a k-means algorithm that finds the centers
% of `K` clusters and groups the input samples around the clusters. As an
% output, `labels(i)` contains a 0-based cluster index for the sample stored
% in the i-th row of the samples matrix.
%
% The function returns the compactness measure that is computed as:
%
%     sum_{i} (|| samples_i - centers_{labels_i} ||^2)
%
% after every attempt. The best (minimum) value is chosen and the
% corresponding labels and the compactness value are returned by the
% function. Basically, you can use only the core of the function, set the
% number of attempts to 1, initialize labels each time using a custom
% algorithm, pass them with the `InitialLabels` option, and then choose the
% best (most-compact) clustering.
%
% ## References
% [Arthur2007]:
% > D. Arthur, S. Vassilvitskii: "k-means++: The Advantages of Careful Seeding".
% > In Proceedings of the eighteenth annual ACM-SIAM symposium
% > on Discrete algorithms, 1027-1035, 2007.
%
% See also: kmeans
%


function area = area_quad ( quad_xy )

%*****************************************************************************80
%
%% AREA_QUAD returns the area of a quadrilateral.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real QUAD_XY(2,4), the coordinates of the nodes.
%
%    Output, real AREA, the area of the quadrilateral.
%
  t1(1:2,1) = quad_xy(1:2,1);
  t1(1:2,2) = quad_xy(1:2,2);
  t1(1:2,3) = quad_xy(1:2,3);

  area1 = triangle_area ( t1 );

  t2(1:2,1) = quad_xy(1:2,3);
  t2(1:2,2) = quad_xy(1:2,4);
  t2(1:2,3) = quad_xy(1:2,1);

  area2 = triangle_area ( t2 );

  if ( area1 < 0.0 | area2 < 0.0 )

    t1(1:2,1) = quad_xy(1:2,2);
    t1(1:2,2) = quad_xy(1:2,3);
    t1(1:2,3) = quad_xy(1:2,4);

    area1 = triangle_area ( t1 );

    t2(1:2,1) = quad_xy(1:2,4);
    t2(1:2,2) = quad_xy(1:2,1);
    t2(1:2,3) = quad_xy(1:2,2);

    area2 = triangle_area ( t2 );

    if ( area1 < 0.0 | area2 < 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'AREA_QUAD - Fatal error!\n' );
      fprintf ( 1, '  The quadrilateral nodes seem to be listed in\n' );
      fprintf ( 1, '  the wrong order, or the quadrilateral is\n' );
      fprintf ( 1, '  degenerate.\n' );
      error ( 'AREA_QUAD - Fatal error!' );
    end

  end

  area = area1 + area2;

  return
end


function [ Tree,Cost ] =  UndirectedMaximumSpanningTree (CostMatrix)
% The function takes CostMatrix as input and returns the maximum spanning tree T
% Uses Kruskal's Algorithm
% Extract the edge weights from the cost matrix
% Sort the edges in a non decreasing order of weights
% This algorithm is revised by Lowell Guangdi at 2009/06/11.

n = size (CostMatrix,1); %Number of vertices
EdgeWeights = 0;         %Edges and corresponding weights
EdgeWeightsCounter = 0;
for i = 1:n
    for j = (i+1):n
        if ((CostMatrix(i,j))~=0)
            EdgeWeightsCounter = EdgeWeightsCounter + 1;
            EdgeWeights(EdgeWeightsCounter,1) = CostMatrix(i,j);
            EdgeWeights(EdgeWeightsCounter,2) = i;
            EdgeWeights(EdgeWeightsCounter,3) = j;
        end
    end
end

SortedEdgeWeights = 0;
SortedEdgeWeights = sortrows(EdgeWeights);
% First column of SortedEdgeWeights are the weights
% Second and third column are the vertices that the edges connect
m = size(SortedEdgeWeights,1); % number of edges

% We use the Disjoint sets data structures to detect cycle while adding new
% edges. Union by Rank with path compression is implemented here.

% Assign parent pointers to each vertex. Initially each vertex points to
% itself. Now we have a conceptual forest of n trees representing n disjoint
% sets
global ParentPointer ;
ParentPointer = 0;
ParentPointer(1:n) = 1:n;

% Assign a rank to each vertex (root of each tree). Initially all vertices
% have the rank zero.
TreeRank = 0;
TreeRank(1:n) = 0;

% Visit each edge in the sorted edges array
% If the two end vertices of the edge are in different sets (no cycle), add
% the edge to the set of edges in maximum spanning tree
MSTreeEdges = 0;
MSTreeEdgesCounter = 0; i = m;
while ((MSTreeEdgesCounter < (n-1)) && (i>=1))
%Find the roots of the trees that the selected edge's two vertices
%belong to. Also perform path compression.
    root1=0; root2=0; temproot=0;
    temproot = SortedEdgeWeights(i,2);
    root1 = FIND_PathCompression(temproot);

    temproot = SortedEdgeWeights(i,3);
    root2 = FIND_PathCompression(temproot);

    if (root1 ~= root2)
        MSTreeEdgesCounter = MSTreeEdgesCounter + 1;
        MSTreeEdges(MSTreeEdgesCounter,1:3) = SortedEdgeWeights(i,:);
        if (TreeRank(root1)>TreeRank(root2))
            ParentPointer(root2)=root1;
        else
            if (TreeRank(root1)==TreeRank(root2))
               TreeRank(root2)=TreeRank(root2) + 1;
            end
            ParentPointer(root1)=root2;
        end
    end
    i = i - 1;
end

MSTreeEdgesCounter = 0;
Tree = 0;
Tree(1:n,1:n)=0;
while (MSTreeEdgesCounter < (n-1))
    MSTreeEdgesCounter = MSTreeEdgesCounter + 1;
    Tree(MSTreeEdges(MSTreeEdgesCounter,2),MSTreeEdges(MSTreeEdgesCounter,3))=1;
    Tree(MSTreeEdges(MSTreeEdgesCounter,3),MSTreeEdges(MSTreeEdgesCounter,2))=1;
end
%T

Cost = 0;
for p = 1:n
    for q = p+1:n
       if Tree( p,q ) == 1
          Cost = Cost + CostMatrix( p,q );
       end
    end
end

end

function [parent] = FIND_PathCompression(temproot)

global ParentPointer;
ParentPointer(temproot);
if (ParentPointer(temproot)~=temproot)
    ParentPointer(temproot) = FIND_PathCompression(ParentPointer(temproot));
end
parent = ParentPointer(temproot);
end


function value = tetrahedron_unit_monomial ( expon )

%*****************************************************************************80
%
%% TETRAHEDRON_UNIT_MONOMIAL integrates a monomial over the unit tetrahedron.
%
%  Discussion:
%
%    This routine integrates a monomial of the form
%
%      product ( 1 <= dim <= 3 ) x(dim)^expon(dim)
%
%    where the exponents are nonnegative integers.  Note that
%    if the combination 0^0 is encountered, it should be treated
%    as 1.
%
%    Integral ( over unit tetrahedron ) x^l y^m z^n dx dy =
%    l% * m% * n% / ( m + n + 3 )%
%
%    The integration region is defined as:
%
%      0 <= X
%      0 <= Y
%      0 <= Z
%      0 <= X + Y + Z <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer EXPON(3), the exponents.
%
%    Output, real VALUE, the integral of the monomial.
%

%
%  The first computation ends with VALUE = 1.0;
%
  value = 1.0;
%
%  The first loop simply calculates 1, so we short circuit it.
%
% k = 0;
%
% for i = 1 : expon(1)
%   k = k + 1;
%   value = value * i / k;
% end

  k = expon(1);
  for i = 1 : expon(2)
    k = k + 1;
    value = value * i / k;
  end

  for i = 1 : expon(3)
    k = k + 1;
    value = value * i / k;
  end

  k = k + 1;
  value = value / k;

  k = k + 1;
  value = value / k;

  k = k + 1;
  value = value / k;

  return
end


function H = ent_g(x, biascorrect)
% ENT_G Entropy of a Gaussian variable in bits
%   H = ent_g(x) returns the entropy of a (possibly
%   multidimensional) Gaussian variable x with bias correction.
%   Rows of x correspond to samples, columns to dimensions/variables.
%   (Samples first axis)
%
%   biascorrect : true / false option (default true) which specifies
%   whether bias correction should be applied to the estimated entropy.

if isvector(x)
    x = x(:);
end
if ndims(x)~=2
    error('ent_g: input arrays should be 2d')
end
[Ntrl, Nvarx] = size(x);

if nargin < 2
    % default is to apply bias correction
    biascorrect = true;
end

% demean data
gx.m = sum(x,1)/Ntrl;
x = bsxfun(@minus,x,gx.m);

% covariance
C = (x'*x) / (Ntrl - 1);
chC = chol(C);

% entropy in nats
HX = sum(log(diag(chC))) + 0.5*Nvarx*(log(2*pi)+1);

ln2 = log(2);
if biascorrect
    psiterms = psi((Ntrl - (1:Nvarx))/2) / 2;
    dterm = (ln2 - log(Ntrl-1)) / 2;
    HX = (HX - Nvarx*dterm - sum(psiterms));
end

% convert to bits
H = HX / ln2;