function [xi,w]=orthoPolyZerosFromRecur(n,a,b,c,mu0)
%%ORTHOPOLYZEROSFROMRECUR Consider a family of polynomials p_n(x) such that
%              p_n(x) is nth order and the polynomials are orthonormal with
%              respect to some weighting function w(x). This means that
%              integral_lowL^upL w(x)*p_n(x)*p_m(x) dx =1 if m=n and is
%              zero otherwise. All such polynomials can be written in
%              recursive form as
%              p_i(x)=(a(i)*x+b(i))*p_{(i-1)}(x)-c(i)*p_{i-2}(x)
%              starting with p_{-1}(x)=0 and p_0(x)=1. This function finds
%              the zeros of the nth polynomial given expressions for the
%              recursion of the polynomials. Parameters for common
%              orthogonal polynomials are given below. The zeros of such
%              orthonormal polynomials can be used as quadrature points for
%              1D integration from lowL to upL of all polynomials up to
%              order 2*n-1 times the weighting function w(x). The
%              associated quadrature weights for such integrals can also be
%              returned by this function if two outputs are requested.
%
%INPUTS: n The order of the polynomial whose zeros are desired.
%    a,b,c There are two formulations for the input. In the first one,
%          a,b, and c are vectors or function handles such that a(i), b(i),
%          and c(i) give the appropriate values for the recursive
%          formulation of the function values in
%          p_i(x)=(a(i)*x+b(i))*p_{(i-1)}(x)-c(i)*p_{i-2}(x)
%          where the values of i used in a and b go from 1 to n, and
%          indices used in  c(i) go from 2 to n. c(1) is not used.
%          Alternatively, if c is omitted or an empty matrix is passed,
%          then a and b are assumed to be coefficients in the three-term
%          expansion of the form
%          b(i)*p_i(x)=(x-a(i))*p_{(i-1)}(x)-b(i-1)*p_{i-2}(x)
%          and can be vectors or function handles. Indices of a run from 1
%          to n and b run from 1 to (n-1).
%      mu0 This parameter is only needed if the second output of this
%          function is desired. The quadrature weights must be scaled by
%          mu0=integral_lowL^upL w(x) dx. This is that mu0.
%
%OUTPUTS: xi The 1Xn set of zeros of the given set of orthonormal
%            polynomials.
%          w The set of n cubature weights associated with the orthonormal
%            polynomials. This requires the input mu0 be specified.
%
%When xi and w are used for quadrature integration, one can write
%integral_lowL^upL w(x)*f(x) dx=sum_{i=1}^n w(i)*f(xi(i))
%where the equality holds for all polynomials up to degree 2*n-1. This is
%the fundamental theorem of Gaussian quadrature. Thus, finding the zeros of
%polynomials that are orthonormal over a region of interest with respect to
%a particular weighting function can be useful.
%
%The algorithm is that of [1]. Because an eigenvalue decomposition is used,
%the results can be numerically unstable for large n.
%
%Examples of values for common polynomials, are given in [2]. Some are:
%1) Hermite polynomials. These are orthonormal with respect to exp(-x^2)
%   integrated over (-Inf,Inf). The recursion from [3] is
% mu0=sqrt(pi);
% a=@(i)2;
% b=@(i)0;
% c=@(i)2*(i-1);
%
%2) Legendre polynomials. These are orthonormal with respect to w(x)=1 for
%   integration over [-1,+1]. The recursion from [3] is
% mu0=2;
% a=@(i)(2*(i-1)+1)/i;
% b=@(i)0;
% c=@(i)(i-1)/i;
%However, the function GaussLegendrePoints1D should provide
%higher-precision zeros than this function.
%
%3) Laguerre polynomials. These are orthonormal with respect to
%   w(x)=exp(-x) for integration over [0,Inf). The recursion from [3] is
% mu0=1;
% a=@(i)-1/i;
% b=@(i)(2*(i-1)+1)/i;
% c=@(i)(i-1)/i;
%
%4) Associated Laguerre polynomials. These are orthonormal with respect to
%   w(x)=x^c1*exp(-x) for integration over [0,Inf). When considering
%   c1>-1, the recursion from [3] is
% mu0=gamma(1+c1);
% a=@(i)(-1/i);
% b=@(i)(2*i-1+c1)/i;
% c=@(i)(i-1+c1)/i;
%
%5) Gegenbauer polynomials. These are orthonormal with respect to
%   w(x)=(1-x^2)^(c1-1/2) for integration over [-1,1]. When considering
%   c1>-(1/2), the recursion from [3] is
% mu0=(sqrt(pi)*gamma((1/2)+c1))/gamma(1+c1);
% a=@(i)2*(i+c1-1)/i;
% b=@(i)0;
% c=@(i)(i+2*c1-2)/i;
%
%6) Modified Hermite polynomials that are orthogonal with respect to the
%   standard normal N(0,1) distribution integrated over (-Inf,Inf). These
%   are for w(x)=1/sqrt(2*pi)*exp(-x^2/2). The recursion [3] is
% mu0=1;
% a=@(i)1;
% b=@(i)0;
% c=@(i)i-1;
%
%REFERENCES:
%[1] G. H. Golub and J. H. Welsh, "Calculation of Gauss quadrature rules,"
%    Mathematics of Computation, vol. 23, pp. 221-230, 1969.
%[2] Weisstein, Eric W. "Orthogonal Polynomials." From MathWorld--A Wolfram
%    Web Resource. http://mathworld.wolfram.com/OrthogonalPolynomials.html
%[3] Abramowitz, M. and Stegun, I. A. (Eds.). "Orthogonal Polynomials."
%    Table 22.7 in Ch. 22 in Handbook of Mathematical Functions with
%    Formulas, Graphs, and Mathematical Tables, 9th printing. New York:
%    Dover, pp. 782, 1972.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

J=zeros(n,n);
if(nargin<4||isempty(c))
    %If alpha and beta are directly given as a and b.
    for i=1:(n-1)
        J(i,i)=a(i);
        J(i,i+1)=b(i);
        J(i+1,i)=b(i);
    end
    J(n,n)=a(n);
else
    %The first row
    %Rows 1 through n-1
    for i=1:(n-1)
        bi=b(i);
        ai=a(i);
        aiNext=a(i+1);
        ciNext=c(i+1);

        %From Equation 2.2 in [1].
        alpha=-bi/ai;
        beta=sqrt(ciNext/(ai*aiNext));

        J(i,i)=alpha;
        J(i,i+1)=beta;
        J(i+1,i)=beta;
    end
    %The final row
    i=n;
    bi=b(i);
    ai=a(i);
    alpha=-bi/ai;
    J(i,i)=alpha;
end

[V,D]=eig(J);

xi=diag(D)';
w=abs(V(1,:)').^2*mu0;

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 p = circle_imp_points_3d ( r, pc, nc, n )

%*****************************************************************************80
%
%% CIRCLE_IMP_POINTS_3D returns points on an implicit circle in 3D.
%
%  Discussion:
%
%    Points P on an implicit circle in 3D satisfy the equations:
%
%      ( P(1) - PC(1) )^2
%    + ( P(2) - PC(2) )^2
%    + ( P(3) - PC(3) )^2 = R^2
%
%    and
%
%      ( P(1) - PC(1) ) * NC(1)
%    + ( P(2) - PC(2) ) * NC(2)
%    + ( P(3) - PC(3) ) * NC(3) = 0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the circle.
%
%    Input, real PC(3,1), the center of the circle.
%
%    Input, real NC(3,1), a nonzero vector that is normal to
%    the plane of the circle.  It is customary, but not necessary,
%    that this vector have unit norm.
%
%    Input, integer N, the number of points desired.
%    N must be at least 1.
%
%    Output, real P(3,N), the coordinates of points
%    on the circle.
%

%
%  Get two unit vectors N1 and N2 which are orthogonal to each other,
%  and to NC.
%
  [ n1, n2 ] = plane_normal_basis_3d ( pc, nc );
%
%  Rotate R units away from PC in the plane of N1 and N2.
%
  for i = 1 : n

    theta = ( 2.0 * pi * ( i - 1 ) ) / n;

    p(1:3,i) = ( pc(1:3,1) ...
      + r * ( cos ( theta ) * n1(1:3,1) ...
            + sin ( theta ) * n2(1:3,1) ) );

  end

  return
end


function[varargout]=cum2mom(varargin)
%CUM2MOM  Convert cumulants to moments.
%
%   [M0,M1,...MN]=CUM2MOM(K0,K1,...KN) converts the first N cumulants
%   K0,K1,...KN into the first N moments M0,M1,...MN.
%
%   The KN and MN are all scalars or arrays of the same size.
%
%   Note for a probability density function, M0=1 and K0=0.
%
%   MCELL=CUM2MOM(KCELL), where KCELL is a cell array whose (N+1)th
%   is the Nth cumulant, returns a similar cell array of moments.
%
%   CUM2MOM is inverted by MOM2CUM.
%
%   See also MOM2CUM, BELLPOLY.
%
%   Usage: [m0,m1,m2]=cum2mom(k0,k1,k2);
%          mcell=cum2mom(kcell);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2008 J.M. Lilly --- type 'help jlab_license' for details

%Tests for CUM2MOM are run by MOM2CUM.
if strcmpi(varargin{1}, '--t'),return,end

if nargin==1&&iscell(varargin{1})
    cum=varargin{1};
else
    cum=varargin;
end
mom=cell(size(cum));
mom{1}=exp(cum{1});

cum=cum(2:end);
mom(2:end)=bellpoly(cum);

for i=2:length(mom)
    mom{i}=mom{i}.*mom{1};
end

if nargin==1&&iscell(varargin{1})
    varargout{1}=mom;
else
    varargout=mom;
end


function proj = projPointOnEllipse(point, elli)
%PROJPOINTONELLIPSE Project a point orthogonally onto an ellipse.
%
%   PROJ = projPointOnEllipse(PT, ELLI)
%   Computes the (orthogonal) projection of point PT onto the ellipse given
%   by ELLI.
%
%
%   Example
%     % create an ellipse and a point
%     elli = [50 50 40 20 30];
%     pt = [60 10];
%     % display reference figures
%     figure; hold on; drawEllipse(elli, 'b');
%     axis equal; axis([0 100 0 100]);
%     drawPoint(pt, 'bo');
%     % compute projected point
%     proj = projPointOnEllipse(pt, elli);
%     drawEdge([pt proj], 'b');
%     drawPoint(proj, 'k*');
%
%   See also
%     ellipses2d, distancePointEllipse, projPointOnLine, ellipsePoint
%     drawEllipse
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2022-07-17, using Matlab 9.12.0.1884302 (R2022a)
% Copyright 2022 INRAE - BIA Research Unit - BIBS Platform (Nantes)

% defaults
nMaxIters = 4;

% compute transform to centered axis-aligned ellipse
center = elli(1:2);
theta = deg2rad(elli(5));
transfo = createRotation(-theta) * createTranslation(-center);
pointT = transformPoint(point, transfo);

% retrieve ellipse semi axis lengths
a = elli(3);
b = elli(4);

% keep absolute values
px = abs(pointT(:,1));
py = abs(pointT(:,2));

% initial guess of solution
tx = 0.707;
ty = 0.707;

% iterate
for i = 1:nMaxIters
    x = a * tx;
    y = b * ty;

    ex = (a*a - b*b) * power(tx, 3) / a;
    ey = (b*b - a*a) * power(ty, 3) / b;

    rx = x - ex;
    ry = y - ey;

    qx = px - ex;
    qy = py - ey;

    r = hypot(ry, rx);
    q = hypot(qy, qx);

    tx = min(1, max(0, (qx .* r ./ q + ex) / a));
    ty = min(1, max(0, (qy .* r ./ q + ey) / b));
    t = hypot(ty, tx);
    tx = tx ./ t;
    ty = ty ./ t;

end

% computes coordinates of projection
projX = a * tx;
projY = b * ty;

% fix sign
projX(pointT(:,1) < 0) = -projX(pointT(:,1) < 0);
projY(pointT(:,2) < 0) = -projY(pointT(:,2) < 0);

% project pack to basis of original ellipse
proj = transformPoint([projX projY], inv(transfo));


function sigma_n = sigma ( n )

%*****************************************************************************80
%
%% SIGMA returns the value of SIGMA(N), the divisor sum.
%
%  Definition:
%
%    SIGMA(N) is the sum of the distinct divisors of N, including 1 and N.
%
%  First values:
%
%     N  SIGMA(N)
%
%     1    1
%     2    3
%     3    4
%     4    7
%     5    6
%     6   12
%     7    8
%     8   15
%     9   13
%    10   18
%    11   12
%    12   28
%    13   14
%    14   24
%    15   24
%    16   31
%    17   18
%    18   39
%    19   20
%    20   42
%
%  Formula:
%
%    SIGMA(U*V) = SIGMA(U) * SIGMA(V) if U and V are relatively prime.
%
%    SIGMA(P^K) = ( P^(K+1) - 1 ) / ( P - 1 ) if P is prime.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the value to be analyzed.
%
%    Output, integer SIGMA_N, the value of SIGMA(N).  If N is less than
%    or equal to 0, SIGMA_N will be returned as 0.  If there is not
%    enough room for factoring N, SIGMA_N is returned as -1.
%
  maxfactor = 20;

  if ( n <= 0 )
    sigma_n = 0;
    return
  end

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

  if ( nleft ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SIGMA - Fatal error!\n' );
    fprintf ( 1, '  Not enough factorization space.\n' );
    sigma_n = -1;
    error ( 'SIGMA - Fatal error!' );
  end

  sigma_n = 1;
  for i = 1 : nfactor
    sigma_n = ( sigma_n * ( factor(i)^( power(i) + 1 ) - 1 ) ) ...
      / ( factor(i) - 1 );
  end

  return
end


function [ x, err, iter, flag ] = sor(A, x, b, w, max_it, tol)
%
% [ x, err, iter, flag ] = sor(A, x, b, w, max_it, tol)
%
% SOR Successive Over-Relaxation Method
%   This function solves linear equation systems such as Ax=b using SOR
%   method (Successive Over-Relaxation).
%   When the relaxation scalar w=1, the method used is Gauss-Seidel.
%
% Input:
%   A - input matrix
%   x - inicial vector
%   b - vector b
%   w - relaxation scalar
%   max_it - maximum number of iterations
%   tol - tolerance
%
% Output:
%   x - solution vector
%   err - norm err estimate
%   iter - nu,ber of performed iterations
%   flag - 0 = a solution was found / 1 = no convergence for max_it
%
% Example:
%   [ x, err, iter, flag ] = sor( [.5 .125; .125 .25], zeros(2,1),
%       ones(2,1), .5, 1e4, 1e-4 )
%
% Author:	Tashi Ravach
% Version:	1.0
% Date:     08/06/2006
%

    if nargin == 3
        w = .5;
        max_it = 1e4;
        tol = 1e-4;
    elseif nargin == 4
        max_it = 1e4;
        tol = 1e-4;
    elseif nargin == 5
        tol = 1e-4;
    elseif nargin ~= 6
        error('sor: invalid input parameters');
    end

    flag = 0;
    iter = 0;

    norma2_b = norm(b);
    if (norma2_b == 0.0)
        norma2_b = 1.0;
    end

    r = b - A * x;
    err = norm(r) / norma2_b;
    if (err < tol)
        return
    end

    % separate A into several matrix for SOR/Gauss-Seidel
    [ M, N, b ] = matsep(A, b, w, 2);

    for iter = 1 : max_it
        x_1 = x;
        x = M \ (N * x + b); % adjust the aproximation
        %err = norm(x - x_1) / norm(x); % compute error
        err = norm(x_1 - x, 1); % compute error
        if (err <= tol) % check for convergence
            break
        end
    end
    b = b / w; % vector b

    if (err > tol) % no convergence
        flag = 1;
    end

end

function [ M, N, b ] = matsep(A, b, w, flag)
%
% [ M, N, b ] = matsep(A, b, w, flag)
%
% MATSEP Matrix Separation
%   Input matrix is splitted into several others in diferent ways depending
%   on the method to be used: Jacobi and SOR (Gauss-Seidel when w = 1)
%
% Input:
%   A - input matrix
%   x - inicial vector
%   b - vector b
%   flag - 1 = Jacobi / 2 = SOR
%
% Output:
%   M - matrix M
%   N - matrix N
%   b - vector b (modified for SOR)
%
% Author:	Tashi Ravach
% Version:	1.0
% Date:     08/06/2006
%

    if nargin ~= 4
        error('matsep: invalid input parameters');
    end

    [ m, n ] = size(A);
    if m ~= n
        error('matsep: input matrix A must have dimension nXn');
    end

    [ l, o ] = size(b);
    if l ~= n && o ~= 1
        error('matsep: input matrix b must have dimension nX1');
    end

    if (flag == 1) % separation for Jacobi
        M = diag(diag(A));
        N = diag(diag(A)) - A;
    elseif (flag == 2) % separation for SOR/Gauss-Seidel
        b = w * b;
        M =  w * tril(A, -1) + diag(diag(A));
        N = -w * triu(A,  1) + (1.0 - w) * diag(diag(A));
    end

end


function value = torus_square_volume_3d ( r1, r2 )

%*****************************************************************************80
%
%% TORUS_SQUARE_VOLUME_3D returns the volume of a square torus in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that:
%
%      R1 - R2 <= SQRT ( X**2 + Y**2 ) <= R1 + R2,
%      -R2 <= Z <= R2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R1, R2, the two radii that define the torus.
%
%    Output, real TORUS_SQUARE_VOLUME_3D, the volume of the torus.
%
  value = 8.0E+00 * pi * r1 * r2 * r2;

  return
end


function mean = binomial_mean ( a, b )

%*****************************************************************************80
%
%% BINOMIAL_MEAN returns the mean of the Binomial PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer A, the number of trials.
%    1 <= A.
%
%    Input, real B, the probability of success on one trial.
%    0.0 <= B <= 1.0.
%
%    Output, real MEAN, the expected value of the number of
%    successes in A trials.
%
  mean = a * b;

  return
end


function w = faddeeva(z,N)
% FADDEEVA   Faddeeva function
%   W = FADDEEVA(Z) is the Faddeeva function, aka the plasma dispersion
%   function, for each element of Z. The Faddeeva function is defined as:
%
%     w(z) = exp(-z^2) * erfc(-j*z)
%
%   where erfc(x) is the complex complementary error function.
%
%   W = FADDEEVA(Z,N) can be used to explicitly specify the number of terms
%   to truncate the expansion (see (13) in [1]). N = 16 is used as default.
%
%   Example:
%       x = linspace(-10,10,1001); [X,Y] = meshgrid(x,x);
%       W = faddeeva(complex(X,Y));
%       figure;
%       subplot(121); imagesc(x,x,real(W)); axis xy square; caxis([-1 1]);
%       title('re(faddeeva(z))'); xlabel('re(z)'); ylabel('im(z)');
%       subplot(122); imagesc(x,x,imag(W)); axis xy square; caxis([-1 1]);
%       title('im(faddeeva(z))'); xlabel('re(z)'); ylabel('im(z)');
%
%   Reference:
%   [1] J.A.C. Weideman, "Computation of the Complex Error Function," SIAM
%       J. Numerical Analysis, pp. 1497-1518, No. 5, Vol. 31, Oct., 1994
%       Available Online: http://www.jstor.org/stable/2158232

if nargin<2, N = []; end
if isempty(N), N = 16; end

w = zeros(size(z)); % initialize output

%%%%%
% for purely imaginary-valued inputs, use erf as is if z is real
idx = real(z)==0; %
w(idx) = exp(-z(idx).^2).*erfc(imag(z(idx)));

if all(idx), return; end
idx = ~idx;

%%%%%
% for complex-valued inputs

% make sure all points are in the upper half-plane (positive imag. values)
idx1 = idx & imag(z)<0;
z(idx1) = conj(z(idx1));

M = 2*N;
M2 = 2*M;
k = (-M+1:1:M-1)'; % M2 = no. of sampling points.
L = sqrt(N/sqrt(2)); % Optimal choice of L.

theta = k*pi/M;
t = L*tan(theta/2); % Variables theta and t.
f = exp(-t.^2).*(L^2+t.^2);
f = [0; f]; % Function to be transformed.
a = real(fft(fftshift(f)))/M2; % Coefficients of transform.
a = flipud(a(2:N+1)); % Reorder coefficients.

Z = (L+1i*z(idx))./(L-1i*z(idx));
p = polyval(a,Z); % Polynomial evaluation.
w(idx) = 2*p./(L-1i*z(idx)).^2 + (1/sqrt(pi))./(L-1i*z(idx)); % Evaluate w(z).

% convert the upper half-plane results to the lower half-plane if necesary
w(idx1) = conj(2*exp(-z(idx1).^2) - w(idx1));


%% SQUAREPOISSON Poisson equation in a square domain.
%
%   squarePoisson computes approximations of the Poisson equation in the
%   unit square on a sequence of meshes obtained by uniform refinement. It
%   plots the approximation error (in L2 norm or H1 norm) vs the number of
%   nodes.
%
% See also Poisson, crack, Lshape
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all; clear all;
%% Parameters
maxIt = 4; N = zeros(maxIt,1);
erruIuh = zeros(maxIt,1);

%% Generate an initial mesh
[node,elem] = squaremesh([0 1 0 1], 0.25);
%bdFlag = setboundary(node,elem,'Dirichlet','~(x==0)','Neumann','x==0');
bdFlag = setboundary(node,elem,'Dirichlet','all','Neumann','y==1');
%bdFlag = setboundary(node,elem,'Dirichlet');
for k = 1:3
    [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%     [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
end

%% Get the data of the pde
pde = sincosdata;
% pde = mixBCdata;
option.solver = 'mg';

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    [u,Du,eqn] = PoissonWG(node,elem,pde,bdFlag,option);
    N(k) = size(elem,1);
    % compute error
    uI = zeros(N(k)+size(eqn.edge,1),1);
    uI(1:N(k)) = pde.exactu((node(elem(:,1),:)+node(elem(:,2),:)+node(elem(:,3),:))/3);
    uI(N(k)+1:end) = pde.exactu((node(eqn.edge(:,1),:)+node(eqn.edge(:,2),:))/2);
    erruIuh(k) = sqrt((u-uI)'*eqn.A*(u-uI));
    % refine mesh
   [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%     [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
end

%% Plot convergence rates
figure(2);
showrate(N,erruIuh);
%% Error table
% The optimal rate of convergence of the H1-norm (1st order) and L2-norm
% (2nd order) is observed. The 2nd order convergent rate between two
% discrete functions ||DuI-Duh|| is known as superconvergence.


% Excersize 1 - Calculate the equilibrium surface temperature of an icy
% body (e.g. a comet) at a given distance from the Sun.

% This sample program is a script that balances the equation of heat
% transfer on the surface of a ball made of ice, to find an equilibrium
% temperature. This script demonstrates how physical units can be used in
% calculations.

%% Set up the workspace with needed variables
si=setUnits;
% We begin by calling the |setUnits| function in order to get the interface
% structure that holds all predefined units.

LSun=3.8e26*si.joule/si.second;
% Total brightness of the Sun. This is the energy radiated by the Sun every
% second. (Tip: si.s is shorthand for si.second.)

dSun=2.5*1.496e11*si.meter;
% Distance from the Sun to the body whose temperature we must calculate. I
% have chosen a distance of 2.5 astronomical units (Earth-Sun distances)
% that puts our "comet" betwen Mars and Jupiter. (Tip: si.astronomical_unit
% is already defined, and si.AU is even easier. Also, si.m is shorthand for
% si.meter.)

KB=1.3806e-23*si.joule/si.K;
% Boltzmann's constant. (Tip: si.boltzmann is defined.)

stefan=5.67e-8*si.joule/si.m^2/si.s/si.K^4;
% The Stefan-Boltzmann constant. (Tip: Guess what, si.stefan_boltzmann.)

Hice=2.8345e6*si.joule/si.kg;
% Latent heat for sublimation of ice.

A=3.56e12*si.newton/si.m^2; B=6141.667*si.kelvin;
% These parameters are used in an empirical formula for the saturation
% vapor pressure. (Tip: si.N and si.K are shorter.)

avogad=6.022e23*si.mole^(-1);
% Avogadro's number. (Tip: Try si.avogadro.)

mH2O=18*si.g/si.mole;
% The molecular weight of water.

%% Now, Lion-hunt for the temperature of equilibrium
% When we write down the equation:
% radiant energy from the Sun = (radiant + sublimation)energy from the body
% we essentially get an implicit function of temperature vs. distance from
% the Sun. This function however cannot be inverted so we need to hunt for
% the temperature that will balance the equation.

tol=1e-3;
% We'll use this as the relative accuracy.

Tlo=1*si.K; Thi=400*si.K;
% The desired temperature is definitely bracketed by these values.

while (Thi-Tlo)/Thi>tol
    T=(Tlo+Thi)/2;
    fT=(stefan*T^4)+(Hice*sqrt(mH2O/(2*avogad*pi*KB*T))*A*exp(-(B/T)))-...
        (0.25*(LSun/(4*pi*dSun^2)));
    if double(fT)>0
        Thi=T;
    else
        Tlo=T;
    end
end

%% That's it. The desired temperature is
fprintf('The equilibrium temperature is\n')
T
% Try to introduce an easy-to-make-hard-to-detect error in one of the above
% statements, and run the script again. Instead of getting a wrong answer
% you will probably get an informative error message that will help you
% trace the "bug".


function C = xeucn( A, T, shape )
% n-dimensional euclidean distance between each window in A and template T.
%
% Similar to normxcorrn, except at each point (i,j) calculates the
% euclidean distance between the T and the window in A surrounding the
% point, storing the result in C(i,j).
%
% USAGE
%  C = xeucn( A, T, [shape] )
%
% INPUTS
%  A           - first d-dimensional matrix
%  T           - second d-dimensional matrix
%  shape       - ['full'] 'valid', or 'same' (see convn)
%
% OUTPUTS
%  C           - correlation matrix
%
% EXAMPLE
%  T=gaussSmooth(rand(20),2); A=repmat(T,[3 3]);
%  C1=normxcorrn(T,A);  C2=xcorrn(A,T);  C3=xeucn(A,T);
%  figure(1); im(C1);  figure(2); im(C2);  figure(3); im(-C3);
%
% See also XCORRN, CONVNFAST
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<3 || isempty(shape)); shape='full'; end
nd = ndims(T);
if(nd~=ndims(A)); error('xeucn: T and A must have same ndims'); end;

% flip for conv purposes
if(nd==2); T=rot90(T,2); else for d=1:nd; T=flipdim(T,d); end; end

% The expression for euclidean distance can be rewritten as:
%   D(k,l) = sumj( (Akj - Tlj).^2 )
%          = sumj( Akj.^2 ) + sumj( Tlj.^2 ) - 2*sumj(Akj.*Tlj);
% T is constant.  Hence simply need square of A in each window, as
% well as each dot product between A and T.
Amag = localSum( A.*A, size(T), shape );     % sum of squares per window
Tmag = T.^2;  Tmag = sum( Tmag(:) );         % sum of squares of T
C = Amag + Tmag - 2 * convnFast(A,T,shape);  % distance squared
% C( Amag<.01 ) = Tmag;  % prevent numerical errors
C = real(sqrt(real(C)));


function h=spreadop(f,coef)
%SPREADOP  Spreading operator
%   Usage: h=spreadop(f,c);
%
%   `spreadop(f,c)` applies the operator with spreading function *c* to the
%   input *f*. *c* must be square.
%
%   `spreadop(f,c)` computes the following for *c* of size $L \times L$:
%
%   ..         L-1 L-1
%     h(l+1) = sum sum c(m+1,n+1)*exp(2*pi*i*l*m/L)*f(l-n+1)
%              n=0 m=0
%
%   .. math:: h\left(l+1\right)=\sum_{n=0}^{L-1}\sum_{m=0}^{L-1}c\left(m+1,n+1\right)e^{2{\pi}ilm/L}f\left(l-n+1\right)
%
%   where $l=0,\ldots,L-1$ and $l-n$ is computed modulo *L*.
%
%   The combined symbol of two spreading operators can be found by using
%   `tconv`. Consider two symbols *c1* and *c2* and define *f1* and *f2* by::
%
%     h  = tconv(c1,c2)
%     f1 = spreadop(spreadop(f,c2),c1);
%     f2 = spreadop(f,h);
%
%   then *f1* and *f2* are equal.
%
%   See also:  tconv, spreadfun, spreadinv, spreadadj
%
%   References: feko98

%   AUTHOR: Peter L. Søndergaard
%   TESTING: TEST_SPREAD
%   REFERENCE: REF_SPREADOP

complainif_argnonotinrange(nargin,2,2,mfilename);

if ndims(coef)>2 || size(coef,1)~=size(coef,2)
    error('Input symbol coef must be a square matrix.');
end;

L=size(coef,1);

% Change f to correct shape.
[f,Ls,W,wasrow,remembershape]=comp_sigreshape_pre(f,'DGT',0);

f=postpad(f,L);

h=zeros(L,W);

if issparse(coef) && nnz(coef)<L

    % The symbol is so sparse that the straighforward definition is
    % the fastest way to apply it.

    [mr,nr,cv]=find(coef);
    h=zeros(L,W);

    % We need mr and nr to be zero-indexed
    mr=mr-1;
    nr=nr-1;

    % This is the basic idea of the routine below
    %for ii=1:length(mr)
    %  for l=0:L-1
    %	h(l+1,:)=h(l+1,:)+cv(ii)*exp(-2*pi*i*l*mr(ii)/L)*f(mod(l-nr(ii),L)+1,:);
    %  end;
    %end;

    l=(-2*pi*i*(0:L-1)/L).';
    for ii=1:length(mr)
        bigmod=repmat(exp(l*mr(ii)),1,W);
        h=h+cv(ii)*(bigmod.*circshift(f,nr(ii)));
    end;

else

  % This version only touches coef one column at a time, and it suited
  % if coef is sparse.

  if issparse(coef)
    for n=0:L-1
      % The 'full' below is required for Matlab compatibility, as
      % Matlab refuses to do an ifft of a sparse matrix.
      cf=ifft(full(coef(:,n+1)))*L;
      modind=mod((0:L-1).'-n,L)+1;
      h=h+repmat(cf,1,W).*f(modind,:);
    end;
  else
    for n=0:L-1
      cf=ifft(coef(:,n+1))*L;
      modind=mod((0:L-1).'-n,L)+1;
      h=h+repmat(cf,1,W).*f(modind,:);
    end;
  end;

end;


function colin = points_colin_2d ( p1, p2, p3 )

%*****************************************************************************80
%
%% POINTS_COLIN_2D estimates the colinearity of 3 points in 2D.
%
%  Discussion:
%
%    The estimate of colinearity that is returned is the ratio
%    of the area of the triangle spanned by the points to the area
%    of the equilateral triangle with the same perimeter.
%
%    This estimate is 1 if the points are maximally noncolinear, 0 if the
%    points are exactly colinear, and otherwise is closer to 1 or 0 depending
%    on whether the points are far or close to colinearity.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(2), P2(2), P3(2), the points.
%
%    Output, real COLIN, the colinearity estimate.
%
  dim_num = 2;

  t(1:dim_num,1:3) = [ p1(1:dim_num); p2(1:dim_num); p3(1:dim_num) ]';

  area_triangle = triangle_area_2d ( t );

  if ( area_triangle == 0.0 )

    colin = 0.0;

  else

    perim = sqrt ( sum ( ( p2(1:dim_num) - p1(1:dim_num) ).^2 ) ) ...
          + sqrt ( sum ( ( p3(1:dim_num) - p2(1:dim_num) ).^2 ) ) ...
          + sqrt ( sum ( ( p1(1:dim_num) - p3(1:dim_num) ).^2 ) );

    side = perim / 3.0;

    area2 = 0.25 * sqrt ( 3.0 ) * side * side;

    colin = abs ( area_triangle ) / area2;

  end

  return
end


function ps = normpdf2( xs, m, C )
% Normal prob. density function (pdf) with arbitrary covariance matrix.
%
% Evaluate the multi-variate density with mean vector m and covariance
% matrix C for the input vector xs.  Assumes that the N datapoints are d
% dimensional.  Then m is dx1 or 1xd, C is dxd, and xs is dxN or NxD where
% N is the number of samples to be evaluated.
%
% USAGE
%  ps = normpdf2( xs, m, C )
%
% INPUTS
%  xs  - points to evaluated (Nxd or dxN)
%  m   - mean vector (dx1 or 1xd)
%  C   - Covariance matrix (dxd)
%
% OUTPUTS
%  ps  - probability density at each x (Nx1)
%
% EXAMPLE
%  ps = normpdf2( randn(10,2), [0 0], eye(2) )
%
% See also NORMPDF
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.30
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

% get dimensions of data
d=length(m);
if( size(xs,1)~=d ); xs=xs'; end
N=size(xs,2);

if( d==1 ) % fast special case
  ps = 1/sqrt(2*pi*C) * exp(-(xs-m).*(xs-m)/(2*C))';

elseif( rcond(C)<eps ) % if matrix is badly conditioned
  warning('normpdf2: Covariance matrix close to singular.'); %#ok<WNTAG>
  ps = zeros(N,1);

else % get probabilities
  xs = (xs-m(:)*ones(1,N))';
  denom = (2*pi)^(d/2)*sqrt(abs(det(C)));
  mahal = sum( (xs/C).*xs, 2 );
  numer = exp(-0.5*mahal);
  ps = numer/denom;
end


function [ x, y, w ] = legendre_2d_set ( a, b, nx, ny )

%*****************************************************************************80
%
%% LEGENDRE_2D_SET: set a 2D Gauss-Legendre quadrature rule.
%
%  Discussion:
%
%    The integral:
%
%      integral ( a(2) <= y <= b(2) ) ( a(1) <= x <= b(1) ) f(x,y) dx dy
%
%    The quadrature rule:
%
%      sum ( 1 <= i <= n ) w(i) * f ( x(i),y(i) )
%
%    where n = nx * ny, the orders of the rule in the X and Y directions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A(2), B(2), the lower and upper integration
%    limits.
%
%    Input, integer NX, NY, the orders in the X and Y directions.
%    NX and NY must be between 1 and 10.
%
%    Output, real X(N), Y(N), the abscissas.
%
%    Output, real W(N), the weights.
%

%
%  Get the rules for [-1,+1].
%
  [ xx, wx ] = legendre_set ( nx );
  [ yy, wy ] = legendre_set ( ny );
%
%  Adjust from [-1,+1] to [A,B].
%
  for i = 1 : nx
    xx(i) = ( ( 1.0 - xx(i) ) * a(1)   ...
            + ( 1.0 + xx(i) ) * b(1) ) ...
            /   2.0;
    wx(i) = wx(i) * ( b(1) - a(1) ) / 2.0;
  end

  for j = 1 : ny
    yy(j) = ( ( 1.0 - yy(j) ) * a(2)   ...
            + ( 1.0 + yy(j) ) * b(2) ) ...
            /   2.0;
    wy(j) = wy(j) * ( b(2) - a(2) ) / 2.0;
  end
%
%  Compute the product rule.
%
  n = nx * ny;
  x = zeros ( n, 1 );
  y = zeros ( n, 1 );
  w = zeros ( n, 1 );

  k = 0;
  for j = 1 : ny
    for i = 1 : nx
      k = k + 1;
      x(k) = xx(i);
      y(k) = yy(j);
      w(k) = wx(i) * wy(j);
    end
  end

  return
end


function W = compute_mesh_weight(vertex,face,type,options)

% compute_mesh_weight - compute a weight matrix
%
%   W = compute_mesh_weight(vertex,face,type,options);
%
%   W is sparse weight matrix and W(i,j)=0 is vertex i and vertex j are not
%   connected in the mesh.
%
%   type is either
%       'combinatorial': W(i,j)=1 is vertex i is conntected to vertex j.
%       'distance': W(i,j) = 1/d_ij^2 where d_ij is distance between vertex
%           i and j.
%       'conformal': W(i,j) = cot(alpha_ij)+cot(beta_ij) where alpha_ij and
%           beta_ij are the adjacent angle to edge (i,j)
%
%   If options.normalize=1, the the rows of W are normalize to sum to 1.
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
[vertex,face] = check_face_vertex(vertex,face);

nface = size(face,1);
n = max(max(face));

if isfield(options, 'verb')
    verb = options.verb;
else
    verb = n>5000;
end

if nargin<3
    type = 'conformal';
end

switch lower(type)
    case 'combinatorial'
        W = triangulation2adjacency(face);
    case 'distance'
        W = my_euclidean_distance(triangulation2adjacency(face),vertex);
        W(W>0) = 1./W(W>0);
        W = (W+W')/2;
    case 'conformal'
        % conformal laplacian
        W = sparse(n,n);
        ring = compute_vertex_face_ring(face);
        for i = 1:n
            if verb
                progressbar(i,n);
            end
            for b = ring{i}
                % b is a face adjacent to a
                bf = face(:,b);
                % compute complementary vertices
                if bf(1)==i
                    v = bf(2:3);
                elseif bf(2)==i
                    v = bf([1 3]);
                elseif bf(3)==i
                    v = bf(1:2);
                else
                    error('Problem in face ring.');
                end
                j = v(1); k = v(2);
                vi = vertex(:,i);
                vj = vertex(:,j);
                vk = vertex(:,k);
                % angles
                alpha = myangle(vk-vi,vk-vj);
                beta = myangle(vj-vi,vj-vk);
                % add weight
                W(i,j) = W(i,j) + cot( alpha );
                W(i,k) = W(i,k) + cot( beta );
            end
        end
    otherwise
        error('Unknown type.')
end

if isfield(options, 'normalize') && options.normalize==1
    W = diag(sum(W,2).^(-1)) * W;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function beta = myangle(u,v);

du = sqrt( sum(u.^2) );
dv = sqrt( sum(v.^2) );
du = max(du,eps); dv = max(dv,eps);
beta = acos( sum(u.*v) / (du*dv) );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function W = my_euclidean_distance(A,vertex)

if size(vertex,1)<size(vertex,2)
    vertex = vertex';
end

[i,j,s] = find(sparse(A));
d = sum( (vertex(i,:) - vertex(j,:)).^2, 2);
W = sparse(i,j,d);


function value = atan4 ( y, x )

%*****************************************************************************80
%
%% ATAN4 computes the inverse tangent of the ratio Y / X.
%
%  Discussion:
%
%    ATAN4 returns an angle whose tangent is ( Y / X ), a job which
%    the built in functions ATAN and ATAN2 already do.
%
%    However:
%
%    * ATAN4 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;
%
%    * ATAN4 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 ATAN4, 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.0E+00 )

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

  elseif ( y == 0.0E+00 )

    if ( 0.0E+00 < x )
      value = 0.0E+00;
    elseif ( x < 0.0E+00 )
      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.0E+00 < x & 0.0E+00 < y )
      value = theta_0;
    elseif ( x < 0.0E+00 & 0.0E+00 < y )
      value = pi - theta_0;
    elseif ( x < 0.0E+00 & y < 0.0E+00 )
      value = pi + theta_0;
    elseif ( 0.0E+00 < x & y < 0.0E+00 )
      value = 2.0E+00 * pi - theta_0;
    end

  end

  return
end


function [A, names] = mk_adj_mat(connections, names, topological)
% MK_ADJ_MAT Make a directed adjacency matrix from a list of connections between named nodes.
%
% A = mk_adj_mat(connections, name)
% This is best explaine by an example:
%   names = {'WetGrass', 'Sprinkler', 'Cloudy', 'Rain'};
%   connections = {'Cloudy', 'Sprinkler'; 'Cloudy', 'Rain'; 'Sprinkler', 'WetGrass'; 'Rain', 'WetGrass'};
% adds the arcs C -> S, C -> R, S -> W, R -> W. Node 1 is W, 2 is S, 3 is C, 4 is R.
%
% [A, names] = mk_adj_mat(connections, name, 1)
% The last argument of 1 indicates that we should topologically sort the nodes (parents before children).
% In the example, the numbering becomes: node 1 is C, 2 is R, 3 is S, 4 is W
% and the return value of names gets permuted to {'Cloudy', 'Rain', 'Sprinkler', 'WetGrass'}.
% Note that topological sorting the graph is only possible if it has no directed cycles.

if nargin < 3, topological = 0; end

n=length(names);
A=zeros(n);
[nr nc] = size(connections);
for r=1:nr
  from = strmatch(connections{r,1}, names, 'exact');
  assert(~isempty(from));
  to = strmatch(connections{r,2}, names, 'exact');
  assert(~isempty(to));
  %fprintf(1, 'from %s %d to %s %d\n', connections{r,1}, from, connections{r,2}, to);
  A(from,to) = 1;
end

if topological
  order = topological_sort(A);
  A = A(order, order);
  names = names(order);
end


function [g]=gsp_design_expwin(G, bmax, a)
%GSP_DESIGN_EXPWIN create an expwin window of lenth N with parameter a
%   Usage: g = gsp_design_expwin(G);
%          g = gsp_design_expwin(G, bmax);
%          g = gsp_design_expwin(G, bmax, a);
%
%   Input parameters:
%       G       : Graph structure
%       bmax    : Maximum relative band (default 0.2)
%       a       : Slope parameter (default 1).
%
%   Output parameters
%       g       : filter
%
%   This function design the following filter:
%
%   .. g(x) = s( (1-x) / bmax /lmax )
%
%   .. math:: g(x) = s\left(\frac{1-x}{\lambda_{\text{max}} b_{\text{max}} }\right)
%
%   where  $s(x)$ is the step function
%
%   ..          /   0                                      if x < -1
%   ..  s(x) = | exp(-a/x) / ( exp(-a/x) + exp(-a/(1-x)) ) if x in [-1, 1]
%   ..          \   1                                      if x > 1
%
%   .. math:: s(x)=\begin{cases} 0 & \mbox{if }x<-1 \\ \frac{e^{-\frac{a}{x}}}{e^{-\frac{a}{x}}+e^{-\frac{a}{1-x}}} & \mbox{if }x\in[-1,1]\\ 1 & \mbox{if }x>1 \end{cases}
%
%   It uses a clever exponential construction to obtain an infinitely
%   differentiable function that is band limited!
%
%   This function will compute the maximum eigenvalue of the laplacian. To
%   be more efficient, you can precompute it using::
%
%       G = gsp_estimate_lmax(G);
%
%   Example:::
%
%         Nf = 4;
%         G = gsp_sensor(100);
%         G = gsp_estimate_lmax(G);
%         g = gsp_design_expwin(G);
%         gsp_plot_filter(G,g);
%

% Author: Nathanael Perraudin
% Date  : 18 December 2014

if nargin<3
    a = 1;
end

if nargin<2
    bmax = 0.2;
end

if numel(bmax)>1
    g = cell(numel(bmax),1);
    for ii = 1:numel(bmax);
        g{ii} = gsp_design_expwin(G,bmax(ii),a);
    end
    return
end

if isstruct(G)
    if ~isfield(G,'lmax')
%         if param.verbose
            fprintf('GSP_DESIGN_EXPWIN has to compute lmax \n')
%         end
        G = gsp_estimate_lmax(G);
    end
   lmax = G.lmax;
else
   lmax = G;
end

g = @(x) gsp_smooth_downstep(x/bmax/lmax, a , 1);

end


function K = intmat(N, varargin)
%INTMAT   Spectral integration matrix.
%   K = INTMAT(N) returns the N x N integration matrix associated with the
%   Chebyshev spectral collocation method at second-kind Chebyshev points.
%
%   K = INTMAT(N, P) returns the N x N integration matrix of order P.
%
%   K = INTMAT(N, P, DOM) scales the integration matrix K to the domain
%   DOM. DOM should be a 1x2 vector.
%
%   K = INTMAT([M N]) returns the M x N first-order rectangular integration
%   matrix which maps from an N-point Chebyshev grid of the second kind to an
%   M-point Chebyshev grid of the same kind.
%
%   K = INTMAT([M N], P) returns an M x N rectangular integration matrix of
%   order P which maps from an N-point to an M-point Chebyshev grid, both of
%   second kind.
%
%   K = INTMAT([M N], P, DOM) returns the same K but scaled to the domain DOM.
%
% See also CUMSUMMAT, DIFFMAT, DIFFROW, INTROW.

% This code has been written for Aurentz and Trefethen,
% "Block operators and spectral discretizations", and is not
% yet fully compliant with standard Chebfun coding practices.

% Copyright 2015 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Parse inputs
[m, n, p, dom] = parseInputs(N, varargin{:});

% Build Lagrange basis
K = chebfun(eye(n),dom);

% Integrate p times
K = cumsum(K,p);

% Evaluate at grid of size m
K = feval(K,chebpts(m,dom));

end

function [m, n, p, dom] = parseInputs(N, varargin)
% Parse the inputs to INTMAT.

p = 1;
dom = [-1 1];

if ( isscalar(N) )
    n = N;
    m = n;
else
    m = N(1);
    n = N(2);
end

for j = 1:numel(varargin)
    v = varargin{j};
    if ( isnumeric(v) )
        if ( isscalar(v) )
            p = v;
        else
            dom = v;
        end
    else
        error('CHEBFUN:intmat:unknown', ...
            'Unknown input of type %s.', class(v));
    end
end

end


function [ Nreg ] = eigRegularizer( N, k )

% -----------------------------------------------------------------
% matrizer regularization by checking eigenvalues
% -----------------------------------------------------------------
%
% N:   input square matrix
% k:   required condition number

% -----------------------------------------------------------------

[V, D] = eig(N); 					 % eigenvectors and eigenvalues
e = diag(D);                         % eigenvalues (ascending order)

% k = 1e6;                           % condition number
% pos = find( e < (e(end)/k) );
% e(pos) = e(end)/k;                 % regularized eigenvalues
% Nreg = V*diag(e)*V';               % regularized matrix

pos = e < (max(e) / k);
e(pos) = max(e) / k;                 % regularized eigenvalues
Nreg = V * diag(e) * V';             % regularized matrix
%Nreg = real(V*diag(e)*V');          % regularized matrix

% -----------------------------------------------------------------

% figure; imagesc(N); colorbar
% figure; imagesc(Nreg); colorbar
% figure; imagesc(N-Nreg); colorbar


% P. Perona and W. T. Freeman, "A factorization approach to grouping",
% In H. Burkardt and B. Neumann, editors, Proc ECCV, pages 655-670, 1998.

% Yair Weiss, "Segmentation Using Eigenvectors: A Unifying View", UC
% Berkeley.

% Asad Ali
% GIK Institute of Engineering Sciences & Technology, Pakistan
% Email: asad_82@yahoo.com

% CONCEPT: Clustering algorithm based on thresholding of the first eigenvector of
% the affinity matrix
clear all;
close all;

THRESHOLD = 0.28; % change the threshold depending on the data.
% generate the data
data = GenerateData(1);
% break the block matrix structure
data = data(3:14,:);
figure,plot(data(:,1), data(:,2),'r+'),title('Original Data Points'); grid on;shg

affinity = CalculateAffinity(data);
figure,imshow(affinity,[]),title('Affinity Matrix')

[eigVectors,eigValues] = eig(affinity);

% first eigenvector is the one which has the highest eigenvalue
sz = size(eigVectors);
firstEig = eigVectors(:,sz(1,2));

% plot the eigen vector corresponding to the largest eigen value
[xx1,yy1,val1] = find(firstEig > THRESHOLD);
[xx2,yy2,val2] = find(firstEig <= THRESHOLD);
figure,
hold on;
plot(data(xx1,1),data(xx1,2),'g*')
plot(data(xx2,1),data(xx2,2),'b*')
hold off;
title('Clustering Results after thresholding the First Eigenvector');
grid on;shg


function volume = ball01_volume ( )

%*****************************************************************************80
%
%% BALL01_VOLUME returns the volume of the unit ball.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VOLUME, the volume of the unit ball.
%
  r = 1.0;
  volume = 4.0 * pi * r ^ 3 / 3.0;

  return
end


function [dStates, A, B, Positions] = Derive_EoM()

%This function derives the equations of motion for a tractor trailer truck.

%The outputs assume that the states and inputs are being listed in the
%order in which they are defined

%% Parameters

Lt = sym('Lt');
Lc = sym('Lc');

%% Inputs

v = sym('v');
psi = sym('psi');

%% States

x = sym('x');
y = sym('y');
th = sym('th');
phi = sym('phi');

%% State Derivatives

Dx = sym('Dx');
Dy = sym('Dy');
Dth = sym('Dth');
Dphi = sym('Dphi');

%% Equations of motion

%Unit Vectors
    i = [1;0];
    j = [0;1];

    An = cos(th)*i + sin(th)*j;
    At = -sin(th)*i + cos(th)*j;
    Bn = cos(phi)*An + sin(phi)*At;
    Bt = -sin(phi)*An + cos(phi)*At;
    Cn = cos(psi)*Bn + sin(psi)*Bt;
    Ct = -sin(psi)*Bn + cos(psi)*Bt;

%Derivatives of Unit Vectors
    dAn = Dth*At;
    dAt = -Dth*An;
    dBn = (Dphi + Dth)*Bt;
    dBt = -(Dphi + Dth)*Bn;

%Position Vectors
    Pa = x*i + y*j;
    Pb = Pa + Lt*At;
    Pc = Pb + Lc*Bt;
    Positions = [Pa,Pb,Pc];

%Derivatives of Position Vectors
    dPa = Dx*i + Dy*j;
    dPb = dPa + Lt*dAt;
    dPc = dPb + Lc*dBt;

%Trailer Back Wheel Constraint
    RHS_1 = 0;
    LHS_1 = Dot(dPa,An);

%Cab Back Wheel Constraint
    RHS_2 = 0;
    LHS_2 = Dot(dPb,Bn);

%Cab Front Wheel Constraints
    RHS_3 = dPc(1);
    LHS_3 = v*Ct(1);

    RHS_4 = dPc(2);
    LHS_4 = v*Ct(2);

%Solve for State Derivatives
    Solution = solve(...
        RHS_1-LHS_1, RHS_2-LHS_2, RHS_3-LHS_3, RHS_4-LHS_4,...
        Dx, Dy, Dth, Dphi);

%Express as Vector [Dx, Dy, Dth, Dphi]

    dStates = [...
        Solution.Dx;
        Solution.Dy;
        Solution.Dth;
        Solution.Dphi];

%% Jacobian Calculations

    J_x = diff(dStates,x);
    J_y = diff(dStates,y);
    J_th = diff(dStates,th);
    J_phi = diff(dStates,phi);
    J_v = diff(dStates,v);
    J_psi = diff(dStates,psi);

    A = [J_x, J_y, J_th, J_phi];
    B = [J_v, J_psi];

end


function [inv_map,bi_inv_map,logdet,invA] = invchol2(A)
% INVCHOL2
% Does a Cholesky decomposition on A and returns logdet, inverse and
% two function handles that respectively map X to A\X and A\X/A.
%

if nargin==0
    test_this();
    return;
end

if isreal(A)

    R = chol(A); %R'*R = A
    inv_map = @(X) R\(R'\X);

    % inv(A)*X*inv(A)
    bi_inv_map = @(X) (inv_map(X)/R)/(R');

    if nargout>2
        logdet = 2*sum(log(diag(R)));
        if nargout>3
            invA = inv_map(eye(size(A)));
        end
    end
else
    inv_map = @(X) A\X;

    % inv(A)*X*inv(A)
    bi_inv_map = @(X) (A\X)/A;

    if nargout>2
        logdet = log(det(A));
        if nargout>3
            invA = inv_map(eye(size(A)));
        end
    end
end

function test_this()
dim = 3;
r = randn(dim,2*dim);
A = r*r';
[inv_map,bi_inv_map,logdet,iA] = invchol2(A);
[logdet,log(det(A))]
X = randn(dim,3);
Y1 = A\X,
Y2 = inv_map(X)

Z1 = (A\X)/A
Z2 = bi_inv_map(X)

iA,
inv(A)


function [t] = LineSearchGoldenSection(func,LB,UB,EPSILON)
%
% Golden Section search for an approximate minimum of unimodal
%         function f(t) using only function evaluations
%
% length of location interval is reduced by 0.318 each iteration
%

narginchk(3,4);
if nargin == 3
    EPSILON = max(abs(UB) + abs(LB),2)*0.5e-4;
end
GR = (sqrt(5)-1)/2; % 0.618

l = UB - (UB-LB)*GR;
r = LB + (UB-LB)*GR;
fl = func(l);
fr = func(r);
while UB - LB >= EPSILON
    if fl < fr
        UB = r;
        r = l;
        fr = fl;
        l = UB - (UB-LB)*GR;
        fl = func(l);
    else
        LB = l;
        l = r;
        fl = fr;
        r = LB + (UB-LB)*GR;
        fr = func(r);
    end
end

t = (UB+LB)/2;

end

github-repos/MATLAB/yuanhao-cui-Must-Reading-on-ISAC/Must-Reading-on-ISAC-34cd6615c52ebca121428a979e756c608b195040/Codes/Fan2018TSP/Codes for DFRC Waveform Design/Waveform Design With Given Radar Beampatterns/LineSearchGoldenSection.m


function value = lrline ( xu, yu, xv1, yv1, xv2, yv2, dv )

%*****************************************************************************80
%
%% LRLINE determines if a point is left of, right or, or on a directed line.
%
%  Discussion:
%
%    The directed line is parallel to, and at a signed distance DV from
%    a directed base line from (XV1,YV1) to (XV2,YV2).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2005
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe,
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, real XU, YU, the coordinates of the point whose
%    position relative to the directed line is to be determined.
%
%    Input, real XV1, YV1, XV2, YV2, the coordinates of two points
%    that determine the directed base line.
%
%    Input, real DV, the signed distance of the directed line
%    from the directed base line through the points (XV1,YV1) and (XV2,YV2).
%    DV is positive for a line to the left of the base line.
%
%    Output, integer VALUE, the result:
%    +1, the point is to the right of the directed line;
%     0, the point is on the directed line;
%    -1, the point is to the left of the directed line.
%
  tol = 100.0 * eps;

  dx = xv2 - xv1;
  dy = yv2 - yv1;
  dxu = xu - xv1;
  dyu = yu - yv1;

  tolabs = tol * max ( abs ( dx ), ...
                 max ( abs ( dy ), ...
                 max ( abs ( dxu ), ...
                 max ( abs ( dyu ), abs ( dv ) ) ) ) );

  t = dy * dxu - dx * dyu + dv * sqrt ( dx * dx + dy * dy );

  if ( tolabs < t )
    value = 1;
  elseif ( -tolabs <= t )
    value = 0;
  else
    value = -1;
  end

  return
end


function variance = uniform_discrete_variance ( a, b )

%*****************************************************************************80
%
%% UNIFORM_DISCRETE_VARIANCE returns the variance of the Uniform discrete PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer A, B, the parameters of the PDF.
%    A <= B.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = ( ( b + 1 - a )^2 - 1 ) / 12.0;

  return
end


function integral = circle01_monomial_integral ( e )

%*****************************************************************************80
%
%% CIRCLE01_MONOMIAL_INTEGRAL: integrals on circumference of unit circle in 2D.
%
%  Discussion:
%
%    The integration region is
%
%      X^2 + Y^2 = 1.
%
%    The monomial is F(X,Y) = X^E(1) * Y^E(2).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Academic Press, 1984, page 263.
%
%  Parameters:
%
%    Input, integer E(2), the exponents of X and Y in the
%    monomial.  Each exponent must be nonnegative.
%
%    Output, real INTEGRAL, the integral.
%
  if ( any ( e(1:2) < 0 ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CIRCLE01_MONOMIAL_INTEGRAL - Fatal error!\n' );
    fprintf ( 1, '  All exponents must be nonnegative.\n' );
    error ( 'CIRCLE01_MONOMIAL_INTEGRAL - Fatal error!' );
  end

  if ( any ( mod ( e(1:2), 2 ) == 1 ) )

    integral = 0.0;

  else

    integral = 2.0;

    for i = 1 : 2
      integral = integral * gamma ( 0.5 * ( e(i) + 1 ) );
    end

    integral = integral / gamma ( 0.5 * sum ( e(1:2) + 1 ) );

  end

  return
end


function omega_en_n = transport_rate(lat, Vn, Ve, h)
% transport_rate: calculates the transport rate in the navigation frame.
%
% INPUT
%	lat, 1x1 latitude (rad).
%	Vn, 1x1 North velocity (m/s).
%   Ve, 1x1 East velocity (m/s).
%   h, altitude (m).
%
% OUTPUT
%	omega_en_n, 3x3 skew-symmetric transport rate matrix (rad/s).
%
%   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:
%
%   Paul D. Groves. Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems. Second Edition.
% Eq. 5.44, page 177.
%
% Version: 002
% Date:    2022/01/25
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

h = abs(h);

[RM,RN] = radius(lat);

om_en_n(1,1) =   Ve / (RN + h);              % North
om_en_n(2,1) = -(Vn / (RM + h));             % East
om_en_n(3,1) = -(Ve * tan(lat) / (RN + h));  % Down

omega_en_n = skewm(om_en_n);

end


function d = distancePointLine3d(point, line)
%DISTANCEPOINTLINE3D Euclidean distance between 3D point and line.
%
%   D = distancePointLine3d(POINT, LINE);
%   Returns the distance between point POINT and the line LINE, given as:
%   POINT : [x0 y0 z0]
%   LINE  : [x0 y0 z0 dx dy dz]
%   D     : (positive) scalar
%
%   See also:
%   lines3d, isPointOnLine3d, distancePointEdge3d, projPointOnLine3d,
%
%
%   References
%   http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html

%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 23/05/2005.
%

%   HISTORY
%   15/01/2007 unify size of input data
%   31/01/2007 typo in data formatting, and replace norm by vecnorm3d
%   12/12/2010 changed to bsxfun implementation - Sven Holcombe

% cf. Mathworld (distance point line 3d)  for formula
d = bsxfun(@rdivide, vectorNorm3d( ...
        crossProduct3d(line(:,4:6), bsxfun(@minus, line(:,1:3), point)) ), ...
        vectorNorm3d(line(:,4:6)));


function variance = hypergeometric_variance ( n, m, l )

%*****************************************************************************80
%
%% HYPERGEOMETRIC_VARIANCE returns the variance of the Hypergeometric PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    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 VARIANCE, the variance of the PDF.
%
  variance = n * m * ( l - m ) * ( l - n ) / ( l * l * ( l - 1 ) );

  return
end


function p = perms_m(n, m)
%perms_m(n, m): return the mth permutation of the digits 1:n
% where m is from 0 to factorial(n) - 1
%
% The ordering is fairly arbitrary, but there is a bijection between m to
% the n! permutations (asking for same m twice gets same permutation), and
% perms_m(n, 0) is 1:n in order.
%
% Example:
%   p = perms(1:4);
%   P = zeros(size(p));
%   for i = 1:size(P,1)
%       P(i, :) = perms_m(4, i-1)';
%   end
%   all(all(sortrows(P) == sortrows(p)))
%
% See also: perms, randperm, uperms, signs_m, nchoosek_m, sample_no_repl

% Copyright 2010 Ged Ridgway
% http://www.mathworks.com/matlabcentral/fileexchange/authors/27434

n = ceil(abs(n)); m = round(abs(m));

M = factorial(n) - 1; % max m
if m < 0 || m > M
    error('perms_m:m_range', 'Require 0 <= m <= %d', M);
end

p = zeros(n,1);
d = 1:n;
F = factorial(n); % (F in loop goes from factorial(n-1) to factorial(0))
for f = 1:n
    F = F / (n + 1 - f); % (here rather than end of loop to avoid div-by-0)
    x = floor(m / F);
    p(f) = d(f + x);
    d(f + x) = d(f);
    m = rem(m, F);
end


function alpha_pq = moment_normalized ( n, x, y, p, q )

%*****************************************************************************80
%
%% MOMENT_NORMALIZED computes a normalized moment of a polygon.
%
%  Discussion:
%
%    Alpha(P,Q) = Integral ( x, y in polygon ) x^p y^q dx dy / Area ( polygon )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carsten Steger,
%    On the calculation of arbitrary moments of polygons,
%    Technical Report FGBV-96-05,
%    Forschungsgruppe Bildverstehen, Informatik IX,
%    Technische Universitaet Muenchen, October 1996.
%
%  Parameters:
%
%    Input, integer N, the number of vertices of the polygon.
%
%    Input, real X(N), Y(N), the vertex coordinates.
%
%    Input, integer P, Q, the indices of the moment.
%
%    Output, real ALPHA_PQ, the normalized moment Alpha(P,Q).
%
  nu_pq = moment ( n, x, y, p, q );
  nu_00 = moment ( n, x, y, 0, 0 );

  alpha_pq = nu_pq / nu_00;

  return
end


function sca=RLSsca(X,loc,p)
%Syntax: sca=RLSsca(X,loc,p)
%___________________________
%
% Calculates the Reweighted Least Squares (RLS) scale parameter
% of the columns of a matrix X.
%
% sca is the RLS estimated vector of scales.
% X is the matrix with the data sets.
% loc is the location vector. Its default value is the LMS location.
% p is the number of parameters. Its default value is 1.
%
% Reference:
% Rousseeuw PJ, Leroy AM (1987): Robust regression and outlier detection. Wiley.
%
%
% Alexandros Leontitsis
% Institute of Mathematics and Statistics
% University of Kent at Canterbury
% Canterbury
% Kent, CT2 7NF
% U.K.
%
% University e-mail: al10@ukc.ac.uk (until December 2002)
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% Sep 3, 2001.

if nargin<1 | isempty(X)==1
   error('Not enough input arguments.');
else
   % X must be 2-dimensional
   if ndims(X)>2
      error('Invalid data set.');
   end
   % If X is a row vector make it a column vector
   if size(X,1)==1
      X=X';
   end

   % n is the length of the data set
   n=size(X,1);
end

% For a single data point there is no need to proceed
if n==1
   sca=0;
else

   % Check loc
   if nargin<2 | isempty(loc)==1
      % If you don't give loc, it is supposed the LMS location
      loc=LMSloc(X);
   else
      % loc must be a scalar/vector
      if min(size(loc))>1
         error('loc must be a scalar/vector.');
      end
      % loc must have the length of the columns of X
      if length(loc)~=size(X,2);
         error('loc must have the length of the columns of X.');
      end
   end

   % Check p
   if nargin<3 | isempty(p)==1
      % If p is omitted give it the value of 1
      p=1;
   else
      % p must be a scalar
      if sum(size(p))>2
         error('p must be a scalar.');
      end
      % p must be an integrer greater than 1
      if round(p)-p~=0 | p<1
         error('p must be an integrer greater than or equal to 1.');
      end
   end

   % Calculate the scale parameter for every column
   for i=1:size(X,2)

      % The differences from the location
      r=X(:,i)-loc(i);

      % Estimate the preliminary scale parameter
      s=LMSsca(r,0,p);
      if s==0
         w=1:n;
      else
         w=find(abs(r)/s<=2.5);
      end
      sca(i)=sqrt(sum(r(w).^2)/(length(w)-p));
   end
end


function a = hankel ( n, x )

%*****************************************************************************80
%
%% HANKEL returns a HANKEL matrix.
%
%  Formula:
%
%    A(I,J) = X(I+J-1)
%
%  Example:
%
%    N = 5, X = ( 1, 2, 3, 4, 5, 6, 7, 8, 9 )
%
%    1  2  3  4  5
%    2  3  4  5  6
%    3  4  5  6  7
%    4  5  6  7  8
%    5  6  7  8  9
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is a Hankel matrix: constant along anti-diagonals.
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real X(2*N-1), the vector that defines A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for j = 1 : n
    a(1:n,j) = x(j:j+n-1);
  end

  return
end


function P = khatrirao(varargin)
%KHATRIRAO Khatri-Rao product of matrices.
%
%   KHATRIRAO(A,B) computes the Khatri-Rao product of matrices A and
%   B that have the same number of columns.  The result is the
%   column-wise Kronecker product
%   [KRON(A(:,1),B(:,1)) ... KRON(A(:,n),B(:,n))]
%
%   KHATRIRAO(A1,A2,...) computes the Khatri-Rao product of
%   multiple matrices that have the same number of columns.
%
%   KHATRIRAO(C) computes the Khatri-Rao product of
%   the matrices in cell array C.
%
%   KHATRIRAO(...,'r') computes the Khatri-Rao product in reverse
%   order.
%
%   Examples
%   A = rand(5,2); B = rand(3,2); C = rand(2,2);
%   khatrirao(A,B) %<-- Khatri-Rao of A and B
%   khatrirao(B,A,'r') %<-- same thing as above
%   khatrirao({C,B,A}) %<-- passing a cell array
%   khatrirao({A,B,C},'r') %<-- same as above
%
%   See also TENSOR, KTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

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

%% Error checking on input and set matrix order
% Note that this next if/else check forces A to be a cell array.
if ischar(varargin{end}) && varargin{end} == 'r'
    if nargin == 2 && iscell(varargin{1})
        % Input is a single cell array
        A = varargin{1};
    else
        % Input is a sequence of matrices
        A = {varargin{1:end-1}};
    end
    matorder = length(A):-1:1;
else
    if nargin == 1 && iscell(varargin{1})
        % Input is a single cell array
        A = varargin{1};
    else
        % Input is a sequence of matrices
        A = varargin;
    end
    matorder = 1:length(A);
end

%% Error check on matrices and compute number of rows in result

% N = number of columns (must be the same for every input)
N = size(A{1},2);

% After loop, M = number of rows in the result
M = 1;

for i = matorder
    if ndims(A) ~= 2
        error('Each argument must be a matrix');
    end
    if (N ~= size(A{i},2))
        error('All matrices must have the same number of columns.')
    end
    M = M * size(A{i},1);
end

%% Computation

% Preallocate
P = zeros(M,N);

% Loop through all the columns
for n = 1:N
    % Loop through all the matrices
    ab = A{matorder(1)}(:,n);
    for i = matorder(2:end)
       % Compute outer product of nth columns
       ab = A{i}(:,n) * ab(:).';
    end
    % Fill nth column of P with reshaped result
    P(:,n) = ab(:);
end


function [rank,WT]=rankSetPartition(q,WT)
%%RANKSETPARTITION Return the set partition with the given rank in the
%           lexicographic ordering of all set parititions having length(n).
%           The partition is a length-n vector q that specifies which
%           partition each item belongs to. The order of the items in the
%           partitions is not important.
%
%INPUTS: q The nX1 set partition vector that should be ranked. Paritions
%          are numbered in increasing order starting from 1. That is, the
%          first occurence of partition numbered i in q is always before
%          the first occurences of partitions numbered j>1.
%       WT The is, optionally, the Williamson table. It is the table T in
%          [1]. The table is a function of length(q). If omitted, then the
%          table will be computed and returned so that futue calls to the
%          function can use
%
%OUTPUTS: rank The rank of the set partition in lexicographic order.
%              0<=rank<BellNumber(n).
%           WT The Williamson table for a length-n set partition. Passing
%              this to rankSetPartition on subsequent calls will speed up
%              the algorithm.
%
%This function implements the ranking algorithm of [1]. See
%getNextSetPartition for more information on set partitions.
%
%REFERENCES:
%[1] S. G. Williamson, "Ranking algorithms for lists of partitions," SIAM
%    Journal on Computing, vol. 5, no. 4, pp. 602-617, 1976.
%
%January 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=length(q);

if(nargin<3||isempty(WT))
    %Compute the Williamson triangle
    WT=zeros(n,n);
    WT(1,1:n)=1;
    for v=2:n
        for mu=1:(n-v+1)
            WT(v,mu)=mu*WT(v-1,mu)+WT(v-1,mu+1);
        end
    end
end

q=q-1;

rank=0;

m=cummax(q);

for t=0:(n-2)
    idx=n-t;

    rank=rank+q(idx)*WT(t+1,m(idx-1)+1);
end

rank=rank+1;

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 r = corr ( n, x, m )

%*****************************************************************************80
%
%% CORR computes the sample correlation of a signal sample.
%
%  Discussion:
%
%    The sample correlation is defined, for 0 <= i < N, as
%
%      R(i) = 1/N * sum ( 0 <= j <= N - 1 - i ) X(i+j) * X(j)
%
%    The sample correlation is an estimate of the correlation function.
%
%    It is usually the case that the signal X is assumed to
%    have zero mean.  Here, we compute the mean and adjust the
%    calculation accordingly:
%
%      R(i) = 1/N * sum ( 0 <= j <= N - 1 - i )
%        ( X(i+j) - Xbar ) * ( X(j) - Xbar )
%
%    Experience suggests that only the first 5 or 10 percent of
%    the lags are statistically reliable, so that one might choose
%    M = N / 20 or M = N / 10, for instance.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 June 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Sophocles Orfanidis,
%    Introduction to Signal Processing,
%    Prentice-Hall, 1995,
%    ISBN: 0-13-209172-0,
%    LC: TK5102.5.O246.
%
%  Parameters:
%
%    Input, integer N, the number of equally spaced signal
%    samples.
%
%    Input, real X(N), the signal samples.
%
%    Input, integer M, the maximum lag to consider.
%    0 <= M < N.
%
%    Output, real R(1:M+1), the sample correlations.
%
  r(1:m+1) = 0.0;

  xbar = sum ( x(1:n) ) / n;

  for i = 0 : m
    for j = 0 : n - i - 1
      r(i+1) = r(i+1) + ( x(i+j+1) - xbar ) * ( x(j+1) - xbar );
    end
  end

  r(1:m+1) = r(1:m+1) / n;

  return
end


function [ o, c, e ] = polynomial_mul ( m, o1, c1, e1, o2, c2, e2 )

%*****************************************************************************80
%
%% POLYNOMIAL_MUL multiplies two polynomials.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer O1, the "order" of polynomial 1.
%
%    Input, real C1(O1), the coefficients of polynomial 1.
%
%    Input, integer E1(O1), the indices of the exponents of polynomial 1.
%
%    Input, integer O2, the "order" of polynomial 2.
%
%    Input, real C2(O2), the coefficients of polynomial 2.
%
%    Input, integer E2(O2), the indices of the exponents of polynomial 2.
%
%    Output, integer O, the "order" of the polynomial product.
%
%    Output, real C(O), the coefficients of the polynomial product.
%
%    Output, integer E(O), the indices of the exponents of the
%    polynomial product.
%
  o = o1 * o2;
  e = zeros ( o, 1 );
  c = zeros ( o, 1 );

  o = 0;
  for j = 1 : o2
    for i = 1 : o1
      o = o + 1;
      c(o) = c1(i) * c2(j);
      f1(1:m) = mono_unrank_grlex ( m, e1(i) );
      f2(1:m) = mono_unrank_grlex ( m, e2(j) );
      f(1:m) = f1(1:m) + f2(1:m);
      e(o) = mono_rank_grlex ( m, f );
    end
  end

  [ c, e ] = polynomial_sort ( o, c, e );
  [ o, c, e ] = polynomial_compress ( o, c, e );

  return
end


% 2-D Transform
% The 2-D Fourier transform is useful for processing 2-D signals and other 2-D data such as images.
% Create and plot 2-D data with repeated blocks.

P = peaks(20);
X = repmat(P,[5 10]);
imagesc(X)

% TODO : Compute the 2-D Fourier transform of the data.
% Shift the zero-frequency component to the center of the output, and
% plot the resulting 100-by-200 matrix, which is the same size as X.

% In the case of Radar signal processing. Convert the signal in MxN matrix, where M is the size of Range FFT samples and N is the size of Doppler FFT samples:
signal  = reshape(X, [100, 200]);

% Run the 2D FFT across both the dimensions.
signal_fft = fft2(signal, 100, 200);

% Shift zero-frequency terms to the center of the array
signal_fft = fftshift(signal_fft);

% Take the absolute value
signal_fft = abs(signal_fft);

% Here since it is a 2D output, it can be plotted as an image. Hence, we use the imagesc function
imagesc(signal_fft);


function [ x, w ] = lobatto_compute ( n )

%*****************************************************************************80
%
%% LOBATTO_COMPUTE computes a Lobatto quadrature rule.
%
%  Discussion:
%
%    The integral:
%
%      Integral ( -1 <= X <= 1 ) F(X) dX
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= N ) W(I) * F ( X(I) )
%
%    The quadrature rule will integrate exactly all polynomials up to
%    X^(2*N-3).
%
%    The Lobatto rule is distinguished by the fact that both endpoints
%    (-1 and 1) are always abscissas of the rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 April 2015
%
%  Author:
%
%    Original MATLAB code by Greg von Winckel.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Claudio Canuto, Yousuff Hussaini, Alfio Quarteroni, Thomas Zang,
%    Spectral Methods in Fluid Dynamics,
%    Springer, 1993,
%    ISNB13: 978-3540522058,
%    LC: QA377.S676.
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%    Daniel Zwillinger, editor,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996,
%    ISBN: 0-8493-2479-3.
%
%  Parameters:
%
%    Input, integer N, the order.
%
%    Output, real X(N), the abscissas.
%
%    Output, real W(N), the weights,
%
 if ( n == 1 )
    x(0) = -1.0;
    w(0) = 2.0;
    return
  end

  x = zeros ( n, 1 );
  w = zeros ( n, 1 );

  tolerance = 100.0 * eps;
%
%  Initial estimate for the abscissas is the Chebyshev-Gauss-Lobatto nodes.
%
  x(1:n,1) = cos ( pi * ( 0 : n - 1 ) / ( n - 1 ) )';
  xold(1:n,1) = 2.0;

  while ( tolerance < max ( abs ( x(1:n,1) - xold(1:n,1) ) ) )

    xold(1:n,1) = x(1:n,1);

    p(1:n,1) = 1.0;
    p(1:n,2) = x(1:n,1);

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

    x(1:n,1) = xold(1:n,1) - ( x(1:n,1) .* p(1:n,n) - p(1:n,n-1) ) ...
             ./ ( n * p(1:n,n) );
  end

  x(1:n) = r8vec_reverse ( n, x );

  w(1:n,1) = 2.0 ./ ( ( n - 1 ) * n * p(1:n,n).^2 );

  return
end


function area = quad_area2_2d ( q )

%*****************************************************************************80
%
%% QUAD_AREA2_2D computes the area of a quadrilateral in 2D.
%
%  Discussion:
%
%    A quadrilateral is a polygon defined by 4 vertices.
%
%    This algorithm computes the area of the related
%    Varignon parallelogram first.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 May 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real Q(2,4), the vertices, specified in
%    counter clockwise order.
%
%    Output, real AREA, the area of the quadrilateral.
%

%
%  Define a parallelogram by averaging consecutive vertices.
%
  p(1:2,1:3) = ( q(1:2,1:3) + q(1:2,2:4) ) / 2.0;
  p(1:2,  4) = ( q(1:2,  4) + q(1:2,1  ) ) / 2.0;
%
%  Compute the area.
%
  area = parallelogram_area_2d ( p );
%
%  The quadrilateral's area is twice that of the parallelogram.
%
  area = 2.0 * area;

  return
end


clear all, close all, clc
n = 256;
w = exp(-i*2*pi/n);

% Slow
for i=1:n
    for j=1:n
        DFT(i,j) = w^((i-1)*(j-1));
    end
end

% Fast
[I,J] = meshgrid(1:n,1:n);
DFT = w.^((I-1).*(J-1));
imagesc(real(DFT))


function [inv_map,bi_inv_map,logdet,invA] = invchol_or_lu(A)
% INVCHOL_OR_LU
% Does a Cholesky decomposition on A and returns logdet, inverse and
% two function handles that respectively map X to A\X and A\X/A.
%

if nargin==0
    test_this();
    return;
end

if isreal(A)

    R = chol(A); %R'*R = A
    inv_map = @(X) R\(R'\X);

    % inv(A)*X*inv(A)
    bi_inv_map = @(X) (inv_map(X)/R)/(R');

    if nargout>2
        logdet = 2*sum(log(diag(R)));
        if nargout>3
            invA = inv_map(eye(size(A)));
        end
    end
else
    [L,T,p] = lu(A,'vector');
    P = sparse(p,1:length(p),1);
                       % P*A = L*U
                       % L is lower triangular, with unit diagonal and unit determinant
                       % T is upper triangular, det(T) = prod(diag(T), may have negative values on diagonal
                       % P is a permuation matrix: P' = inv(P) and det(P) is +1 or -1
                       %

    inv_map = @(X) T\(L\(P*X));

    % inv(A)*X*inv(A)
    bi_inv_map = @(X) ((inv_map(X)/T)/L)*P;

    if nargout>2
        logdet = sum(log(diag(T)))-log(det(P));
        if nargout>3
            invA = T\(L\P);
        end
    end
end

function test_this()
dim = 3;
A = randn(dim)+sqrt(-1)*randn(dim);
[inv_map,bi_inv_map,logdet,iA] = invchol_or_lu(A);
[logdet,log(det(A))]
X = randn(dim,3);
Y1 = A\X,
Y2 = inv_map(X)

Z1 = (A\X)/A
Z2 = bi_inv_map(X)

iA,
inv(A)


function [ u, v ] = uv_spiral ( n, x, y, c )

%*****************************************************************************80
%
%% UV_SPIRAL computes a spiral velocity vector field.
%
%  Discussion:
%
%    Note that the continuous velocity field (U,V)(X,Y) that is discretely
%    sampled here satisfies the homogeneous continuity equation, that is,
%    it has zero divergence.  In other words:
%
%      dU/dX + dV/dY = 0.
%
%    This is by construction, since we have
%
%      U(X,Y) =  10 * d/dY ( PHI(X) * PHI(Y) )
%      V(X,Y) = -10 * d/dX ( PHI(X) * PHI(Y) )
%
%    which guarantees zero divergence.
%
%    The underlying function PHI is defined by
%
%      PHI(Z) = ( 1 - cos ( C * pi * Z ) ) * ( 1 - Z )^2
%
%    where C is a parameter.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), Y(N), the coordinates of the evaluation points.
%
%    Input, real C, a parameter, typically between 0 and 2 * PI.
%
%    Output, real U(N), V(N), the velocity components.
%
  u = zeros ( n, 1 );
  v = zeros ( n, 1 );

  u =   10.0 * ( 1.0 - cos ( c * pi * x ) ) ...
           .* ( 1.0 - x ).^2 ...
           .* ( ...
               c * pi * sin ( c * pi * y ) .* ( 1.0 - y ).^2 ...
             - ( 1.0 - cos ( c * pi * y ) ) ...
             .* 2.0 .* ( 1.0 - y ) ...
             );

  v = - 10.0 * ( 1.0 - cos ( c * pi * y ) ) ...
           .* ( 1.0 - y ).^2 ...
           .* ( ...
               c * pi * sin ( c * pi * x ) .* ( 1.0 - x ).^2 ...
             - ( 1.0 - cos ( c * pi * x ) ) ...
             .* 2.0 .* ( 1.0 - x ) ...
             );

  return
end


function [RM,RN] = radius(lat)
% radius: calculates meridian and normal radii of curvature.
%
% INPUT
%   lat, Nx1 latitude (rad).
%
% OUTPUT
%   RM, Nx1 meridian radius of curvature (North-South)(m).
%   RN, Nx1 normal radius of curvature (East-West) (m).
%
%   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. 2.6 and 2.7.
%
% 	Groves, P. (2008). Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems. Artech House, UK.
% Eq. 2.65 and 2.66.
%
%  	R. Gonzalez, J. Giribet, and H. Patiño. An approach to
% benchmarking of loosely coupled low-cost navigation systems,
% Mathematical and Computer Modelling of Dynamical Systems, vol. 21,
% issue 3, pp. 272-287, 2015. Eq. 11.
%
% Version: 006
% Date:    2021/03/19
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

a = 6378137.0;                  % WGS84 Equatorial radius in meters
e = 0.0818191908425;            % WGS84 eccentricity

e2 = e^2;
den = 1 - e2 .* (sin(lat)).^2;

% Meridian radius of curvature: radius of curvature for North-South motion.
RM = a .* (1-e2) ./ (den).^(3/2);

% Normal radius of curvature: radius of curvature for East-West motion.
% AKA transverse radius.
RN = a ./ sqrt(den);

end


function diameter = polygon_diameter_2d ( n, v )

%*****************************************************************************80
%
%% POLYGON_DIAMETER_2D computes the diameter of a polygon in 2D.
%
%  Discussion:
%
%    The diameter of a polygon is the maximum distance between any
%    two points on the polygon.  It is guaranteed that this maximum
%    distance occurs between two vertices of the polygon.  It is
%    sufficient to check the distance between all pairs of vertices.
%    This is an N^2 algorithm.  There is an algorithm by Shamos which
%    can compute this quantity in order N time instead.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of vertices of the polygon.
%
%    Input, real V(2,N), the vertices.
%
%    Output, real DIAMETER, the diameter of the polygon.
%
  diameter = 0.0;

  for i = 1 : n

    for j = i + 1 : n
      diameter = max ( diameter, ...
        sqrt ( ( v(1,i) - v(1,j) ).^2 + ( v(2,i) - v(2,j) ).^2 ) );
    end

  end

  return
end


function [ w, t ] = circle_rule ( nt )

%*****************************************************************************80
%
%% CIRCLE_RULE computes a quadrature rule for the unit circle.
%
%  Discussion:
%
%    The unit circle is the region:
%
%      x * x + y * y = 1.
%
%    The integral I(f) is then approximated by
%
%      Q(f) = 2 * pi * sum ( 1 <= i <= NT ) W(i) * F ( cos(T(i)), sin(T(i)) ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NT, the number of angles to use.
%
%    Output, real W(NT), the weights for the rule.
%
%    Output, real T(NT), the angles for the rule.
%
  w(1:nt) = 1.0 / nt;
  for it = 1 : nt
    t(it) = 2.0 * pi * ( it - 1 ) / nt;
  end

  return
end


function [axis, theta] = rotation3dAxisAndAngle(mat)
%ROTATION3DAXISANDANGLE Determine axis and angle of a 3D rotation matrix.
%
%   [AXIS, ANGLE] = rotation3dAxisAndAngle(MAT)
%   Where MAT is a 4-by-4 matrix representing a rotation, computes the
%   rotation axis (containing the points that remain invariant under the
%   rotation), and the rotation angle around that axis.
%   AXIS has the format [DX DY DZ], constrained to unity, and ANGLE is the
%   rotation angle in radians.
%
%   Note: this method use eigen vector extraction. It would be more precise
%   to use quaternions, see:
%   http://www.mathworks.cn/matlabcentral/newsreader/view_thread/160945
%
%
%   Example
%     origin = [1 2 3];
%     direction = [4 5 6];
%     line = [origin direction];
%     angle = pi/3;
%     rot = createRotation3dLineAngle(line, angle);
%     [axis angle2] = rotation3dAxisAndAngle(rot);
%     angle2
%     angle2 =
%           1.0472
%
%   See also
%   transforms3d, vectors3d, angles3d, eulerAnglesToRotation3d
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-08-11,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% extract the linear part of the rotation matrix
A = mat(1:3, 1:3);

% extract eigen values and eigen vectors
[V, D] = eig(A - eye(3));

% we need the eigen vector corresponding to eigenvalue==1
[dummy, ind] = min(abs(diag(D)-1)); %#ok<ASGLU>

% extract corresponding eigen vector
vector = V(:, ind)';

% compute rotation angle
t = [A(3,2)-A(2,3) , A(1,3)-A(3,1) , A(2,1)-A(1,2)];
theta = atan2(dot(t, vector), trace(A)-1);

% If angle is negative, invert both angle and vector direction
if theta<0
    theta  = -theta;
    vector = -vector;
end

% try to get a point on the line
% seems to work, but not sure about stability
[V, D] = eig(mat-eye(4)); %#ok<ASGLU>
origin = V(1:3,4)'/V(4, 4);

% create line corresponding to rotation axis
axis = [origin vector];


function k = pascal_to_i4 ( i, j )

%*****************************************************************************80
%
%% PASCAL_TO_I4 converts Pacal triangle coordinates to a linear index.
%
%  Discussion:
%
%    We describe the grid points in a Pascal triangle in two ways:
%
%    As a linear index K:
%
%                     1
%                   2   3
%                 4   5   6
%               7   8   9   10
%
%    As elements (I,J) of Pascal's triangle:
%
%                     0,0
%                  1,0   0,1
%               2,0   1,1    0,2
%            3,0   2,1   1,2    0,3
%
%  Example:
%
%     K  I  J
%
%     1  0  0
%     2  1  0
%     3  0  1
%     4  2  0
%     5  1  1
%     6  0  2
%     7  3  0
%     8  2  1
%     9  1  2
%    10  0  3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, J, the row and column indices.  I and J must
%    be nonnegative.
%
%    Output, integer K, the linear index of the (I,J) element.
%
  if ( i < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PASCAL_TO_I4 - Fatal error!\n' );
    fprintf ( 1, '  I < 0.\n' );
    fprintf ( 1, '  I = %d\n', i );
    error ( 'PASCAL_TO_I4 - Fatal error!' );
  elseif ( j < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PASCAL_TO_I4 - Fatal error!\n' );
    fprintf ( 1, '  J < 0.\n' );
    fprintf ( 1, '  J = %d\n', j );
    error ( 'PASCAL_TO_I4 - Fatal error!' );
  end

  d = i + j;

  k = ( d * ( d + 1 ) ) / 2 + j + 1;

  return
end


function integral = ball01_monomial_integral ( e )

%*****************************************************************************80
%
%% BALL01_MONOMIAL_INTEGRAL returns monomial integrals in the unit ball.
%
%  Discussion:
%
%    The integration region is
%
%      X^2 + Y^2 + Z^2 <= 1.
%
%    The monomial is F(X,Y,Z) = X^E(1) * Y^E(2) * Z^E(3).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gerald Folland,
%    How to Integrate a Polynomial Over a Sphere,
%    American Mathematical Monthly,
%    Volume 108, Number 5, May 2001, pages 446-448.
%
%  Parameters:
%
%    Input, integer E(3), the exponents of X, Y and Z in the
%    monomial.  Each exponent must be nonnegative.
%
%    Output, real INTEGRAL, the integral.
%
  if ( any ( e(1:3) < 0 ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BALL01_MONOMIAL_INTEGRAL - Fatal error!\n' );
    fprintf ( 1, '  All exponents must be nonnegative.\n' );
    error ( 'BALL01_MONOMIAL_INTEGRAL - Fatal error!' );
  end
%
%  Integrate over the surface.
%
  if ( all ( e(1:3) == 0 ) )

    integral = 2.0 * sqrt ( pi ^ 3 ) / gamma ( 1.5 );

  elseif ( any ( mod ( e(1:3), 2 ) == 1 ) )

    integral = 0.0;

  else

    integral = 2.0;

    for i = 1 : 3
      integral = integral * gamma ( 0.5 * ( e(i) + 1 ) );
    end

    integral = integral / gamma ( 0.5 * ( sum ( e(1:3) + 1 ) ) );

  end
%
%  The surface integral is now adjusted to give the volume integral.
%
  r = 1.0;
  s = sum ( e(1:3) ) + 3;

  integral = integral * r ^ s / s;

  return
end


%%
%   Author: Yash Bansod
%
% GitHub: <https://github.com/YashBansod>
%
% Demonstrates the calculation of Jacobian for a 3DOF 2D Planar Robot

%% Clear the environment and the command line
clear;
clc;
close all;

%% Calculation of Jacobian for 3DOF 2D Planar Robot
syms t1 t2 t3 L1 L2 L3
orgX = 0;
orgY = 0;

X(1)= orgX+ L1*cos(t1);
Y(1)= orgY+ L1*sin(t1);
X(2)= X(1)+ L2*cos(t1 + t2);
Y(2)= Y(1)+ L2*sin(t1 + t2);
X(3)= X(2)+ L3*cos(t1 + t2 + t3);
Y(3)= Y(2)+ L3*sin(t1 + t2 + t3);

Jacob = [diff(X(3),t1) , diff(X(3), t2) , diff(X(3), t3);
            diff(Y(3), t1), diff(Y(3), t2) , diff(Y(3), t3)]


function fx = p06_fx ( x )

%*****************************************************************************80
%
%% P06_FX evaluates exp ( x ) - 2 - 1 / ( 10 * x )^2 + 2 / ( 100 * x )^3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(*), the point at which F is to be evaluated.
%
%    Output, real FX(*), the value of the function at X.
%
  fx = exp ( x ) - 2.0 - 1.0 ./ ( 10.0 * x ).^2 + 2.0 ./ ( 100.0 * x ).^3;

  return
end


%% RATE OF CONVERGENCE OF LINEAR ELEMENT FOR POISSON EQUATION
%
% This example is to show the rate of convergence of linear finite element
% approximation of the Poisson equation on the unit cube with the
% following boundary conditions:
%
% - Non-empty Dirichlet boundary condition.
% - Pure Neumann boundary condition.
% - Robin boundary condition.
%
% The basis, data structure and numerical test is summarized in <a
% href="matlab:ifem Poisson3femrate">Poisson3femrate</a>.
%
% See also Poisson3P2femrate, Poissonfemrate
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

clear variables

%% Setting
[node,elem] = cubemesh([0,1,0,1,0,1],0.5);
mesh = struct('node',node,'elem',elem);
option.L0 = 1;
option.maxIt = 4;
option.elemType = 'P1';
option.printlevel = 1;
option.plotflag = 1;

%% Non-empty Dirichlet boundary condition.
fprintf('Mixed boundary conditions. \n');
pde = sincosdata3;
mesh.bdFlag = setboundary3(node,elem,'Dirichlet','~(x==0)','Neumann','x==0');
% bdFlag = setboundary3(node,elem,'Dirichlet');
femPoisson3(mesh,pde,option);

%% Pure Neumann boundary condition.
fprintf('Pure Neumann boundary condition. \n');
option.plotflag = 0;
pde = sincosdata3Neumann;
mesh.bdFlag = setboundary3(node,elem,'Neumann');
femPoisson3(mesh,pde,option);

%% Robin boundary condition.
fprintf('Robin boundary condition. \n');
pde = sincosRobindata3;
mesh.bdFlag = setboundary3(node,elem,'Robin');
femPoisson3(mesh,pde,option);

%% Conclusion
%
% The optimal rate of convergence of the H1-norm (1st order) and L2-norm
% (2nd order) is observed. The 2nd order convergent rate between two
% discrete functions ||DuI-Duh|| is known as superconvergence.
%
% MGCG converges uniformly in all cases.
%
% Note that for Neumann and Robin boundary condition, the order of the error in the
% maximum norm is not close to 2. Neumann problem is around 1.5 and Robin
% is near 1.3.


%% Optimizing the operation of a Hydroelectric Dam
%
% This demo maximizes the revenue generated from a hydroelectric dam using
% Quadratic Programming.  We compute the optimal flow rate to the turbine
% as well as the optimal spill flow rate.
%
% This problem is constrained such that:
%
%  1)  0 < Turbine Flow < 25,000 CFS
%  2)  0 < Spill Flow
%  3)  Max change in Turbine and Spill Flow < 500 CFS
%  4)  Combined Turbine and Spill Flow > 1000 CFS
%  5)  50000 < Reservoir Storage < 100000 Acre-Feet
%  6)  The final reservoir level must equal the initial level (90000 AF)
%
% Two solvers are used to solve this problem: FMINCON and QUADPROG
% FMINCON is used first as it takes less time to set-up the problem for
% ths solver.  Supplying the Gradient and Hessian to the FMINCON algorithm
% significantly speeds up the solution of the problem.
% QUADPROG ionterior-point-convex is used third to provide a faster
% algorithm for solving this type of problem.  This solver is also better
% suited for handling a larger version of the same problem.
%
% Copyright (c) 2012, MathWorks, Inc.

%% 1 - Load data
% Two variables are loaded from data set:
%   inFlow - flow rate into the reservoir measured hourly (CFS)
%   price - price of electricity measured hourly ($/MWh)
load('FlowAndPriceData.mat');

%% 2 - Define Starting Point and Constants
% We wish to find the vector x of optimal decision variables.
% For this problem:
% x = [turbineFlow(1) ... turbineFlow(N) spillFlow(1) ... spillFlow(N)]

% Our initial starting point is turbineFlow = inFlow and spillFlow = 0
x0 = [inFlow; zeros(size(inFlow))];

stor0 = 90000; % initial vol. of water stored in the reservoir (Acre-Feet)

k1 = 0.00003; % K-factor coefficient
k2 = 9; % K-factor offset

MW2kW = 1000; % MW to kW
C2A = 1.98347/24; % Convert from CFS to AF/HR

C = [C2A k1 k2 MW2kW]; % Vector of constants

N = length(inFlow);

%% 3 - Define Linear Inequality Constraints and Bounds
DefineConstraints;

%% 4 - Objective function
% Use symbolic math for creating the objective function and then convert
% into a MATLAB function that can be evaluated.
%% 4.1 - Derive Objective Function using Symbolic Math
% First define the decision variables as X, which is a vector consisting
% of [TurbineFlow, SpillFlow] and all the parameters as symbolic objects
X = sym('x',[2*N,1]);   % turbine flow and spill flow
F = sym('F',[N,1]);     % flow into the reservoir
P = sym('P',[N,1]);     % price
s0 = sym('s0');         % initial storage
c = sym({'c1','c2','c3','c4'}.','r');   % constants

%%
% Total out flow equation
TotFlow = X(1:N)+X(N+1:end);

%%
% Storage Equations
S = cell(N,1);
S{1} = s0 + c(1)*(F(1)-TotFlow(1));

for ii = 2:N
    S{ii} = S{ii-1} + c(1)*(F(ii)-TotFlow(ii));
end

%%
% K factor equation
k = c(2)*([s0; S(1:end-1)]+ S)/2+c(3);

% MWh equation
MWh = k.*X(1:N)/c(4);

%%
% Revenue equation
R = P.*MWh;

%%
% Total Revenue (Minus sign changes from maximization to minimization)
totR = -sum(R);

%% 4.2 - Create Function File that can be Evaluated in MATLAB
% Create a function that when evaluated returns the value at a point

% Perform substitutions so that X is the only remaining variable
totR = subs(totR,[c;s0;P;F],[C';stor0;price;inFlow]);

% Generate a MATLAB file from the equation
matlabFunction(totR,'vars',{X},'file','objFcn');

%% 5 - Optimize with FMINCON
options = optimset('MaxFunEvals',Inf,'MaxIter',5000,...
    'Algorithm','interior-point','Display','iter');

tic
[x1, fval1] = fmincon(@objFcn,x0,A,b,Aeq,beq,LB,UB,[],options);
toc

% Visualize Results
plotResults(price, x1, N);

%% 6 - Optimize with FMINCON with supplied Gradient and Hessian
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Let's see if we can speed up the optimization by supplying the gradient
% and Hessian.  Passing these to the algorithm allows it to make more
% accurate decisions because it no longer has to approximate the gradient
% and hessian (approximation also takes longer as it requires many
% objective function evaluations).

%% 6.1 - Gradient
% Use the GRADIENT function from Symbolic Math to calculate the gradient
gradObj = gradient(totR,X);

% Create a function that when evaluated returns the gradient at a point
matlabFunction(gradObj,'vars',{X},'file','grad');

% The optimization routine expects two return arguments for a user-supplied
% gradient.  The first argument is the value of the objective function and
% the second argument is the gradient.  The DEAL function is used to return
% multiple output arguments through a function handle.
ValAndGrad = @(x) deal(objFcn(x),grad(x));

%% 6.2 - Hessian
% Use the HESSIAN function from Symbolic Math to calculate the Hessian
hessObj = hessian(totR,X);

% Create a function that when evaluated returns the Hessian at a point
matlabFunction(hessObj,'vars',{X},'file','Hess');

% The Hessian function has two input arguments (the current point and the
% Lagrange Multipliers) and returns the value of the Hessian at the current
% point.
hfcn = @(x,lambda) Hess(x);

%% 6.3 - Optimize
options = optimset('Disp','iter','Algorithm','interior-point',...
                   'MaxFunEvals',500,'MaxIter',5000,...
                   'GradObj','on','Hessian','user-supplied',...
                   'HessFcn',hfcn);

tic
[x2,fval2] = fmincon(ValAndGrad,x0,A,b,Aeq,beq,LB,UB,[],options);
toc

% Visualize Results
plotResults(price, x2, N);

%% 7 - Optimize using the QUADPROG Solver
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% QUADPROG minimizes functions of the type (1/2) * x' * H * x  +  f' * x
% First perform some substitutions into the revenue equation
Rsub = subs(R,[c;s0;P;F],[C';stor0;price;inFlow]);

% Total revenue (minus sign to flip maximization to minimization)
Rtot = -sum(Rsub);

%% 7.2 - Calculate and evaluate the linear component
fsym = gradient(Rtot,X);
f = double(subs(fsym,X,zeros(size(X))));

%% 7.1 - Calculate and evaluate the quadratic matrix
H = 0.5.*double(hessian(Rtot,X));

%% 7.3 - Optimize
qpoptions = optimset('Algorithm','interior-point-convex','Disp','iter');

tic
[x3,fval3] = quadprog(H,f,A,b,Aeq,beq,LB,UB,[],qpoptions);
toc

% Visualize Results
plotResults(price, x3, N);


function TDD = divdiff(X, Y)
%
% TDD = divdiff(X, Y)
%
% DIVDIFF
%   Newton's Method for Divided Differences.
%
%   The following formula is solved:
%       Pn(x) = f(x0) + f[x0,x1](x-x0) + f[x0,x1,x2](x-x0)(x-x1) + ...
%           + f[x0,x1,..,xn](x-x0)(x-x1)..(x-x[n-1])
%       where f[x0,x1] = (f(x1-f(x0))/(x1-x0)
%             f[x0,x1,..,xn] = (f[x1,..,xn]-f[x0,..,x_[n-1]])/(xn-x0)
%
% NOTE: f^(n+1)(csi)/(n+1)! aprox. = f[x0,x1,..,xn,x_[n+1]]
%
% Input::
%	X = [ x0 x1 .. xn ] - object vector
%	Y = [ y0 y1 .. yn ] - image vector
%
% Output:
%   TDD - table of divided differences
%
% Example:
%   TDD = divdiff( [ 1.35 1.37 1.40 1.45 ], [ .1303 .1367 .1461 .1614 ])
%
% Author:	Tashi Ravach
% Version:	1.0
% Date:     22/05/2006
%

    if nargin ~= 2
        error('divdiff: invalid input parameters');
    end

    [ p, m ] = size(X); % m points, polynomial order <= m-1
    if p ~= 1 || p ~=size(Y, 1) || m ~= size(Y, 2)
        error('divdiff: input vectors must have the same dimension');
    end

    TDD = zeros(m, m);
    TDD(:, 1) = Y';
    for j = 2 : m
        for i = 1 : (m - j + 1)
            TDD(i,j) = (TDD(i + 1, j - 1) - TDD(i, j - 1)) / (X(i + j - 1) - X(i));
        end
    end

end


function [detjacobian,invjacobian]=Jacobian(nnel,dhdr,dhds,xcoord,ycoord)

%------------------------------------------------------------------------
%  Purpose:
%     determine the Jacobian for two-dimensional mapping
%
%  Synopsis:
%     [detjacobian,invjacobian]=Jacobian(nnel,dhdr,dhds,xcoord,ycoord)
%
%  Variable Description:
%     jacobian - Jacobian for one-dimension
%     nnel - number of nodes per element
%     dhdr - derivative of shape functions w.r.t. natural coordinate r
%     dhds - derivative of shape functions w.r.t. natural coordinate s
%     xcoord - x axis coordinate values of nodes
%     ycoord - y axis coordinate values of nodes
%------------------------------------------------------------------------

 jacobian=zeros(2,2);

 for i=1:nnel
 jacobian(1,1) = jacobian(1,1)+dhdr(i)*xcoord(i);
 jacobian(1,2) = jacobian(1,2)+dhdr(i)*ycoord(i);
 jacobian(2,1) = jacobian(2,1)+dhds(i)*xcoord(i);
 jacobian(2,2) = jacobian(2,2)+dhds(i)*ycoord(i);
 end

 detjacobian = det(jacobian) ;  % Determinant of Jacobian matrix
 invjacobian = inv(jacobian) ;  % Inverse of Jacobian matrix