function [y,exitCode]=geometricMedian(x,eta,AbsTol,RelTol,maxIter,test4Duplicates)
%%GEOMETRICMEDIAN Compute the weighted geometric median of the (possibly
%          multivariate) points x. This finds the vector y that minimizes
%          sum_i eta(i)*norm(y-x(:,i),1) where all eta(i)>0.
%          The geometric median is also known as the L1-median and the
%          spatial median.
%
%INPUTS: x The xDimXN set of N real vectors whose geometric median is
%          desired.
%      eta The NX1 or 1XN set of all positive weights. If this parameter is
%          omitted or an empty matrix is passed, then uniform weights are
%          used.
% AbsTol, RelTol Absolute and relative tolerances for determining
%          convergence. If diff is the difference between the estimate at
%          the current and the previous iteration, then convergence is
%          declared if diff<AbsTol||diff<RelTol*norm(y). These same
%          tolerances are used in a similar manner to determine whether y
%          equals any of the points in x during an iterative update and
%          whether any of the vectors in x are duplicates of each other.
%          The defaults if omitted or empty matrices are passed are
%          RelTol=1e-9, AbsTol=1e-12.
%  maxIter The maximum number of iterations to perform. The default if
%          ommitted or an empty matrix is passed is max(1000,20*xDim).
% test4Duplicates The algorithms assume that none of the vectors in x are
%          duplicated of each other. test4Duplicates can be set to false if
%          it is known that there are no duplicated vectors in x. The
%          default if omitted or an empty matrix is passed is true.
%
%OUTPUTS: y The xDimX1 geometric median of the vectors in x.
%  exitCode A value indicating how the iterative algorithm terminated.
%           Possible values are:
%           0 The algorithm converged.
%           1 The algorithm did not convergence withing maxiter iteration.
%
%This function uses the modified Weiszfeld algorithm, which is described in
%[1]. The algorithm is inistialized with the Convergence is linear. A
%quasi-Newton algorithm would presumably be better for very large problems.
%
%EXAMPLE:
%Here, we have a set of data with an outlier. one will see that the mean is
%pulled towards the outlier, whereas the geometric median is more robust.
% x=[-26,   5,   1,  -2,  13,  -1,   0,   3,  -2, -12,  120;
%      0,  12,  12,  19,  -8, -16,  -5,   3,   4,   6,  100];
% yGM=geometricMedian(x);
% yM=mean(x,2);
% costGM=sum(sqrt(sum((bsxfun(@minus,yGM,x)).^2,1)))
% costM=sum(sqrt(sum((bsxfun(@minus,yM,x)).^2,1)))
% figure(1)
% clf
% hold on
% scatter(x(1,:),x(2,:),'ok')
% scatter(yGM(1),yGM(2),'r','filled')
% scatter(yM(1),yM(2),'b','filled')
% legend('Points','Geometric Median','Mean')
% h1=xlabel('x');
% h2=ylabel('y');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] Y. Vardi and C.-H. Zhang, "The multivariate l1-median and associated
%    data depth," Proceedings of the National Academy of Sciences of the
%    United States of America, vol. 97, no. 4, pp. 1423-1426, 23 Feb. 2000.
%
%August 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(x,2);
xDim=size(x,1);

if(nargin<6||isempty(test4Duplicates))
    test4Duplicates=true;
end

if(nargin<5||isempty(maxIter))
    maxIter=max(1000,20*xDim);
end

if(nargin<4||isempty(RelTol))
    RelTol=1e-9;
end

if(nargin<3||isempty(AbsTol))
    AbsTol=1e-12;
end

if(nargin<2||isempty(eta))
    eta=ones(1,n);
end

%Get rid of duplicate values and adjust the weights.
if(test4Duplicates)
    xUnique=zeros(xDim,n);
    xUnique(:,1)=x(:,1);
    etaUnique=zeros(1,n);
    etaUnique(1)=eta(1);
    numUnique=1;

    xMag=sqrt(sum(x.*x,1));
    for curX=2:n

        d=x(:,curX)-xUnique(:,1:numUnique);
        dMag=sqrt(sum(d.^2,1));
        idx=find(dMag<AbsTol|dMag<RelTol*xMag(curX),1);

        if(isempty(idx))%It is unique.
          numUnique=numUnique+1;
          xUnique(:,numUnique)=x(:,curX);
          etaUnique(numUnique)=eta(curX);
        else%It is not unique.
            etaUnique(idx)=etaUnique(idx)+eta(curX);
        end
    end

    x=xUnique;
    eta=etaUnique;
    n=numUnique;
end

%The special case when given a single value.
if(n==1)
  y=x;
  exitCode=0;
  return
end

eta=eta(:).';
xMag=sqrt(sum(x.*x,1));%Magnitude

%Start with the weighted mean as the initial estimate.
y=sum(bsxfun(@times,(eta/sum(eta)),x),2);

exitCode=1;
for curIter=1:maxIter
  yPrev=y;

  %Duplicates were removed (weights combined) prior to beginning iterations, so
  %there should be at most 1 point coincident with x. We will use absolute and
  %relative tolerances to determine equality.
  d=bsxfun(@minus,y,x);
  dMag=sqrt(sum(d.^2,1));
  idx=find(dMag<AbsTol|dMag<RelTol*xMag,1);

  if(isempty(idx))
    %If no points coincide with x, then use Weiszfeld's original iteration.

    %Defined after Equation 2.3
    w=eta./dMag;
    w=w/sum(w);

    %Equation 2,3
    y=sum(bsxfun(@times,w,x),2);
  else
    %If any point in x coincides with y, then use the modified Weiszfeld
    %iteration.

    %Equation 2,5. The only nonzero weight is the one  where the values
    %coincide.
    etay=eta(idx);

    %w as in Equation 23, but not normalized.
    w=eta./dMag;
    w(idx)=0;
    %The normalization constant.
    wNorm=sum(w);

    %Equation 2,4
    Ty=sum(bsxfun(@times,(w/wNorm),x),2);

    %Equation 2,7
    ry=norm(sum(bsxfun(@times,w,-d),2),1);

    %Equation 2.6
    y=max(0,(1-etay/ry))*Ty+min(1,etay/ry)*y;
  end

  yNorm=norm(y);
  diff=norm(y-yPrev);

  if(diff<AbsTol||diff<RelTol*yNorm)
      exitCode=0;
      break;
  end
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 lambda = kms_eigenvalues ( alpha, n )

%*****************************************************************************80
%
%% KMS_EIGENVALUES returns the eigenvalues of the KMS matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    William Trench,
%    Spectral decomposition of Kac-Murdock-Szego matrices,
%    Unpublished technical document.
%
%  Parameters:
%
%    Input, real ALPHA, the scalar that defines A.
%    Eigenvalue computations require 0 <= ALPHA <= 1.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  theta(1:n,1) = kms_eigenvalues_theta ( alpha, n );

  lambda(1:n,1) = ( 1.0 + alpha ) * ( 1.0 - alpha ) ...
    ./ ( 1.0 - 2.0 * alpha * cos ( theta(1:n,1) ) + alpha * alpha );

  return
end


function value = r8_cinh ( x )

%*****************************************************************************80
%
%% R8_CINH: alternate hyperbolic cosine integral Cinh of an R8 argument.
%
%  Discussion:
%
%    Cinh ( x ) = Integral ( 0 <= t <= x ) ( cosh ( t ) - 1 ) dt / t
%
%    The original text of this program had a mistake:
%      y = x * x / 9.0 - 1.0
%    has been corrected to
%      y = x * x / 4.5 - 1.0
%    JVB, 27 March 2010
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the hyperbolic cosine integral Cinh
%    evaluated at X.
%
  persistent cinhcs
  persistent eul
  persistent ncinh
  persistent xmin
  persistent xsml

  eul = 0.57721566490153286060651209008240;

  if ( isempty ( ncinh ) )

    cinhcs = [ ...
      0.1093291636520734431407425199795917, ...
      0.0573928847550379676445323429825108, ...
      0.0028095756978830353416404208940774, ...
      0.0000828780840721356655731765069792, ...
      0.0000016278596173914185577726018815, ...
      0.0000000227809519255856619859083591, ...
      0.0000000002384484842463059257284002, ...
      0.0000000000019360829780781957471028, ...
      0.0000000000000125453698328172559683, ...
      0.0000000000000000663637449497262300, ...
      0.0000000000000000002919639263594744, ...
      0.0000000000000000000010849123956107, ...
      0.0000000000000000000000034499080805, ...
      0.0000000000000000000000000094936664, ...
      0.0000000000000000000000000000228291, ...
      0.0000000000000000000000000000000484 ]';

    ncinh = r8_inits ( cinhcs, 16, 0.1 * r8_mach ( 3 ) );
    xsml = sqrt ( r8_mach ( 3 ) );
    xmin = 2.0 * sqrt ( r8_mach ( 1 ) );

  end

  absx = abs ( x );

  if ( x == 0.0 )
    value = 0.0;
  elseif ( absx <= xmin )
    value = 0.0;
  elseif ( x <= xsml )
    y = - 1.0;
    value = x * x * ( 0.25 + r8_csevl ( y, cinhcs, ncinh ) );
  elseif ( x <= 3.0 )
    y = x * x / 4.5 - 1.0;
    value = x * x * ( 0.25 + r8_csevl ( y, cinhcs, ncinh ) );
  else
    value = r8_chi ( absx ) - eul - log ( absx );
  end

  return
end


function value = r8_atan ( y, x )

%*****************************************************************************80
%
%% R8_ATAN computes the inverse tangent of the ratio Y / X.
%
%  Discussion:
%
%    R8_ATAN returns an angle whose tangent is ( Y / X ), a job which
%    the built in functions ATAN and ATAN2 already do.
%
%    However:
%
%    * R8_ATAN always returns a positive angle, between 0 and 2 PI,
%      while ATAN and ATAN2 return angles in the interval [-PI/2,+PI/2]
%      and [-PI,+PI] respectively;
%
%    * R8_ATAN accounts for the signs of X and Y, (as does ATAN2).  The ATAN
%     function by contrast always returns an angle in the first or fourth
%     quadrants.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real Y, X, two quantities which represent the tangent of
%    an angle.  If Y is not zero, then the tangent is (Y/X).
%
%    Output, real VALUE, an angle between 0 and 2 * PI, whose tangent is
%    (Y/X), and which lies in the appropriate quadrant so that the signs
%    of its cosine and sine match those of X and Y.
%

%
%  Special cases:
%
  if ( x == 0.0 )

    if ( 0.0 < y )
      value = pi / 2.0;
    elseif ( y < 0.0 )
      value = 3.0 * pi / 2.0;
    elseif ( y == 0.0 )
      value = 0.0;
    end

  elseif ( y == 0.0 )

    if ( 0.0 < x )
      value = 0.0;
    elseif ( x < 0.0 )
      value = pi;
    end
%
%  We assume that ATAN2 is correct when both arguments are positive.
%
  else

    abs_y = abs ( y );
    abs_x = abs ( x );

    theta_0 = atan2 ( abs_y, abs_x );

    if ( 0.0 < x && 0.0 < y )
      value = theta_0;
    elseif ( x < 0.0 && 0.0 < y )
      value = pi - theta_0;
    elseif ( x < 0.0 && y < 0.0 )
      value = pi + theta_0;
    elseif ( 0.0 < x && y < 0.0 )
      value = 2.0 * pi - theta_0;
    end

  end

  return
end


function [E,GReuss,GHill] = shearModulus(S,h,u)
% shear modulus for an compliance tensor
%
% Syntax
%
%   [GV,GR,GVRH] = shearModulus(S) % the isotropic case
%
%   E = shearModulus(S,h,u) % the anisotropic case with plane h and shear direction  u
%   E = shearModulus(S,[],u)
%   E = shearModulus(S,h,[])
%
% Input
%  C - elastic @stiffnessTensor
%  h - shear plane @vector3d
%  u - shear direction @vector3d
%
% Output
%  E - shear modulus
%  GVoigt - Voigt effective shear modulus, upper bound
%  GReuss - Reuss effective shear modulus, lower bound
%  GHill - Hill effective shear modulus
%
% Description
%
% $$E = \frac{1}{4 S_{ijkl} h_i u_j h_k u_l}$$
%
% See also
% complianceTensor/YoungsModulus complianceTensor/volumeCompressibility complianceTensor/ChristoffelTensor

if nargin == 1 % the isotropic case

  % compute stifness tensor as 6x6 matrices
  C = matrix(inv(S),'voigt');
  S = matrix(S,'voigt');

  % the Voigt upper bound
  % GV = ((C(1,1)+C(2,2)+C(3,3))-(C(1,2)+C(2,3)+C(3,1))+3*(C(4,4)+C(5,5)+C(6,6)))/15
  GVoigt = ((C(1,1,:) + C(2,2) + C(3,3,:)) ...
    - (C(1,2,:) + C(2,3,:) + C(3,1,:)) ...
    + 3 * (C(4,4,:) + C(5,5,:) + C(6,6,:))) ./ 15;

  % the Reuss lower bound
  % GR = 15/(4*(S(1,1)+S(2,2)+S(3,3))-4*(S(1,2)+S(2,3)+S(3,1))+3*(S(4,4)+S(5,5)+S(6,6)))
  GReuss = 15 ./ (4 * (S(1,1,:) + S(2,2,:) + S(3,3,:)) ...
    - 4 * (S(1,2,:) + S(2,3,:) + S(3,1,:)) ...
    + 3 * (S(4,4,:) + S(5,5,:) + S(6,6,:)));

  % Voigt Reuss Hill average
  GHill = 0.5.*(GVoigt + GReuss);
  E = GVoigt;

elseif nargin == 2 || isempty(h)

  E = S2FunHarmonicSym.quadrature(@(u) shearModulus(S,h,u),'bandwidth',4,S.CS);

elseif isempty(u)

  E = S2FunHarmonicSym.quadrature(@(u) shearModulus(S,h,u),'bandwidth',4,S.CS);

else

  % the anisotropic shear modulus
  E = 0.25./EinsteinSum(S,[-1 -2 -3 -4],h,-1,u,-2,h,-3,u,-4);

end


function cdf = hypergeometric_cdf ( x, n, m, l )

%*****************************************************************************80
%
%% HYPERGEOMETRIC_CDF evaluates the Hypergeometric CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X, the argument of the CDF.
%
%    Input, integer N, the number of balls selected.
%    0 <= N <= L.
%
%    Input, integer M, the number of white balls in the population.
%    0 <= M <= L.
%
%    Input, integer L, the number of balls to select from.
%    0 <= L.
%
%    Output, real CDF, the value of the CDF.
%
  c1_log = binomial_coef_log ( l - m, n );
  c2_log = binomial_coef_log ( l, n );

  pdf = exp ( c1_log - c2_log );
  cdf = pdf;

  for x2 = 0 : x - 1

    pdf = pdf * ( m - x2 ) * ( n - x2 ) ...
      / ( ( x2 + 1 ) * ( l - m - n + x2 + 1 ) );

    cdf = cdf + pdf;

  end

  return
end


function oeprint(mu, oev)

% print six classical orbital elements
% (orbital period in days, sma in kilometers)

% input

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

% Orbital Mechanics with MATLAB

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

rtd = 180 / pi;

% unload orbital elements array

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

arglat = mod(tanom + argper, 2.0 * pi);

period = 2.0 * pi * sma * sqrt(sma / mu) / 86400.0;

% print orbital elements

fprintf ('\n      sma (km)        eccentricity     inclination (deg)    argper (deg)');

fprintf ('\n %12.10e  %12.10e  %12.10e  %12.10e \n', sma, ecc, inc * rtd, argper * rtd);

fprintf ('\n     raan (deg)     true anomaly (deg)   arglat (deg)       period (days)');

fprintf ('\n %12.10e  %12.10e  %12.10e  %12.10e \n', raan * rtd, tanom * rtd, arglat * rtd, period);


function [v,lambda] = trideigs ( a, b, TOL, Nmax, vects )

%QRST           determine all of the eigenvalues (and optionally all of the
%               eigenvectors) of a symmetric tridiagonal matrix using the
%               QR algorithm with Wilkinson shift
%
%     calling sequences:
%             [lambda, v] = qrst ( a, b, TOL, Nmax, vects )
%             [lambda, v] = qrst ( a, b, TOL, Nmax )
%             lambda = qrst ( a, b, TOL, Nmax )
%             qrst ( a, b, TOL, Nmax, vects )
%             qrst ( a, b, TOL, Nmax )
%
%     inputs:
%             a       vector containing elements along the main diagonal
%                     of the symmetric tridiagonal matrix whose eigenvalues
%                     are to be determined
%             b       vector containing elements along the off diagonal
%                     of the symmetric tridiagonal matrix whose eigenvalues
%                     are to be determined
%             TOL     convergence tolerance
%             Nmax    maximum number of iterations
%             vects   optional input argument
%                     matrix containing eigenvector information produced
%                     during the reduction of the original symmetric
%                     matrix to symmetric tridiagonal form
%                     - this input is needed only if computation of the
%                       eigenvectors is requested (by including the second
%                       output argument) and the original matrix was not in
%                       symmetric tridiagonal form
%
%     output:
%             lambda  vector containing the eigenvalues of the symmetric
%                     tridiagonal matrix determined by the vectors a and b
%             v       optional output argument
%                     matrix containing the eigenvectors of the symmetric
%                     tridiagonal matrix determined by the vectors a and b
%                     - the i-th column of this matrix is an eigenvector
%                       which corresponds to the i-th eigenvalue in the
%                       vector lambda
%                     - eigenvectors will be not computed if this second
%                       output argument is omitted
%
%     NOTE:
%             if the maximum number of iterations is exceeded, a message
%             to this effect will be displayed, along with the number of
%             eigenvalues which had been determined - these eigenvalues
%             will be returned in the last entries of the output vector
%             lambda
%

n = length(a);
if ( length(b) == n-1 ) b(2:n) = b(1:n-1); end;

c = zeros ( 1, n );
s = zeros ( 1, n );
shift = 0;
togo = n;

if ( nargout >= 2 )
   if ( nargin >= 5 )
      v = vects;
   else
      v = eye(n);
   end;
end;

for its = 1 : Nmax

    if ( togo == 1 )
	   lambda(1) = a(1) + shift;
	   disp ( its );
	   return;
	end;

    trace = a(togo-1) + a(togo);
	det   = a(togo-1)*a(togo) - b(togo)*b(togo);
	disc  = sqrt ( trace*trace - 4*det );
	mu1 = (1/2) * ( trace + disc );
	mu2 = (1/2) * ( trace - disc );
	if ( abs ( mu1 - a(togo) ) < abs ( mu2 - a(togo) ) )
	   s = mu1;
	else
	   s = mu2;
	end;

    shift = shift + s;
	for i = 1:togo
	    a(i) = a(i) - s;
	end;

	oldb = b(2);
    for i = 2:togo
        j = i-1;
	    r = sqrt ( a(j)^2 + oldb^2 );
	    c(i) = a(j) / r;
	    s(i) = oldb / r;
	    a(j) = r;
	    temp1 = c(i)*b(i) + s(i)*a(i);
	    temp2 = -s(i)*b(i) + c(i)*a(i);
	    b(i) = temp1;
	    a(i) = temp2;
	    if ( i ~= togo ) oldb = b(i+1); b(i+1) = c(i)*b(i+1); end;
    end;

    a(1) = c(2)*a(1) + s(2)*b(2);
    b(2) = s(2)*a(2);
    for i = 2:togo-1
        a(i) = s(i+1)*b(i+1) + c(i)*c(i+1)*a(i);
	    b(i+1) = s(i+1)*a(i+1);
    end;
    a(togo) = c(togo)*a(togo);

	if ( nargout >= 2 )
	   for i = 2 : togo
	       col1 = v(:,i-1) * c(i) + v(:,i) * s(i);
		   v(:,i) = -s(i) * v(:,i-1) + c(i) * v(:,i);
		   v(:,i-1) = col1;
	   end;
	end;

	if ( abs(b(togo)) < TOL )
	   lambda(togo) = a(togo) + shift;
	   disp([lambda(togo) its]);
	   togo = togo - 1;
	end;

end;

disp ( 'qrst error: Maximum number of iterations exceeded' );
disp ( sprintf ( '%d eigenvalues determined \n', n-togo ) );


function DataSet = prtDataGenXor(N)
%prtDataGenXor  Generates XOR example data
%
%   DATASET = prtDataGenXor returns a prtDataSetClass with randomly
%   generated data according to the following distribution.
%
%       H0: N([-1 -1],eye(2))
%       H1: N([2 2],[1 .5; .5 1])
%
%   % Example
%
%   ds = prtDataGenXor;
%   plot(ds)
%
%   See also: prtDataSetClass, prtDataGenBiModal, prtDataGenIris,
%   prtDataGenMary, prtDataGenNoisySinc, prtDataGenOldFaithful,
%   prtDataGenSpiral, prtDataGenUnimodal, prtDataGenUnimodal, prtDataGenXor

if nargin == 0
    nSamples = 200;
else
    nSamples = N;
end
if nargin < 5
    mu01 = [0 3];
    sigma01 = eye(2)/3;
    mu02 = [3 0];
    sigma02 = eye(2)/3;

    mu11 = [3 3];
    sigma11 = eye(2)/3;
    mu12 = [0 0];
    sigma12 = eye(2)/3;
end
rv(1) = prtRvMvn('mu',mu01,'sigma',sigma01);
rv(2) = prtRvMvn('mu',mu02,'sigma',sigma02);

rv(3) = prtRvMvn('mu',mu11,'sigma',sigma11);
rv(4) = prtRvMvn('mu',mu12,'sigma',sigma12);

X = cat(1,draw(rv(1),nSamples/2),draw(rv(2),nSamples/2),draw(rv(3),nSamples/2),draw(rv(4),nSamples/2));
Y = prtUtilY(nSamples,nSamples);

DataSet = prtDataSetClass(X,Y,'name','XOR Data');


function [a,b,c,d]=getplanefrom3pt(plane)
%
% [a,b,c,d]=getplanefrom3pt(plane)
%
% define a plane equation ax+by+cz+d=0 from three 3D points
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%    plane: a 3x3 matrix with each row specifying a 3D point (x,y,z)
%
% output:
%    a,b,c,d: the coefficient for plane equation ax+by+cz+d=0
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

x=plane(:,1);
y=plane(:,2);
z=plane(:,3);

% compute the plane equation a*x + b*y +c*z +d=0

a=y(1)*(z(2)-z(3))+y(2)*(z(3)-z(1))+y(3)*(z(1)-z(2));
b=z(1)*(x(2)-x(3))+z(2)*(x(3)-x(1))+z(3)*(x(1)-x(2));
c=x(1)*(y(2)-y(3))+x(2)*(y(3)-y(1))+x(3)*(y(1)-y(2));
d=-det(plane);


function [theta,axis] = dq2rot(dq)

% DQ2ROT     extracts the rotation axis and angle of a rotation dual
%            quaternion
%
%       [THETA,AXIS] = DQ2ROT(DQ) returns the rotation angle, THETA [deg],
%       and the rotation axis, AXIS, of a rotation dual quaternion DQ.
%       - DQ is a rotation dual quaternion. It is a 8-vector or an 8*N
%       array (each column is a rotation dual quaternion) where N is the
%       number of rotation dual quaternions.
%       - THETA is the rotation angle [deg]. It is comprised between 0 and
%       180deg. THETA is a scalar or an 1*N array (element i is the
%       rotation angle of dual quaternion i).
%       - AXIS is the unitary (norm equal to 1) rotation axis. It is a
%       3-vector or an 3*N array (column i is the rotation axis of
%       dual quaternion i).
%
% See also DQ2TRANS, DQ2SCREW, ROT2DQ

sdq = size(dq);
if sdq == [1 8]
    dq = dq.';
    sdq = size(dq);
end
n = sdq(2);

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

% Extraction of the rotation parameters
theta = 2*acos(dq(1,:));
axis = sym([ones(1,n) ; zeros(2,n)]);
repsin = repmat(sin(theta(n)/2),3,1);
axis(:,n) = dq(2:4,n)./repsin;


function euler_m = dcm2euler_m(DCMnb_m)
% dcm2euler_m: transforms a nav-to-body DCM matrix to an Euler angles matrix.
%
% INPUT
%   DCMnb_m: Nx9 matrix with nav-to-body direct cosine matrices (DCM).
% Each row of DCMnb_m contains the 9 elements of a particular DCMnb
% matrix ordered as [a11 a21 a31 a12 a22 a32 a13 a23 a33].
%
% OUTPUT
%   euler_m: Nx3 Euler angles ordered by column, [roll pitch yaw] (rad, rad, rad).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
% 	Titterton, D.H. and Weston, J.L. (2004). Strapdown
% Inertial Navigation Technology (2nd Ed.). Institution
% of Engineering and Technology, USA. Eq. 11.4. Eq. 3.66, p. 46.
%
%   R. Gonzalez, J. Giribet, and H.D. Patiño,. An approach to
% benchmarking of loosely coupled low-cost navigation systems,
% Mathematical and Computer Modelling of Dynamical Systems, vol. 21,
% issue 2, pp. 272-287. Eq. 15.
%
% Version: 001
% Date:    2020/12/17
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

[N,~] = size (DCMnb_m);

euler_m = zeros(N,3);

for i=1:N

    DCMnb = reshape(DCMnb_m (i,:), 3, 3);
    DCMbn = DCMnb';                         % nav-to-body > body-to-nav

    euler_m (i,:) = dcm2euler(DCMbn);       % phi theta psi
end

end


function imchi=kkimbook2(omega,rechi,alpha)
%The program inputs are the vector of the frequency (or energy)
%components, the vector of the real part of the susceptibility
%under examination, and the value of the moment considered.
%The two vectors must have the same length
%and the frequency vector omega must be equispaced.
%If not, apply MATLAB functions such as interp.
%If rechi is the real part of a linear susceptibility,
%alpha must be 0.
%If rechi is the real part of the nth
%harmonic generation susceptibility, alpha=0,1,..2n.
%If rechi is the real part of a pump and probe
%susceptibility, alpha=0 or 1.
%This files accompanies the book
%"Kramers-Kronig Relations in Optical Materials Research"
%by Lucarini, V., Saarinen, J.J., Peiponen, K.-E., Vartiainen, E.M.
%Springer, Heidelberg, 2005
%where the theory and applications are fully developed.
%The output is the estimate of the imaginary part as obtained
%with K-K relations.
%This software is distributed under the GNU licence agreement
%by Valerio Lucarini
%email: lucarini@alum.mit.edu
%University of Camerino
%Department of Mathematics and Computer Science
%Camerino, Italy

if size(omega,1)>size(omega,2);
omega=omega';
end; if size(rechi,1)>size(rechi,2);
rechi=rechi';
end;
%Here the program rearranges the two vectors so that,
%whichever their initial shape, they become row vectors.

g=size(omega,2);
%Size of the vectors.%

imchi=zeros(size(rechi));
%The output is initialized.

a=zeros(size(rechi));
b=zeros(size(rechi));
%Two vectors for intermediate calculations are initialized

deltaomega=omega(2)-omega(1);
%Here we compute the frequency (or energy) interval

j=1;
beta1=0;
for k=2:g;
b(1)=beta1+rechi(k)*omega(k)^(2*alpha)/(omega(k)^2-omega(1)^2);
beta1=b(1);
end;
imchi(1)=-2/pi*deltaomega*b(1)*omega(1)^(1-2*alpha);
%First element of the output: the principal part integration
%is computed by excluding the first element of the input

j=g;
alpha1=0;
for k=1:g-1;
a(g)=alpha1+rechi(k)*omega(k)^(2*alpha)/(omega(k)^2-omega(g)^2);
alpha1=a(g);
end;
imchi(g)=-2/pi*deltaomega*a(g)*omega(g)^(1-2*alpha);
%Last element of the output: the principal part integration
%is computed by excluding the last element of the input.

for j=2:g-1; ;
%Loop on the inner components of the output vector.
alpha1=0;
beta1=0;
for k=1:j-1;
a(j)=alpha1+rechi(k)*omega(k)^(2*alpha)/(omega(k)^2-omega(j)^2);
alpha1=a(j);
end;
for k=j+1:g;
b(j)=beta1+rechi(k)*omega(k)^(2*alpha)/(omega(k)^2-omega(j)^2);
beta1=b(j);
end;
imchi(j)=-2/pi*deltaomega*(a(j)+b(j))*omega(j)^(1-2*alpha);
%Last element of the output: the principal part integration
%is computed by excluding the last element of the input
end;


function [A,Phi] = inverse_cht(Am,Nphi)
%INVERSE_CHT inverse circular harmonics transform (ICHT)
%
%   Usage: [A,Phi] = inverse_cht(Am,[Nphi])
%
%   Input parameters:
%       Am      - circular harmonics coefficients [N x (2*M+1)]
%       Nphi    - number of equi-angular distributed angles, for which the ICHT
%                 is computed, optional, default: 2*M+1
%
%   Output parameters:
%       A       - inverse circular harmonics transform [N x Nphi]
%       Phi     - corresponding angle of the ICHT [1 x Nphi]
%
%   See also: pwd_imp_circexp

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 1;
nargmax = 2;
narginchk(nargmin,nargmax);
isargmatrix(Am);
if nargin == nargmin
    Nphi = size(Am, 2);
else
    isargpositivescalar(Nphi);
end

%% ===== Computation ==================================================
M = (size(Am,2)-1)/2;
N = size(Am,1);

% Implementation of
%           ___
%           \
% A(phi) =  /__    A  e^(+i*m*n*2*pi/Nphi)
%         m=-M..M   m

% Spatial IFFT
A = zeros(N, Nphi);
% this handles cases where Nphi < M
for l=1:N
    A(l,:) = sum(buffer(Am(l,:),Nphi),2);
end
A = circshift(A,[0,-M]);  % m = 0, ..., M, ..., -M, ..., -1
A = ifft(A,[],2) * Nphi;  % IFFT includes factor 1/Nphi

% Axis corresponding to ICHT
if nargout>1
    Phi = 0:2*pi / Nphi:2*pi*(1-1/Nphi);
end


function value = lp_value ( n, o, x )

%*****************************************************************************80
%
%% LP_VALUE evaluates a Legendre polynomial at several points X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, integer O, the degree of the polynomial.
%    0 <= O.
%
%    Input, real X(N,1), the evaluation points.
%
%    Output, real VALUE(N,1), the value of the Legendre polynomial
%    of degree O at the points X.
%
  x = x ( : );

  v = zeros ( n, o + 1 );

  v(1:n,1) = 1.0;

  if ( 1 <= o )

    v(1:n,2) = x(1:n,1);

    for j = 2 : o

      v(1:n,j+1) = ( ( 2 * j - 1 ) * x(1:n,1) .* v(1:n,j)   ...
                  -  (     j - 1 ) *             v(1:n,j-1) ) ...
                  /  (     j     );

    end

  end

  value(1:n,1) = v(1:n,o+1);

  return
end


function [r,varargout]=circfit(x,y)
%CIRCFIT  Least squares fit of X-Y data to a circle.
%   R = CIRCFIT(X,Y) returns scalar radius R of a fitted circle. X and Y are 1-D
%   arrays of position data in a rectilinear coordinate system. X and Y must be
%   the same length and must contain at least three non-colinear points in order
%   for a valid solution to be found.
%
%   [R,ERR] = CIRCFIT(X,Y) additionally returns the scalar root mean squared
%   error of the fitted circle radius and center relative to the position data.
%
%   [R,XC,YC] = CIRCFIT(X,Y) additionally returns the scalar positions, XC and
%   YC, of center of the fitted circle.
%
%   [R,XC,YC,ERR] = CIRCFIT(X,Y) returns both the center positions of the circle
%   as well as the root mean squared error.
%
%   Examples:
%       % Fit of just five noisy points
%       x1=[1 0 -1 0 1]+0.05*randn(1,5); y1=[0 1 0 -1 0]+0.05*randn(1,5);
%       r1=circfit(x1,y1)
%
%       % CIRCFIT can sometimes perfom poorly with less than 180-degree arc
%       t=0:0.1:pi; lt=length(t);
%       x2=cos(t)+0.04*randn(1,lt); y2=sin(t)+0.04*randn(1,lt);
%       r2_90deg=circfit(x2(1:floor(lt/2)),y2(1:floor(lt/2)))
%       r2_180deg=circfit(x2,y2)

%   Andrew D. Horchler, adh9 @ case . edu, Created 5-12-7
%   Revision: 1.3, 4-6-16

% Check inputs
if nargout > 4
    error('circfit:circfit:TooManyOutputs','Too many output arguments.');
end

if ~isvector(x) || ~isfloat(x) || ~isreal(x) || ~all(isfinite(x))
    error('circfit:circfit:NonFiniteRealVectorX',...
          'X must be a finite real vector of floating point numbers.');
end
if ~isvector(y) || ~isfloat(y) || ~isreal(y) || ~all(isfinite(y))
    error('circfit:circfit:NonFiniteRealVectory',...
          'Y must be a finite real vector of floating point numbers.');
end

lx=length(x);
if lx ~= length(y)
    error('circfit:circfit:LengthMismatch',...
          'The vectors X and Y must have the same length.');
end
if lx < 3
    error('circfit:circfit:Min3Points',...
          'The vectors X and Y must contain at least three points.');
end
x=x(:);
y=y(:);

% Check collinearity, assume with sufficient points, some will be non-collinear
if rank(diff([x([1:min(50,lx) 1]) y([1:min(50,lx) 1])])) == 1
    if lx <= 50 || rank(diff([x y;x(1) y(1)])) == 1
        error('circfit:circfit:Colinearity',...
             ['The points in vectors X and Y must not all be collinear, or '...
              'nearly collinear, with each other.']);
    end
end

xx=x.*x;
yy=y.*y;
xy=x.*y;
xxyy=xx+yy;
sx=sum(x);
sy=sum(y);
sxx=sum(xx);
syy=sum(yy);
sxy=sum(xy);

% Solve linear system without inverting
% a=[sx sy lx;sxy syy sy;sxx sxy sx]\[sxx+syy;sum(xxyy.*y);sum(xxyy.*x)];
[L,U]=lu([sx sy lx;sxy syy sy;sxx sxy sx]);
a=U\(L\[sxx+syy;sum(xxyy.*y);sum(xxyy.*x)]);

xc=0.5*a(1);          	% X-position of center of fitted circle
yc=0.5*a(2);          	% Y-position of center of fitted circle
r=sqrt(xc^2+yc^2+a(3));	% Radius of fitted circle

% Set variable outputs
if nargout > 2
    varargout{1}=xc;
    varargout{2}=yc;
end
if nargout == 2 || nargout == 4
    varargout{nargout-1}=sqrt(mean((sqrt((x-xc).^2+(y-yc).^2)-r).^2));	% RMSE
end


function prob_test0744 ( )

%*****************************************************************************80
%
%% TEST0744 tests FRECHET_CDF, FRECHET_CDF_INV and FRECHET_PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
  seed = 1213456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0744\n' );
  fprintf ( 1, '  For the Frechet PDF:\n' );
  fprintf ( 1, '  FRECHET_CDF evaluates the CDF;\n' );
  fprintf ( 1, '  FRECHET_CDF_INV inverts the CDF.\n' );
  fprintf ( 1, '  FRECHET_PDF evaluates the PDF;\n' );

  alpha = 3.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter ALPHA =         %f\n', alpha );

  fprintf ( 1, '\n' );
  fprintf ( 1,'       X            PDF           CDF            CDF_INV\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ x, seed ] = frechet_sample ( alpha, seed );

    pdf = frechet_pdf ( x, alpha );

    cdf = frechet_cdf ( x, alpha );

    x2 = frechet_cdf_inv ( cdf, alpha );

    fprintf ( 1, '  %12f  %12f  %12f  %12f\n',x, pdf, cdf, x2 );

  end

  return
end


function [H,F] = make_bank_DFT(p,N)

% make_bank_DFT     Generate DFT Filter Bank with N Subbands
%
% Arguments:
% p                 Prototype filter (impulse response)
% N                 Number of subbands
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

p = p(:);                            % Make column
flen = max(size(p));                 % Length of prototype filter
H = zeros(flen,N/2+1);               % Assume N is an even number
n = (0:flen-1)';

k = 0; H(:,k+1) = p;                 % i = 0
k = N/2; H(:,k+1) = p.*((-1).^n);    % i = N/2+1

for k = 1:(N/2-1)
    H(:,k+1) = p.*exp(j*2*pi/N*k*n); % Complex modulation
end

D = H(:,2:N/2);
H = [H, conj(fliplr(D))];            % Complex-conjugate pair

% Synthesis filters are the same as the analysis filters

F = H;


function f = bohach2_xy ( x, y )

%*****************************************************************************80
%
%% BOHACH2_XY evaluates the Bohachevsky function #2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, real X, Y, the argument of the function.
%
%    Output, real F, the value of the function at X.
%
  f =       x * x ...
    + 2.0 * y * y ...
    - 0.3 * cos ( 3.0 * pi * x ) * cos ( 4.0 * pi * y ) ...
    + 0.3;

  return
end


function z = nmi(x, y)
% Compute normalized mutual information I(x,y)/sqrt(H(x)*H(y)) of two discrete variables x and y.
% Input:
%   x, y: two integer vector of the same length
% Ouput:
%   z: normalized mutual information z=I(x,y)/sqrt(H(x)*H(y))
% Written by Mo Chen (sth4nth@gmail.com).
assert(numel(x) == numel(y));
n = numel(x);
x = reshape(x,1,n);
y = reshape(y,1,n);

l = min(min(x),min(y));
x = x-l+1;
y = y-l+1;
k = max(max(x),max(y));

idx = 1:n;
Mx = sparse(idx,x,1,n,k,n);
My = sparse(idx,y,1,n,k,n);
Pxy = nonzeros(Mx'*My/n); %joint distribution of x and y
Hxy = -dot(Pxy,log2(Pxy));

% hacking, to elimative the 0log0 issue
Px = nonzeros(mean(Mx,1));
Py = nonzeros(mean(My,1));

% entropy of Py and Px
Hx = -dot(Px,log2(Px));
Hy = -dot(Py,log2(Py));

% mutual information
MI = Hx + Hy - Hxy;

% normalized mutual information
z = sqrt((MI/Hx)*(MI/Hy));
z = max(0,z);


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


% Random directed graph construction
% Note 1: if p is omitted, p=0.5 is default
% Note 2: no self-loops, no double edges
%
% INPUTS:  n - number of nodes
%          p - probability, 0<=p<=1
% Output: adjacency matrix, nxn
%
% GB: last updated, Oct 21 2012

function adj = randomDirectedGraph(n,p)

adj=zeros(n); % initialize adjacency matrix

if nargin==1; p=0.5; end; % default probability

% splitting j = 1:i-1,i+1,n avoids the if statement i==j

for i=1:n

  for j=1:i-1
    if rand<=p; adj(i,j)=1; end;
  end

  for j=i+1:n
    if rand<=p; adj(i,j)=1; end;
  end

end


function g = sigmoid(z)
%SIGMOID Compute sigmoid functoon
%   J = SIGMOID(z) computes the sigmoid of z.

% You need to return the following variables correctly
g = zeros(size(z));

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

f=@(h) 1 ./ (1 + exp(-h)); % 1 / (1 + exp(-h));

g = f(z);

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

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


function prob_test063 ( )

%*****************************************************************************80
%
%% TEST063 tests EXPONENTIAL_MEAN, EXPONENTIAL_SAMPLE, EXPONENTIAL_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST063\n' );
  fprintf ( 1, '  For the Exponential PDF:\n' );
  fprintf ( 1, '  EXPONENTIAL_MEAN computes the mean;\n' );
  fprintf ( 1, '  EXPONENTIAL_SAMPLE samples;\n' );
  fprintf ( 1, '  EXPONENTIAL_VARIANCE computes the variance.\n' );

  a = 1.0;
  b = 10.0;

  check = exponential_check ( a, b );

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

  mean = exponential_mean ( a, b );
  variance = exponential_variance ( a, b );

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

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

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

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

  return
end


%% 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 p = multipdf(x,theta)
%MULTIPDF Multinomial probability density function.
%   p = multipdf(x,theta) returns the probabilities of
%   vector x, under the multinomial distribution
%   with parameter vector theta.
%
%   Author: David Ross

%--------------------------------------------------------
% Check the arguments.
%--------------------------------------------------------
error(nargchk(2,2,nargin));

% make sure theta is a vector
if ndims(theta) > 2 | all(size(theta) > 1)
    error('theta must be a vector');
end

% make sure x is of the appropriate size
if ndims(x) > 2 | any(size(x) ~= size(theta))
    error('columns of X must have same length as theta');
end

%--------------------------------------------------------
% Main...
%--------------------------------------------------------
p = prod(theta .^ x);
p = p .* factorial(sum(x)) ./ prod(factorial_v(x));

%--------------------------------------------------------
% Function factorial_v(x): computes the factorial function
% on each element of x
%--------------------------------------------------------
function r = factorial_v(x)

if size(x,2) == 1
    x = x';
end

r = [];
for y = x
    r = [r factorial(y)];
end


function centroids = computeCentroids(X, idx, K)
%COMPUTECENTROIDS returns the new centroids by computing the means of the
%data points assigned to each centroid.
%   centroids = COMPUTECENTROIDS(X, idx, K) returns the new centroids by
%   computing the means of the data points assigned to each centroid. It is
%   given a dataset X where each row is a single data point, a vector
%   idx of centroid assignments (i.e. each entry in range [1..K]) for each
%   example, and K, the number of centroids. You should return a matrix
%   centroids, where each row of centroids is the mean of the data points
%   assigned to it.
%

% Useful variables
[m n] = size(X);

% You need to return the following variables correctly.
centroids = zeros(K, n);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every centroid and compute mean of all points that
%               belong to it. Concretely, the row vector centroids(i, :)
%               should contain the mean of the data points assigned to
%               centroid i.
%
% Note: You can use a for-loop over the centroids to compute this.
%

for k = 1:K
    xPos = find(idx == k);
    if(size(xPos,1)>0)
      avg = mean(X(xPos,:));
      centroids(k, :) = avg;
    end
end

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

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  a = randint(m,n,a,b)
        %RANDINT  Randomly generated integral matrix.
        %         randint(m,n) returns an m-by-n such matrix with entries
        %         between 0 and 9.
        %         rand(m,n,a,b) return entries between integers  a  and  b .
        if nargin < 3, a = 0; b = 9; end
        a = floor((b-a+1)*rand(m,n)) + a;


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


numNoisePoints = 40;
dots = rand(numNoisePoints,2);

numClus = 2;
clusCtr = rand(numClus,2);

stdMin = .04;
stdMax = .06;
clusStdX = rand(numClus)*stdMax+stdMin;
clusStdY = rand(numClus)*stdMax+stdMin;

sizeMin = 30;
sizeMax = 50;
clusSize = randi(sizeMax, numNoisePoints)+sizeMin;

for i = 1:numClus
    clusDotsX = normrnd(clusCtr(i,1),clusStdX(i),clusSize(i),1);
    clusDotsY = normrnd(clusCtr(i,2),clusStdY(i),clusSize(i),1);
    clusDots = [clusDotsX clusDotsY];
    dots = vertcat(dots, clusDots);
end

figure;
scatter(dots(:,1),dots(:,2),5,'MarkerFaceColor','k','MarkerEdgeColor','k');

%% kmeans 1
numK1 = 15;
idx_k1 = kmeans(dots,numK1);
colorsK1 = lines(numK1);
figure; hold on;
for i = 1:length(dots)
    scatter(dots(i,1),dots(i,2),5,'MarkerFaceColor',colorsK1(idx_k1(i),:),...
        'MarkerEdgeColor',colorsK1(idx_k1(i),:));
end

%% kmeans 2
numK2 = 5;
figure; hold on;
for j = 1:numK1
    dots_k1 = dots(idx_k1 == j,:);
    idx_k2 = kmeans(dots_k1,numK2);
    colorsK2 = lines(numK2*numK1);
    for i = 1:length(dots_k1)
        ci = (j-1)*numK1+idx_k2(i);
    scatter(dots_k1(i,1),dots_k1(i,2),5,'MarkerFaceColor',colorsK2(idx_k2(ci),:),...
        'MarkerEdgeColor',colorsK2(idx_k2(ci),:));
    end
end


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 test_quadratic_constrained
%% Tests the solvers on a simple constrained quadratic function
%{
    Solve:

    minimize_x  c'x + x'Dx/2
    subject to  ||x||_1 <= 10

    as an example of using TFOCS without the "SCD" interface
    (since the objective is smooth and we can project, there's
     no need to smooth and solve the dual)

    It's also an example of using 2 objective functions

%}

%% Now, add in constraints

randn( 'state', sum('quadratic test') );
N = 100;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CONSTRUCT THE TEST PROBLEM %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

c = randn(N,1);
D = randn(N,N);
D = D * D' + .5*eye(N);
s = svd(D);
% x_star = - D \ c;
% L = max(s);
% mu = min(s);
% if mu < eps, disp('WARNING: may have 0 eigenvalues'); end
% f_star = 0.5 * c' * x_star;
% n_star = norm( x_star );
x0 = zeros(N,1);

% f       = @(x) c'*x + x'*D*x/2;
% grad_f  = @(x) c + D*x;
% smoothF = @(x) wrapper_objective( f, grad_f, x );
%   -- or --
% smoothF = smooth_quad(D,c);
%   -- or (demonstrating how to use 2 objective functions) --
f1      = @(x) c'*x;
f2      = @(x) x'*D*x/2;
g1      = @(x) c;
g2      = @(x) D*x;
smoothF = { @(x) wrapper_objective(f1,g1,x); @(x) wrapper_objective(f2,g2,x) };
linearF = { 1 ; 1 };

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SET UP THE TEST PARAMETERS %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

opts = [];
opts.tol        = 1e-16;
opts.maxits     = 3000;
opts.restart    = 100;

%%%%%%%%%%%%%%%%%%%%%
% ADD IN CONSTRAINT %
%%%%%%%%%%%%%%%%%%%%%
% projectorF      = proj_simplex(10); % x >=0, sum(x) = 10
projectorF      = proj_l1(10);  % l1 ball, radius 10 (e.g. norm(x,1) <= 10)

%%%%%%%%%%%%%%%%%
% RUN THE TESTS %
%%%%%%%%%%%%%%%%%
[ x, out, optsOut ] = tfocs( smoothF, linearF, projectorF,x0, opts );
% Check that we are within allowable bounds

function [v,gr] = wrapper_objective(f,g,x)
v = f(x);
if nargout > 1
    gr = g(x);
end
% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


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 D = dmatrix(P)

% Distance Matrix (Eucledean distance)
%
% A simple function to calculate the distances of all pairs of points in a
% given group, as rows of a matrix P. The advantage of this method is that it
% exhibits a small computational cost.
%
% Created by Dr. Zacharias Voulgaris, 2009

N = size(P,1);
d(N,N) = 0;

for i = 1:(N-1)
    X = ones((N-i),1)*P(i,:);
    d((i+1):N,i) = sqrt( sum((X - P((i+1):N,:)).^2,2) );
end

D =d + d';


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