%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                           NACA airfoil generator
%                   coded by Manuel Diaz, NTU, 2015.04.27
%
% Refs:
% [1] Geometry for Aerodynamicists, Appendix A. Accessible online at:
%     http://www.dept.aoe.vt.edu/~mason/Mason_f/CAtxtAppA.pdf
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all;

%-------------------------------------------------------------------------%
% Define all the parameters
%-------------------------------------------------------------------------%

Na = 50;
M = 30;
A = 10; %Clustering points near leading edge, inf = no clustering
B = 2; %Clustering points near the surface, 1 = no clustering
t = .12;
c = 1;
L = 1;
N = floor(2*c/(2*c+L)*Na);
x1 = c*ones(1,N+1);
y1 = zeros(1,N+1);
x2 = x1;
y2 = y1;
y2(1) = c;
s1 = y1;
s2 = s1;
N2 = 250;

%-------------------------------------------------------------------------%
% Solve Algebraic Method
%-------------------------------------------------------------------------%
for i = 2:N2+1
    x1(i) = c*(1-(i-1)/N2);
    y1(i) = max([t/.2*(.2969*x1(i)^.5-.126*x1(i)-.3516*x1(i)^2+.2843*x1(i)^3-.1015*x1(i)^4) 0]);
    s1(i) = s1(i-1)+sqrt((x1(i)-x1(i-1))^2+(y1(i)-y1(i-1))^2);
    x2(i) = c*(1-2*(i-1)/N2);
    if x2(i) >= 0;
        y2(i) = c;
    else
        y2(i) = sqrt(c^2-(x2(i))^2);
    end
    s2(i) = s2(i-1)+sqrt((x2(i)-x2(i-1))^2+(y2(i)-y2(i-1))^2);
end
S1 = s1(end)*(1-exp(-(0:N)/A))/(1-exp(-(N)/A));
S1(end)/s1(end)
S2 = (0:N)*s2(end)/N;
for i = 1:N+1
    X1(i) = interp1(s1,x1,S1(i));
    Y1(i) = interp1(s1,y1,S1(i));
    X2(i) = interp1(s2,x2,S2(i));
    Y2(i) = interp1(s2,y2,S2(i));
end
r = exp(((1:(M+1))-M+1)/((M+1)/B));
r = r-r(1);
r = r/max(r);
for i = 1:N+1
    for j = 1:M+1
        x(i,j) = X1(i) + r(j)*(X2(i)-X1(i));
        y(i,j) = Y1(i) + r(j)*(Y2(i)-Y1(i));
    end
end
N = Na-N;
x = [((L+c):-L/N:(c+L/N))'*ones(1,M+1); x];
y = [(c*r'*ones(1,N))'; y];
x = [x; x(end-1:-1:1,:)];
y = [y ;-y(end-1:-1:1,:)];
[n,m] = size(x);

%-------------------------------------------------------------------------%
% Plot Algebraic Method
%-------------------------------------------------------------------------%
mesh(x,y,zeros(n,m))
view(0,90)
colormap([0 0 0])
axis('equal','tight');
xlabel('Length [unitless]','FontSize',14);
ylabel('Length [unitless]','FontSize',14);
title('Algebraic Method ','FontSize',18);

%-------------------------------------------------------------------------%
% Predetermined Constants
%-------------------------------------------------------------------------%
de = 1/M;
de2 = de^2;
dn = 1/Na;
dn2 = dn^2;
dedn = 2/(M*Na);
P = 100; %number of iterations for the elliptic solver.

%-------------------------------------------------------------------------%
% Solve Elliptic Method
%-------------------------------------------------------------------------%

for k = 1:P
    for i = 2:2*Na-1
        for j = 2:M-1
            xn = (x(i+1,j)-x(i-1,j))/(2*dn);
            yn = (y(i+1,j)-y(i-1,j))/(2*dn);
            xe = (x(i,j+1)-x(i,j-1))/(2*de);
            ye = (y(i,j+1)-y(i,j-1))/(2*de);
            a(i,j) = xn^2+yn^2;
            b(i,j) = xe*xn+ye*yn;
            c(i,j) = xe^2+ye^2;
        end
    end
    for i = 2:2*Na-1
        for j = 2:M-1
            x(i,j) = (a(i,j)/de2*(x(i+1,j)+x(i-1,j))+c(i,j)/dn2*(x(i,j+1)+x(i,j-1))...
                -b(i,j)/dedn*(x(i+1,j+1)-x(i+1,j-1)+x(i-1,j-1)-x(i-1,j+1)))...
                /(2*(a(i,j)/de2+c(i,j)/dn2));
            y(i,j) = (a(i,j)/de2*(y(i+1,j)+y(i-1,j))+c(i,j)/dn2*(y(i,j+1)+y(i,j-1))...
                -b(i,j)/dedn*(y(i+1,j+1)-y(i+1,j-1)+y(i-1,j-1)-y(i-1,j+1)))...
                /(2*(a(i,j)/de2+c(i,j)/dn2));
        end
    end
end
xn = ones(Na,M)/0;
yn = xn;
xe = xn;
ye = xn;
for i = 2:Na-1
    for j = 2:M-1
        xn(i,j) = (x(i+1,j)-x(i-1,j))/(2*dn);
        yn(i,j) = (y(i+1,j)-y(i-1,j))/(2*dn);
        xe(i,j) = (x(i,j+1)-x(i,j-1))/(2*de);
        ye(i,j) = (y(i,j+1)-y(i,j-1))/(2*de);
        J(i,j) = yn(i,j)*xe(i,j)-xn(i,j)*ye(i,j); % Jacobian
    end
end
figure(2)
mesh(x,y,zeros(n,m))
xlabel('Length [unitless]','FontSize',14);
ylabel('Length [unitless]','FontSize',14);
title('Elliptic Method ','FontSize',18);
axis('equal','tight');

%-------------------------------------------------------------------------%
% Plot Elliptic Method
%-------------------------------------------------------------------------%

view(0,90)
colormap([0 0 0])

%-------------------------------------------------------------------------%
% Plot Jacobian
%-------------------------------------------------------------------------%

figure(3)
x=2:30; y=2:50; [X,Y]=meshgrid(x,y);
surf(X,Y,J);
view(-45,45)
title('Jacobian ','FontSize',18);


function r=geodistance(ci,cf,m)

%GEODISTANCE: Calculates the distance in meters between two points on earth surface.
%
% Usage:  r = geodistance( coordinates1 , coordinates2 , method ) ;
%
%	  Where coordinates1 = [longitude1,latitude1] defines the
%	  initial position and coordinates2 = [longitude2,latitude2]
%	  defines the final position.
%	  Coordinates values should be specified in decimal degrees.
%	  Method can be an integer between 1 and 23, default is m = 6.
%         Methods 1 and 2 are based on spherical trigonometry and a
%         spheroidal model for the earth, respectively.
%	  Methods 3 to 24 use Vincenty's formulae, based on ellipsoid
%         parameters.
%         Here it follows the correspondence between m and the type of
%         ellipsoid:
%
%         m =  3 -> ANS ,        m =  4 -> GRS80,    m = 5 -> WGS72,
%         m =  6 -> WGS84,       m =  7 -> NSWC-9Z2,
%         m =  8 -> Clarke 1866, m =  9 -> Clarke 1880,
%         m = 10 -> Airy 1830,
%         m = 11 -> Bessel 1841 (Ethiopia,Indonesia,Japan,Korea),
%         m = 12 -> Bessel 1841 (Namibia),
%         m = 13 -> Sabah and Sarawak (Everest,Brunei,E.Malaysia),
%         m = 14 -> India 1830, m = 15 -> India 1956,
%         m = 16 -> W. Malaysia and Singapore 1948,
%         m = 17 -> W. Malaysia 1969,
%         m = 18 -> Helmert 1906, m = 19 -> Helmert 1960,
%         m = 20 -> Hayford International 1924,
%         m = 21 -> Hough 1960, m = 22 -> Krassovsky 1940,
%         m = 23 -> Modified Fischer 1960,
%         m = 24 -> South American 1969.
%
%	  Important notes:
%
%	 1)South latitudes are negative.
%	 2)East longitudes are positive.
%	 3)Great circle distance is the shortest distance between two points
%          on a sphere. This coincides with the circumference of a circle which
%          passes through both points and the centre of the sphere.
%	 4)Geodesic distance is the shortest distance between two points on a spheroid.
%	 5)Normal section distance is formed by a plane on a spheroid containing a
%          point at one end of the line and the normal of the point at the other end.
%          For all practical purposes, the difference between a normal section and a
%          geodesic distance is insignificant.
%	 6)The method m=2 assumes a spheroidal model for the earth with an average
%          radius of 6364.963 km. It has been derived for use within Australia.
%          The formula is estimated to have an accuracy of about 200 metres over 50 km,
%          but may deteriorate with longer distances.
%          However, it is not symmetric when the points are exchanged.
%
%  Examples: A = [150 -30]; B = [150 -31]; L = [151 -80];
%            [geodistance(A,B,1) geodistance(A,B,2) geodistance(A,B,3)]
%            [geodistance(A,L,1) geodistance(A,L,2) geodistance(A,L,3)]
%            geodistance([0 0],[2 3])
%            geodistance([2 3],[0 0])
%            geodistance([0 0],[2 3],1)
%            geodistance([2 3],[0 0],1)
%            geodistance([0 0],[2 3],2)
%            geodistance([2 3],[0 0],2)
%            for m = 1:24
%            r(m) = geodistance([150 -30],[151 -80],m);
%            end
%            plot([1:m],r), box on, grid on
%
%***************************************************************************************
% Second version: 07/11/2007
% Third  version: 03/08/2010
%
% Contact: orodrig@ualg.pt
%
% Any suggestions to improve the performance of this
% code will be greatly appreciated.
%
% Reference: Geodetic Calculations Methods
%            Geoscience Australia
%            (http://www.ga.gov.au/geodesy/calcs/)
%
%***************************************************************************************
% Changed a little bit by Mao,
% So, it works in vector computaiton now.
%
% Contact: argansos@hotmail.com
% Ganquan Mao, 15/09/2011
%
%***************************************************************************************

if size(ci,2)~=2 || size(cf,2)~=2
    error('ci cf must be a n*2 matrix!')
end

if size(ci,1)~=size(cf,1)
    error('ci cf must have same size!')
end

r=[];%for convergence

if nargin==2
    m=6;
end

lambda1=ci(:,1)*pi/180;
   phi1=ci(:,2)*pi/180;

lambda2=cf(:,1)*pi/180;
   phi2=cf(:,2)*pi/180;

L=lambda2-lambda1;

if m==1%great circle distance, based on spherical trigonometry
    r=180*1.852*60*acos(sin(phi1).*sin(phi2)...
      +cos(phi1).*cos(phi2).*cos(lambda2-lambda1))./pi;
    r=1000*abs(r);

elseif m==2%spheroidal model for the earth
    term1=111.08956*(ci(:,2)-cf(:,2)+0.000001);
    term2=cos(phi1+((phi2-phi1)/2));
    term3=(cf(:,1)-ci(:,1)+0.000001)/(cf(:,2)-ci(:,2)+0.000001);
    r=1000*abs(term1./cos(atan(term2.*term3)));

else%apply Vincenty's formulae (as long as the points are not coincident)
    alla=[0,0,6378160,6378137,6378135,6378137,6378145,6378206.4,...
          6378249.145,6377563.396,6377397.155,6377483.865,6377298.556,...
          6377276.345,6377301.243,6377304.063,6377295.664,6378200,...
          6378270,6378388,6378270,6378245,6378155,6378160];

    allf=[0,0,1/298.25,1/298.257222101,1/298.26,1/298.257223563,...
          1/298.25,1/294.9786982,1/293.465,1/299.3249646,1/299.1528128,...
          1/299.1528128,1/300.8017,1/300.8017,1/300.8017,1/300.8017,...
          1/300.8017,1/298.3,1/297,1/297,1/297,1/298.3,1/298.3,1/298.25];

    a=alla(m);
    f=allf(m);

    b=a*(1-f);

    axa=a^2;
    bxb=b^2;

    U1=atan((1-f)*tan(phi1));
    U2=atan((1-f)*tan(phi2));

    lambda=L;
    lambda_old=sqrt(-1);%there is no way a complex number is going to coincide with a real number!

    ntrials=0;%just in case...

    while any(abs(lambda-lambda_old)>1e-9)

        ntrials=ntrials+1;

        lambda_old=lambda;
        sin_sigma=sqrt((cos(U2).*sin(lambda)).^2+(cos(U1).*sin(U2)...
                  -sin(U1).*cos(U2).*cos(lambda)).^2);
        cos_sigma=sin(U1).*sin(U2)+cos(U1).*cos(U2).*cos(lambda);
        sigma=atan2(sin_sigma,cos_sigma);
        sin_alpha=cos(U1).*cos(U2).*sin(lambda)./sin_sigma;
        cos2_alpha=1-sin_alpha.^2;
        cos_2sigmam=cos_sigma-2*sin(U1).*sin(U2)./cos2_alpha;

        C=(f/16)*cos2_alpha.*(4+f*(4-3*cos2_alpha));

        lambda=L+f*(1-C).*sin_alpha.*(sigma+C.*sin_sigma.*(cos_2sigmam...
               +C.*cos_sigma.*(-1+2*(cos_2sigmam).^2)));

       % stop the function if convergence is not achieved
        if ntrials>1000
            disp('Convergence failure...')
            return
        end

    end

    % convergence achieved? get the distance
    uxu=cos2_alpha*(axa-bxb)/bxb;
    A=1+(uxu/16384).*(4096+uxu.*(-768+uxu.*(320-175*uxu)));
    B=(uxu/1024).*(256+uxu.*(-128+uxu.*(74-47*uxu)));
    delta_sigma=B.*sin_sigma.*(cos_2sigmam+(B/4).*(cos_sigma.*(-1+...
                2*cos_2sigmam.^2)-(B/6).*cos_2sigmam.*(-3+...
                4*sin_sigma.^2).*(-3+4*cos_2sigmam.^2)));
    r=b*A.*(sigma-delta_sigma);

end

r(isnan(r))=0;

end


function circle_segment_test08 ( )

%*****************************************************************************80
%
%% CIRCLE_SEGMENT_TEST08 tests CIRCLE_SEGMENT_CONTAINS_POINT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 May 2013
%
%  Author:
%
%    John Burkardt
%
  n = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_SEGMENT_TEST08\n' );
  fprintf ( 1, '  CIRCLE_SEGMENT_CONTAINS_POINT reports whether\n' );
  fprintf ( 1, '  a circle segment contains a point.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Pick a circle segment at random.\n' );
  fprintf ( 1, '  Compute %d sample points in the surrounding box.\n', n );
  fprintf ( 1, '  Compare the area of the segment to the percentage of points\n' );
  fprintf ( 1, '  contained in the circle segment.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       N          Omega1          Omega2           Area         Estimate\n' );
  fprintf ( 1, '\n' );

  r = 1.0;
  c = [ 0.0; 0.0 ];

  for test = 1 : 5

    [ u1, seed ] = r8_uniform_01 ( seed );
    [ u2, seed ] = r8_uniform_01 ( seed );
    omega1 = 2.0 * pi * u1;
    omega2 = 2.0 * pi * u2;

    if ( omega2 < omega1 )
      omega2 = omega2 + 2.0 * pi;
    end

    [ xy, seed ] = r8mat_uniform_01 ( 2, n, seed );
    xy = 2.0 * xy - 1.0;

    inout = zeros ( n, 1 );
    for j = 1 : n
      inout(j) = circle_segment_contains_point ( r, c, omega1, omega2, xy(1:2,j) );
    end

    theta = circle_segment_angle_from_chord_angles ( omega1, omega2 );
    area = circle_segment_area_from_angle ( r, theta );
    area_est = 4.0 * sum ( inout(1:n) ) / n;

    fprintf ( 1, '  %6d  %14g  %14g  %14g  %14g\n', n, omega1, omega2, area, area_est );

  end

  return
end


function aList=genAllBinCombinations(n,t,algorithm)
%%GENALLBINARYCOMBINATIONS Generate all combinations of t items chosen from
%              a set of n items, presenting the results as binary strings.
%
%INPUTS: n The number of items from which t items are chosen .
%        t The number of items chosen, t<=n.
% algorithm An optional parameter affecting which algorithm is used (and
%          thus the ordering of the codes). Possible values are:
%          0 (The default if omitted or an empty matrix is passed). Use
%            Algorithm C in Chapter 7.2.1.3 of [1]. The combinations are
%            given in the order of Chase's sequence.
%          1 Use Algorithm 2 in [2]. The combinations are given as a gray
%            code. Consecutive combinations differ in only 2 bits.
%
%OUTPUTS: aList An nXnumCombos list of all of the binary strings of n bits
%               choosing t of them to be 1.
%
%There is a total of numCombos=binomial(n,t) combinations.
%
%EXAMPLE:
% aList0=genAllBinCombinations(4,2,0)
% aList1=genAllBinCombinations(4,2,1)
%One will find that
% aList0=[0, 1, 0, 0, 1, 1;
%         0, 0, 1, 1, 0, 1;
%         1, 0, 0, 1, 1, 0;
%         1, 1, 1, 0, 0, 0];
% aList1=[1, 0, 1, 0, 0, 1;
%         1, 1, 0, 0, 1, 0;
%         0, 1, 1, 1, 0, 0;
%         0, 0, 0, 1, 1, 1];
%
%REFERENCES:
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%[2] J. R. Bitner, G. Ehrlich, and E. M. Reingold, "Efficient generation of
%    the binary reflected gray code and its applications," Communications
%    of the ACM, vol. 19, no. 9, pp. 517-521, Sep. 1976.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(algorithm))
    algorithm=0;
end

switch(algorithm)
    case 0
        aList=genAllBinCombinationsKnuth(n,t);
    case 1
        aList=genAllBinCombinationsGray(n,t);
    otherwise
        error('Unknown algorithm specified.')
end

end

function aList=genAllBinCombinationsKnuth(n,t)
%%GENALLBINARYCOMBINATIONS Generate all combinations of t items chosen from
%              a set of n items, presenting the results as binary strings.
%
%INPUTS: n The number of items from which t items are chosen.
%        t The number of items chosen, t<=n.
%
%OUTPUTS: aList An nXnumCombos list of all of the binary strings of n bits
%               choosing t of them to be 1.
%
%There is a total of numCombos=binomial(n,t) combinations. Algorithm C in
%Chapter 7.2.1.3 of [1] is used to generate the strings in the order of
%Chase's sequence.
%
%EXAMPLE:
% aList=genAllBinCombinations(4,2)
%One will find that
% aList=[0,1,0,0,1,1;
%        0,0,1,1,0,1;
%        1,0,0,1,1,0;
%        1,1,1,0,0,0];
%
%REFERENCES:
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

if(t==n)
    aList=ones(n,1);
    return
elseif(t==0)
    aList=zeros(n,1);
    return
end

s=n-t;

numCombos=binomial(n,t);
aList=zeros(n,numCombos);

%Step C1. a and w are indexed from 0 in [1].
a=[zeros(s,1);ones(t,1)];%
w=ones(n+1,1);
r=s;

for curCombo=1:numCombos
    %Step C1
    aList(:,curCombo)=a;

    %Step C3
    j=r;
    while(w(j+1)==0)
        w(j+1)=1;
        j=j+1;
        if(j==n)
            return;
        end
    end

    w(j+1)=0;
    if(mod(j,2)==1)%j is odd
        if(a(j+1)~=0)
            %Step C4
            a(j-1+1)=1;
            a(j+1)=0;
            if(r==j&&j>1)
                r=j-1;
            elseif(r==j-1)
                r=j;
            end
        else
            %Step C7
            if(a(j-1+1)~=0)
                %Step C6
                a(j+1)=1;
                a(j-1+1)=0;
                if(r==j&&j>1)
                    r=j-1;
                elseif(r==j-1)
                    r=j;
                end
            else
               a(j+1)=1;
               a(j-2+1)=0;
               if(r==j-2)
                   r=j;
               elseif(r==j-1)
                   r=j-2;
               end
            end
        end
    else%j is even
        if(a(j+1)~=0)
            %Step C5
            if(a(j-2+1)~=0)
                %Step C4
                a(j-1+1)=1;
                a(j+1)=0;
                if(r==j&&j>1)
                    r=j-1;
                elseif(r==j-1)
                    r=j;
                end
            else
                a(j-2+1)=1;
                a(j+1)=0;
                if(r==j)
                    r=max(j-2,1);
                elseif(r==j-2)
                    r=j-1;
                end
            end
        else
            %Step C6
            a(j+1)=1;
            a(j-1+1)=0;
            if(r==j&&j>1)
                r=j-1;
            elseif(r==j-1)
                r=j;
            end
        end
    end

end

end

function theCodes=genAllBinCombinationsGray(n,k)
%%GENALLBINCOMBINATIONSGRAY Generate all combinations of k items chosen
%              from a set of n items, presenting the results as binary
%              strings. Subsequent combinations differ by 2 bits.
%
%INPUTS: n The number of items from which k items are chosen.
%        t The number of items chosen, t<=n.
%
%OUTPUTS: aList An nXnumCombos list of all of the binary strings of n bits
%               choosing k of them to be 1.
%
%There is a total of numCombos=binomial(n,t) combinations. Algorithm 2 in
%[1] is used to generate all combinations are a gray code. Subsequent
%combinations differ in only 2 bits.
%
%REFERENCES:
%[1] J. R. Bitner, G. Ehrlich, and E. M. Reingold, "Efficient generation of
%    the binary reflected gray code and its applications," Communications
%    of the ACM, vol. 19, no. 9, pp. 517-521, Sep. 1976.
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

if(k==0)
    theCodes=zeros(n,1);
    return;
end

numCodes=binomial(n,k);
theCodes=zeros(n,numCodes);

g=[ones(k,1);zeros(n-k+2,1)];
tau=2:(n+2);
t=k;
tau(1)=k+1;
i=0;

curGrayCode=1;
while(i<n+1)
    theCodes(:,curGrayCode)=g(1:n);
    curGrayCode=curGrayCode+1;

    i=tau(1);
    tau(1)=tau(i);
    tau(i)=i+1;

    if(g(i)==1)
        if(t~=0)
            g(t)=1-g(t);
        else
            g(i-1)=1-g(i-1);
        end
        t=t+1;
    else
        if(t~=1)
            g(t-1)=1-g(t-1);
        else
            g(i-1)=1-g(i-1);
        end
        t=t-1;
    end

    g(i)=1-g(i);

    if(t==i-1||t==0)
        t=t+1;
    else
        t=t-g(i-1);
        tau(i-1)=tau(1);

        if(t==0)
            tau(1)=i-1;
        else
            tau(1)=t+1;
        end
    end
end
end

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


function [ area, radin, side ] = polygon_outrad_data ( n, radout )

%*****************************************************************************80
%
%% POLYGON_OUTRAD_DATA determines polygonal data from its outer radius.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sides of the polygon.
%    N must be at least 3.
%
%    Input, real RADOUT, the outer radius of the polygon, that is,
%    the radius of the smallest circle that can be described
%    around the polygon.
%
%    Output, real AREA, the area of the regular polygon.
%
%    Output, real RADIN, the inner radius of the polygon, that is,
%    the radius of the largest circle that can be inscribed
%    within the polygon.
%
%    Output, real SIDE, the length of one side of the polygon.
%
  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGON_OUTRAD_DATA - Fatal error!\n' );
    fprintf ( 1, '  Input value of N must be at least 3.\n' );
    fprintf ( 1, '  but your input value was N = %d\n', n );
    error ( 'POLYGON_OUTRAD_DATA - Fatal error!' );
  end

  angle = pi / n;
  area = 0.5 * n * radout * radout * sin ( 2.0 * angle );
  side = 2.0 * radout * sin ( angle );
  radin = 0.5 * side / tan ( angle );

  return
end


function [xi,w]=transformSimplexTriPts(xi,w,v)
%%TRANSFORMSIMPLEXTRIPTSTOTRI Given cubature points and weights for
%   integration over a the standard simplex triangle with vertices (1,0),
%   (0,1), and (0,0), transform the points and weights for integration over
%   the triangle with vertices given in v.
%
%INPUTS: xi The 2XnumCubPoints set of cubature points for the standard 2D
%           simplex triangle.
%         w The numCubPointsX1 associated cubature weights for the standard
%           2D simplex triangle.
%         v The 2X3 set of vertices of the new triangle.
%
%OUTPUTS: xi The 2XnumCubPoints set of cubature points for the specified
%            triangle.
%          w The 2XnumCubPoints set of transformed weights.
%
%With cubature formulae as in [1], the weights must sum to the area of the
%triangle. Thus, the first step here is to scale the weights by 2*the
%triangle area (the 2 is because the area of the basic simplex triangle is
%1/6). An affine transformation, as described in Chapter 5 of [2], is then
%performed to move the cubature weights into the new triangle.
%
%EXAMPLE:
%In this example, we find the integral of x^3*y^2 using fifth-order simplex
%points. The exact solution is 128/5 and one can see that the relative
%error between the solution found and the exact solution is within what one
%would expect due to finite precision limits.
% vertices=[0,2,4;
%           0,2,0];
% [xi,w]=fifthOrderSimplexCubPoints(2);
% [xi,w]=transformSimplexTriPts(xi,w,vertices);
% alpha=[3;2];
% cubSol=findMomentFromSamp(alpha,xi,w)
% exactSol=128/5
%
%REFERENCES:
%[1] A.H. Stroud, Approximate Calculation of Multiple Integrals. Cliffs,
%    NJ: Prentice-Hall, Inc., 1971.
%[2] K. Anjyo and H. Ochiai, Mathematical Basics of Motion and Deformation
%    in Computer Graphics, 2nd ed. Morgan and Claypool Publishers, 2017.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Adjust w for the triangle area.
T=triangleArea(v(:,1),v(:,2),v(:,3),true);
w=w*(2*T);

%Transforms the points into the new triangle.
A=[v(:,1)-v(:,3),v(:,2)-v(:,3)];
xi=A*xi+v(:,3);

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 [theta,axis] = dquat2rot(dq)

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

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

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

% check that it is a rotation dual quaternion
tol = 1e-5;
[maxval,imax] = max(max(abs(dq(5:8,:))));
if maxval > tol
    warning('DualQuaternion:dquat2rot:wrongFormat',...
        'At least one dual quaternion is not a rotation dual quaternion (tol = %.1e).\n Indices of max values: %d \n Max value = %.2e',...
        tol,imax,maxval);
end
normDQ = sqrt(sum(dq(1:4,:).^2));
[maxval2,imax2] = max(abs(normDQ-1));
if maxval2 > tol
       warning('DualQuaternion:dquat2rot:wrongFormatForRotation',...
        'At least one dual quaternion is not a rotation dual quaternion (is it a rotation dual quaternion?) (tol = %.1e).\n Indices of max values: %d \n Max value = %.2e',...
        tol,imax2,maxval2);
end

indpos = find(dq(1,:) > 1);
dq(1,indpos) = ones(1,length(indpos));
indneg = find(dq(1,:) < -1);
dq(1,indneg) = ones(1,length(indneg));

% Extraction of the rotation parameters
theta = 2*acosd(dq(1,:)); % acosd: [-1,1] --> [0,180]
theta = mod(theta,360);
indthetaOK = find(theta > 0);
axis = [ones(1,n) ; zeros(2,n)];
if length(indthetaOK > 0)
    repsin = repmat(sind(theta(indthetaOK)/2),3,1);
    axis(:,indthetaOK) = dq(2:4,indthetaOK)./repsin;
end


function area = trimeshSurfaceArea(v, e, f)
%TRIMESHSURFACEAREA Surface area of a triangular mesh.
%
%   S = trimeshSurfaceArea(V, F)
%   S = trimeshSurfaceArea(V, E, F)
%   Computes the surface area of the mesh specified by vertex array V and
%   face array F. Vertex array is a NV-by-3 array of coordinates.
%   Face array is a NF-by-3, containing vertex indices of each face.
%
%   Example
%     % Compute area of an octahedron (equal to 2*sqrt(3)*a*a, with
%     % a = sqrt(2) in this case)
%     [v f] = createOctahedron;
%     trimeshSurfaceArea(v, f)
%     ans =
%         6.9282
%
%     % triangulate a compute area of a unit cube
%     [v f] = createCube;
%     f2 = triangulateFaces(f);
%     trimeshSurfaceArea(v, f2)
%     ans =
%         6
%
%   See also
%   meshes3d, meshSurfaceArea, trimeshMeanBreadth, triangulateFaces

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

% check input number
if nargin == 2
    f = e;
end

% compute two direction vectors, using first vertex of each face as origin
v1 = v(f(:, 2), :) - v(f(:, 1), :);
v2 = v(f(:, 3), :) - v(f(:, 1), :);

% area of each triangle is half the cross product norm
vn = vectorNorm3d(crossProduct3d(v1, v2));

% sum up and normalize
area = sum(vn) / 2;


function cube_plots ( )

%*****************************************************************************80
%
%% CUBE_PLOTS plots the surface and the R(Theta) function for a cube.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CUBE_PLOTS:\n' );
  fprintf ( 1, '  For a 3D cube defined by a 0/1 characteristic function,\n' );
  fprintf ( 1, '  plot the surface, and R(Theta),\n' );
  fprintf ( 1, '  using a centered point, and then an offcentered point.\n' );
%
%  1): Plot the surface, using a centered base point.
%
  n1 = 31;
  n2 = 61;
  theta1 = linspace ( 0.0,       pi, n1 );
  theta2 = linspace ( 0.0, 2.0 * pi, n2 );

  [ t1, t2 ] = meshgrid ( theta2, theta1 );
  r = zeros ( n1, n2 );

  x0 = [ 0.0, 0.0, 0.0 ];
  for i = 1 : n1
    for j = 1 : n2
      r(i,j) = bisect_characteristic ( x0, [ t1(i,j); t2(i,j) ], @cube_characteristic );
    end
  end

  x1 = r .* cos ( t1 );
  x2 = r .* sin ( t1 ) .* cos ( t2 );
  x3 = r .* sin ( t1 ) .* sin ( t2 );

  figure ( 1 )
  mesh ( x3, x2, x1, 'FaceColor', 'interp' );
  axis equal
  grid on
  xlabel ( '<---X--->', 'FontSize', 16 );
  ylabel ( '<---Y--->', 'FontSize', 16 );
  zlabel ( '<---Z--->', 'FontSize', 16 );
  title ( 'Cube transition surface', 'FontSize', 24 )
  hold off
  filename =  'cube_centered_surface.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  2): Plot R as a function of Theta for a centered base point.
%
  figure ( 2 )
  mesh ( t1, t2, r, 'FaceColor', 'Interp' );
  axis ( [0, 2.0 * pi, 0.0, pi, 0.0, 1.5 ] );
  xlabel ( '<---Theta2--->', 'FontSize', 16 )
  ylabel ( '<---Theta1--->', 'FontSize', 16 );
  zlabel ( '<---R(Theta1,Theta2)--->', 'FontSize', 16 )
  title ( 'R(*,*) from base point to surface', 'FontSize', 24 )
  grid on
  filename = 'cube_centered_plot.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  3): Plot the surface, using an offcentered base point.
%
  n1 = 31;
  n2 = 61;
  theta1 = linspace ( 0.0,       pi, n1 );
  theta2 = linspace ( 0.0, 2.0 * pi, n2 );

  [ t1, t2 ] = meshgrid ( theta2, theta1 );
  r = zeros ( n1, n2 );

  x0 = [ +0.7, -0.2, -0.5 ];
  for i = 1 : n1
    for j = 1 : n2
      r(i,j) = bisect_characteristic ( x0, [ t1(i,j); t2(i,j) ], @cube_characteristic );
    end
  end

  x1 = r .* cos ( t1 );
  x2 = r .* sin ( t1 ) .* cos ( t2 );
  x3 = r .* sin ( t1 ) .* sin ( t2 );

  figure ( 3 )
  c = zeros ( n1, n2 );
  colormap ( 'gray' )
  mesh ( x3, x2, x1, c );
  axis equal
  grid on
  xlabel ( '<---X--->', 'FontSize', 16 );
  ylabel ( '<---Y--->', 'FontSize', 16 );
  zlabel ( '<---Z--->', 'FontSize', 16 );
  title ( 'Cube transition surface', 'FontSize', 24 )
  hold off
  filename =  'cube_offcentered_surface.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  4): Plot R as a function of Theta for an offcentered base point.
%
  figure ( 4 )
  mesh ( t1, t2, r, 'FaceColor', 'Interp' );
  xlabel ( '<---Theta2--->', 'FontSize', 16 )
  ylabel ( '<---Theta1--->', 'FontSize', 16 );
  zlabel ( '<---R(Theta1,Theta2)--->', 'FontSize', 16 )
  title ( 'R(*,*) from base point to surface', 'FontSize', 24 )
  grid on
  filename = 'cube_offcentered_plot.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Note variation in R for the offcenter case:\n' );
  fprintf ( 1, '  Min R = %g\n', min ( min ( r ) ) );
  fprintf ( 1, '  Max R = %g\n', max ( max ( r ) ) );
  return
end


function [model, llh] = hmmEm(x, init)
% EM algorithm to fit the parameters of HMM model (a.k.a Baum-Welch algorithm)
% Input:
%   x: 1 x n integer vector which is the sequence of observations
%   init: model or k
% Output:s
%   model: trained model structure
%   llh: loglikelihood
% Written by Mo Chen (sth4nth@gmail.com).
n = size(x,2);
X = sparse(x,1:n,1);
d = size(X,1);
if isstruct(init)   % init with a model
    A = init.A;
    E = init.E;
    s = init.s;
elseif numel(init) == 1  % random init with latent k
    k = init;
    s = normalize(rand(k,1),1);
    A = normalize(rand(k,k),2);
    E = normalize(rand(k,d),2);
end
tol = 1e-4;
maxIter = 1000;
llh = -inf(1,maxIter);
for iter = 2:maxIter
    M = E*X;
%     E-step
    [gamma,alpha,beta,c] = hmmSmoother(M,A,s);
    llh(iter) = mean(log(c));
    if abs(llh(iter)-llh(iter-1)) < tol*abs(llh(iter-1)); break; end   % check likelihood for convergence
%     M-step
    s = gamma(:,1);                                                                             % 13.18
    A = normalize(A.*(alpha(:,1:n-1)*(beta(:,2:n).*M(:,2:n)./c(2:n))'),2);      % 13.19 13.43 13.65
    E = (gamma*X')./sum(gamma,2);                            % 13.23
end
model.s = s;
model.A = A;
model.E = E;
llh = llh(2:iter);

function [gamma, alpha, beta, c] = hmmSmoother(M, A, s)
[K,T] = size(M);
At = A';
c = zeros(1,T);
alpha = zeros(K,T);
[alpha(:,1),c(1)] = normalize(s.*M(:,1),1);
for t = 2:T
    [alpha(:,t),c(t)] = normalize((At*alpha(:,t-1)).*M(:,t),1);  % 13.59
end
beta = ones(K,T);
for t = T-1:-1:1
    beta(:,t) = A*(beta(:,t+1).*M(:,t+1))/c(t+1);   % 13.62
end
gamma = alpha.*beta;                  % 13.64


function fx = p43_fun ( n, x )

%*****************************************************************************80
%
%% P43_FUN evaluates the integrand for problem 43.
%
%  Discussion:
%
%    The problem has a parameter ALPHA that can be set by calling
%    P43_PARAM_SET.
%
%    The suggested parameter range is 0.1 <= ALPHA <= 2.0.
%
%    The integrand has an algebraic endpoint singularity at X = 1
%    times a singular factor.
%
%  Interval:
%
%    0 <= x <= 1
%
%  Integrand:
%
%    ( ln ( 1 / x ) )^( alpha - 1 )
%
%  Exact Integral:
%
%    Gamma(alpha)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Piessens, Elise de Doncker-Kapenga,
%    Christian Ueberhuber, David Kahaner,
%    QUADPACK: A Subroutine Package for Automatic Integration,
%    Springer, 1983, page 84.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the integrand values.
%
  alpha = p43_param_get ( );

  fx(1:n) = ( log ( 1.0 ./ x(1:n) ) ).^( alpha - 1.0 );

  i = find ( x == 0.0 | x == 1.0 );
  fx(i) = 0.0;

  return
end


function A=vech2Mat(theVec,isSymmetric,offDiagScalFact)
%%VECH2MAT Given the main diagonal and lower-triangular elements of a
%          matrix stacked column-wise, obtain the original
%          symmetric or lower-triangular matrix. This function is
%          essentially the inverse of the vech function when applied to a
%          symmetric or lower-triangular matrix with the proper
%          parameterization.
%
%INPUTS: theVect An nX1 or 1Xn vector holding the diagonal and
%                lower-triangular components of a symmetric matrix stacked
%                column-wise.
%    isSymmetric An optional boolean parameter specifying whether the
%                matrix being recreated is symmetric. If false, a lower-
%                triangular matrix is formed. The default if this parameter
%                is omitted or an empty matrix is passed is true.
%   offDiagScalFact An optional scalar factor by which the off-diagonal
%                elements are scaled. If this parameter is omitted or an
%                empty matrix is passed, then the default value of 1 is
%                used.
%
%OUTPUTS: A The matrix impled by theVec and isSymmetric. This is a dXd
%           matrix, where n=d*(d+1)/2.
%
%The opposite of this function is vech.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(offDiagScalFact))
    offDiagScalFact=1;
end

if(nargin<2||isempty(isSymmetric))
   isSymmetric=true;
end

n=length(theVec);
theVec=theVec(:);

d=(1/2)*(-1+sqrt(1+8*n));
A=zeros(d,d);

curStart=1;
if(isSymmetric)
    for curCol=1:d
        %The diagonal element
        A(curCol,curCol)=theVec(curStart);
        span=(curStart+1):(curStart+d-curCol);

        %The lower-triangular part
        A((curCol+1):end,curCol)=theVec(span)*offDiagScalFact;
        %The upper triangular part.
        A(curCol,(curCol+1):end)=theVec(span)'*offDiagScalFact;
        curStart=curStart+d-curCol+1;
    end
else
    for curCol=1:d
        %The diagonal element
        A(curCol,curCol)=theVec(curStart);
        span=(curStart+1):(curStart+d-curCol);

        %The lower-triangular part
        A((curCol+1):end,curCol)=theVec(span)*offDiagScalFact;

        %The upper-triangular part is just zero.
        curStart=curStart+d-curCol+1;
    end
end
end

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


function varargout = createCube()
%CREATECUBE Create a 3D mesh representing the unit cube
%
%   [V, E, F] = createCube
%   Create a unit cube, as a polyhedra representation.
%   c has the form [V E F], where V is a 8-by-3 array with vertices
%   coordinates, E is a 12-by-2 array containing indices of neighbour
%   vertices, and F is a 6-by-4 array containing vertices array of each
%   face.
%
%   [V, F] = createCube;
%   Returns only the vertices and the face vertex indices.
%
%   MESH = createCube;
%   Returns the data as a mesh structure, with fields 'vertices', 'edges'
%   and 'faces'.
%
%   Example
%   [n, e, f] = createCube;
%   drawMesh(n, f);
%
%   See also
%   meshes3d, drawMesh
%   createOctahedron, createTetrahedron, createDodecahedron
%   createIcosahedron, createCubeOctahedron
%

% ---------
% author : David Legland
% e-mail: david.legland@inra.fr
% INRA - TPV URPOI - BIA IMASTE
% created the 10/02/2005.

%   HISTORY
%   04/01/2007: remove unused variables

x0 = 0; dx= 1;
y0 = 0; dy= 1;
z0 = 0; dz= 1;

nodes = [...
    x0 y0 z0; ...
    x0+dx y0 z0; ...
    x0 y0+dy z0; ...
    x0+dx y0+dy z0; ...
    x0 y0 z0+dz; ...
    x0+dx y0 z0+dz; ...
    x0 y0+dy z0+dz; ...
    x0+dx y0+dy z0+dz];

edges = [1 2;1 3;1 5;2 4;2 6;3 4;3 7;4 8;5 6;5 7;6 8;7 8];

% faces are oriented such that normals point outwards
faces = [1 3 4 2;5 6 8 7;2 4 8 6;1 5 7 3;1 2 6 5;3 7 8 4];

% format output
varargout = formatMeshOutput(nargout, nodes, edges, faces);


function Y = sym_sne(X, d, perplexity)
%SNE Implementation of symmetric Stochastic Neighbor Embedding
%
%   mappedX = sym_sne(X, no_dims, perplexity)
%
% Runs the symmetric Stochastic Neighbor Embedding algorithm. The
% high-dimensional datapoints are specified by X. The target dimensionality
% is specified in no_dims (default = 2), and the perplexity of the Gaussian
% kernel is specified in perplexity (default = 30).
% The function returns the embedded points in mappedX.
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    if ~exist('d', 'var') || isempty(d)
        d = 2;
    end
    if ~exist('perplexity', 'var') || isempty(perplexity)
        perplexity = 30;
    end

    % Initialize some variables
    n = size(X, 1);                 % number of instances
    eta = .05;                      % learning rate
    max_iter = 2000;                % maximum number of iterations
    jitter = 0.3;                   % initial jitter
    jitter_decay = 0.99;            % jitter decay
    momentum = 0.5;                 % initial momentum
    final_momentum = 0.8;           % final momentum
    mom_switch_iter = 750;          % iteration where momentum changes

    % Initialize embedding coordinates randomly (close to origin)
    Y = 0.0001 * rand(n, d);
    dC = zeros(n, d);
    y_incs = zeros(n, d);

    % Compute Gaussian kernel for high-dimensional data representation
    P = x2p(X, perplexity, 1e-5);                                        % use fixed perplexity
    P = P + P';
    P = P ./ sum(P(:));
    P = max(P, eps);

    % Iterating loop
    for iter=1:max_iter

        % Compute Gaussian kernel for low-dimensional data representation
        sum_Y = sum(Y .^ 2, 2);                                                     % precomputation for pairwise distances
        Q = exp(-bsxfun(@plus, sum_Y, bsxfun(@plus, sum_Y', -2 * Y * Y')) ./ 2);    % Gaussian probabilities
        Q = Q ./ repmat(sum(Q, 2), [1 n]);
        Q = max(Q, eps);

        % Compute cost function between P and Q
        if ~rem(iter, 20)
            costs = sum(P .* log((P + eps) ./ (Q + eps)), 2) ./ n;              % division by n corrects for # of datapoints
            cost = sum(costs);
            disp(['Iteration ' num2str(iter) ': error is ' num2str(cost)]);
        end

        % Compute gradient
        PQ = P - Q;
        for i=1:n
            dC(i,:) = sum(bsxfun(@times, bsxfun(@minus, Y(i,:), Y), PQ(i,:)'), 1);
        end

        % Perform the gradient search
        y_incs = momentum * y_incs - eta * dC;
        Y = Y + y_incs;
		Y = Y + jitter * randn(size(Y));
        Y = bsxfun(@minus, Y, mean(Y, 1));

        % Reduce jitter over time and change momentum
        jitter = jitter * jitter_decay;
        if iter == mom_switch_iter
            momentum = final_momentum;
        end
    end


function [ x, seed ] = quasigeometric_sample ( a, b, seed )

%*****************************************************************************80
%
%% QUASIGEOMETRIC_SAMPLE samples the Quasigeometric PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, the probability of 0 successes.
%    0.0 <= A <= 1.0.
%
%    Input, real B, the depreciation constant.
%    0.0 <= B < 1.0.
%
%    Input, integer SEED, a seed for the random
%    number generator.
%
%    Output, integer X, a sample of the PDF.
%
%    Output, integer SEED, a seed for the random
%    number generator.
%
  [ cdf, seed ] = r8_uniform_01 ( seed );

  x = quasigeometric_cdf_inv ( cdf, a, b );

  return
end


function ref = physical_to_reference_tet4 ( t, n, phy )

%*****************************************************************************80
%
%% PHYSICAL_TO_REFERENCE_TET4 maps physical points to reference points.
%
%  Discussion:
%
%    Given the vertices of an order 4 physical tetrahedron and a point
%    (X,Y,Z) in the physical tetrahedron, the routine computes the value
%    of the corresponding point (R,S,T) in the reference tetrahedron.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(3,4), the coordinates of the vertices of the
%    physical tetrahedron.  The vertices are assumed to be the images of
%    (1,0,0), (0,1,0), (0,0,1) and (0,0,0) respectively.
%
%    Input, integer N, the number of points to transform.
%
%    Input, real PHY(3,N), the coordinates of physical points
%    to be transformed.
%
%    Output, real REF(3,N), the coordinates of the corresponding
%    points in the reference tetrahedron.
%
  a(1:3,1:3) = t(1:3,1:3);
  for i = 1 : 3
    a(i,1:3) = a(i,1:3) - t(i,4);
  end

  for i = 1 : 3
    ref(i,1:n) = phy(i,1:n) - t(i,4);
  end

  ref(1:3,1:n) = a(1:3,1:3) \ ref(1:3,1:n);

  return
end


function B=autocorr2d(H)

% Compute the 2D  autocorrelation of matrix ( Image )
% Algorithm based on Wiener - Khintchine Theorem*.
%
% * :http://mathworld.wolfram.com/Wiener-KhinchinTheorem.html
%
% Special Case :
% 2D zero mean Gaussian process with variance=1:
%           ==========================
%           | P=randn(400,600);      |
%           | CorrFunc=autocorr2d(H);|
%           ==========================
% CorrFunc is symmetric with Dirac impulsion in the center.
% with max(CorrFunc(:))=1
% July, 24, 2012
% KHMOU Youssef

[n m]=size(H);
% Divide by the size for normalization

B=abs(fftshift(ifft2(fft2(H).*conj(fft2(H)))))./(n*m);
figure, surf(B);
shading interp;


function [ w, x ] = line_o05 ( )

%*****************************************************************************80
%
%% LINE_O05 returns a 5 point quadrature rule for the unit line.
%
%  Discussion:
%
%    The integration region is:
%
%    - 1.0 <= X <= 1.0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carlos Felippa,
%    A compendium of FEM integration formulas for symbolic work,
%    Engineering Computation,
%    Volume 21, Number 8, 2004, pages 867-890.
%
%  Parameters:
%
%    Output, real W(5), the weights.
%
%    Output, real X(5), the abscissas.
%
  w(1:5) = [ ...
    0.118463442528095, ...
    0.239314335249683, ...
    0.284444444444444, ...
    0.239314335249683, ...
    0.118463442528095 ];

  x(1:5) = [ ...
    -0.90617984593866399280, ...
    -0.53846931010568309104, ...
     0.00000000000000000000, ...
     0.53846931010568309104, ...
     0.90617984593866399280 ];

  return
end


function [x,tspan] = eulermaruyama(f,L,tspan,x0,Q)
%% EULERMARUYAMA - Numerical SDE solver: The Euler-Maruyama method
%
% Syntax:
%   [x,tspan] = eulermaruyama(f,L,tspan,x0,Q)
%
% In:
%   f      - Drift function, f(x,t)
%   L      - Diffusion function, L(x,t)
%   tspan  - Time steps to simulate, [t0,...,tend]
%   x0     - Initial condition
%   Q      - Spectral density (default: standard Brownian motion)
%
% Out:
%   x      - Solved values
%   tspan  - Time steps
%
% Description:
%   Integrates the system of stochatic differential equations
%     dx = f(x,t) dt + L(x,t) dbeta,  for x(0) = x0
%   over the time interval defined in tspan.
%
% Copyright:
%   2018 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

%%

  % Check if Q given
  if nargin<5 || isempty(Q), Q = eye(size(L(x0,tspan(1)),2)); end

  % Cholesky factor of Q
  cQ = chol(Q,'lower');

  % Number of steps
  steps = numel(tspan);

  % Allocate space
  x = zeros(size(x0,1),steps);

  % Initial state
  x(:,1) = x0;

  % Iterate
  for k=2:steps

    % Time discretization
    dt = tspan(k)-tspan(k-1);

    % Increment
    db = sqrt(dt)*cQ*randn(size(Q,1),1);

    % Step
    x(:,k) = x(:,k-1) + ...
        f(x(:,k-1),tspan(k-1))*dt + ...
        L(x(:,k-1),tspan(k-1))*db;

  end


%computes the lag of the average magnitude difference function
%> called by ::ComputePitch
%>
%> @param x: audio signal
%> @param iBlockLength: block length in samples
%> @param iHopLength: hop length in samples
%> @param f_s: sample rate of audio data
%>
%> @retval f_0 amdf lag (in Hz)
%> @retval t time stamp of f_0 estimate (in s)
% ======================================================================
function [f_0, t] = PitchTimeAmdf(x, iBlockLength, iHopLength, f_s)

    % blocking
    [x_b, t] = ToolBlockAudio(x, iBlockLength, iHopLength, f_s);
    iNumOfBlocks = size(x_b, 1);

    % allocate memory
    f_0 = zeros(1, iNumOfBlocks);

    % initialization
    f_max = 2000;
    f_min = 50;
    eta_min = round(f_s/f_max);
    eta_max = round(f_s/f_min);

    for n = 1:iNumOfBlocks

        if (sum(abs(x_b(n, :))) == 0)
            f_0(n) = 0;
            continue;
        end

        % calculate the amdf_I minimum
        afAMDF = amdf_I(x_b(n, :), eta_max);
        [fDummy, T_0(n)] = min(afAMDF(1+eta_min:end));

        % convert to Hz
        f_0(n) = f_s ./ (T_0(n) + eta_min);
    end
end

function [AMDF] = amdf_I(x, eta_max)
    K = length(x);

    AMDF = ones(1, K);

    for eta=0:min(K-1,eta_max-1)
        AMDF(eta+1) = sum(abs(x(1:K-1-eta) - x(eta+2:end))) / K;
    end
end


function julia(arg1,arg2)
%
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%
%               Julia Fractals Using Matlab
%               Written By Sridharan, Mithun Aiyswaryan
%               Christian Albrechts Universität zu Kiel, Germany
%               Mail Your Comments At: s.mithun@indiatimes.com
%
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%               Help :
%               This program takes two arguments and computes the
%               Julia sets using the provided values.
%               For the best viewing results, varhoose the arguments such that:
%
%               Argument 1 > 10     &             Argument 2 > 100
%
%               Note : The generation of fractals is  computationally
%                      very intensive and it may take you a while before
%                      you observe the results on the screen!
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

xaxis=0;
yaxis=0;
l=1.5;
x=linspace(xaxis-l,xaxis+l,arg2);
y=linspace(yaxis-l,yaxis+l,arg2);
[xtrans,ytrans]=meshgrid(x,y);
var= -.745429;
ztrans=xtrans+i*ytrans;
for k=1:arg1;
ztrans=ztrans.^2+var;
t=exp(-abs(ztrans));
end
colormap prism(256)
pcolor(t);
shading flat;
axis('square','equal','off');


function [x, infos] = alternating_onmf(V, rank, in_options)
% Orthogonal two-block coordinate descent (2-BCD) for non-negative matrix factorization (Alt-Orth-NMF).
%
% The problem of interest is defined as
%
%       min || V - WH ||_F^2,
%       where
%       {V, W, H} >= 0, and W or H is orthogonal, i.e., HH^T = I_r.
%
% Given a non-negative matrix V, factorized non-negative matrices {W, H} are calculated.
%
%
% Inputs:
%       matrix      V
%       rank        rank
%
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       F. Pompili, N. Gillis, P.-A. Absil and F. Glineur,
%       "Two Algorithms for Orthogonal Nonnegative Matrix Factorization
%       with Application to Clustering,"
%       Neurocomputing 141, pp. 15-25, 2014.
%
%
% This file is part of NMFLibrary.
%
% This file has been ported from
% alternatingONMF.m at https://gitlab.com/ngillis/nmfbook/-/tree/master/algorithms
% by Nicolas Gillis (nicolas.gillis@umons.ac.be)
%
% Change log:
%
%       June 14, 2022 (Hiroyuki Kasai): Ported initial version
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.orth_h = true;
    local_options.delta = 0;
    local_options.special_stop_condition = @(epoch, infos, options, stop_options) alt_nmf_stop_func(epoch, infos, options, stop_options);

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W; % use only W

    % initialize
    method_name = 'ALT-ONMF';
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % initialize for this algorithm
    % Xn: normalized version of X, ||Xn(:,j)||_2  1 for all j
    norm2x = sqrt(sum(V.^2,1));
    Vn = V .* repmat(1./(norm2x+1e-16), m, 1);
    normV2 = sum(V(:).^2);
    stop_options.normV2 = normV2;
    H = nnls_orth(V, W, Vn);

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, W, H, [], options, [], epoch, grad_calc_count, 0);
    orth_val = norm(H*H' - eye(rank), 'fro');
    [infos.orth] = orth_val;
    infos.prev_error =  Inf;

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag, infos] = check_stop_condition(epoch, infos, options, stop_options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % update H
        % H = argmin_H ||X-WH||_F, H >= 0, rows H orthogonal up to a scaling of the rows of H
        H = nnls_orth(V, W, Vn);

        % normalize rows of H
        norm2h = sqrt(sum(H'.^2, 1)) + 1e-16;
        H = repmat(1./norm2h', 1, n) .* H;

        % update W
        W = V * H';

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, W, H, [], options, infos, epoch, grad_calc_count, elapsed_time);
        orth_val = norm(H*H' - eye(rank), 'fro');
        [infos.orth] = [infos.orth orth_val];

        % display info
        display_info(method_name, epoch, infos, options);

        % check convergence (by original code)
        e = sqrt( (normV2-sum(sum(W.^2)))/normV2 );
        if (epoch > 2) && (abs(e_prev-e) < options.delta)
            %break;
        else
            e_prev = e;
        end
    end

    x.W = W;
    x.H = H;

end

function [stop_flag, reason, infos] = alt_nmf_stop_func(epoch, infos, options, stop_options)

    stop_flag = false;
    reason = [];

    normV2 = stop_options.normV2;
    W = infos.final_W;

    error = sqrt( (normV2-sum(sum(W.^2)))/normV2 );
    if (epoch > 2) && (abs(infos.prev_error - error) < options.delta)
        stop_flag = true;
        reason = sprintf('Relative solution change tolerance reached: %.4e < %.4e\n', abs(infos.prev_error - error), options.delta);
    else
        infos.prev_error = error;
    end

end


% Copyright (C) 2000 Paul Kienzle  <pkienzle@users.sf.net>
% Copyright (C) 2007 Peter L. Soendergaard
%
% This program is free software; you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation; either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
% FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program; if not, see <http://www.gnu.org/licenses/>.

% -*- texinfo -*-
% @deftypefn {Function File} {@var{h} =} hilbert (@var{f}, @var{N}, @var{dim})
% Analytic extension of real valued signal.
%
% @code{@var{h} = hilbert (@var{f})} computes the extension of the real
% valued signal @var{f} to an analytic signal. If @var{f} is a matrix,
% the transformation is applied to each column. For N-D arrays,
% the transformation is applied to the first non-singleton dimension.
%
% @code{real (@var{h})} contains the original signal @var{f}.
% @code{imag (@var{h})} contains the Hilbert transform of @var{f}.
%
% @code{hilbert (@var{f}, @var{N})} does the same using a length @var{N}
% Hilbert transform. The result will also have length @var{N}.
%
% @code{hilbert (@var{f}, [], @var{dim})} or
% @code{hilbert (@var{f}, @var{N}, @var{dim})} does the same along
% dimension @var{dim}.
% @end deftypefn

function f = oc_hilbert(f, N, dim)

% ------ PRE: initialization and dimension shifting ---------

if (nargin<1 || nargin>3)
    error('Invalid call');
end
if (nargin < 3)
    dim = [];
end
if (nargin < 2)
    N = [];
end

if ~isreal(f)
    warning ('HILBERT: ignoring imaginary part of signal');
    f = real (f);
end

D=ndims(f);

% Dummy assignment.
order=1;

if isempty(dim)
    dim=1;

    if sum(size(f)>1)==1
        % We have a vector, find the dimension where it lives.
        dim=find(size(f)>1);
    end

else
    if (numel(dim)~=1 || ~isnumeric(dim))
        error('HILBERT: dim must be a scalar.');
    end
    if rem(dim,1)~=0
        error('HILBERT: dim must be an integer.');
    end
    if (dim<1) || (dim>D)
        error('HILBERT: dim must be in the range from 1 to %d.',D);
    end

end

if (numel(N)>1 || ~isnumeric(N))
    error('N must be a scalar.');
elseif (~isempty(N) && rem(N,1)~=0)
    error('N must be an integer.');
end

if dim>1
    order=[dim, 1:dim-1,dim+1:D];

    % Put the desired dimension first.
    f=permute(f,order);

end

Ls=size(f,1);

% If N is empty it is set to be the length of the transform.
if isempty(N)
    N=Ls;
end

% Remember the exact size for later and modify it for the new length
permutedsize=size(f);
permutedsize(1)=N;

% Reshape f to a matrix.
f=reshape(f,size(f,1),numel(f)/size(f,1));
W=size(f,2);

if ~isempty(N)
    f = oc_postpad(f,N);
end

% ------- actual computation -----------------
if N>2
    f=fft(f);

    if rem(N,2)==0
        f=[f(1,:);
            2*f(2:N/2,:);
            f(N/2+1,:);
            zeros(N/2-1,W)];
    else
        f=[f(1,:);
            2*f(2:(N+1)/2,:);
            zeros((N-1)/2,W)];
    end

    f=ifft(f);
end

% ------- POST: Restoration of dimensions ------------

% Restore the original, permuted shape.
f=reshape(f,permutedsize);

if dim>1
    % Undo the permutation.
    f=ipermute(f,order);
end

end

%!demo
%! % notice that the imaginary signal is phase-shifted 90 degrees
%! t=linspace(0,10,256);
%! z = hilbert(sin(2*pi*0.5*t));
%! grid on; plot(t,real(z),';real;',t,imag(z),';imag;');

%!demo
%! % the magnitude of the hilbert transform eliminates the carrier
%! t=linspace(0,10,1024);
%! x=5*cos(0.2*t).*sin(100*t);
%! grid on; plot(t,x,'g;z;',t,abs(hilbert(x)),'b;|hilbert(z)|;');


function r = tenRank( A, tol )
%TENRANK Calculates the multilinear rank.
%   R = TENRANK( A, TOL ) calculates the multilinear rank R of the
%   given tensor A. All singular values below the specified tolerance
%   TOL are considered to be zero.
%
%   R = TENRANK( A ) calculates the multilinear rank R of the given
%   tensor A. The default tolerance 1e-5 is chosen for the cutoff.
%

%   GeomCG Tensor Completion. Copyright 2013 by
%   Michael Steinlechner
%   Questions and contact: michael.steinlechner@epfl.ch
%   BSD 2-clause license, see LICENSE.txt

    if ~exist('tol','var')
        tol = 1e-5;
    end

    r = zeros( 1, ndims( A ) );

    for i = 1:ndims( A )
        r(i) = rank( double( tenmat( A, i ) ), tol);
    end
end


function moment = truncated_normal_ab_moment ( order, mu, s, a, b )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_AB_MOMENT returns a moment of the truncated Normal PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Phoebus Dhrymes,
%    Moments of Truncated Normal Distributions,
%    May 2005.
%
%  Parameters:
%
%    Input, integer ORDER, the order of the moment.
%    0 <= ORDER.
%
%    Input, real MU, S, the mean and standard deviation of the
%    parent Normal distribution.
%    0 < S.
%
%    Input, real A, B, the lower and upper truncation limits.
%    A < B.
%
%    Output, real MOMENT, the moment of the PDF.
%
  if ( order < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRUNCATED_NORMAL_AB_MOMENT - Fatal error!\n' );
    fprintf ( 1, '  ORDER < 0.\n' );
    error ( 'TRUNCATED_NORMAL_AB_MOMENT - Fatal error!\n' );
  end

  if ( s <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRUNCATED_NORMAL_AB_MOMENT - Fatal error!\n' );
    fprintf ( 1, '  S <= 0.0.\n' );
    error ( 'TRUNCATED_NORMAL_AB_MOMENT - Fatal error!\n' );
  end

  if ( b <= a )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRUNCATED_NORMAL_AB_MOMENT - Fatal error!\n' );
    fprintf ( 1, '  B <= A.\n' );
    error ( 'TRUNCATED_NORMAL_AB_MOMENT - Fatal error!\n' );
  end

  a_h = ( a - mu ) / s;
  a_pdf = normal_01_pdf ( a_h );
  a_cdf = normal_01_cdf ( a_h );

  if ( a_cdf == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRUNCATED_NORMAL_AB_MOMENT - Fatal error!\n' );
    fprintf ( 1, '  PDF/CDF ratio fails, because A_CDF is too small.\n' );
    fprintf ( 1, '  A_PDF = %g\n', a_pdf );
    fprintf ( 1, '  A_CDF = %g\n', a_cdf );
    error ( 'TRUNCATED_NORMAL_AB_MOMENT - Fatal error!\n' );
  end

  b_h = ( b - mu ) / s;
  b_pdf = normal_01_pdf ( b_h );
  b_cdf = normal_01_cdf ( b_h );

  if ( b_cdf == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRUNCATED_NORMAL_AB_MOMENT - Fatal error!\n' );
    fprintf ( 1, '  PDF/CDF ratio fails, because B_CDF too small.\n' );
    fprintf ( 1, '  B_PDF = %g\n', b_pdf );
    fprintf ( 1, '  B_CDF = %g\n', b_cdf );
    error ( 'TRUNCATED_NORMAL_AB_MOMENT - Fatal error!\n' );
  end

  moment = 0.0;
  irm2 = 0.0;
  irm1 = 0.0;

  for r = 0 : order

    if ( r == 0 )
      ir = 1.0;
    elseif ( r == 1 )
      ir = - ( b_pdf - a_pdf ) / ( b_cdf - a_cdf );
    else
      ir = ( r - 1 ) * irm2 ...
        - ( b_h ^ ( r - 1 ) * b_pdf - a_h ^ ( r - 1 ) * a_pdf ) ...
        / ( b_cdf - a_cdf );
    end

    moment = moment + r8_choose ( order, r ) ...
      * mu ^ ( order - r ) ...
      * s ^ r * ir;

    irm2 = irm1;
    irm1 = ir;

  end

  return
end


function r=rot4x(theta)
    % r = rot4x(theta)
    %
    % rotx produces a 4x4 rotation matrix representing
    % a rotation by theta radians about the x axis.
    %
    %	Argument definitions:
    %
    %	theta = rotation angle in radians
    c = cos(theta);
    s = sin(theta);
    r = [1  0  0 0;
        0  c -s 0;
        0  s  c 0;
        0  0  0 1];


function value = pyramid_unit_volume_3d ( )

%*****************************************************************************80
%
%% PYRAMID_UNIT_VOLUME_3D returns the volume of a unit pyramid in 3D.
%
%  Integration region:
%
%    - ( 1 - Z ) <= X <= 1 - Z
%    - ( 1 - Z ) <= Y <= 1 - Z
%              0 <= Z <= 1.
%
%  Discussion:
%
%    A pyramid with square base can be regarded as the upper half of a
%    3D octahedron.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, the volume of the pyramid.
%
  value = 4.0 / 3.0;

  return
end


function a = bab ( n, alpha, beta )

%*****************************************************************************80
%
%% BAB returns the BAB matrix.
%
%  Example:
%
%    N = 5
%    ALPHA = 5, BETA = 2
%
%    5  2  .  .  .
%    2  5  2  .  .
%    .  2  5  2  .
%    .  .  2  5  2
%    .  .  .  2  5
%
%  Properties:
%
%    A is banded, with bandwidth 3.
%
%    A is tridiagonal.
%
%    Because A is tridiagonal, it has property A (bipartite).
%
%    A is Toeplitz: constant along diagonals.
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    CM da Fonseca, J Petronilho,
%    Explicit Inverses of Some Tridiagonal Matrices,
%    Linear Algebra and Its Applications,
%    Volume 325, 2001, pages 7-21.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real ALPHA, BETA, the parameters.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    a(i,i) = alpha;
  end

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

  return
end


function [cm, cSq] = DiscreteFrechetDist(P,Q,dfcn)
% Calculates the discrete Frechet distance between curves P and Q
%
% [cm, cSq] = DiscreteFrechetDist(P,Q)
% [cm, cSq] = DiscreteFrechetDist(...,dfcn)
%
% P and Q are two sets of points that define polygonal curves with rows of
% vertices (data points) and columns of dimensionality. The points along
% the curves are taken to be in the order as they appear in P and Q.
%
% Returned in cm is the discrete Frechet distance, aka the coupling
% measure, which is zero when P equals Q and grows positively as the curves
% become more dissimilar.
%
% The optional dfcn argument allows the user to specify a function with
% which to calculate distance between points in P and Q. If not provided,
% the L2 norm is used.
%
% The secondary output, cSq, is the coupling sequence, that is, the
% sequence of steps along each curve that must be followed to achieve the
% minimum coupling distance, cm. The output is returned in the form of a
% matrix with column 1 being the index of each point in P and column 2
% being the index of each point in Q. (NOTE: the coupling sequence is not
% unique in general)
%
% Explanation:
% The Frechet distance is a measure of similarity between to curves, P and
% Q. It is defined as the minimum cord-length sufficient to join a point
% traveling forward along P and one traveling forward along Q, although the
% rate of travel for either point may not necessarily be uniform.
%
% The Frechet distance, FD, is not in general computable for any given
% continuous P and Q. However, the discrete Frechet Distance, also called
% the coupling measure, cm, is a metric that acts on the endpoints of
% curves represented as polygonal chains. The magnitude of the coupling
% measure is bounded by FD plus the length of the longest segment in either
% P or Q,  and approaches FD in the limit of sampling P and Q.
%
% This function implements the algorithm to calculate discrete Frechet
% distance outlined in:
% T. Eiter and H. Mannila. Computing discrete Frechet distance. Technical
% Report 94/64, Christian Doppler Laboratory, Vienna University of
% Technology, 1994.
%
%
%
% EXAMPLE:
% % create data
% t = 0:pi/8:2*pi;
% y = linspace(1,3,6);
% P = [(2:7)' y']+0.3.*randn(6,2);
% Q = [t' sin(t')]+2+0.3.*randn(length(t),2);
% [cm, cSq] = DiscreteFrechetDist(P,Q);
% % plot result
% figure
% plot(Q(:,1),Q(:,2),'o-r','linewidth',3,'markerfacecolor','r')
% hold on
% plot(P(:,1),P(:,2),'o-b','linewidth',3,'markerfacecolor','b')
% title(['Discrete Frechet Distance of curves P and Q: ' num2str(cm)])
% legend('Q','P','location','best')
% line([2 cm+2],[0.5 0.5],'color','m','linewidth',2)
% text(2,0.4,'dFD length')
% for i=1:length(cSq)
%   line([P(cSq(i,1),1) Q(cSq(i,2),1)],...
%        [P(cSq(i,1),2) Q(cSq(i,2),2)],...
%        'color',[0 0 0]+(i/length(cSq)/1.35));
% end
% axis equal
% % display the coupling sequence along with each distance between points
% disp([cSq sqrt(sum((P(cSq(:,1),:) - Q(cSq(:,2),:)).^2,2))])
%
%
%
% %%% ZCD June 2011 %%%
% %%% edits ZCD May 2013: 1) remove excess arguments to internal functions
% and persistence for speed, 2) added example, 3) allowed for user defined
% distance function, 4) added aditional output option for coupling sequence
%

% size of the data curves
sP = size(P);
sQ = size(Q);

% check validity of inputs
if sP(2)~=sQ(2)
    error('Curves P and Q must be of the same dimension')
elseif sP(1)==0
    cm = 0;
    return;
end

% initialize CA to a matrix of -1s
CA = ones(sP(1),sQ(1)).*-1;

% distance function
if nargin==2
    dfcn = @(u,v) sqrt(sum( (u-v).^2 ));
end

% final coupling measure value
cm = c(sP(1),sQ(1));

% obtain coupling measure via backtracking procedure
if nargout==2
    cSq = zeros(sQ(1)+sP(1)+1,2);    % coupling sequence
    CApad = [ones(1,sQ(1)+1)*inf; [ones(sP(1),1)*inf CA]];  % pad CA
    Pi=sP(1)+1; Qi=sQ(1)+1; count=1;  % counting variables
    while Pi~=2 || Qi~=2
        % step down CA gradient
        [v,ix] = min([CApad(Pi-1,Qi) CApad(Pi-1,Qi-1) CApad(Pi,Qi-1)]);
        if ix==1
            cSq(count,:) = [Pi-1 Qi];
            Pi=Pi-1;
        elseif ix==2
            cSq(count,:) = [Pi-1 Qi-1];
            Pi=Pi-1; Qi=Qi-1;
        elseif ix==3
            cSq(count,:) = [Pi Qi-1];
            Qi=Qi-1;
        end
        count=count+1;
    end
    % format output: remove extra zeroes, reverse order, subtract off
    % padding value, and add in the last point
    cSq = [flipud(cSq(1:find(cSq(:,1)==0,1,'first')-1,:))-1; sP(1) sQ(1)];
end

% debug
% assignin('base','CAw',CA)

function CAij = c(i,j)
    % coupling search function
    if CA(i,j)>-1
        % don't update CA in this case
        CAij = CA(i,j);
    elseif i==1 && j==1
        CA(i,j) = dfcn(P(1,:),Q(1,:));     % update the CA permanent
        CAij = CA(i,j);                    % set the current relevant value
    elseif i>1 && j==1
        CA(i,j) = max( c(i-1,1), dfcn(P(i,:),Q(1,:)) );
        CAij = CA(i,j);
    elseif i==1 && j>1
        CA(i,j) = max( c(1,j-1), dfcn(P(1,:),Q(j,:)) );
        CAij = CA(i,j);
    elseif i>1 && j>1
        CA(i,j) = max( min([c(i-1,j), c(i-1,j-1), c(i,j-1)]),...
            dfcn(P(i,:),Q(j,:)) );
        CAij = CA(i,j);
    else
        CA(i,j) = inf;
    end
end     % end function, c

end     % end main function


% Chapter 17 - Neural Networks.
% Programs_17e - Bifurcation Diagram for a Simple Bistable Neuromodule.
% Copyright Birkhauser 2013. Stephen Lynch.

% Bifurcation diagram for a two-neuron module. (See Figure 17.15).
% Vary bias b1.
clear all
format long;
halfN=1000;N=2*halfN+1;N1=1+halfN;Max=10;a=1;alpha=0.3;
x=zeros(1,N);y=zeros(1,N);
b2=3;w11=7;w12=-4;w21=5;start=-5;
x(1)=-10;y(1)=-3;

% Ramp the power up
for n=1:halfN
    b1=(start+n*Max/halfN);
    x(n+1)=b1+w11*(exp(a*x(n))-exp(-a*x(n)))/(exp(a*x(n))+exp(-a*x(n)))+w12*(exp(alpha*y(n))-exp(-alpha*y(n)))/(exp(alpha*y(n))+exp(-alpha*y(n)));
    y(n+1)=b2+w21*(exp(a*x(n))-exp(-a*x(n)))/(exp(a*x(n))+exp(-a*x(n)));
end

% Ramp the power down
for n=N1:N
    b1=(start+2*Max-n*Max/halfN);
    x(n+1)=b1+w11*(exp(a*x(n))-exp(-a*x(n)))/(exp(a*x(n))+exp(-a*x(n)))+w12*(exp(alpha*y(n))-exp(-alpha*y(n)))/(exp(alpha*y(n))+exp(-alpha*y(n)));
    y(n+1)=b2+w21*(exp(a*x(n))-exp(-a*x(n)))/(exp(a*x(n))+exp(-a*x(n)));
end

% Plot the bifurcation diagrams
fsize=14;
subplot(2,1,1)
hold on
set(gca,'XTick',0:halfN/2:N,'FontSize',fsize);
set(gca,'YTick',-Max:5:Max,'FontSize',fsize);
plot(x(1:N),'-','MarkerSize',1,'color','k')
xlabel('Number of Iterations','FontSize',fsize);
ylabel('x_n','FontSize',fsize);
hold off
x1=zeros(1,N);w=zeros(1,N);

for n=1:halfN
    x1(n)=x(N+1-n);
    w(n)=start+n*Max/halfN;
end

subplot(2,1,2)
hold on
set(gca,'XTick',start:5:start+Max,'FontSize',fsize);
set(gca,'YTick',-Max:5:Max,'FontSize',fsize);
plot(w(1:halfN),x(1:halfN),'-','MarkerSize',1,'color','k');
plot(w(1:halfN),x1(1:halfN),'-','MarkerSize',1,'color','k');
xlabel('b_1','FontSize',fsize);
ylabel('x_n','FontSize',fsize);
hold off

% End of Programs_17e.


% EASY12   Detailed explanations of a Matlab implementation
%          of the Lambda method. The notation follows the one
%          introduced in Strang and Borre, pages 495--499

%Kai Borre 30-06-2008
%Copyright (c) by Kai Borre
%$Revision: 1.0 $  $Date: 2008/06/30  $

fprintf('\n')
echo on
% Test example
hat_I = [5.45;3.1;2.97];
%hat_I =[0; 1];
n = size(hat_I,1);
Q_hat_I = [6.29 5.978 .544; 5.978 6.292 2.34; .544 2.34 6.288];
%Q_hat_I = [53.40 38.40;38.40 28.00];
echo off

% Given float ambiguities
fprintf('\nFloat ambiguities hat_I')
for i = 1:n
    fprintf('\n%12.3f', hat_I(i,1))
end

% Original covariance matrix for hat_I
fprintf('\n\nCovariance matrix for hat_I')
for i = 1:n
    fprintf('\n')
    for j = 1:n
        fprintf('%12.3f', Q_hat_I(i,j))
    end
end

fprintf('\n\n')
echo on
% Shift of hat_I in order to secure that -1 < hat_I <= +1
echo off

shifts = hat_I-rem(hat_I,1);
fprintf('\n\nInteger shifts of hat_I')
for i = 1:n
    fprintf('\n%12.0f', shifts(i,1))
end

% Remainders of hat_I
hat_I_r = rem(hat_I,1);
fprintf('\n\nRemainders of hat_I')
for i = 1:n
    fprintf('\n%12.3f', hat_I_r(i,1))
end

% L^T*D*L factorization of Q_hat_I
% For consistency we do not use the Matlab ldl function
[L,D] = ldldecom(Q_hat_I);
fprintf('\n\nQ_hat_I is factorized into L^T*D*L')
fprintf('\n\nThe lower triangular L')
for i = 1:n
    fprintf('\n')
    for j = 1:n
        fprintf('%12.3f',L(i,j))
    end
end
fprintf('\n\nThe diagonal matrix D\n')
for i = 1:n
    fprintf('%12.3f',D(i))
end

% Computing the size of the search volume
chi2 = chistart(D,L,hat_I_r,1);
fprintf('\n\nchi^2 = %5.3f\n',chi2)

% Doing the decorrelation
[Q_bar_I,Z,L_t,D_t] = decorrel(Q_hat_I,hat_I_r);
fprintf('\nInteger transformation matrix Z')
for i = 1:n
    fprintf('\n')
    for j = 1:n
        fprintf('%12.0f',Z(i,j))
    end
end

% hat_I transformed: I_s = Z'*hat_I_r
fprintf('\n\nTransformed, shifted ambiguities I_s = Z^T*hat_I_r')
I_s = Z'*hat_I_r;
for i = 1:n
    fprintf('\n%12.3f',I_s(i))
end

% The transformed, decorrelated covariance matrix: Q_bar_I = Z^T*Q_hat_I*Z
fprintf('\n\nThe transformed, decorrelated covariance matrix Q_bar_I = Z^T*Q_hat_I*Z.')
fprintf('\n\nQ_bar_I = Z^T*Q_hat_I*Z')
for i = 1:n
    fprintf('\n')
    for j = 1:n
        fprintf('%12.3f',Q_bar_I(i,j))
    end
end
fprintf('\nNote the deminished off-diagonal terms!')
fprintf('\n\nThe lower triangular L_t used in the search')
for i = 1:n
    fprintf('\n')
    for j = 1:n
        fprintf('%12.3f',L_t(i,j))
    end
end
fprintf('\n\nThe diagonal matrix D_t used in the seach\n')
for i = 1:n
    fprintf('%12.3f',D_t(i))
end

fprintf('\n\n')
echo on
% Determining the size of the search volume for the transformed L_t and D_t
echo off

chi2 = chistart(D_t,L_t,I_s,1);
fprintf('\n\nchi^2 for the transformed problem  =  %5.3f\n',chi2)

fprintf('\nThe search domain is defined as')
fprintf('\n(I-hat_I_r)^T*(Z^T*Q_hat_I*Z)*(I-hat_I_r) < chi^2,  for I integer')

[bar_I,sqnorm,ierr] = lsearch(I_s,L_t,D_t,chi2,1);
%bar_I = (bar_I' * inv(Z))'+shifts;
bar_I = (bar_I'/Z)' + shifts;
fprintf('\n\nFixed ambiguities bar_I')
for i = 1:n
    fprintf('\n%12.0f', bar_I(i,1))
end
fprintf('\n\n')
%%%%%%%%%%%%%%%%%%%%%%%%%%% end easy12.m  %%%%%%%%%%%%


u0 = double(imread('58468899e2951a4b87b00897.png'));
u0 = mean(u0,3);

[M N] = size(u0);
MN = M*N;
I=reshape([1:M*N],M,N);
east=[I(:,2:end), I(:,end)];
north=[I(2:end,:); I(end,:)];
D1 = sparse(I,east,1,MN,MN) -speye(MN,MN);
D2 = sparse(I,north,1,MN,MN) -speye(MN,MN);
D = [D1 ; D2];

h0 = [ 1 2 1 ];
h = h0
for i=1:5 %% level of blur
    h = conv(h0,h);
end
size(h)
h = h'*h;
h = h/sum(sum(h));

stddev=5;
g = filter2(h,u0,'valid');
[Ms Ns]=size(g);
g = g + stddev*randn(Ms,Ns);

figure(1);
imagesc(u0); colormap(gray); %drawnow();
figure(2);
imagesc(g); colormap(gray); drawnow();

u=zeros(size(u0));
L2 = 8;
tau = 1; %% max value for tau = 1;
sig = 1/tau/L2;
lambda = .2;

p = zeros(2*MN,1);
%% primal-dual
for i=1:1000
    um = u;
    u = u-tau*(conv2(h,filter2(h,u,'valid')-g,'full') + reshape(D'*p,M,N));
    um = 2*u-um;
    p = p + sig*D*um(:);
    no= max(1,hypot(p(1:MN),p(MN+1:end))/lambda);
    p = p./[no;no];
    if mod(i,20)==0
        i
        figure(3);
        imagesc(u);
        colormap(gray); drawnow();
    end
end


function [M, H, noiseFractions] = hyperNacp(M, h, w)
% HYPERMNF Performs the noise adjusted principal component transform (NACP)
%  hyperMnf performs the noise adjust principal component transform on the
% data and uses spatial (row) offsets of the data to estimate the
% covariance matrix of the data.
%
% Usage
%   M = hyperNacp(M, h, w)
% Inputs
%   M - 2D matrix (p x N)
%   h - height of image in pixels
%   w - width of image in pixels
% Outputs
%   M - 2D transformed data
%   H - 2D transformation matrix
%   noiseFractions - Estimates of the noise fraction for each band
%
% References
%   C-I Change and Q Du, "Interference and Noise-Adjusted Principal
% Components Analysis," IEEE TGRS, Vol 36, No 5, September 1999.

[p, N] = size(M);

% Remove mean from data
u = mean(M.').';
for k=1:N
    M(:,k) = M(:,k) - u;
end

% Compute to rotation of the signal+noise
sigmaZ = hyperCov(M);
M = hyperConvert3d(M, h, w, p);

% Estimate the covariance of the noise.
dX = zeros(h-1, w, p);
for i=1:(h-1)
    dX(i, :, :) = M(i, :, :) - M(i+1, :, :);
end
dX = hyperConvert2d(dX);

% Compute the covariance of the noise signal estimate.
sigmaN = hyperCov(dX);

% Orthonormalize the noise subspace.
[U,deltaN,E] = svd(sigmaN);
F = E*inv(sqrt(deltaN));  % Rotation components of noise orthonormalized
% F now whitens the noise.

% Rotates the signal+noise cov so that the noise is whitened (all noise
% powers are equal)
sigmaAdj = F'*sigmaZ*F;

[U,gammaAdj,G] = svd(sigmaAdj);
H = G*F;

% Compute noise fractions
noiseFractions = diag(gammaAdj);

% Perform transform
M = H*hyperConvert2d(M);


function glscf = CO_glscf(y,alpha,beta,tau)
% CO_glscf  The generalized linear self-correlation function of a time series.
%
% This function was introduced in Queiros and Moyano in Physica A, Vol. 383, pp.
% 10--15 (2007) in the paper "Yet on statistical properties of traded volume:
% Correlation and mutual information at different value magnitudes"
% https://www.sciencedirect.com/science/article/pii/S0378437107004645
%
% The function considers magnitude correlations.
%
%---INPUTS:
% y, the input time series
% alpha and beta are real and nonzero parameters
% tau is the time-delay (can also be 'tau' to set to first zero-crossing of the ACF)
%
% When alpha = beta estimates how values of the same order of magnitude are
% related in time
% When alpha ~= beta, estimates correlations between different magnitudes of the
% time series.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
%% Check inputs and set defaults
% ------------------------------------------------------------------------------
if nargin < 4 || isempty(tau)
    tau = 'tau';
end

% Set tau to first zero-crossing of the autocorrelation function with the input 'tau'
if strcmp(tau,'tau')
    tau = CO_FirstCrossing(y,'ac',0,'discrete');
end

% Take magnitudes of time-delayed versions of the time series:
y1 = abs(y(1:end-tau));
y2 = abs(y(1+tau:end));

glscf = (mean((y1.^alpha).*(y2.^beta)) - mean(y1.^alpha)*mean(y2.^beta)) / ...
     		    (sqrt(mean(y1.^(2*alpha)) - mean(y1.^alpha)^2) ...
     		          * sqrt(mean(y2.^(2*beta)) - mean(y2.^beta)^2));

end


function [H_o_j, A_j, H_x_j] = calcHoj(p_f_G, msckfState, camStateIndices)
%CALCHOJ Calculates H_o_j according to Mourikis 2007
% Inputs: p_f_G: feature location in the Global frame
%         msckfState: the current window of states
%         camStateIndex: i, with camState being the ith camera pose in the window
% Outputs: H_o_j, A

N = length(msckfState.camStates);
M = length(camStateIndices);
H_f_j = zeros(2*M, 3);
H_x_j = zeros(2*M, 12 + 6*N);

c_i = 1;
for camStateIndex = camStateIndices
    camState = msckfState.camStates{camStateIndex};

    C_CG = quatToRotMat(camState.q_CG);
    %The feature position in the camera frame
    p_f_C = C_CG*(p_f_G - camState.p_C_G);

    X = p_f_C(1);
    Y = p_f_C(2);
    Z = p_f_C(3);

    J_i = (1/Z)*[1 0 -X/Z; 0 1 -Y/Z];

    H_f_j((2*c_i - 1):2*c_i, :) = J_i*C_CG;

    H_x_j((2*c_i - 1):2*c_i,12+6*(camStateIndex-1) + 1:12+6*(camStateIndex-1) + 3) = J_i*crossMat(p_f_C);
    H_x_j((2*c_i - 1):2*c_i,(12+6*(camStateIndex-1) + 4):(12+6*(camStateIndex-1) + 6)) = -J_i*C_CG;

    c_i = c_i + 1;
end

A_j = null(H_f_j');
H_o_j = A_j'*H_x_j;

end


function [H]=hessianScalar(varargin)

% function [H]=hessianScalar(U,v,cellOpt)
% ------------------------------------------------------------------------
%
% This function computes the Hessian matrix of the scalar function U for
% each point in U. U may be a vector, a 2D matrix or a 3D matrix. The
% vector v denotes the points spacing between the data entries in U. If v
% is not supplied the spacing is assumed to be homogeneous and unity. If
% the input is n dimensional array consisting of m entries then the output
% is a matrix (if cellOpt==0) the size of mx(n^2) (whereby the colum
% entries define the entries in a Hessian matrix and row entries relate to
% elements in the input array). If cellOpt==1 then the output is reformed
% into a cell array that matches the size of the input aray. Each cell
% entry then contains the nxn Hessian matrix.
%
% See also: |gradient|,|hessian|,|jacobian|,|cellEig|
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2014/10/10 Updated
%------------------------------------------------------------------------

%% Parse input

switch nargin
    case 1
        U=varargin{1};
        v=[];
        cellOpt=0;
    case 2
        U=varargin{1};
        v=varargin{2};
        cellOpt=0;
    case 3
        U=varargin{1};
        v=varargin{2};
        cellOpt=varargin{3};
end
%%

nDims=ndims(U);
if isvector(U)
    nDims=1;
end

if isempty(v)
    v=ones(1,nDims); %Use default
end

if nDims<=3
    switch nDims
        case 1
            %Compute first order derivative
            [dUdx] = gradient(U,v);

            %Compute second order derivatives
            [dUdxdx] = gradient(dUdx,v);

            H_mat=dUdxdx(:);
        case 2
            %Compute first order derivative
            [dUdx,dUdy] = gradient(U,v(1),v(2));

            %Compute second order derivatives
            [dUdxdx,dUdxdy] = gradient(dUdx,v(1),v(2));
            [dUdydx,dUdydy] = gradient(dUdy,v(1),v(2));

            H_mat=[dUdxdx(:) dUdxdy(:)...
                dUdydx(:) dUdydy(:)];
        case 3
            %Compute first order derivative
            [dUdx,dUdy,dUdz] = gradient(U,v(1),v(2),v(3));

            %Compute second order derivatives
            [dUdxdx,dUdxdy,dUdxdz] = gradient(dUdx,v(1),v(2),v(3));
            [dUdydx,dUdydy,dUdydz] = gradient(dUdy,v(1),v(2),v(3));
            [dUdzdx,dUdzdy,dUdzdz] = gradient(dUdz,v(1),v(2),v(3));

            H_mat=[dUdxdx(:) dUdxdy(:) dUdxdz(:)...
                   dUdydx(:) dUdydy(:) dUdydz(:)...
                   dUdzdx(:) dUdzdy(:) dUdzdz(:)];
    end

    if cellOpt==1
        H_mat=reshape(H_mat',nDims,nDims,size(H_mat,1));

        %Cell array of Hessian matrices
        H=reshape(mat2cell(H_mat,nDims,nDims,ones(size(H_mat,3),1)),size(U));
    else
        H=H_mat;
    end
else
    warning('Function only defined for 1D, 2D or 3D scalar valued arrays');
end

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


% Test script for solving the 2D advection
Globals2D;

% Set polynomial order to use
N = 7;

% Read and initiate circular mesh
filename = 'circA01.neu';
[Nv, VX, VY, K, EToV, BCType] = MeshReaderGambitBC2D(filename);

StartUp2D;
BuildBCMaps2D;

% Push all boundary faces to unit cylinder
[k,f] = find(BCType);
curved = sort(unique(k));
MakeCylinder2D([k,f], 1, 0, 0);

% Set initial conditions
% First 6 modes of eigenmodes with 6 azimuthal periods
alpha = [9.936109524217684,13.589290170541217,17.003819667816014,...
         20.320789213566506,23.586084435581391,26.820151983411403];

% choose radial mode
alpha0 = alpha(2);
theta = atan2(y,x);
rad   = sqrt(x.^2+y.^2);

Ez = besselj(6, alpha0*rad).*cos(6*theta);
Hx = zeros(Np, K); Hy = zeros(Np, K);

% Solve Problem for exactly one period
FinalTime = .5;
[Hx,Hy,Ez,time] = MaxwellCurved2D(Hx,Hy,Ez,FinalTime);

exactEz = besselj(6, alpha0*rad).*cos(6*theta)*cos(alpha0*time(end));
maxabserror = max(max(abs(Ez-exactEz)))


function K = exponential_correlation ( s, t )

%*****************************************************************************80
%
%% EXPONENTIAL_CORRELATION evaluates the exponential correlation function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real S(*), T(*), pairs of argument values.
%
%    Output, real K(*), the correlation function values
%
  K = exp ( - abs ( s - t ) );

  return
end


function I = intersectionHull(varargin)
%intersectionHull - computes a bounded convex polyhedron resulting from the
%intersection of other convex polyhedra. The other polyhedra need not individually
%be bounded. Only the final intersection resulting from them must be bounded.
%Any number of polyhedra can be input to the intersection operation. They can
%be expressed using either vertices or linear in/equalities, according to
%the input scheme,
%
%
%   I = intersectionHull('vert', V1, 'lcon', A2,b2, 'lcon', A3,b3,Aeq3,,beq3,...)
%
%The arguments specifying different polyhedra are separated using labels
%'vert' and 'lcon'. The label 'vert' signifies that an input polyhedron will
%be expressed using vertices and is to be followed by any string of input arguments
%accepted by vert2lcon(). The label 'lcon' signifies that an input polyhedron will
%be expressed using linear constraints and is to be followed by any string of
%input arguments accepted by lcon2vert().
%
%The output, I, is a struct containing fields
%
%   I.vert: A matrix whose rows are the vertices of the polyhedron formed from
%           the intersection.
%   I.lcon: The quadruplet of linear constraint data {A,b,Aeq,beq}
%           describing the polyhedral intersection.
%
%EXAMPLE 1: This example computes the intersection of a unit square and an
%oblique 2D line segment, both expressed in terms of their vertices.
%
%     V1=dec2bin(0:2^2-1,2)-'0';   %vertices of unit square
%
%     V2=[1,1;0,-1];               %vertices of 2D line segment
%
%     I=intersectionHull('vert',V1,'vert',V2);  %compute intersection
%
%The intersection is another line segment with vertices
%
%        >> I.vert
%
%         ans =
%
%             0.5000         0
%             1.0000    1.0000
%
%EXAMPLE 2: This example computes the intersection of a unit cube, expressed in
%terms of its vertices, and an infinite oblique 3D line, expressed in terms of linear equalities.
%Note that the line is an unbounded polyhedron. This is okay, since we know in advance
%that the final polyhedron formed from the intersection is bounded.
%
%     V=dec2bin(0:2^3-1,3)-'0';    %vertices of unit cube
%
%     Aeq=[1 -1 0; 0 1 -1]; beq=[0;0]; %oblique line in 3D
%
%     I=intersectionHull('vert',V,'lcon',[],[],Aeq,beq);  %compute intersection
%
%Once again, the intersection is a line segment. Its vertices are
%
%      >> I.vert   %vertices of line segment of intersection
%
%         ans =
%
%             0.0000    0.0000    0.0000
%             1.0000    1.0000    1.0000

%%%%begin parsing

if isnumeric(varargin{1})
   TOL=varargin{1};
   varargin(1)=[];
else
    TOL=[];
end

N=length(varargin);
idxType = [find(cellfun(@ischar,varargin)),N+1];

L=length(idxType)-1;
S(L).type=[];
S(L).args={};
S(L).A=[];
S(L).b=[];
S(L).Aeq=[];
S(L).beq=[];

for i=1:L

    j=idxType(i);
    k=idxType(i+1);
    S(i).type=varargin{j};
    S(i).args=varargin(j+1:k-1);

    if isempty(S(i).args)
     error 'Syntax error - arguments missing'
    end

    lcon=cell(1,4);

      switch S(i).type

          case 'vert'

              [lcon{1:4}] = vert2lcon(S(i).args{:});

          case 'lcon'

               lcon(1:k-j-1) = S(i).args;

          case 'qlcon' %deliberately undocumented - no point in using this

               lcon(1:k-j-1) = S(i).args(2:end);

          otherwise

            error(['Unrecognized representation label of polyhedron ' num2str(i)]);

      end

    [S(i).A, S(i).b, S(i).Aeq, S(i).beq] = deal(lcon{:});

end

%%%%end parsing

   A=vertcat(S.A);
   b=vertcat(S.b);
   Aeq=vertcat(S.Aeq);
   beq=vertcat(S.beq);

   [V,nr,nre]=lcon2vert(A,b,Aeq,beq,TOL);

   I.vert=V;
   I.lcon={A(nr,:),b(nr,:), Aeq(nre,:),beq(nre,:)};


%VL_NNBNORM CNN batch normalisation.
%   Y = VL_NNBNORM(X,G,B) applies batch normalization to the input
%   X. Batch normalization is defined as:
%
%      Y(i,j,k,t) = G(k) * (X(i,j,k,t) - mu(k)) / sigma(k) + B(k)
%
%   where:
%
%      mu(k) = mean_ijt X(i,j,k,t),
%      sigma2(k) = mean_ijt (X(i,j,k,t) - mu(k))^2,
%      sigma(k) = sqrt(sigma2(k) + EPSILON)
%
%   are respectively the per-channel mean, variance, and standard
%   deviation of each feature channel in the data X. The parameters
%   G(k) and B(k) are multiplicative and additive constants use to
%   scale each data channel.
%
%   Means and variances are accumulated across all the data items
%   (images) stored in the 4D tensor X (from which the name batch
%   normalization is derived). The constant EPSILON is used to
%   regularize the computation of sigma(k) and to avoid division by
%   zero.
%
%   [DZDX,DZDG,DZDB] = VL_NNBNORM(X,G,B,DZDY) computes the derviatives
%   of the block projected onto DZDY. DZDX, DZDG, DZDB and DZDY have
%   the same dimensions as X, G, B, and Y respectivey.
%
%   Optionally, [Y,MOMENTS] = VL_NNBNORM(...) and
%   [DZDX,DZDG,DZDB,MOMENTS] = VL_NNBNORM(...,DZDY) return the values
%   of the vectors mu and sigma in the formulas above. Here, MOMENTS
%   is a DEPTH x 2 array [MU, SIGMA].
%
%   VL_NNBNROM(..., 'Option', value) takes the following options:
%
%   `Epsilon`:: 1e-4
%       Specifies the constant EPSILON in the formuals above.
%
%   `Moments`:: unspecified
%       Specifies an array MOMENTS with the values of mu and sigma to
%       use instead of computing them according to the equations
%       above. This is useful to disable batch normalization during
%       testing.
%
%   `CuDNN`:: specified
%       If specified, turns on CuDNN. CuDNN is on by default. This
%       option can be useful to undo the effect of a previous
%       `NoCuDNN` option in the argument list.
%
%   `NoCuDNN`:: not specified
%       If specified, turns off CuDNN.
%
%   See also: VL_NNNORMALIZE().

% Copyright (C) 2015 Sébastien Ehrhardt, Karel Lenc and Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


function x = exponential_01_cdf_inv ( cdf )

%*****************************************************************************80
%
%% EXPONENTIAL_01_CDF_INV inverts the Exponential 01 CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%    0.0 <= CDF <= 1.0.
%
%    Output, real X, the corresponding argument.
%
  if ( cdf < 0.0 | 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EXPONENTIAL_01_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'EXPONENTIAL_01_CDF_INV - Fatal error!' );
  end

  x = - log ( 1.0 - cdf );

  return
end


function circular_arc ( xcenter, ycenter, radius, angmin, angmax )

%*****************************************************************************80
%
%% CIRCULAR_ARC draws a circular arc of a given angular size and radius.
%
%  Discussion:
%
%    It is assumed that a plot has already been begun.
%
%  Example:
%
%    r = rand ( 100, 1 );
%    t = pi * rand ( 100, 1 );
%    plot ( r .* cos ( t ), r .* sin ( t ), 'b*' );
%    axis equal
%    axis square
%    circular_arc ( 0.0, 0.0, 1.0, 0, 180 );
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real XCENTER, YCENTER, the X and Y coordinates of
%    the center of the circle on which the arc lies.
%
%    Input, real RADIUS, the radius of the circle on which the arc lies.
%
%    Input, real ANGMIN, ANGMAX, the minimum and maximum
%    angles of the circle.  ANGMIN and ANGMAX are both
%    measured in degrees.  ANGMIN and ANGMAX determine the
%    portion of the circle to be drawn.  If ANGMIN=0.0 and
%    ANGMAX=90.0, for instance, an arc of 90 degrees will be drawn.
%
  color = [ 0.75, 0.75, 0.75 ];

  nval = 65;

  if ( radius == 0.0 )
    return
  end
%
%  Set up the data defining the circular arc, using NVAL equally
%  spaced points along the circumference.
%
  if ( nval == 1 )
    angle(1) = 0.5 * ( angmax + angmin );
  else
    angle(1:nval) = linspace ( angmin, angmax, nval );
  end

  xval(1:nval) = xcenter + radius * cos ( angle * pi / 180 );
  yval(1:nval) = ycenter + radius * sin ( angle * pi / 180 );

  line ( xval, yval, 'Color', color );

  return
end


% ir_denoise_test_sep1
% Examine denoising with ordinary vs separable finite differences
% i.e. |Ch X|_1 + |Cv X|_1 vs |Ch X Cv|_1
% Uses ADMM algorithm to optimize denoising cost function.

if ~isvar('yi'), printm 'generate noisy image'
	xt = ellipse_im(2^7);
	xt = zeros(2^6); xt(20:60,20:48) = 9;
	sig = 2^0; clim = [0 9];

	snr = @(x) -20*log(norm(x(:)-xt(:)) / norm(xt(:)));

	rng(7)
	yi = xt + sig * randn(size(xt));

	im plc 2 4
	im(1, xt, clim, 'True')
	im(2, yi, clim, 'Noisy')
	xlabelf('SNR = %4.1f dB', snr(yi))
end

if ~isvar('Cs'), printm 'C2 and Cs'
	arg.Ch = Cdiffs(size(xt,1), 'type_diff', 'circshift', 'offsets', 1);
	arg.Cv = Cdiffs(size(xt,2), 'type_diff', 'circshift', 'offsets', 1);
	Cs = fatrix2('arg', arg, 'idim', size(xt), 'odim', size(xt), ...
		'forw', @(arg, x) arg.Ch * x * arg.Cv, ... % separable!
		'back', @(arg, y) arg.Ch' * y * arg.Cv');
	C2 = Cdiffs(size(xt), 'type_diff', 'circshift', 'offsets', '2d:hv');

	im(3, 'col', 1, C2 * xt, 'Usual'), cbar
	im(4, Cs * xt, 'Separable'), cbar
	drawnow
end

if ~isvar('x2'), printm 'run admm with usual 2d finite differences'
	[x2s snr2] = ir_denoise_admm1(yi, 'beta', 2^+1, ...
		'stop_diff_tol', 1e-4, 'chat', 0, ...
		'isave', 'all', ...
		'userfun', @(x, iter) snr(x), ...
		'niter', 2^5, 'shrink', [], 'rho', 1);
	x2 = x2s(:,:,end);
end

if ~isvar('x1'), printm 'run admm with separable finite differences'
	[x1s snr1] = ir_denoise_admm1(yi, 'beta', 2^+1, ...
		'stop_diff_tol', 1e-4, 'chat', 0, ...
		'C', 'sep1', ...
		'isave', 'all', ...
		'userfun', @(x, iter) snr(x), ...
		'niter', 2^5, 'shrink', [], 'rho', 1);
	x1 = x1s(:,:,end);
end

if 1 % figures
	im(7, x2, clim, 'Denoised usual')
	xlabelf('SNR = %4.1f dB', snr(x2))
	im(8, x1, clim, 'Denoised separ.')
	xlabelf('SNR = %4.1f dB', snr(x1))

%	im subplot 5
	subplot(223)
	iters = 0:numel(snr1)-1;
	plot(iters, snr2, '-o', iters, snr1, '-x')
	snr_lim = [floor(min([snr1; snr2])) ceil(max([snr1; snr2]))];
	axis([0 numel(iters) snr_lim])
	ytick(snr_lim)
	xlabelf 'ADMM iteration'
	ylabelf 'SNR (dB)'
	legend('usual', 'separable', 'location', 'southeast')
end