function corr_coef = RegreesionLIVE(objectiveValues,mos)
%this script is used to calculate the pearson linear correlation
%coefficient and root mean sqaured error after regression

%get the objective scores computed by the IQA metric and the subjective
%scores provided by the dataset
% matData = load('VSIOnLIVE.mat');
% VSIOnLIVE = matData.VSIOnLIVE;
% objectiveValues = VSIOnLIVE(:,1);
% mos = VSIOnLIVE(:,2);

%plot objective-subjective score pairs
% p = plot(objectiveValues,mos,'+');
% set(p,'Color','blue','LineWidth',1);

%initialize the parameters used by the nonlinear fitting function
beta(1) = max(mos);
beta(2) = min(mos);
beta(3) = mean(objectiveValues);
beta(4) = 0.1;
beta(5) = 40;

%fitting a curve using the data
[bayta ehat,J] = nlinfit(objectiveValues,mos,@logistic,beta);
%given a ssim value, predict the correspoing mos (ypre) using the fitted curve
[ypre junk] = nlpredci(@logistic,objectiveValues,bayta,ehat,J);

RMSE = sqrt(sum((ypre - mos).^2) / length(mos));%root meas squared error
corr_coef = corr(mos, ypre, 'type','Pearson'); %pearson linear coefficient

%draw the fitted curve
% t = min(objectiveValues):0.01:max(objectiveValues);
% [ypre junk] = nlpredci(@logistic,t,bayta,ehat,J);
% hold on;
% p = plot(t,ypre);
% set(p,'Color','black','LineWidth',2);
% legend('Images in LIVE','Curve fitted with logistic function', 'Location','NorthEast');
% xlabel('Objective score by VSI');
% ylabel('MOS');


function [ result, seed ] = sphere01_quad_mc ( f, h, seed, n )

%*****************************************************************************80
%
%% SPHERE01_QUAD_MC uses the Monte Carlo rule for sphere quadrature.
%
%  Discussion:
%
%    A number of points N are chosen at random on the sphere, with N
%    being determined so that, if the points were laid out on a regular
%    grid, the average spacing would be no more than H.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, function v = F ( n, x ), evaluates the integrand.
%
%    Input, real H, the maximum length of a side of the spherical
%    quadrilaterals.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Input, integer N, the number of points to use.
%
%    Output, real RESULT, the approximate integral.
%
  sphere_area = 4.0 * pi;

  [ x, seed ] = sphere01_sample_3d ( n, seed );

  v = f ( n, x )

  result = sphere_area * sum ( v(1:n) ) / n;

  return
end


function [ T, W ] = rd_lin_spline ( w0, t_array, n, c_array )

%*****************************************************************************80
%
%% RD_LIN_SPLINE discretizes and solves a 1D reaction diffusion problem.
%
%  Discussion:
%
%    The problem includes homogeneous Neumann boundary conditions at both ends.
%
%    The dynamics are given by the following reaction/diffusion equation
%    for the function W(T,X):
%
%      W_t = W_xx + NL(W,c),
%
%    where NL(W,c) is a polynomial in W:
%
%       NL(W,c) = c(1) + c(2) * W + c(3) * W^2 + c(4) * W^3
%
%    with Neumann boundary conditions at X = 0.0 and X = 1.0:
%
%      W_x(T,0.0) = 0.0
%      W_x(T,1.0) = 0.0
%
%    and initial condition at T = 0.0:
%
%      W(0,X) = sin ( pi * X ).
%
%    The problem is to be solved for 0.0 <= T <= 4.0, 0.0 <= X <= 1.0.
%
%    We use a finite element approximation with piecewise linear "hat"
%    functions. The resulting ODE model is:
%
%      M * w-dot(t) = - K * w(t) + NL(w, c)
%
%    where M is the mass matrix, K the stiffness matrix, and NL the nonlinear
%    term.
%
%      NL(w, c_array)=c_array(1)+ c_array(2)*w + c_array(3)*w^2 + c_array(4)*w^3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 April 2011
%
%  Author:
%
%   Eugene Cliff
%
%  Reference:
%
%    Jeffrey Borggaard, John Burkardt, John Burns, Eugene Cliff,
%    Working Notes on a Reaction Diffusion Model: a Finite Element Formulation.
%
%  Parameters:
%
%    w0     - handle to initial function (on [0, 1])
%
%    t_array- array of times for output
%
%    n      - grid  parameter ( > 2)
%
%    c_array- coefficients in nonlinear term
%

%
%  Assemble the mass matrix.
%
    M = spdiags([ones(n+1,1) [2;4*ones(n-1,1);2] ones(n+1,1)], ...
                                                 [-1 0 1], n+1, n+1)/(6*n);
%
%  Assemble the stiff matrix.
%
    K = n*spdiags([-ones(n+1,1) [1;2*ones(n-1,1);1] -ones(n+1,1)], ...
                                                 [-1 0 1], n+1, n+1);
%
%  Set the initial condition.
%
   wr  = zeros(n+1,1);
   h_wr = @(x) w0(x).*basic_hat(n*x );
   wr(1) = quad(h_wr, 0, 1/n);                    % < w_0, \phi_1 >
   h_wr = @(x) w0(x).*basic_hat(n*x - n);
   wr(n+1) = quad(h_wr, (n-1)/n, 1);              % < w_0, \phi_n+1 >
   for jj=2:n
      h_wr = @(x) w0(x).*basic_hat(n*x + 1 - jj);
      wr(jj) = quad(h_wr, (jj-2)/n, jj/n);        % < w_0, \phi_i >
   end

   w_0 = M \ wr;

   ODE_opt = odeset('Mass', M);                  % constant mass matrix
   h_rhs = @(t, w) -K*w  + NL(w, c_array, n, M); % handle for rhs function

   [T, W] = ode15s(h_rhs, t_array, w_0, ODE_opt);% invoke ODE solver

  return
end
function val = NL (w, c, n, M )

%*****************************************************************************80
%
%% NL evaluates the nonlinear term in the reaction-diffusion equation.
%
      val = (c(1)/n)* [1; 2*ones(n-1,1); 1] ... % constant
          +  c(2) * M * w     ...               % linear
          +  c(3) * Nq(w, n)  ...               % quadratic
          +  c(4) * Nc(w, n) ;                  % cubic

  return
end
function val = Nq(w, n)

%*****************************************************************************80
%
%% NQ evaluates a quadratic nonlinear finite element function.
%
     w2 = w(:).^2; wx = (w(1:end-1,1)+ w(2:end,1)).^2;

     val = [2*w2(1)+wx(1); wx(1:end-1)+4*w2(2:end-1)+wx(2:end) ; ...
                                                 wx(end)+2*w2(end)]/(12*n);

  return
end
function val = Nc(w, n)

%*****************************************************************************80
%
%% NC evaluates a cubiic nonlinear finite element function.
%
     w2 = w(:).*w(:); w3 = w(:).*w2(:); wx = (w(1:end-1,1)+w(2:end,1)).^3;

     val = [3*w3(1)+wx(1)-w(1)*w(2)^2;
   wx(1:end-1)+6*w3(2:end-1)+wx(2:end)-w(2:end-1).*(w2(1:end-2)+w2(3:end));
                               wx(end)+3*w3(end)-w(end)*w(end-1)^2]/(20*n);

  return
end


function trans = createTranslation(varargin)
%CREATETRANSLATION Create the 3*3 matrix of a translation.
%
%   TRANS = createTranslation(DX, DY);
%   Returns the translation corresponding to DX and DY.
%   The returned matrix has the form :
%   [1 0 TX]
%   [0 1 TY]
%   [0 0  1]
%
%   TRANS = createTranslation(VECTOR);
%   Returns the matrix corresponding to a translation by the vector [x y].
%
%
%   See also
%   transforms2d, transformPoint, createRotation, createScaling

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2004-04-06
% Copyright 2004-2022 INRA - TPV URPOI - BIA IMASTE

% process input arguments
if isempty(varargin)
    tx = 0;
    ty = 0;
elseif length(varargin)==1
    var = varargin{1};
    tx = var(1);
    ty = var(2);
else
    tx = varargin{1};
    ty = varargin{2};
end

% create the matrix representing the translation
trans = [1 0 tx ; 0 1 ty ; 0 0 1];


function [] = draw_crystal_lattice
% File:      draw_crystal_lattice.m
% Author:    Ioannis Filippidis, jfilippidis@gmail.com
% Date:      2011.01.30 -
% Language:  MATLAB R2012a
% Purpose:   draw crystal lattices
% Copyright: Ioannis Filippidis, 2011-

selection = 'diamond'; % 'diamond', 'copper'

a = 1;
c = a /4;

r_diamond_cubic = c *[0, 0, 0;
        2, 0, 0;
        0, 2, 0;
        2, 2, 0;
        1, 1, 0; %
        1, 0, 1;
        0, 1, 1;
        1, 2, 1;
        2, 1, 1; %
        0, 0, 2;
        2, 0, 2;
        0, 2, 2;
        2, 2, 2;
        1, 1, 2; %
        0.5, 0.5, 0.5;
        1.5, 1.5, 0.5;
        0.5, 1.5, 1.5;
        1.5, 0.5, 1.5]';

r_fcc = c *[0, 0, 0;
        2, 0, 0;
        0, 2, 0;
        2, 2, 0;
        1, 1, 0; %
        1, 0, 1;
        0, 1, 1;
        1, 2, 1;
        2, 1, 1; %
        0, 0, 2;
        2, 0, 2;
        0, 2, 2;
        2, 2, 2;
        1, 1, 2]'; %

i_diamond_cubic =[15, 1;
     15, 5;
     15, 6;
     15, 7; %
     16, 5;
     16, 4;
     16, 8;
     16, 9; %
     17, 7;
     17, 12;
     17, 14;
     17, 8; %
     18, 6;
     18, 9;
     18, 14;
     18, 11];
%

i_fcc = [1, 2;
    2, 4;
    2, 3;
    1, 3;
    1, 4;
    3, 4; %
    1, 10;
    2, 10;
    2, 11;
    1, 11;
    10, 11; %
    3, 12;
    4, 13;
    12, 13;
    4, 12;
    3, 13; %
    10, 12;
    10, 13;
    13, 11;
    12, 11;
    1, 12;
    3, 10; %
    2, 13;
    4, 11];

switch selection
    case 'diamond'
        r = r_diamond_cubic;
        i1 = i_diamond_cubic;
    case 'copper'
        r = r_fcc;
        i1 = i_fcc;
    otherwise
        disp('Unknown material selection.')
end

ax = newax;
hold(ax, 'on')
for i=1:size(r, 2)
    h = drawSphere(r(:, i), 0.05);
    set(h, 'FaceColor', 'r')
end
grid(ax, 'on')
box(ax, 'on')
axis(ax, 'equal')
axis(ax, 'tight')
view(ax, 3)
%maximize(get(ax, 'Parent') )

for i=1:size(i1, 1)
    a = i1(i, 1);
    b = i1(i, 2);
    drawCylinder([r(:, a)', r(:, b)', 0.01] )
end

fname = selection;
fig2u3d(gca, fname, '-pdf');


function a_cum = r8vec_cum0 ( n, a )

%*****************************************************************************80
%
%% R8VEC_CUM0 computes the cumulutive sums of an R8VEC.
%
%  Example:
%
%    Input:
%
%      A = (/ 1.0, 2.0, 3.0, 4.0 /)
%
%    Output:
%
%      A_CUM = (/ 0.0, 1.0, 3.0, 6.0, 10.0 /)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real A(N), the vector to be summed.
%
%    Output, real A_CUM(1:N+1), the cumulative sums.
%
  a_cum(1) = 0.0;

  for i = 1 : n
    a_cum(i+1) = a_cum(i) + a(i);
  end

  return
end


function Y=tensor2Mat(X,param2,C)
%%TENSOR2MAT Unfold a (real or complex) tensor into a matrix. This can
%            either be done as an n-way matricization using a standard
%            ordering of the modes, or one can explicitly specify the
%            ordering of the modes. The tensor is just a hypermatrix. The
%            unfolding produces a 2D matrix as the output. The
%            transformation of tensors into matrices plays a role in
%            numerous multilinear algorithms, such as the multilinear
%            singular value decomposition.
%
%INPUTS: X The hypermatrix (tensor) that is to be unfolded. This should
%          have 2 or more modes. For example, a 4-mode hyper matrix would
%          be addressable as X(i1,i2,i3,i4). The matrix can be real or
%          complex.
% param2, C If the algorithm is called with only two parameters, then
%          param2=n is the number n of the mode of the unfolding and an
%          n-mode matricization is performed. The mode number ranges
%          from 1 through N --the number of modes of the matrix X (the
%          number of indices needed to address an element in X). The
%          permutation of the modes to the columns is as in [1], which is
%          [1:1:(n-1),(n+1):1:N] and the number of rows of the matrix Y
%          returned is equal to the number of elements in the nth way. On
%          the other hand, some authors, such as in [2], use a different
%          permutation. In such an instance, the explicit ordering can be
%          specified using param2=R and also providing C as in [3]. R
%          contains the indices of the modes that are to be mapped to rows
%          and C contains the indices of modes that are to be mapped to
%          columns. For example, for a 4-mode matrix one might use R=[2;3],
%          C=[4;1], which makes the dimensionality of the rows of the
%          output equal to the sum of the dimensionalities of
%          modes 2 and 3.
%
%OUTPUT: Y A 2D matrix containing the elements of the hypermatrix X
%          arranged according to the inputs.
%
%The algorithm can be called as
%Y=tensor2Mat(X,n);
%for a standard n-mode matricization or as
%Y=tensor2Mat(X,R,C);
%for general ordering to be used. The standard n-mode matricization is
%consistent with the orderings used in the nModeProd function.
%
%As an example, consider the 3X2X3 hypermatrix
% A=zeros(3,2,3);
% A(1,1,1)=1;
% A(1,1,2)=1;
% A(2,1,1)=1;
% A(2,1,2)=-1;
% A(2,1,3)=2;
% A(3,1,1)=2;
% A(3,1,3)=2;
% A(1,2,1)=2;
% A(1,2,2)=2;
% A(2,2,1)=2;
% A(2,2,2)=-2;
% A(2,2,3)=4;
% A(3,2,1)=4;
% A(3,2,3)=4;
% %A standard 1-mode matrix unfolding is
% Y=tensor2Mat(A,1)
%Providing the result
% Y=[1     2     1     2     0     0;
%    1     2    -1    -2     2     4;
%    2     4     0     0     2     4];
%On the other hand, the example in [2] gives a different answer, because
%they use a different ordering. That is, they rearrange, the columns in the
%unfolding differently. Here, one can get the same answer using
% Y=tensor2Mat(A,1,[3,2])
%which returns
% Y=[1     1     0     2     2     0;
%    1    -1     2     2    -2     4;
%    2     0     2     4     0     4];
%
%Introductions to tensor operations are in [3] and [4].
%
%REFERENCES:
%[1] J. Salmi, A. Richter, and V. Koivunen, "Sequential unfolding SVD for
%    tensors with applications in array signal processing," IEEE
%    Transactions on Signal Processing, vol. 57, no. 12, pp. 4719-4733,
%    Dec. 2009.
%[2] L. de Lathauwer, B. de Moore, and J. Vandewalle, "A multilinear
%    singular value decomposition," SIAM Journal on Matrix Analysis and
%    Applications, vol. 21, no. 4, pp. 1253-1278, 2000.
%[3] T. G. Kolda, "Multilinear operators for higher-order decompositions,"
%    Sandia National Laboratories, Tech. Rep. SAND2006-2081, Apr. 2006.
%    [Online]. Available: http://www.sandia.gov/~tgkolda/pubs/pubfiles/SAND2006-2081.pdf
%[4] R. G. Kolda and B. W. Bader, "Tensor decompositions and applications,"
%    SIAM Review, vol. 51, no. 3, pp. 455-500, 2009.
%
%June 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

I=size(X);%The dimensionality of each way of X.

%If a standard n-mode matricization is being performed
if(nargin<3)
    d=length(I);
    C=[1:1:(param2-1),(param2+1):1:d];
end

R=param2;
J=prod(I(R));
K=prod(I(C));
Y=reshape(permute(X,[R(:);C(:)]),J,K);%Convert X to the matrix Y.

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 ypp = fppcube ( x )

%*****************************************************************************80
%
%% FPPCUBE sets the value of the second derivative of the cubic function.
%
%  Discussion:
%
%    Y(X) = ( ( X + 2 ) * X + 3 ) * X + 4
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real YPP, the value of the second derivative of the cubic function.
%
  ypp = 6.0E+00 * x + 4.0E+00;

  return
end


function [PBarUVals,dPBarUValsdTheta,d2PBarUValsdTheta2]=NALegendreCosRat(theta,M,scalFactor)
%NALEGENDRECOSRAT Evaluate \bar{P}_{nm}(cos(theta))/u^m for all n from
%                 0 to M and for each n for all m from 0 to n, where
%                 u=abs(sin(theta)). \bar{P}_{nm}(x) is the fully
%                 normalized associated Legendre function of x of degree n
%                 and order m. Also evaluate
%                 D{\bar{P}_{nm}(cos(theta))}/u^m and
%                 D2{\bar{P}_{nm}(cos(theta))}/u^m where D{} is the first
%                 derivative operator with respect to theta and D2{} is the
%                 second derivative operator with respect to theta. All of
%                 the values can be scaled by a factor of scalFac, if
%                 desired, to help prevent overflows with high degrees and
%                 orders.
%
%INPUTS: theta An angle in radians.
%       maxDeg The maximum degree and order of the output. This must be
%              >=0.
%   scalFactor A scale factor to help prevent overflow of the results. In
%              [1], discussed below, a value of 10^(-280) is used.
%
%OUTPUTS: PBarUVals An instance of the CountingClusterSet class such that
%                   PBarUVals(n+1,m+1)=scalFac*\bar{P}_{nm}(cos(theta))/u^m.
%  dPBarUValsdTheta An instance of the CountingClusterSet class such that
%                   dPBarUValsdTheta(n+1,m+1)=scalFac*D{\bar{P}_{nm}(cos(theta))}/u^m
% d2PBarUValsdTheta2 An instance of the CountingClusterSet class such that
%                   d2PBarUValsdTheta2(n+1,m+1)=scalFac*D2{\bar{P}_{nm}(cos(theta))}/u^m
%
%The modified forward row (MFR) algorithm of [1] is used to compute
%PBarUVals and dPBarUValsdTheta. For d2PBarUValsdTheta2, the algorithm of
%[2] is used. However, the paper contains a typo. The first term of the
%first unnumbered equation should be multiplied by an additional (1/u).
%This function uses the correct formulation, which is also described in
%Appendix F of [3].
%
%With the notation P^m_n(x) for an associated Legendre function of degree n
%and order m evaluated at the point x, one generally means
%P^m_n(x)=(-1)^m*(1-x^)^(m/2)*D_m{P_n(x)}
%where P_n(x) is a Legendre polynomial of degree n evaluated at x and
%D_m{P_n(x)} is the mth derivative of the polynomial. These associated
%Legendre functions are the same as those used in the International
%Geomagnetic Reference Field. On the other hand, with the notation
%P_{nm}(x), one generally means (-1)^mP^m_n(x). This notation is also
%called an associated Legendre function. Matlab's built-in function
%legendre(n,x) returns all P^m_n(x) for m=0 to m=n. On the other hand, the
%notation \bar{P}_{nm}(x), refers to a fully normalized associated Legendre
%function. Multiple definitions of what is normalized exist. In this
%function, the normalization is such that
%\bar{P}_{nm}(x)=P_{nm}(x)*sqrt(k*(2*n+1)*factorial(n-m)/factorial(n+m))
%where k=1 if m=0 and k=2 otherwise. This differs from the normalization
%constant that Matlab uses in the normalized version of its built-in
%associated Legendre function. It is the same as the normalization
%constant the NGA uses in its coefficients for the EGM96 and the
%EGM2008 gravitation models.
%
%The fully normalized associated Legendre function ratios that this
%function computes can be used in the synthesis of spherical harmonic
%coefficients.
%
%REFERENCES:
%[1] S. A. Holmes and W. E. Featherstone, "A unified approach to the
%    Clenshaw summation and the recursive computation of very high degree
%    and order normalised associated Legendre functions," Journal of
%    Geodesy, vol. 76, no. 5, pp. 279-299, May 2002.
%[2] S. A. Holmes and W. E. Featherstone, "Short note: Extending simplified
%    high-degree synthesis methods to second latitude derivatives of
%    geopotential," Journal of Geodesy, vol. 76, no. 8, pp. 447-450, Nov.
%    2002.
%[3] D. F. Crouse, "An overview of major terrestrial, celestial, and
%    temporal coordinate systems for target tracking," Naval Research
%    Laboratory, Washington, DC, Tech. Rep. NRL/FR/5344-16-10,279, 10 Aug.
%    2016.
%
%December 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(M<0)
       error('M must be >=0.')
    end

    u=sin(theta);
    t=cos(theta);

    %Allocate space for the return variables. n ranges from 0 to M and m
    %ranges from 0 to n. Thus, the number of elements for each n ranges
    %from 1 to M+1. This means that a total of
    %(M+1)*(M+2)/2 elements are necessary.
    numPBarU=(M+1)*(M+2)/2;
    totalP=zeros(numPBarU,1);
    PBarUVals=CountingClusterSet(totalP);

    %The value of PBar_{0,0}(cos(theta)) is independent of theta and is
    %one.
    PBarUVals(0+1,0+1)=1*scalFactor;

    if(M>0)
        %Set the seed value for PBar_{1,1}(cos(theta))/u from which the
        %other values will be computed.
        PBarUVals(1+1,1+1)=sqrt(3)*scalFactor;

        jTerm=1/sqrt(2);
        %First, deal with the case where n=1, m=0;
        n=1;
        m=0;
        %g is given in Equation 18 of the first Holmes and Featherstone paper.
        g=2*(m+1)/sqrt((n-m)*(n+m+1));
        PBarUVals(n+1,m+1)=jTerm*g*t*PBarUVals(n+1,m+1+1);

        %Compute the values along the main diagonal, where m=n
        %starting from m=n=2. This implements equation 28 in the first
        %Holmes and Featherstone paper for the normalized associated
        %Legendre function ratio.
        for m=2:M
            PBarUVals(m+1,m+1)=sqrt((2*m+1)/(2*m))*PBarUVals(m-1+1,m-1+1);
        end

        %Recursively compute the values using Equation 27 from the
        %first Holmes and Featherstone paper, taking into account the
        %fact that the first element of the recursion only has one term.

        %Next, evaluate the values for all other valid n and m.
        for n=2:M
            %Deal with the first element of the recursion,which is
            %where m=n-1.
            m=n-1;
            g=2*(m+1)/sqrt((n-m)*(n+m+1));
            PBarUVals(n+1,m+1)=g*t*PBarUVals(n+1,m+1+1);

            %Recursively compute the values of the rest of the m terms.
            for m=(n-2):-1:1
                g=2*(m+1)/sqrt((n-m)*(n+m+1));
                %h is given in Equation 19 of the first Holmes and
                %Featherstone paper.
                h=sqrt((n+m+2)*(n-m-1)/((n-m)*(n+m+1)));
                PBarUVals(n+1,m+1)=g*t*PBarUVals(n+1,m+1+1)-h*u^2*PBarUVals(n+1,m+2+1);
            end

            %Deal with the special m=0 case.
            m=0;
            g=2*(m+1)/sqrt((n-m)*(n+m+1));
            h=sqrt((n+m+2)*(n-m-1)/((n-m)*(n+m+1)));
            PBarUVals(n+1,m+1)=jTerm*(g*t*PBarUVals(n+1,m+1+1)-h*u^2*PBarUVals(n+1,m+2+1));
        end
    end

    %If the first derivative is desired.
    if(nargout>1)
        %Allocate space.
        dPBarUValsdTheta=CountingClusterSet(totalP);
        %The first derivative of PBar_{0,0}(cos(theta)) is just zero.
        dPBarUValsdTheta(1,1)=0;

        if(M>0)
            m=1;
            n=1;
            %From Equation 30 in the first Holmes and Featherstone paper. This
            %is the seed value from which other values will be computed.
            dPBarUValsdTheta(1+1,1+1)=m*(t/u)*PBarUVals(1+1,1+1);

            n=1;
            m=0;
            %e is given in Equation 22 of the first Holmes and Featherstone
            %paper.
            e=sqrt((n+m+1)*(n-m)/2);
            %This is Equation 30 of the first Holmes and Featherstone paper for
            %m=0.
            dPBarUValsdTheta(n+1,m+1)=-e*u*PBarUVals(n+1,m+1+1);

            %Compute the values along the main diagonal, where m=n starting
            %from m=n=2. This implements Equation 30 in the frist Holmes and
            %Featherstone paper for the ratio of the first derivative.
            for m=2:M
                dPBarUValsdTheta(m+1,m+1)=m*(t/u)*PBarUVals(m+1,m+1);
            end

            %Next, evaluate the values for all other valid n and m.
            for n=2:M
                %Recursively compute the values of the m terms for m>0.
                for m=(n-1):-1:1
                    e=sqrt((n+m+1)*(n-m));
                    dPBarUValsdTheta(n+1,m+1)=m*(t/u)*PBarUVals(n+1,m+1)-e*u*PBarUVals(n+1,m+1+1);
                end
                %Deal with the special m=0 case.
                m=0;
                e=sqrt((n+m+1)*(n-m)/2);
                dPBarUValsdTheta(n+1,m+1)=-e*u*PBarUVals(n+1,m+1+1);
            end
        end
    end

    %If the second derivative is desired
    if(nargout>2)
        %Allocate space
        d2PBarUValsdTheta2=CountingClusterSet(totalP);

        for n=0:M
            for m=0:n
                %From the first (un-numbered) equation in the second Holmes
                %and Featherstone paper AFTER correction.
                d2PBarUValsdTheta2(n+1,m+1)=(m^2/u^2-n*(n+1))*PBarUVals(n+1,m+1)-(t/u)*dPBarUValsdTheta(n+1,m+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 q = ik (K)
% Anthropomorphic arm with 3 DOF
% It calculates the Inverse Kinematic of an Anthropomorphic arm with 3 DOF.
% 'q' is the solutions in radiant and K is the direct Kinematic matrix.
%
%               K = [ n s a p;
%                     0 0 0 1]
% where n, s, a are three vectors fo 3 elements that represents the
% end-effector's orientation, and p is the desired end-effector position.

% Denavit-Hartenberg's Parameters
a1=0;           % [m]
a2=0.2;         % [m]
a3=0.2;         % [m]
alfa1=pi/2;     % [rad]
alfa2=0;        % [rad]
alfa3=0;        % [rad]

dk=K;          % Direct kinematics matrix

% Inverse Kinematic
pw_x=dk(1,4);   % Vector's components that representes the end-effector position
pw_y=dk(2,4);
pw_z=dk(3,4);

c3=(pw_x^2+pw_y^2+pw_z^2-a2^2-a3^2)/(2*a2*a3);  % cos(teta3)
s3=-sqrt(1-c3^2);        % sin(teta3)
teta3=atan2(s3,c3);

c2=(sqrt(pw_x^2+pw_y^2)*(a2+a3*c3)+pw_z*a3*s3)/(a2^2+a3^2+2*a2*a3*c3);      % cos(teta2)
s2=(pw_z*(a2+a3*c3)-sqrt(pw_x^2+pw_y^2)*a3*s3)/(a2^2+a3^2+2*a2*a3*c3);      % sin(teta2)
teta2=atan2((a2+a3*c3)*pw_z-a3*s3*sqrt(pw_x^2+pw_y^2),(a2+a3*c3)*sqrt(pw_x^2+pw_y^2)+a3*s3*pw_z);

teta1=atan2(pw_y,pw_x);

q=[teta1 teta2 teta3]';     % Solutions in radiant


function [ W ] = log_gmm_div_w(X,Priors,Mu,Sigma )
% LOG_GMM_DIV_W Computes the responsibility factor of each GMM component ot
%
%
%   input -----------------------------------------------------------------
%
%       o X         : (D x N),      Samples
%
%       o Priors    : (1 x K),      Weights
%
%       o Mu        : (D x K),      Means
%
%       o Sigma     : (D x D x K),  Covariance
%
%
%   output ----------------------------------------------------------------
%
%       o W         : (N x K),  responsibility factor
%

K = size(Priors,2);
D = size(Mu,1);
N = size(X,2);

% likelihood for all data points
% (N x 1)
Lik = gmm_pdf(X,Priors,Mu,Sigma);

% (N x K)
lik_k = zeros(N,K);

if D==1
    for k=1:K
        lik_k(:,k) = gaussPDF(X,Mu(k),Sigma(k))';
    end
else
    for k=1:K
        lik_k(:,k) = gaussPDF(X,Mu(:,k),squeeze(Sigma(:,:,k)))';
    end
end

%(N x K)
W = (lik_k .* repmat(Priors,N,1)) ./ repmat(Lik,1,K);

% nbStates = K;
% for i=1:nbStates
%     %Compute probability p(x|i)
%     Pxi(:,i) = gaussPDF(X, Mu(:,i), squeeze(Sigma(:,:,i)));
% end
% %Compute posterior probability p(i|x)
% Pix_tmp = repmat(Priors,[N 1]).*Pxi;
% Pix = Pix_tmp ./ repmat(sum(Pix_tmp,2),[1 nbStates]); % -> W
% %Compute cumulated posterior probability
% E = sum(Pix);

end


%FITELLIPSE  Fits an ellipse around a set of 2D points
%
%     rct = cv.fitEllipse(points)
%     rct = cv.fitEllipse(points, 'OptionName',optionValue, ...)
%
% ## Input
% * __points__ Input 2D point set, stored in numeric array
%   (Nx2/Nx1x2/1xNx2) or cell array of 2-element vectors (`{[x,y], ...}`).
%   There should be at least 5 points to fit the ellipse.
%
% ## Output
% * __rct__ Output rotated rectangle struct with the following fields:
%   * __center__ The rectangle mass center `[x,y]`.
%   * __size__ Width and height of the rectangle `[w,h]`.
%   * __angle__ The rotation angle in a clockwise direction. When the angle is
%     0, 90, 180, 270 etc., the rectangle becomes an up-right rectangle.
%
% ## Options
% * __Method__ One of:
%   * __Linear__ Linear (LIN) conic fitting method. This is the default.
%   * __Direct__ Direct least square (DLS) method.
%   * __AMS__ Approximate mean square (AMS) method.
%
% ### Method = Linear
%
% The function calculates the ellipse that fits (in a least-squares sense) a
% set of 2D points best of all. It returns the rotated rectangle in which the
% ellipse is inscribed. The first algorithm described by [Fitzgibbon95] is
% used. Developer should keep in mind that it is possible that the returned
% ellipse/rotatedRect data contains negative indices, due to the data points
% being close to the border of the containing Mat element.
%
% ### Method = Direct
%
% The function calculates the ellipse that fits a set of 2D points.
% It returns the rotated rectangle in which the ellipse is inscribed.
% The Direct least square (Direct) method by [Fitzgibbon1999] is used.
%
% For an ellipse, this basis set is `chi = (x^2, x*y, y^2, x, y, 1)`, which is
% a set of six free coefficients `A^T = {A_xx, A_xy, A_yy, A_x, A_y, A_0}`.
% However, to specify an ellipse, all that is needed is five numbers; the
% major and minor axes lengths `(a,b)`, the position `(x_0,y_0)`, and the
% orientation `theta`. This is because the basis set includes lines,
% quadratics, parabolic and hyperbolic functions as well as elliptical
% functions as possible fits.
%
% The Direct method confines the fit to ellipses by ensuring that
% `4*A_xx*A_yy - A_xy^2 > 0`. The condition imposed is that
% `4*A_xx*A_yy - A_xy^2 = 1` which satisfies the inequality and as the
% coefficients can be arbitrarily scaled is not overly restrictive.
%
%     epsilon^2 = A^T * D^T * D * A
%     with A^T * C * A = 1
%     and C = [0 0 2 0 0 0; 0 -1 0 0 0 0; 2 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0]
%
% The minimum cost is found by solving the generalized eigenvalue problem.
%
%     D^T * D * A = lambda * (C) * A
%
% The system produces only one positive eigenvalue `lambda` which is chosen as
% the solution with its eigenvector `u`. These are used to find the
% coefficients:
%
%     A = sqrt(1 / (u^T * C * u)) * u
%
% The scaling factor guarantees that  `A^T * C * A = 1`.
%
% Note: If the determinant of `A` is too small, the method fallsback to
% 'Linear'.
%
% ### Method = AMS
%
% The function calculates the ellipse that fits a set of 2D points.
% It returns the rotated rectangle in which the ellipse is inscribed.
% The Approximate Mean Square (AMS) proposed by [Taubin1991] is used.
%
% For an ellipse, this basis set is `chi = (x^2, x*y, y^2, x, y, 1)`, which is
% a set of six free coefficients `A^T = {A_xx, A_xy, A_yy, A_x, A_y, A_0}`.
% However, to specify an ellipse, all that is needed is five numbers; the
% major and minor axes lengths `(a,b)`, the position `(x_0,y_0)`, and the
% orientation `theta`. This is because the basis set includes lines,
% quadratics, parabolic and hyperbolic functions as well as elliptical
% functions as possible fits.
%
% If the fit is found to be a parabolic or hyperbolic function then the
% 'Direct' method is used. The AMS method restricts the fit to parabolic,
% hyperbolic and elliptical curves by imposing the condition that
% `A^T * (D_x^T * D_x + D_y^T * D_y) * A = 1` where the matrices `Dx` and `Dy`
% are the partial derivatives of the design matrix `D` with respect to x and y.
% The matrices are formed row by row applying the following to each of the
% points in the set:
%
%     D(i,:)   = {x_i^2, x_i y_i, y_i^2, x_i, y_i, 1}
%     D_x(i,:) = {2*x_i, y_i, 0, 1, 0, 0}
%     D_y(i,:) = {0, x_i, 2*y_i, 0, 1, 0}
%
% The AMS method minimizes the cost function
%
%     epsilon^2 = (A^T * D^T * D * A) / (A^T * (D_x^T * D_x + D_y^T * D_y) * A^T)
%
% The minimum cost is found by solving the generalized eigenvalue problem.
%
%     D^T * D * A = lambda * (D_x^T * D_x + D_y^T * D_y) * A
%
% Note: If the determinant of `A` is too small, the method fallsback to
% 'Linear'.
%
% ## References
% [Fitzgibbon95]:
% > Andrew W Fitzgibbon and Robert B Fisher.
% > "A buyer's guide to conic fitting". In Proceedings of the 6th British
% > conference on Machine vision (Vol. 2), pages 513-522. BMVA Press, 1995.
% > [PDF](http://www.bmva.org/bmvc/1995/bmvc-95-050.pdf)
%
% [Fitzgibbon1999]:
% > Andrew Fitzgibbon, Maurizio Pilu, and Robert B. Fisher.
% > "Direct least square fitting of ellipses". IEEE Transactions on Pattern
% > Analysis and Machine Intelligence, 21(5):476-480, 1999.
% > [PDF](https://www.microsoft.com/en-us/research/wp-content/uploads/2016/02/ellipse-pami.pdf)
%
% [Taubin1991]:
% > Gabriel Taubin. "Estimation of planar curves, surfaces, and nonplanar
% > space curves defined by implicit equations with applications to edge and
% > range image segmentation". IEEE Transactions on Pattern Analysis and
% > Machine Intelligence, 13(11):1115-1138, 1991.
%
% See also: cv.minAreaRect
%


function lagrange_nd_test09 ( )

%*****************************************************************************80
%
%% LAGRANGE_ND_TEST09 tests LAGRANGE_PARTIAL in 3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LAGRANGE_ND_TEST09\n' );
  fprintf ( 1, '  LAGRANGE_PARTIAL determines\n' );
  fprintf ( 1, '  the Lagrange interpolating polynomials L(x)\n' );
  fprintf ( 1, '  for ND points in D dimensions, assuming that\n' );
  fprintf ( 1, '  the number of points is less than or equal to\n' );
  fprintf ( 1, '  R = Pi(D,N), the number of monomials of degree N or less\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  For this example, the data points are the same as those\n' );
  fprintf ( 1, '  used by the level 2 Clenshaw Curtis sparse grid in 3D.\n' );

  sq2h = sqrt ( 2.0 ) / 2.0;

  d = 3;
  n = 4;
  r = mono_upto_enum ( d, n );
  nd = 25;
  xd = [ ...
      0.0,   0.0,   0.0; ...
     -1.0,   0.0,   0.0; ...
      1.0,   0.0,   0.0; ...
      0.0,  -1.0,   0.0; ...
      0.0,   1.0,   0.0; ...
      0.0,   0.0,  -1.0; ...
      0.0,   0.0,   1.0; ...
     -sq2h,  0.0,   0.0; ...
      sq2h,  0.0,   0.0; ...
     -1.0,  -1.0,   0.0; ...
      1.0,  -1.0,   0.0; ...
     -1.0,   1.0,   0.0; ...
      1.0,   1.0,   0.0; ...
      0.0,  -sq2h,  0.0; ...
      0.0,   sq2h,  0.0; ...
     -1.0,   0.0,  -1.0; ...
      1.0,   0.0,  -1.0; ...
     -1.0,   0.0,   1.0; ...
      1.0,   0.0,   1.0; ...
      0.0,  -1.0,  -1.0; ...
      0.0,   1.0,  -1.0; ...
      0.0,  -1.0,   1.0; ...
      0.0,   1.0,   1.0; ...
      0.0,   0.0,  -sq2h; ...
      0.0,   0.0,   sq2h ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension D = %d\n', d );
  fprintf ( 1, '  Maximum degree N = %d\n', n );
  fprintf ( 1, '  Number of monomials R = %d\n', r );
  fprintf ( 1, '  Number of data points ND = %d\n', nd );

  r8mat_transpose_print ( d, nd, xd, '  Data points XD:' );

  [ po, pc, pe ] = lagrange_partial ( d, n, r, nd, xd );
%
%  Print the polynomials.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Lagrange polynomials for XD data points:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : nd
    o = po(i);
    label = sprintf ( '  P(%d)(x) =', i );
    polynomial_print ( d, o, pc(i,1:o), pe(i,1:o), label );
  end
%
%  Evaluate the polynomials at XD.
%
  value = zeros ( nd, nd );

  for j = 1 : nd
    o = po(j);
    label = sprintf ( '  P(%d)(x) =', j );
    value(1:nd,j) = polynomial_value ( d, o, pc(j,1:o), pe(j,1:o), nd, xd );
  end

  err = r8mat_is_identity ( nd, value );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm of Lagrange matrix error = %g\n', err );

  return
end


function arccosh_values_test ( )

%*****************************************************************************80
%
%% ARCCOSH_VALUES_TEST demonstrates the use of ARCCOSH_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 June 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ARCCOSH_VALUES_TEST:\n' );
  fprintf ( 1, '  ARCCOSH_VALUES stores values of \n' );
  fprintf ( 1, '  the hyperbolic arc cosine function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = arccosh_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16g\n', x, fx );

  end

  return
end


function value = p02_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P02_F evaluates the integrand for problem 02.
%
%  Dimension:
%
%    DIM_NUM arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integrand:
%
%    ( sum ( 2 * x(1:dim_num) - 1 ) )**4
%
%  Exact Integral:
%
%    DIM_NUM * (5*DIM_NUM-2) / 15
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  value(1:point_num) = 0.0;

  for point = 1 : point_num
    value(point) = ( sum ( 2.0 * x(1:dim_num,point) - 1.0 ) )^4;
  end

  p02_i4 ( 'I', '#', point_num );

  return
end


function [rx,sx,tx,ry,sy,ty,rz,sz,tz,J] = GeometricFactors3D(x,y,z,Dr,Ds,Dt)

% function [rx,sx,tx,ry,sy,ty,rz,sz,tz,J] = GeometricFactors3D(x,y,z,Dr,Ds,Dt)
% Purpose  : Compute the metric elements for the local mappings of the elements

% calculate geometric factors
xr = Dr*x; xs = Ds*x; xt = Dt*x;
yr = Dr*y; ys = Ds*y; yt = Dt*y;
zr = Dr*z; zs = Ds*z; zt = Dt*z;

J = xr.*(ys.*zt-zs.*yt) - yr.*(xs.*zt-zs.*xt) + zr.*(xs.*yt-ys.*xt);
rx =  (ys.*zt - zs.*yt)./J; ry = -(xs.*zt - zs.*xt)./J; rz = (xs.*yt - ys.*xt)./J;
sx = -(yr.*zt - zr.*yt)./J; sy =  (xr.*zt - zr.*xt)./J; sz = -(xr.*yt - yr.*xt)./J;
tx =  (yr.*zs - zr.*ys)./J; ty = -(xr.*zs - zr.*xs)./J; tz = (xr.*ys - yr.*xs)./J;
return;


function plane = medianPlane(p1, p2)
%MEDIANPLANE Create a plane in the middle of 2 points
%
%   PLANE = medianPlane(P1, P2)
%   Creates a plane in the middle of 2 points.
%   PLANE is perpendicular to line (P1 P2) and contains the midpoint of P1
%   and P2.
%   The direction of the normal of PLANE is the same as the vector from P1
%   to P2.
%
%   See also:
%   planes3d, createPlane
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 18/02/2005.
%

%   HISTORY
%   28/06/2007: add doc, and manage multiple inputs

% unify data dimension
if size(p1, 1)==1
    p1 = repmat(p1, [size(p2, 1) 1]);
elseif size(p2, 1)==1
    p2 = repmat(p2, [size(p1, 1) 1]);
elseif size(p1, 1)~=size(p2, 1)
    error('data should have same length, or one data should have length 1');
end

% middle point
p0  = (p1 + p2)/2;

% normal to plane
n   = p2-p1;

% create plane from point and normal
plane = createPlane(p0, n);


function [theta, J_history] = gradientDescent(X, y, theta, alpha, num_iters)
%GRADIENTDESCENT Performs gradient descent to learn theta
%   theta = GRADIENTDESCENT(X, y, theta, alpha, num_iters) updates theta by
%   taking num_iters gradient steps with learning rate alpha

% Initialize some useful values
m = length(y); % number of training examples
J_history = zeros(num_iters, 1);

for iter = 1:num_iters
    theta1=theta(1,1)-alpha/m*sum(X*theta-y);
    theta2=theta(2,1)-alpha/m*sum((X*theta-y) .* X(:,2));
    theta(1,1)=theta1;
    theta(2,1)=theta2;

    % ====================== YOUR CODE HERE ======================
    % Instructions: Perform a single gradient step on the parameter vector
    %               theta.
    %
    % Hint: While debugging, it can be useful to print out the values
    %       of the cost function (computeCost) and gradient here.
    %

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

    % Save the cost J in every iteration
    J_history(iter) = computeCost(X, y, theta);

end

end


function drawArcEllipse(x,y,a,b,theta0,theta1,color)
%(x,y):= coordinates of the center
% r: = radius of the circle
angle = theta0+0.01:0.01:theta1;
xp = x+ a*cos(angle);
yp = y+ b*sin(angle);
plot(xp,yp,color);

% plot the symetric arc
angle = theta0+pi+0.01:0.01:theta1+pi;
xp = x+ a*cos(angle);
yp = y+ b*sin(angle);
plot(xp,yp,color);


function Pvalue=myfisher23(x)
%P=MYFISHER23(X)- Fisher's Exact Probability Test on 2x3 matrix.
%Fisher's exact test of 2x3 contingency tables permits calculation of
%precise probabilities in situation where, as a consequence of small cell
%frequencies, the much more rapid normal approximation and chi-square
%calculations are liable to be inaccurate. The Fisher's exact test involves
%the computations of several factorials to obtain the probability of the
%observed and each of the more extreme tables. Factorials growth quickly,
%so it's necessary use logarithms of factorials. This computations is very
%easy in Matlab because x!=gamma(x+1) and log(x!)=gammaln(x+1). This
%function is now fully vectorized to speed up the computation.
%
% Syntax: 	myfisher23(x)
%
%     Inputs:
%           X - 2x3 data matrix
%     Outputs:
%           - Three p-values
%
%   Example:
%
%                A   B   C
%           -------------------
%      X         0   3   2
%           -------------------
%      Y         6   5   1
%           -------------------
%
%
%   x=[0 3 2; 6 5 1];
%
%   Calling on Matlab the function:
%             myfisher23(x)
%
%   Answer is:
%
% 2x3 matrix Fisher's exact test: 18 tables were evaluated
% -----------------------------------------------------------------
% 		 p-value (2-tails): 0.0882352941
% -----------------------------------------------------------------
%
%           Created by Giuseppe Cardillo
%           giuseppe.cardillo-edta@poste.it
%
% To cite this file, this would be an appropriate format:
% Cardillo G. (2007) MyFisher23: a very compact routine for Fisher's exact
% test on 2x3 matrix
% http://www.mathworks.com/matlabcentral/fileexchange/15399

%Input Error handling
if ~isequal(size(x),[2 3])
    if isequal(size(x),[3 2])
        x=x';
    else
        error('Input matrix must be a 2x3 matrix')
    end
end
if ~all(isfinite(x(:))) || ~all(isnumeric(x(:)))
    error('Warning: all X values must be numeric and finite')
end
if ~isequal(x(:),round(x(:)))
    error('Warning: X data matrix values must be whole numbers')
end

Rs=sum(x,2); %rows sum
Cs=sum(x); %columns sum
N=sum(Rs); %Total observations

%If necessed, rearrange matrix
if ~issorted(Cs)
    [Cs,ind]=sort(Cs);
    x=x(:,ind);
    clear ind
end
if ~issorted(Rs)
    [Rs,ind]=sort(Rs);
    x=x(ind,:);
    clear ind
end

%recall that Fisher's P=[Prod(Rs!)*Prod(Cs!)]/[N!*prod(X(i,j)!)]
%Log(A*B)=Log(A)+Log(B) and Log(A/B)=Log(A)-Log(B)
%Costruct all possible tables
%A 2x3 matrix has 2 degrees of freedom...
A=0:1:min(Rs(1),Cs(1)); %all possible values of X(1,1)
B=min(Cs(2),Rs(1)-A); %max value of X(1,2) given X(1,1)
et=sum(B+ones(size(B))); %tables to evaluate
Tables=zeros(et,6); %Matrix preallocation
%compute the index
stop=cumsum(B+1);
start=[1 stop(1:end-1)+1];
%In the first round of the for cycle, Column 1 assignment should be skipped
%because it is already zero. So, modify the cycle...
Tables(start(1):stop(1),2)=0:1:B(1); %Put in the Column2 all the possible values of X(1,2) given X(1,1)
for I=2:length(A)
    Tables(start(I):stop(I),1)=A(I); %replicate the A(I) value for B(I)+1 times
    %Put in the Column2 all the possible values of X(1,2) given X(1,1)
    Tables(start(I):stop(I),2)=0:1:B(I);
end
clear A B start stop
%The degrees of freedom are finished, so complete the table...
%...Put all the possible values of X(1,3) given X(1,1) and X(1,2)
Tables(:,3)=Rs(1)-sum(Tables(:,1:2),2);
%Complete the second row given the first row
Tables(:,4:6)=repmat(Cs,et,1)-Tables(:,1:3);

%Compute log(x!) using the gammaln function
zf=gammaln(Tables+1); %compute log(x!)
K=sum(gammaln([Rs' Cs]+1))-gammaln(N+1); %The costant factor K=log(prod(Rs!)*prod(Cs!)/N!)
np=exp(K-sum(zf,2)); %compute the p-value of each possible matrix
[tf,obt]=ismember(x(1,:),Tables(:,1:3),'rows'); %Find the observed table
clear zf K tf
%Finally compute the probability for 2-tailed test
P=sum(np(np<=np(obt)));

%display results
tr=repmat('-',1,65); %Set up the divisor
disp(' ')
fprintf('2x3 matrix Fisher''s exact test: %0.0f tables were evaluated\n',et)
disp(tr)
fprintf('\t\t p-value (2-tails): %0.10f\n',P);
disp(tr)
fprintf('Mid-p correction: %0.10f\n',0.5*np(obt)+sum(np(np<np(obt))));
disp(tr)

if nargout
    Pvalue=P;
end


function c = connectpoly(x,y)
%CONNECTPOLY Connects the vertices of a polygon with straight lines.
%   C = CONNECTPOLY(X,Y) connects the points in X and Y with straight
%   lines, where X and Y are vectors containing the row and column
%   coordinates of the polygon vertices. These points are assumed to be
%   ordered in the clockwise or counterclockwise direction. The output,
%   C, is np-by-2 matrix whose rows are the (row,col) coordinates of the
%   boundary of the polygon, which are in the same direction as the
%   input vertices. The last point in the sequence is equal to the
%   first, thus producing a polygon that is both fully connected and
%   closed.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

v = [x(:),y(:)];

% Close the polygon.
if ~isequal(v(end,:),v(1,:))
   v(end + 1,:) = v(1,:);
end

% Connect the vertices.
segments = cell(1,length(v) - 1);
for k = 2:length(v)
   [x,y] = intline(v(k - 1,1),v(k,1),v(k - 1,2),v(k,2));
   segments{k - 1} = [x,y];
end

c = cat(1,segments{:});


function [newLine, isOrthogonal] = projLineOnPlane(line, plane)
%PROJLINEONPLANE Return the orthogonal projection of a line on a plane
%
%   NEWLINE = PROJLINEONPLANE(LINE, PLANE) Returns the orthogonal
%   projection of LINE or multiple lines on the PLANE.
%
%   [..., ISORTHOGONAL] = PROJLINEONPLANE(LINE, PLANE) Also returns if the
%   LINE is orthogonal to the PLANE.
%
%   Example
%     plane = [.1 .2 .3 .4 .5 .6 .7 .8 .9];
%     lines = [0 .3 0 1 0 0;0  .5 .5 0 0 1;...
%         .4 .1 .5 1 0 2;.2 .7 .1 0 1 0;...
%         plane(1:3) planeNormal(plane)];
%     [newLines, isOrthogonal] = projLineOnPlane(lines, plane);
%     figure('color','w'); axis equal; view(3)
%     drawLine3d(lines,'b')
%     drawPlane3d(plane)
%     drawLine3d(newLines(~isOrthogonal,:), 'r')
%
%   See also:
%   planes3d, lines3d, intersectLinePlane, projPointOnPlane
%
% ---------
% Author: oqilipo
% Created: 2017-08-06
% Copyright 2017

p1 = projPointOnPlane(line(:,1:3), plane);
p2 = projPointOnPlane(line(:,1:3)+line(:,4:6), plane);

newLine=createLine3d(p1, p2);
isOrthogonal = ismembertol(p1,p2,'ByRows',true);


function asa266_test03 ( )

%*****************************************************************************80
%
%% TEST03 tests DIGAMMA, R8_PSI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 August 2010
%
%  Author:
%
%    John Burkardt
%
  ntest = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03\n' );
  fprintf ( 1, '  digamma(X) = d ( Log ( Gamma ( X ) ) ) / dX.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  DIGAMMA and\n' );
  fprintf ( 1, '  R8_PSI compute the digamma function:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  X  DIGAMMA   R8_PSI\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ntest

    x = i / ntest;

    fprintf ( 1, '  %12f  %12f  %12f\n', x, digamma ( x ), r8_psi ( x ) );

  end

  return
end


function [E,v] = km_kpca_icd(X,m,ktype,kpar,precision)
% KM_KPCA_ICD performs kernel principal component analysis (KPCA) on a data
% set X, using Incomplete Cholesky Decomposition to approximate the large
% kernel matrix.
%
% Input:	- X: data matrix in column format (each data point is a row)
%			- m: the number of principal components to return. If m is
%			smaller than 1, it is interpreted as the fraction of the signal
%			energy that is to be contained within the returned principal
%			components.
%			- ktype: string representing kernel type.
%			- kpar: vector containing the kernel parameters.
% Output:	- E: matrix containing the principal components.
%			- v: array containing the eigenvalues.
% USAGE: [E,v] = km_kpca_icd(X,m,ktype,kpar,precision)
%
% Author: Steven Van Vaerenbergh (steven *at* gtas.dicom.unican.es), 2010.
%
% This file is part of the Kernel Methods Toolbox for MATLAB.
% https://github.com/steven2358/kmbox

if nargin<5
	precision = 10^-6;
end
n = size(X,1);

G = km_kernel_icd(X,ktype,kpar,m,precision);
m1 = min(m,size(G,2));
R = G'*G;
[Er,Vr] = eig(R);
v = diag(Vr);

E = G*Er*diag(1./sqrt(v));
[v,ind] = sort(v,'descend');
E = E(:,ind(1:m1));	% principal components
v = v(1:m1);
for i=1:m1
	E(:,i) = E(:,i)/sqrt(n*v(i));	% normalization
end


% makegausslpfir        Gaussian LP filter
%
% win = makegausslpfir( Fc, Fs, s )
%
% Fc    corner frequency [Hz]
% Fs    sampling frequency [Hz]
% s     support [SD], minimum 3, default 4
%
% win   fir
%
% see also  firfilter

% 02-oct-13 ES

function gwin = makegausslpfir( Fc, Fs, s )

nargs = nargin;
if nargs < 1 || isempty( Fc ), Fc = 100; end
if nargs < 2 || isempty( Fs ), Fs = 1000; end
if nargs < 3 || isempty( s ), s = 4; end
s = max( s, 3 );

sd = Fs / ( 2 * pi * Fc );
x = -ceil( s * sd ) : ceil( s * sd );
gwin = 1/( 2 * pi * sd ) * exp( -( x.^2/2/sd.^2 ) );
gwin = gwin / sum( gwin );

return

% EOF

% to determine the proper support:
cwin = cumsum( gwin );
length( cwin ), [ find( cwin >= 0.001, 1, 'first' ) - 1, find( cwin <= 0.999, 1, 'last' ) + 1 ]
% 3 SD are usually enough


%% ALL NONLINEAR VERSION
clc
clear
% Objective & Gradient
 fun = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
 grad = @(x)[2*x(1)-400*x(1)*(x(2)-x(1)^2)-2,200*x(2)-200*x(1)^2];

 % Constraints
A = [-1 1; 1 1];
rl = [-Inf;5];
ru = [-1;5];
lb = [0;0]; ub = [4;4];

% Nonlinear Constraint, Jacobian & Structure
 nlcon = @(x) A*x;
 nljac = @(x) sparse(A);
 jacstr = @() sparse(double(A~=0));

% Starting Guess
 x0 = [2;2];

% Build Function Structure
 funcs.objective = fun;
 funcs.gradient = grad;
 funcs.constraints = nlcon;
 funcs.jacobian = nljac;
 funcs.jacobianstructure = jacstr;

% Build Options Structure
 opts.lb = lb;
 opts.ub = ub;
 opts.cl = rl;
 opts.cu = ru;
 opts.ipopt.hessian_approximation = 'limited-memory';

% Call IPOPT
[x,output] = bonmin(x0,funcs,opts)

%% ALL LINEAR VERSION
clc
clear all
% Objective & Gradient
 fun = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
 grad = @(x)[2*x(1)-400*x(1)*(x(2)-x(1)^2)-2,200*x(2)-200*x(1)^2];

 % Constraints
A = [-1 1; 1 1];
rl = [-Inf;5];
ru = [-1;5];
lb = [0;0]; ub = [4;4];

% Starting Guess
 x0 = [2;2];

% Build Function Structure
 funcs.objective = fun;
 funcs.gradient = grad;

% Build Options Structure
 opts.lb = lb;
 opts.ub = ub;
 opts.A = sparse(A);
 opts.rl = rl;
 opts.ru = ru;
 opts.ipopt.hessian_approximation = 'limited-memory';
 opts.display = 2;

% Call IPOPT
[x,output] = bonmin(x0,funcs,opts)

%% NL INEQ, LIN EQ
clc
clear all
% Objective & Gradient
 fun = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
 grad = @(x)[2*x(1)-400*x(1)*(x(2)-x(1)^2)-2,200*x(2)-200*x(1)^2];

 % Constraints
Aeq = [1 1];
rl = 5;
ru = 5;
lb = [0;0]; ub = [4;4];

% Nonlinear Constraint, Jacobian & Structure
 A = [-1 1];
 nlcon = @(x) [A]*x;
 nljac = @(x) sparse([A]);
 jacstr = @() sparse(double([A]~=0));
 cl = -Inf;
 cu = -1;

% Starting Guess
 x0 = [2;2];

% Build Function Structure
 funcs.objective = fun;
 funcs.gradient = grad;
 funcs.constraints = nlcon;
 funcs.jacobian = nljac;
 funcs.jacobianstructure = jacstr;

% Build Options Structure
 opts.lb = lb;
 opts.ub = ub;
 opts.A = sparse(Aeq);
 opts.rl = rl;
 opts.ru = ru;
 opts.cl = cl;
 opts.cu = cu;
 opts.ipopt.hessian_approximation = 'limited-memory';
  opts.display = 2;

% Call IPOPT
% for i = 1:1000
    [x,output] = bonmin(x0,funcs,opts)
% end

%% NL EQ, LIN INEQ
clc
clear all
% Objective & Gradient
 fun = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
 grad = @(x)[2*x(1)-400*x(1)*(x(2)-x(1)^2)-2,200*x(2)-200*x(1)^2];

 % Constraints
A = [-1 1];
rl = -Inf;
ru = -1;
lb = [0;0]; ub = [4;4];

% Nonlinear Constraint, Jacobian & Structure
 Aeq = [1 1];
 nlcon = @(x) [Aeq]*x;
 nljac = @(x) sparse([Aeq]);
 jacstr = @() sparse(double([Aeq]~=0));
 cl = 5;
 cu = 5;

% Starting Guess
 x0 = [2;2];

% Build Function Structure
 funcs.objective = fun;
 funcs.gradient = grad;
 funcs.constraints = nlcon;
 funcs.jacobian = nljac;
 funcs.jacobianstructure = jacstr;

% Build Options Structure
 opts.lb = lb;
 opts.ub = ub;
 opts.A = sparse(A);
 opts.rl = rl;
 opts.ru = ru;
 opts.cl = cl;
 opts.cu = cu;
 opts.ipopt.hessian_approximation = 'limited-memory';
  opts.display = 1;

% Call IPOPT
% for i = 1:1000
    [x,output] = bonmin(x0,funcs,opts)
% end

%%
clc
%Objective
obj = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
% Constraints
A = [-1 1];
b = -1;
Aeq = [1 1];
beq = 5;
lb = [0;0]; ub = [4;4];
% Solve
x0 = [2;2];
opts = optiset('solver','bonmin','display','iter');
Opt = opti('obj',obj,'ndec',2,'bounds',lb,ub,'ineq',A,b,'eq',Aeq,beq,'ivars',2,'options',opts)
[x,fval,ef,info] = solve(Opt,x0)
%Plot
% plot(Opt,[],1)

%%
% Objective & Gradient
fun = @(x) -sum(x);
grad = @(x) [-1 -1 -1 -1];

% Linear constraints
n = 4;
A = randn(n,4);
rl = zeros(n,1);
ru = zeros(n,1);

% Nonlinear Constraint, Jacobian & Structure
nlcon = @(x) x(:).^4;
nljac = @(x) sparse(diag(4*x(:).^3));
jacstr = @() speye(4);
cl = [-inf;-inf;-inf;-inf];
cu = [1;1;1;1];

% Starting Guess
x0 = [0;0;0;0];
% Build Function Structure
funcs.objective = fun;
funcs.gradient = grad;
funcs.constraints = nlcon;
funcs.jacobian = nljac;
funcs.jacobianstructure = jacstr;

% Build Options Structure
lb = [-10;-10;-10;-10];
ub = [40;40;40;40];
opts.lb = lb;
opts.ub = ub;
opts.rl = rl;
opts.ru = ru;
opts.cl = cl;
opts.cu = cu;
opts.A = sparse(A);
opts.ipopt.hessian_approximation = 'limited-memory';
opts.display = 2;

% Call IPOPT
[x,output] = bonmin(x0,funcs,opts)


function Test_Mahal(file)
if nargin<1
    file='sample10k.csv';
end
UmapUtil.Initialize;
SAMPLE_FILE = 'sample10k.csv';
file = UmapUtil.RelocateExamples(file);
data= File.ReadCsv(file);

cov = nancov(data);
disp(['The covariance matrix for the data from ' SAMPLE_FILE ' is:']);
disp(cov);

invCov = inv(cov);
disp(['The inverse of the covariance matrix for the data from ' SAMPLE_FILE ' is:']);
disp(invCov);

X1 = knnsearch(data, data, 'K', 15, 'Distance', 'mahalanobis');
X2 = knnsearch(data, data, 'K', 15, 'Distance', 'mahalanobis', 'Cov', cov);

if isequal(X1, X2)
    disp('MATLAB''s knnsearch.m confirms that we correctly computed the covariance matrix!');
else
    disp('Hmm... did we calculate the covariance matrix incorrectly?');
end
X3 = KnnFind.Approximate(data, 15, 'mahalanobis', invCov, false, 3);
X4 = KnnFind.Approximate(data, 15, 'mahalanobis', [], false, 3);

acc = KnnFind.AssessApproximation(X1, X3);
disp(['nn_descent found nearest neighbors with ' num2str(100*acc) ' percent accuracy!']);


function trinomial_test ( )

%*****************************************************************************80
%
%% TRINOMIAL_TEST tests TRINOMIAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 April 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRINOMIAL_TEST\n' );
  fprintf ( 1, '  TRINOMIAL evaluates the trinomial coefficient:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  T(I,J,K) = (I+J+K)! / I! / J! / K!\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     I     J     K    T(I,J,K)\n' );
  fprintf ( 1, '\n' );

  for k = 0 : 4
    for j = 0 : 4
      for i = 0 : 4
        t = trinomial ( i, j, k );
        fprintf ( 1, '  %4d  %4d  %4d  %8d\n', i, j, k, t );
      end
    end
  end

  return
end


function chebyshev_series_test03 ( )

%*****************************************************************************80
%
%% CHEBYSHEV_SERIES_TEST03 considers an odd Chebyshev series for SINH(X).
%
%  Discussion:
%
%    TABLE5ODD contains the odd Chebyshev series coefficients for
%    sinh(x) over -1 <= x <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 April 2014
%
%  Author:
%
%    Manfred Zimmer
%
%  Reference:
%
%    Charles Clenshaw,
%    Mathematical Tables, Volume 5,
%    Chebyshev series for mathematical functions,
%    London, 1962.
%
  nc = 9;

  table5odd = [ ...
    1.13031820798497005442, ...
    0.04433684984866380495, ...
    0.00054292631191394375, ...
    0.00000319843646240199, ...
    0.00000001103677172552, ...
    0.00000000002497956617, ...
    0.00000000000003991263, ...
    0.00000000000000004741, ...
    0.00000000000000000004 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBYSHEV_SERIES_TEST03:\n' );
  fprintf ( 1, '  ODDCHEBSER2 computes an odd Chebyshev series approximation.\n' );
  fprintf ( 1, '  and its first two derivatives.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Errors of an odd Chebyshev series y(x) approximating sinh(x):\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    x        err(y)       err(y'')       err(y")\n' );
  fprintf ( 1, '\n' );

  for i = 0 : 10
    x = i / 10.0;
    [ s, s1, s2 ] = oddchebser2 ( x, table5odd, nc );
    y = sinh ( x );
    y1 = cosh ( x );
    s = s - y;
    s1 = s1 - y1;
    s2 = s2 - y;
    fprintf ( 1, '%7.4f  %14.6g  %14.6g  %14.6g\n', x, s, s1, s2 );
  end

  return
end


function z = rel_entr( x, y )

%REL_ENTR   Scalar relative entropy.
%   REL_ENTR(X,Y) returns an array of the same size as X+Y with the
%   relative entropy function applied to each element:
%                      { X.*LOG(X./Y) if X >  0 & Y >  0,
%      REL_ENTR(X,Y) = { 0            if X == 0 & Y >= 0,
%                      { +Inf         otherwise.
%   X and Y must either be the same size, or one must be a scalar. If X and
%   Y are vectors, then SUM(REL_ENTR(X,Y)) returns their relative entropy.
%   If they are PDFs (that is, if X>=0, Y>=0, SUM(X)==1, SUM(Y)==1) then
%   this is equal to their Kullback-Liebler divergence SUM(KL_DIV(X,Y)).
%   -SUM(REL_ENTR(X,1)) returns the entropy of X.
%
%   Disciplined convex programming information:
%       REL_ENTR(X,Y) is convex in both X and Y, nonmonotonic in X, and
%       nonincreasing in Y. Thus when used in CVX expressions, X must be
%       real and affine and Y must be concave. The use of REL_ENTR(X,Y) in
%       an objective or constraint will effectively constrain both X and Y
%       to be nonnegative, hence there is no need to add additional
%       constraints X >= 0 or Y >= 0 to enforce this.

error(nargchk(2,2,nargin));
if ~isreal( x ) || ~isreal( y ),
    error( 'Arguments must be real.' );
end
t1 = x < 0  | y <= 0;
t2 = x == 0 & y >= 0;
x  = max( x, realmin );
y  = max( y, realmin );
z  = x .* log( x ./ y );
z( t1 ) = +Inf;
z( t2 ) = 0;

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [ n_data, n, c ] = moebius_values ( n_data )

%*****************************************************************************80
%
%% MOEBIUS_VALUES returns some values of the Moebius function.
%
%  Discussion:
%
%    MU(N) is defined as follows:
%
%      MU(N) = 1 if N = 1;
%              0 if N is divisible by the square of a prime;
%              (-1)^K, if N is the product of K distinct primes.
%
%    In Mathematica, the function can be evaluated by:
%
%      MoebiusMu[n]
%
%  First values:
%
%     N  MU(N)
%
%     1    1
%     2   -1
%     3   -1
%     4    0
%     5   -1
%     6    1
%     7   -1
%     8    0
%     9    0
%    10    1
%    11   -1
%    12    0
%    13   -1
%    14    1
%    15    1
%    16    0
%    17   -1
%    18    0
%    19   -1
%    20    0
%
%    As special cases, MU(N) is -1 if N is a prime, and MU(N) is 0
%    if N is a square, cube, etc.
%
%  Formula:
%
%    The Moebius function is related to Euler's totient function:
%
%      PHI(N) = Sum ( D divides N ) MU(D) * ( N / D ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the argument of the Moebius function.
%
%    Output, integer C, the value of the Moebius function.
%
  n_max = 20;

  c_vec = [ ...
      1,  -1,  -1,   0,  -1,   1,  -1,   0,   0,   1, ...
     -1,   0,  -1,   1,   1,   0,  -1,   0,  -1,   0 ];

  n_vec = [ ...
      1,   2,   3,   4,   5,   6,   7,   8,   9,  10, ...
     11,  12,  13,  14,  15,  16,  17,  18,  19,  20 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    c = 0;
  else
    n = n_vec(n_data);
    c = c_vec(n_data);
  end

  return
end


function value = r8_choose ( n, k )

%*****************************************************************************80
%
%% R8_CHOOSE computes the combinatorial coefficient C(N,K).
%
%  Discussion:
%
%    Real arithmetic is used, and C(N,K) is computed directly, via
%    Gamma functions, rather than recursively.
%
%    C(N,K) is the number of distinct combinations of K objects
%    chosen from a set of N distinct objects.  A combination is
%    like a set, in that order does not matter.
%
%  Example:
%
%    The number of combinations of 2 things chosen from 5 is 10.
%
%    C(5,2) = ( 5 * 4 * 3 * 2 * 1 ) / ( ( 3 * 2 * 1 ) * ( 2 * 1 ) ) = 10.
%
%    The actual combinations may be represented as:
%
%      (1,2), (1,3), (1,4), (1,5), (2,3),
%      (2,4), (2,5), (3,4), (3,5), (4,5).
%
%  Formula:
%
%    C(N,K) = N! / ( (N-K)! * K! )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the value of N.
%
%    Input, integer K, the value of K.
%
%    Output, real VALUE, the value of C(N,K)
%
  n = floor ( n );
  k = floor ( k );

  if ( n < 0 )

    value = 0.0;

  elseif ( k == 0 )

    value = 1.0;

  elseif ( k == 1 )

    value = n;

  elseif ( 1 < k && k < n-1 )

    facn = gammaln ( n + 1 );
    fack = gammaln ( k + 1 );
    facnmk = gammaln ( n - k + 1 );

    value = round ( exp ( facn - fack - facnmk ) );

  elseif ( k == n-1 )

    value = n;

  elseif ( k == n )

    value = 1.0;

  else

    value = 0.0;

  end

  return
end


function [coeffs,termMat]=polyFitMultiDim(varVals,funVals,degree)
%%POLYFITMULTIDIM Obtain a least-squares multivariate polynomial fit of a
%                 specified order to a given set of data.
%
%INPUTS: varVals This is a numDimXnumPoints set oif dtapoints where the
%                function was evaluated.
%        funVals This is a numPointsX1 or 1XnumPoints vector of the values
%                of the functions at the specified data points.
%        degree The degree of the desired interpolating polynomial. The
%               degree must be less than the total number of monomial
%               terms, which is
%               (1+degree)*binomial(degree+numDim,numDim-1)/numDim.
%
%OUTPUTS: coeffs A hypermatrix of the coefficients for the multivariate
%               polynomial that can be used in the polyValMultiDim
%               function. These are arranged such that
%               coeffs(a1,a2,a3...an) corresponds to the coefficient of an
%               x1^(a1-1)*x2^(a2-1)*x3^(a3-1)...xn^(an-1) term.  Thus, the
%               number of indices coeffs takes is equal to the
%               dimensionality of x (not counting singleton dimensions at
%               the end of coeffs). Note that this ordering is the reverse
%               that used in the 1D polyval function that is built into
%               Matlab. The number of elements for each index in coeffs is
%               the maximum order of that dimension +1.
%       termMat An (n+1)XnumTerms matrix such that
%               termMat(:,i)=[c,a1,a2,...,an] is a monomial term where c is
%               the value of of the monomial coefficient and the monomial
%               is x1^(a1-1)*x2^(a2-1)*x3^(a3-1)...xn^(an-1).
%
%Multivariate polynomials consist of a sum of monomial terms of the form
%c*x1^(a1)*x2^(a2)*x3^(a3)...xn^(an)
%Here, the variables x are known as are the values z of the polynomial
%evaluated at those points. Thus, the unknowns are the c terms, the
%coefficients of the monomials. This function evaluates the monomial terms
%and build a matrix with all of the monomial term values for all of the
%equations. The solution to the coefficient values is thus the solution to
%a linear system of equations. The
%
%EXAMPLE:
%Here, we have a two-dimensional function evaluated at a number of random
%points and we want to fit a polynomial to it. In this problem, we do not
%add noise to the function.
% points=linspace(-1,1,10);
% [x,y]=meshgrid(points,points);
% z=2*x.^3+2*x.*y-3*y.^3+4*x.^2.*y-5*y.^2.*x.^3;
% varVals=[x(:)';y(:)'];
% funVals=z(:);
% degree=5;
% [~,termMat]=polyFitMultiDim(varVals,funVals,degree);
% %One will get coefficients that are almost exact. Given that we know that
% %the coefficients are integers, we can eliminate all small coefficients by
% %rounding.
% termMat=round(termMat);
% %Get rid of terms that are numerically zero.
% sel=termMat(1,:)~=0;
% termMat=termMat(:,sel);
% terms2String(termMat,{'x','y'})
%
%November 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDim=size(varVals,1);
numEq=size(varVals,2);

%The total number of monomial terms for the multivariate polynomial. This
%is the sum of binomial(degCur+numDim-1,numDim-1) from degCur=0 to degree,
%which is the number of compositions of the degree degCur into n parts.
totalNumMonomials=(1+degree)*binomial(degree+numDim,numDim-1)/numDim;

if(totalNumMonomials>numEq)
   error('The total number of monomial coefficients to find is greater than the number of data points provided.')
end

termMat=zeros(numDim+1,totalNumMonomials);

V=zeros(numEq,totalNumMonomials);

%x^0,y^0, etc. value.
V(:,1)=1;
curTerm=2;
for curDegree=1:degree
    numMonomials=binomial(curDegree+numDim-1,numDim-1);

    for j=0:(numMonomials-1)
        curMonomial=unrankTComposition(j,numDim,curDegree+numDim,true)-1;
        termMat(2:end,curTerm)=curMonomial;
        for curEq=1:numEq
            V(curEq,curTerm)=prod(varVals(:,curEq).^curMonomial);
        end
        curTerm=curTerm+1;
    end
end

termMat(1,:)=V\funVals(:);
%Get rid of terms that are numerically zero.
sel=termMat(1,:)~=0;
termMat=termMat(:,sel);
coeffs=terms2MultiDimPolyMat(termMat);

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.


%%%%%%%VERSION 3
%%ANOTHER DESCRIBTION OF GABOR FILTER

%The Gabor filter is basically a Gaussian (with variances sx and sy along x and y-axes respectively)
%modulated by a complex sinusoid (with centre frequencies U and V along x and y-axes respectively)
%described by the following equation
%%
%               1                -1     x  ^    y  ^
%%% Gi(x,y) = ---------- * exp ([----{(----) 2+(----) 2}])*Mi(x,y,f);
%            2*pi*sx*sy           2    sx       sy
%%% i =1,2
%%% M1(x,y,f) = cos[2*pi*f*sqrt(x^2+y^2)];
%%% M2(x,y,f) = cos[2*pi*f*(x*cos(theta) + y*sin(theta)];

%% Describtion :

%% I : Input image
%% Sx & Sy : Variances along x and y-axes respectively
%% f : The frequency of the sinusoidal function
%% theta : The orientation of Gabor filter

%% G1 & G2 : The output filters as described above
%% gabout1 & gabout2 : The output filtered images

%%  Author : Ahmad poursaberi  e-mail : a.poursaberi@ece.ut.ac.ir
%%          Faulty of Engineering, Electrical&Computer Department,Tehran
%%          University,Iran,June 2004

function [G1,G2,gabout1,gabout2] = gaborfilter(I,Sx,Sy,f,theta);

if isa(I,'double')~=1
    I = double(I);
end

for x = -fix(Sx):fix(Sx)
    for y = -fix(Sy):fix(Sy)
        M1 = cos(2*pi*f*sqrt(x^2+y^2));
        M2 = cos(2*pi*f*(x*cos(theta)+y*sin(theta)));
        G1(fix(Sx)+x+1,fix(Sy)+y+1) = (1/(2*pi*Sx*Sy)) * exp(-.5*((x/Sx)^2+(y/Sy)^2))*M1;
        G2(fix(Sx)+x+1,fix(Sy)+y+1) = (1/(2*pi*Sx*Sy)) * exp(-.5*((x/Sx)^2+(y/Sy)^2))*M2;
    end
end

Imgabout1 = conv2(I,double(imag(G1)),'same');
Regabout1 = conv2(I,double(real(G1)),'same');

Imgabout2 = conv2(I,double(imag(G2)),'same');
Regabout2 = conv2(I,double(real(G2)),'same');

gabout1 = sqrt(Imgabout1.*Imgabout1 + Regabout1.*Regabout1);
gabout2 = sqrt(Imgabout2.*Imgabout2 + Regabout2.*Regabout2);


function [K, sK, n1] = dexpKernCompute(kern, x1, x2)

% DEXPKERNCOMPUTE Compute the double exponential kernel,
%
% k(x_i, x_j) = 0.5 * sigma2 * theta * exp(-theta*abs(x_i - x_j)),
%
% given the parameters (theta and sigma), and t.
%
% FORMAT
% DESC computes the kernel parameters for the double exponential kernel
% given the input matrices associated with rows and columns.
% ARG kern : the kernel structure for which the kernel matrix is computed.
% ARG x1 : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN K : the kernel matrix computed at the given points.
% RETURN sK: normalised kernel matrix (i.e. variance set to 1).
% RETURN n1: L1 distance between each row of x1 and x2.
%
% FORMAT
% DESC computes the kernel matrix for the double exponential kernel
% given a matrix of inputs.
% ARG kern : the kernel structure for which the kernel matrix is computed.
% ARG x1 : the input matrix associated with the rows and the columns.
% RETURN K : the kernel matrix computed at the given points.
% RETURN sK: normalised kernel matrix (i.e. no multiplication by the decay
% or variance of the exponential).
% RETURN n1: L1 distance between each row of x1 and x2.
%
% SEEALSO : ouKernParamInit, kernCompute, kernCreate, ouKernDiagCompute,
%
% COPYRIGHT : David Luengo, 2009
%
% COPYRIGHT : Neil D. Lawrence, 2009

% KERN

if nargin < 3
  n1 = sqrt(dist2(x1, x1));
else
  n1 = sqrt(dist2(x1, x2));
end

sK = 0.5 * exp(-kern.decay * n1);
K = sK * kern.decay * kern.variance;


function geometry_test039 ( )

%*****************************************************************************80
%
%% TEST039 tests LINES_EXP_INT_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST039\n' );
  fprintf ( 1, '  LINES_EXP_INT_2D finds intersections of\n' );
  fprintf ( 1, '    two explicit lines in 2D.\n' );
  fprintf ( 1, '\n' );

  for itest = 1 : 3
%
%  x + 2y - 4 = 0
%  x - y - 1 = 0
%
    if ( itest == 1 )

      p1(1:2,1) = [ 0.0;  2.0 ];
      p2(1:2,1) = [ 4.0;  0.0 ];
      q1(1:2,1) = [ 0.0; -1.0 ];
      q2(1:2,1) = [ 1.0;  0.0 ];
%
%  x + 2y - 4 = 0
%  2x + 4y - 1 = 0
%
    elseif ( itest == 2 )

      p1(1:2,1) = [ 0.00; 2.00 ];
      p2(1:2,1) = [ 4.00; 0.00 ];
      q1(1:2,1) = [ 0.00; 0.25 ];
      q2(1:2,1) = [ 0.50; 0.00 ];
%
%  x + 2y - 4 = 0
%  -3x - 6y +12 = 0
%
    elseif ( itest == 3 )

      p1(1:2,1) = [ 0.0; 2.0 ];
      p2(1:2,1) = [ 4.0; 0.0 ];
      q1(1:2,1) = [ 0.0; 2.0 ];
      q2(1:2,1) = [ 4.0; 0.0 ];

    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  P1  %8f  %8f\n', p1(1:2,1) );
    fprintf ( 1, '  P2  %8f  %8f\n', p2(1:2,1) );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Q1  %8f  %8f\n', q1(1:2,1) );
    fprintf ( 1, '  Q2  %8f  %8f\n', q2(1:2,1) );

    [ ival, p ] = lines_exp_int_2d ( p1, p2, q1, q2 );

    if ( ival == 1 )
      fprintf ( 1, '  Intersection at %8f  %8f\n', p(1:2,1) );
    elseif ( ival == 0 )
      fprintf ( 1, '  Lines are parallel, no intersection.\n' );
    elseif ( ival == 2 )
      fprintf ( 1, '  Lines are coincident.\n' );
    else
      fprintf ( 1, '  Unknown return value of IVAL = %d\n', ival );
    end

  end

  return
end


classdef GaussianMixtureD
%%GAUSSIANMIXTURED Function to handle scalar or multivariate real Gaussian
%                  mixture distributions.
%Implemented methods are: mean, cov, PDF, PDFS, PDFI, rand, randS.
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

methods(Static)
function meanVal=mean(w,mu)
%%MEAN Obtain the mean of the Gaussiax mixture distribution.
%
%INPUTS: w The 1XN or NX1 set of weights of the distribution components
%          such that all w>=0 and sum(w)=1.
%       mu The xDimXN set of mean values of the distribution
%          components.
%
%OUTPUTS: meanVal The xDimX1 mean of the Gaussian mixture distribution.
%
%It is simple to show that the mean of the distribution is the weighted
%sum of the means of the components.
%
%EXAMPLE:
%In this example,
% w=[0.4,0.6];
% mu=[1,-0.5;
%    -1, 1];
% Sigma=zeros(2,2,2);
% Sigma(:,:,1)=[4/9,  14/45;
%               14/45,4/9];
% Sigma(:,:,2)=[4/9, 0;
%                 0, 4/9];
% N=10000;
% analyticMean=GaussianMixtureD.mean(w,mu);
% sampMean=mean(GaussianMixtureD.rand(N,w,mu,Sigma),2);
% RelErr=abs((sampMean-analyticMean)./analyticMean)
%In this example, the relative error will typically be 3% or better.
%Increasing the number of samples will typically decrease the relative
%error, indicating that the analytic and sample means are converging.
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

    meanVal=sum(bsxfun(@times,w(:).',mu),2);
end

function [covMat,meanVal]=cov(w,mu,Sigma)
%%COV Obtain the covariance matrix of the Gaussian mixture distribution
%     (the variance if it is scalar).
%
%INPUTS: w The 1XN or NX1 set of weights of the distribution components
%          such that all w>=0 and sum(w)=1.
%       mu The xDimXN set of mean values of the distribution
%          components.
%    Sigma The xDimXxDimXN set of covariance matrices of the
%          distribution components.
%
%OUTPUTS: covMat The xDimXxDim covariance matrix of the Gaussian
%                mixture.
%        meanVal The xDimX1 mean value of the distribution.
%
%The function calcMixtureMoments is called to find the covariance
%matrix.
%
%EXAMPLE:
% w=[0.4,0.6];
% mu=[1,-0.5;
%    -1, 1];
% Sigma=zeros(2,2,2);
% Sigma(:,:,1)=[4/9,  14/45;
%               14/45,4/9];
% Sigma(:,:,2)=[4/9, 0;
%                 0, 4/9];
% N=10000;
% analyticCov=GaussianMixtureD.cov(w,mu,Sigma);
% [~,sampleCov]=calcMixtureMoments(GaussianMixtureD.rand(N,w,mu,Sigma));
% RelErr=abs((sampleCov-analyticCov)./analyticCov)
%The relative error of each element will tend to be less than 1% and
%will decrease as the number of samples increases
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

    [meanVal,covMat]=calcMixtureMoments(mu,w,Sigma);
end

function vals=PDF(z,w,mu,Sigma)
%%PDF Evaluate the PDF of a scalar or multivariate Gaussian
%     distribution at specified points.
%
%INPUTS: z The xDimXnumPoints set of points at which the PDF should be
%          evaluated.
%        w The 1XN or NX1 set of weights of the distribution components
%          such that all w>=0 and sum(w)=1.
%       mu The xDimXN set of mean values of the distribution
%          components.
%    Sigma The xDimXxDimXN set of covariance matrices of the
%          distribution components.
%
%OUTPUTS: vals The 1XN set of PDF values of the Gaussian mixture PDF
%              evaluated at the points in z.
%
%The PDF is the weighted sum of the PDFs of the individual components.
%
%EXAMPLE:
%Here, we validate the PDF by generating random samples and comparing
%the PDF plot with a histogram of the random samples.
% w=[0.4,0.6];
% mu=[1,-0.5];
% sigma=zeros(1,1,2);
% sigma(:,:,1)=4/9;
% sigma(:,:,2)=5/9;
% N=5000;
% figure(1)
% clf
% histogram(GaussianMixtureD.randS(N,w,mu,sigma),'Normalization','pdf','BinLimits',[-3,3])
% hold on
% numPoints=1000;
% x=linspace(-3,3,numPoints);
% vals=GaussianMixtureD.PDF(x,w,mu,sigma.^2);
% plot(x,vals,'linewidth',2)
% axis([-3,3,0,0.45])
% h1=xlabel('x');
% h2=ylabel('PDF(x)');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

    n=length(w);
    numMeas=size(z,2);

    vals=zeros(1,numMeas);
    for k=1:n
        vals=vals+w(k)*GaussianD.PDF(z,mu(:,k),Sigma(:,:,k));
    end
end

function vals=PDFS(z,w,mu,S)
%%PDF Evaluate the PDF of a scalar or multivariate Gaussian
%     distribution at specified points given the weights, means and
%     lower-triangular covariance matrices of the individual
%     components.
%
%INPUTS: z The xDimXnumPoints set of points at which the PDF should be
%          evaluated.
%        w The 1XN or NX1 set of weights of the distribution components
%          such that all w>=0 and sum(w)=1.
%       mu The xDimXN set of mean values of the distribution
%          components.
%        S The xDimXxDimXN set of lower-triangular square-root
%          covariance matrices of the distribution components.
%
%OUTPUTS: vals The 1XN set of PDF values of the Gaussian mixture PDF
%              evaluated at the points in z.
%
%The PDF is the weighted sum of the PDFs of the individual components.
%
%EXAMPLE:
%Here, we validate the PDF by generating random samples and comparing
%the PDF plot with a histogram of the random samples.
% w=[0.4,0.6];
% mu=[1,-0.5];
% sigma=zeros(1,1,2);
% sigma(:,:,1)=4/9;
% sigma(:,:,2)=5/9;
% N=5000;
% figure(1)
% clf
% histogram(GaussianMixtureD.randS(N,w,mu,sigma),'Normalization','pdf','BinLimits',[-3,3])
% hold on
% numPoints=1000;
% x=linspace(-3,3,numPoints);
% vals=GaussianMixtureD.PDFS(x,w,mu,sigma);
% plot(x,vals,'linewidth',2)
% axis([-3,3,0,0.45])
% h1=xlabel('x');
% h2=ylabel('PDF(x)');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%One will see that the histogram matches well with the plot.
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

    n=length(w);
    numMeas=size(z,2);

    vals=zeros(1,numMeas);
    for k=1:n
        vals=vals+w(k)*GaussianD.PDFS(z,mu(:,k),S(:,:,k));
    end
end

function vals=PDFI(z,w,mu,SigmaInv,SigmaInvDet)
%%PDFI Evaluate the PDF of a scalar or multivariate Gaussian
%      distribution at specified points given the weights, means and
%      the inverses of the covariance matrices of the individual
%      components.
%
%INPUTS: z The xDimXnumPoints set of points at which the PDF should be
%          evaluated.
%        w The 1XN or NX1 set of weights of the distribution components
%          such that all w>=0 and sum(w)=1.
%       mu The xDimXN set of mean values of the distribution
%          components.
% SigmaInv The xDimXxDimXN set of inverse of covariance matrices of the
%          distribution components.
% SigmaInvDet Optionally, a length-N set of determinants of the matrices
%          in SigmaInv can be passed so as to speed up the computation. If
%          omitted or an empty matrix is passed, determinants will be taken
%          as needed.
%
%OUTPUTS: vals The 1XN set of PDF values of the Gaussian mixture PDF
%              evaluated at the points in z.
%
%The PDF is the weighted sum of the PDFs of the individual components.
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

    n=length(w);
    numMeas=size(z,2);

    vals=zeros(1,numMeas);
    if(nargin<5||isempty(SigmaInvDet))
        for k=1:n
            vals=vals+w(k)*GaussianD.PDFI(z,mu(:,k),SigmaInv(:,:,k));
        end
    else
        for k=1:n
            vals=vals+w(k)*GaussianD.PDFI(z,mu(:,k),SigmaInv(:,:,k),SigmaInvDet(k));
        end
    end
end

function vals=CDF(z,w,mu,varVals)
%%CDF Evaluate the cumulative distribution function (CDF) of a scalar
%     Gaussian mixture at the specified points.
%
%INPUTS: z A matrix of the point(s) at which the CDF should be
%          evaluated.
%        w The 1XN or NX1 set of weights of the distribution components
%          such that all w>=0 and sum(w)=1.
%       mu The length N set of mean values of the distribution
%          components.
%  varVals The length-N set of variances of the distribution
%          components.
%
%OUTPUTS: val The scalar value(s) of the Gaussian mixture distribution
%             evaluated at the point(s) z.
%
%The CDF of the mixture is the weighted sum of the CDFs of the
%individual Gaussian components.
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

    n=length(w);

    vals=zeros(size(z));
    for k=1:n
        vals=vals+w(k)*GaussianD.CDF(z,mu(k),varVals(k));
    end
end

function vals=rand(N,w,mu,P)
%%RAND Generate multivariate Gaussian mixture samples with given
%      weights, means and lower-triangular square root covariance
%      matrices.
%
%INPUTS: N The number of random variables to generate.
%        w The 1XN or NX1 set of weights of the distribution components
%          such that all w>=0 and sum(w)=1.
%       mu The xDimXN set of mean values of the distribution
%          components.
%        P The xDimXxDimXN set of covariance matrices of the
%          distribution components. If all of them are the same, then a
%          single xDimXxDim matrix can be passed.
%
%OUTPUT: x An xDimXN matrix of random instances of the multivariate
%          Gaussian distribution.
%
%The distribution is sampled by sampling the empirical distribution of
%weights, to determine which Gaussian component to sample, and then
%sampling the chosen Gausian distribution. Lower-triangular square
%roots of the componets must be found for sampling, so if these are
%available, then the randS method will be faster.
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    %The number of components of the mixture.
    n=length(w);
    xDim=size(mu,1);

    if(size(P,3)==1)
        P=repmat(P,[1,1,n]);
    end

    vals=zeros(xDim,N);

    %Determine which components are to be sampled.
    idx=EmpiricalD.rand([N,1],1:n,w);

    %Sample the Gaussian components that were selected.
    S=zeros(xDim,xDim,n);
    componentUsed=false(n,1);
    for curSamp=1:N
        curComp=idx(curSamp);
        %Get the lower-triangular square roots only for the components
        %that are used.
        if(componentUsed(curComp)==false)
            S(:,:,curComp)=chol(P(:,:,curComp),'lower');
            componentUsed(curComp)=true;
        end

        vals(:,curSamp)=mu(:,curComp)+S(:,:,curComp)*randn(xDim,1);
    end
end

function vals=randS(N,w,mu,S)
%%RANDS Generate multivariate Gaussian mixture samples with given
%       weights, means and lower-triangular square root covariance
%       matrices.
%
%INPUTS: N The number of random variables to generate.
%        w The 1XN or NX1 set of weights of the distribution components
%          such that all w>=0 and sum(w)=1.
%       mu The xDimXN set of mean values of the distribution
%          components.
%        S The xDimXxDimXN set of lower-triangular square-root
%          covariance matrices of the distribution components.
%
%OUTPUT: x An xDimXN matrix of random instances of the multivariate
%          Gaussian distribution.
%
%The distribution is sampled by sampling the empirical distribution of
%weights, to determine which Gaussian component to sample, and then
%sampling the chosen Gausian distribution.
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    %The number of components of the mixture.
    n=length(w);
    xDim=size(mu,1);

    if(size(S,3)==1)
        S=repmat(S,[1,1,n]);
    end

    vals=zeros(xDim,N);

    %Determine which components are to be sampled.
    idx=EmpiricalD.rand([N,1],1:n,w);

    %Sample the Gaussian components that were selected.
    for curSamp=1:N
        curComp=idx(curSamp);

        vals(:,curSamp)=mu(:,curComp)+S(:,:,curComp)*randn(xDim,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.


close all;
clearvars;
clc;
[LoD,HiD,LoR,HiR] = wfilters('db4');

load noisdopp;

% perform 4 level decomposition
[coefficients, levels] = wavedec(noisdopp,4, LoD,HiD);

% plot approximation and detail components
figure;
plot(coefficients); title('Coefficients');

export_fig images/noisdopp_db4_l4_decomposition.png -r120 -nocrop;

% reconstruct the signal
reconstructed = waverec(coefficients, levels, LoR, HiR);

% measure the maximum difference
max_abs_diff = max(abs(noisdopp-reconstructed))

figure;
for level=0:4
    level_app_coeffs = appcoef(coefficients, levels, LoR, HiR, level);
    subplot(511+level);
    plot(level_app_coeffs);
    title(sprintf('Approximation coefficients @ level-%d', level));
end

export_fig images/noisdopp_db4_l4_appcoeffs.png -r120 -nocrop;


function [minPoint,minValue,exitCode]=goldenSectionSearch(g,xSpan,XTol,maxIter)
%%GOLDENSECTIONSEARCH Perform a golden section search to find the minimum
%                     of a continuous, real, function that is unimodal in a
%                     given interval. The golden section search linearly
%                     decreases the search region. The algorithm is
%                     considered more efficient than a ternary search.
%                     Unlike Matlab's built-in fminbnd function, quadratic
%                     interpolation is not used to try to speed up the
%                     convergence rate.
%
%INPUTS: g The handle to a real function to minimize. The function takes a
%          scalar input and returns a real scalar output.
%    xSpan The 2X1 or 1X2 span of values in which the minimum is to be
%          found xSpan(1)<xSpan(2).
%     XTol The absolute tolerance on the argument of g. When the region of
%          uncertainty of the minimum goes below xTol, then convergence is
%          declared. If omitted or an empty matrix is passed, XTol=1e-9 is
%          used.
%  maxIter The maximum number of iterations to use. If this parameter is
%          omitted or an empty matrix is passed, then maxIter=100; is used.
%          One to 2 function evaluations are performed each iteration.
%
%OUTPUTS: minPoint The value such that g(minVal) is minimized after the
%                  search.
%         minValue The value g(minVal).
%         exitCode A value indicating how the function terminated. Possible
%                  values are:
%                  0 The tolerance XTol in the argument of g was achieved.
%                  1 The maximum number of iterations was reached.
%
%The golden section search algorithm is implemented as described in
%Appendix C of [1]. The convergence rate is linear. This function evaluates
%g at the bounds of the interval.
%
%As an example, consdier finding the minimum of the function
%g(x)=(x-3)*x^3*(x-6)^4.
%This function has multiple minima and maxima. However, if one minimum can
%be bounded, then the golden section search can be used to find it. If
%multiple minima are bounded, then the golden section search can be used,
%but it might not convergence to the best minimum. We will consider the
%the case of three ranges.
% %Example 1
% alphaSpan=[0;3];
% g=@(x)(x-3)*x^3*(x-6)^4;
% [minPoint,minValue]=goldenSectionSearch(g,alphaSpan);
% %Here we find a minPoint of about 1.7354 and a min value of about -2186.1.
% %Next, we consider bounding a second minimum.
% %Example 2
% alphaSpan=[3;7];
% [minPoint1,minValue1]=goldenSectionSearch(g,alphaSpan);
% %Here, we get a minimum point of 6 with a value of 0. Now, if we take a
% %span that covers both minimum:
% %Example 3:
% alphaSpan=[1;7];
% [minPoint2,minValue2]=goldenSectionSearch(g,alphaSpan);
% %We also get a minimum point of 6 even though that is not the absolute
% %minimum over the entire region. This is because the entire region is not
% %unimodal. If we used alphaSpan=[0;7]; then the first minimum (the lower
% one) in that span would have been found.
%
%REFERENCES:
%[1] D. P. Bertsekas, Nonlinear Programming, 2nd ed. Belmont, MA: Athena
%    Science, 1999.
%
%January 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(maxIter))
    maxIter=100;
end

if(nargin<3||isempty(XTol))
	XTol=1e-9;
end

tau=(3-sqrt(5))/2;

alpha=xSpan(1);
alphaBar=xSpan(2);

if(alphaBar<alpha)
   error('alphaSpan(2) is less than alphaSpan(1)');
end

b=alpha+tau*(alphaBar-alpha);
bBar=alphaBar-tau*(alphaBar-alpha);

gAlpha=g(alpha);
gAlphaBar=g(alphaBar);

gb=g(b);
gbBar=g(bBar);

exitCode=1;
for curIter=1:maxIter
    alphaPrev=alpha;
    alphaBarPrev=alphaBar;
    if(gb<gbBar)
        if(gAlpha<=gb)
            alphaBar=b;
            gAlphaBar=gb;

            bBar=alphaBar-tau*(alphaBar-alpha);
            gbBar=g(bBar);
        else
            alphaBar=bBar;
            gAlphaBar=gbBar;

            %Because of the use of the Golden ratio for tau, only one
            %function evaluation is needed in this instance.
            bBar=b;
            gbBar=gb;
        end

        b=alpha+tau*(alphaBar-alpha);
        gb=g(b);
    elseif(gb>gbBar)
        if(gbBar>=gAlphaBar)
            alpha=bBar;
            gAlpha=gbBar;

            b=alpha+tau*(alphaBar-alpha);
            gb=g(b);
        else
            alpha=b;
            gAlpha=gb;

            %Because of the use of the golden ratio for tau, only one
            %function evaluation is needed in this instance.
            b=bBar;
            gb=gbBar;
        end

        bBar=alphaBar-tau*(alphaBar-alpha);
        gbBar=g(bBar);
    else
        alpha=b;
        alphaBar=bBar;

        gAlpha=gb;
        gAlphaBar=gbBar;

        b=alpha+tau*(alphaBar-alpha);
        gb=g(b);
        bBar=alphaBar-tau*(alphaBar-alpha);
        gbBar=g(bBar);
    end

    %If the accuracy bound has been achieved. The second condition deals
    %with finite precision limitiations if XTol is very close to 0.
    if(abs(alphaBar-alpha)<=XTol||(alphaPrev==alpha&&alphaBarPrev==alphaBar))
        exitCode=0;
        break;
    end
end

%After the iterations, we have the function evaluated at alpha, b, bBar,
%and alphaBar. We will just return the point/ value that is the smallest.

points=[alpha;b;bBar;alphaBar];
values=[gAlpha;gb;gbBar;gAlphaBar];

[values,idx]=sort(values,'ascend');
minPoint=points(idx(1));
minValue=values(1);
end

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


function mb_bfgs

% 2010 m.bangert@dkfz.de
% fitting a gaussian distribution to data with a quasi newton method using
% a bfgs or sr1 update for the hessian matrix
%
% example : mb_bfgs (function does not take any argument)

% set options
mode          = 'BFGS'; % update rule for hessian: either 'SR1' or 'BFGS'
visLineSearch = 1;
visBool       = 1;

fprintf(['\nFitting a gaussian distribution to data ...\n' ...
           'Using ' mode ' Hessian update\n']);

% sample data from a gaussian distribution
randn('seed',12345); % set the seed of the random number generator - if you do not want to do that comment...
numOfSamples = 250;
data(:,1)    = linspace(-2,4,numOfSamples) + 6/numOfSamples*randn(1,numOfSamples);
data(:,2)    = exp(-(linspace(-2,4,250)-1.5).^2) + .05*randn(1,250);

% plot data
if visBool
    close all
    figure
    hold on
    plot(data(:,1),data(:,2),'b+')
    xlabel('X')
    ylabel('Y')
    visHandle = [];
end

% specify input
x       = [1 1 1]'; % starting value for parameters
H       = eye(3);   % initial hessian approximation

% 1st calculation of objective function and gradient
objFunc         = @(x) sum((x(3).*exp(-(data(:,1)-x(1)).^2./x(2))-data(:,2)).^2);
objFuncValue    = objFunc(x);
oldObjFuncValue = objFuncValue + 1;
dx              = mb_numDiff(objFunc,x);

% iterate
iter      = 0;
numOfIter = 100;
prec      = 1e-5;

% convergence if gradient smaller than prec, change in objective function
% smaller than prec or maximum number of iteration reached...
while iter < numOfIter && abs((oldObjFuncValue-objFuncValue)/objFuncValue)>prec && norm(dx)>prec

    % increment iteration counter
    iter = iter + 1;

    % remember odl objective function value
    oldObjFuncValue = objFuncValue;

    % implementation of BFGS according to
    % http://en.wikipedia.org/wiki/BFGS_method
    % numbers corresponds

    % 1 obtain search direction
    dir = -(H\dx); % this is the efficient matlab notation for -inv(H)*dx

    % 2.1 linesearch to to find acceptable stepsize alpha
    %alpha = mb_backtrackingLineSearch(objFunc,objFuncValue,x,dx,dir);
    alpha = mb_nocLineSearch(objFunc,@(x) mb_numDiff(objFunc,x),x,dir,dir'*dx,objFuncValue);

    % 3
    p = alpha*dir;

        % visualize the line search (optional) used for debugging
        if visLineSearch
            figure
            hold on
            alphas   = linspace(0,2*alpha,100);
            alphaVis = NaN*alphas;
            for i = 1:100
                alphaVis(i) = objFunc(x+alphas(i)*dir);
            end
            plot(alphas,alphaVis,'b') % function in direction of line search
            plot(alpha,objFunc(x+alpha*dir),'r.') % the steplength found
            plot(alphas,objFuncValue + 1e-1*alphas*(dir'*dx),'g') % constraint
            pause(.5)
            close
        end

    % 2.2 update x
    x = x + p;

    % update objective function (and remember old objective function to
    % check for convergence)
    objFuncValue = objFunc(x);

    % 4 calculate difference of gradients
    dx_old = dx;
    dx     = mb_numDiff(objFunc,x);
    q      = dx-dx_old;

    % update hessian
    if strcmp(mode,'BFGS')
        H = H + (q*q')/(q'*p) - ((H*p)*(H*p)')/(p'*H*p);
    elseif strcmp(mode,'SR1')
        H = H + ((q-H*p)*(q-H*p)')/((q-H*p)'*p);
    end

    % update function with new x and plot
    if visBool
        pause(0.5)
        fitFunc = @(t) x(3).*exp(-(t-x(1)).^2./x(2));
        delete(visHandle);
        visHandle = plot(data(:,1),fitFunc(data(:,1)),'r','LineWidth',4);
        drawnow;
    end

    fprintf(1,'Iteration %d: alpha_min=%f, OF=%f\n',iter,alpha,objFuncValue);

end

fprintf(['\n' num2str(iter) ' iteration(s) performed to converge\n'])

fprintf(1,'Final solution: x=(%f,%f,%f)\n',x(1),x(2),x(3));

end