function result = sphere_monomial_int_nd ( n, r, e )

%*****************************************************************************80
%
%% SPHERE_MONOMIAL_INT_ND integrates a monomial on a sphere in ND.
%
%  Discussion:
%
%    The sphere may have nonunit radius, but it must be centered at 0.
%
%    The integration region is
%
%      sum ( x(i)**2 ) = R**2.
%
%    The monomial is F(X) = X(1)**E(1) * X(2)**E(2) * ... * X(N)**E(N).
%
%    This routine is useful for testing the accuracy of quadrature
%    rules on the sphere.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Academic Press, 1984, page 263.
%
%  Parameters:
%
%    Input, integer N, the dimension of the space.
%
%    Input, real R, the radius of the sphere.
%
%    Input, integer E(N), the exponents of X, Y and Z in the monomial.
%    Each exponent must be nonnegative.
%
%    Output, real RESULT, the integral.
%
  if ( any ( e(1:n) < 0 ) )
    result = -r8_huge ( result );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SPHERE_MONOMIAL_INT_ND - Fatal error!\n' );
    fprintf ( 1, '  All exponents must be nonnegative.\n' );
    error ( 'SPHERE_MONOMIAL_INT_ND - Fatal error!' );
  end

  if ( all ( e(1:n) == 0 ) )

    result = 2.0 * sqrt ( pi^n ) / gamma ( 0.5 * n );

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

    result = 0.0;

  else

    result = 2.0;

    for i = 1 : n
      result = result * gamma ( 0.5 * ( e(i) + 1 ) );
    end

    result = result / gamma ( 0.5 * ( sum ( e(1:n) + 1 ) ) );

  end

  result = result * r^( sum ( e(1:n) ) + 2 );

  return
end


function test_pdfs_vbmc()
%TEST_PDFS_VBMC Test pdfs introduced in the VBMC package.

lb = [-1.1,-4.1];
ub = [3.2,-2.8];
a = [-1,-4];
b = [3,-3];
n = 1e6;

tolerr = 1e-3;      % Error tolerance on normalization constant
tolrmse = 0.05;     % Error tolerance on histogram vs pdf

%% Test multivariate uniform box distribution
pdf1 = @(x) munifboxpdf(x,a(1),b(1));
pdf2 = @(x) munifboxpdf(x,a,b);
pdf1log = @(x) exp(munifboxlogpdf(x,a(1),b(1)));
pdf2log = @(x) exp(munifboxlogpdf(x,a,b));
pdfrnd = @(n) munifboxrnd(a,b,n);
name = 'munifbox';

test_pdf1_normalization(pdf1,lb,ub,tolerr,name);
test_pdf2_normalization(pdf2,lb,ub,tolerr,name);
test_pdf1_normalization(pdf1log,lb,ub,tolerr,name);
test_pdf2_normalization(pdf2log,lb,ub,tolerr,name);
test_rnd(pdfrnd,pdf1,a,b,n,tolrmse,name);

%% Test multivariate trapezoidal distribution
u = [-0.5,-3.8];
v = [1.5,-3.4];
pdf1 = @(x) mtrapezpdf(x,a(1),u(1),v(1),b(1));
pdf2 = @(x) mtrapezpdf(x,a,u,v,b);
pdf1log = @(x) exp(mtrapezlogpdf(x,a(1),u(1),v(1),b(1)));
pdf2log = @(x) exp(mtrapezlogpdf(x,a,u,v,b));
pdfrnd = @(n) mtrapezrnd(a,u,v,b,n);
name = 'mtrapez';
test_pdf1_normalization(pdf1,lb,ub,tolerr,name);
test_pdf2_normalization(pdf2,lb,ub,tolerr,name);
test_pdf1_normalization(pdf1log,lb,ub,tolerr,name);
test_pdf2_normalization(pdf2log,lb,ub,tolerr,name);
test_rnd(pdfrnd,pdf1,a,b,n,tolrmse,name);

%% Test multivariate spline trapezoidal distribution
pdf1 = @(x) msplinetrapezpdf(x,a(1),u(1),v(1),b(1));
pdf2 = @(x) msplinetrapezpdf(x,a,u,v,b);
pdf1log = @(x) exp(msplinetrapezlogpdf(x,a(1),u(1),v(1),b(1)));
pdf2log = @(x) exp(msplinetrapezlogpdf(x,a,u,v,b));
pdfrnd = @(n) msplinetrapezrnd(a,u,v,b,n);
name = 'msplinetrapez';
test_pdf1_normalization(pdf1,lb,ub,tolerr,name);
test_pdf2_normalization(pdf2,lb,ub,tolerr,name);
test_pdf1_normalization(pdf1log,lb,ub,tolerr,name);
test_pdf2_normalization(pdf2log,lb,ub,tolerr,name);
test_rnd(pdfrnd,pdf1,a,b,n,tolrmse,name);

%% Test multivariate smoothbox distribution
sigma = [0.7,0.45];
pdf1 = @(x) msmoothboxpdf(x,a(1),b(1),sigma(1));
pdf2 = @(x) msmoothboxpdf(x,a,b,sigma);
pdf1log = @(x) exp(msmoothboxlogpdf(x,a(1),b(1),sigma(1)));
pdf2log = @(x) exp(msmoothboxlogpdf(x,a,b,sigma));
pdfrnd = @(n) msmoothboxrnd(a,b,sigma,n);
name = 'msmoothbox';
lb = [-5,-7];
ub = [5,0];
test_pdf1_normalization(pdf1,lb,ub,tolerr,name);
test_pdf2_normalization(pdf2,lb,ub,tolerr,name);
test_pdf1_normalization(pdf1log,lb,ub,tolerr,name);
test_pdf2_normalization(pdf2log,lb,ub,tolerr,name);
test_rnd(pdfrnd,pdf1,a,b,n,tolrmse,name);

%close all;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function test_pdf1_normalization(pdf1,lb,ub,tol,name)
%TEST_PDF1_NORMALIZATION Test normalization of univariate pdf.

% Check 1D integral
y = integral(@(x) pdf1(x), lb(1), ub(1), 'ArrayValued', true);
fprintf('%s: 1D integral: %.6f\n', name, y);
assert(abs(y - 1) < tol, ['Test error: univariate ' name ' does not integrate to 1.']);

end

function test_pdf2_normalization(pdf2,lb,ub,tol,name)
%TEST_PDF2_NORMALIZATION Test normalization of bivariate pdf.

% Check 2D integral
y = integral2(@(x1,x2) reshape(pdf2([x1(:),x2(:)]),size(x1)), ...
    lb(1), ub(1), lb(2), ub(2));
fprintf('%s: 2D integral: %.6f\n', name, y);
assert(abs(y - 1) < tol, ['Test error: bivariate ' name ' does not integrate to 1.']);

end

function test_rnd(pdfrnd,pdf1,a,b,n,tol,name)
%TEST_RND Test random sample generation (histogram vs pdf).

r = pdfrnd(n);
h = histogram(r(:,1),100,'BinLimits',[a(1),b(1)],'Normalization','pdf');
x = 0.5*(h.BinEdges(1:end-1) + h.BinEdges(2:end))';
y = pdf1(x)';
rmse = sqrt(sum(((y - h.Values)).^2*h.BinWidth));
fprintf('%s: histogram rmse: %.6f\n', name, rmse);
assert(rmse < tol, ['Test error: generated histogram does not match ' name ' pdf.']);

end


function degree = r8poly_degree ( na, a )

%*****************************************************************************80
%
%% R8POLY_DEGREE returns the degree of a polynomial in power sum form.
%
%  Discussion:
%
%    The power sum form of a polynomial is:
%
%      p(x) = a(0) + a(1) * x + ... + a(n-1) * x**(n-1) + a(n) * x**(n)
%
%    The degree of a polynomial is the index of the highest power
%    of X with a nonzero coefficient.
%
%    The degree of a constant polynomial is 0.  The degree of the
%    zero polynomial is debatable, but this routine returns the
%    degree as 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NA, the dimension of A.
%
%    Input, real A(1:NA+1), the coefficients of the polynomials.
%
%    Output, integer DEGREE, the degree of A.
%
  degree = na;

  while ( 0 < degree )

    if ( a(degree+1) ~= 0.0 )
      return
    end

    degree = degree - 1;

  end

  return
end


function fem2d_pack_test17 ( )

%*****************************************************************************80
%
%% TEST17 demonstrates S_L2NORM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  element_num = 40;
  psi_num = element_num + 1;
  quad_num = 5;

  x_max = 1.0;
  x_min = 0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST17\n' );
  fprintf ( 1, '  S_L2NORM computes the L2 norm of a scalar function\n' );
  fprintf ( 1, '  S(X) over a region (of any dimension), assuming:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  * that there is a set of finite element basis\n' );
  fprintf ( 1, '    functions PSI;\n' );
  fprintf ( 1, '  * that the region is decomposed into a number of\n' );
  fprintf ( 1, '    elements whose areas are known;\n' );
  fprintf ( 1, '  * that the integral is to be computed by a quadrature\n' );
  fprintf ( 1, '    rule applied in the same way to each element;\n' );
  fprintf ( 1, '  * that the value of the basis functions is given\n' );
  fprintf ( 1, '    at every quadrature node in every element;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Our example will have one spatial dimension.\n' );
  fprintf ( 1, '  XMIN = %f\n', x_min );
  fprintf ( 1, '  XMAX = %f\n', x_max );
  fprintf ( 1, '  The number of intervals will be    %d\n', element_num );
  fprintf ( 1, '  The number of basis functions is   %d\n', psi_num );
  fprintf ( 1, '  The number of quadrature points is %d\n', quad_num );
%
%  Set the nodes.
%
  for i = 1 : psi_num
    node_x(i) = ( ( psi_num - i     ) * x_min   ...
                + (           i - 1 ) * x_max ) ...
                / ( psi_num     - 1 );
  end
%
%  Set the element "areas".
%
  for i = 1 : element_num
    element_area(i) = node_x(i+1) - node_x(i);
  end
%
%  Set the quadrature weights and abscissas.
%
  quad_weight(1) = 0.236926885056189087514264040720;
  quad_weight(2) = 0.478628670499366468041291514836;
  quad_weight(3) = 0.568888888888888888888888888889;
  quad_weight(4) = 0.478628670499366468041291514836;
  quad_weight(5) = 0.236926885056189087514264040720;

  quad_weight(1:5) = quad_weight(1:5) / 2.0;

  quad_x(1) = - 0.906179845938663992797626878299;
  quad_x(2) = - 0.538469310105683091036314420700;
  quad_x(3) =   0.0;
  quad_x(4) =   0.538469310105683091036314420700;
  quad_x(5) =   0.906179845938663992797626878299;
%
%  Set the finite element coefficients of S.  In our formulation,
%  these finite element coefficients are simply the function values
%  at the nodes.
%
  for i = 1 : psi_num
    s_coef(i) = sin ( node_x(i) );
  end
%
%  For each basis function I,
%    for each element J,
%      for each quadrature point K,
%
%  ...evaluate the basis function in the element at the quadrature point.
%
  psi_quad(1:psi_num,1:element_num,1:quad_num) = 0.0;

  for psi = 1 : psi_num

    for element = 1 : element_num

      if ( element < psi - 1 )

        continue

      elseif ( element == psi - 1 )

        xl = node_x(element);
        xr = node_x(element+1);

        for quad = 1 : quad_num

          x = ( ( 1.0 - quad_x(quad) ) * xl   ...
              + ( 1.0 + quad_x(quad) ) * xr ) ...
              /   2.0;

          psi_quad(psi,element,quad) = ( x - xl ) / ( xr - xl );

        end

      elseif ( element == psi )

        xl = node_x(element);
        xr = node_x(element+1);

        for quad = 1 : quad_num

          x = ( ( 1.0 - quad_x(quad) ) * xl   ...
              + ( 1.0 + quad_x(quad) ) * xr ) ...
              /   2.0;

          psi_quad(psi,element,quad) = ( xr - x ) / ( xr - xl );

        end

      elseif ( psi < element )
        continue
      end

    end

  end
%
%  Ask S_L2NORM to compute the integral of L2 norm of S.
%
  l2norm = s_l2norm ( psi_num, element_num, quad_num, element_area, ...
    quad_weight, psi_quad, s_coef );

  fprintf ( 1, '\n');
  fprintf ( 1, '  The computed L2 norm is %f\n', l2norm );
%
%  The integral of (sin(x))**2 is x/2 - sin(2x)/4
%
  exact = sqrt ( ...
      ( 0.5 * x_max - 0.25 * sin ( 2.0 * x_max ) ) ...
    - ( 0.5 * x_min - 0.25 * sin ( 2.0 * x_min ) ) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The exact value is      %f\n', exact );

  return
end


% Name: tanner_LDPC.m
% Description: This script displays different parameters of an LDPC code
% given his A-list format file.
% Copyright (c) 2009. Robert Morelos-Zaragoza. All rights reserved.

% clear all
%
% name = input('enter file name: ');
name = 'NR_ldpc';
[H] = alist2sparse('NR_ldpc.alist')

figure(1)
spy(H)
title( strcat('H matrix of LDPC code with A-list file name: ', name))

n1 = size(H,1); n2 = size(H,2);

% Adjacency matrix as defined in Matlab
A = [ zeros(n2) H'; H zeros(n1) ];

xy = [];
for i=1:n2
    xy = [ xy; i 2 ];                 % Locations of code nodes
end
for i=1:n1
    xy = [ xy; i 1 ];                 % Locations of check nodes
end

figure(2)
gplot(A,xy)
axis([0 n2+1 0.9 2.1])
title( strcat('Tanner graph of LDPC code with A-list file name: ', name))

figure(3)
for i=1:n1
    cnd(i) = sum(H(i,:));
end
stem(cnd)
title('Check node degree distribution')

figure(4)
for j=1:n2
    vnd(j) = sum(H(:,j));
end
stem(vnd)
title('Variable node degree distribution')

fprintf('Density = %e\n',nnz(H)/(n1*n2));


% Duffing_Bif - Bifurcation diagram for the Duffing equation.
% Copyright Springer 2013 Stephen Lynch.
% Make sure Duffing is in your directory.
clear
global Gamma;
Max=120;step=0.001;interval=Max*step;a=1;b=0;
% Ramp the amplitude up.
for n=1:Max
    Gamma=step*n;
    [t,x]=ode45('Duffing',0:(2*pi/1.25):(4*pi/1.25),[a,b]);
    a=x(2,1);
    b=x(2,2);
    rup(n)=sqrt((x(2,1))^2+(x(2,2))^2);
end
% Ramp the amplitude down.
for n=1:Max
    Gamma=interval-step*n;
    [t,x]=ode45('Duffing',0:(2*pi/1.25):(4*pi/1.25),[a,b]);
    a=x(2,1);
    b=x(2,2);
    rdown(n)=sqrt((x(2,1))^2+(x(2,2))^2);
end
hold on
rr=step:step:interval;
plot(rr,rup)
plot(interval-rr,rdown)
hold off
fsize=15;
axis([0 .12 0 2])
xlabel('\Gamma','FontSize',fsize)
ylabel('r','FontSize',fsize)
title('Bifurcation Diagram of the Duffing System. Bistable Region.')


function value = i4_modp ( i, j )

%*****************************************************************************80
%
%% I4_MODP returns the nonnegative remainder of I4 division.
%
%  Discussion:
%
%    If
%      NREM = I4_MODP ( I, J )
%      NMULT = ( I - NREM ) / J
%    then
%      I = J * NMULT + NREM
%    where NREM is always nonnegative.
%
%    The MOD function computes a result with the same sign as the
%    quantity being divided.  Thus, suppose you had an angle A,
%    and you wanted to ensure that it was between 0 and 360.
%    Then mod(A,360) would do, if A was positive, but if A
%    was negative, your result would be between -360 and 0.
%
%    On the other hand, I4_MODP(A,360) is between 0 and 360, always.
%
%  Example:
%
%        I     J     MOD  I4_MODP    Factorization
%
%      107    50       7       7    107 =  2 *  50 + 7
%      107   -50       7       7    107 = -2 * -50 + 7
%     -107    50      -7      43   -107 = -3 *  50 + 43
%     -107   -50      -7      43   -107 =  3 * -50 + 43
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the number to be divided.
%
%    Input, integer J, the number that divides I.
%
%    Output, integer VALUE, the nonnegative remainder when I is
%    divided by J.
%
  if ( j == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_MODP - Fatal error!\n' );
    fprintf ( 1, '  Illegal divisor J = %d\n', j );
    error ( 'I4_MODP - Fatal error!' );
  end

  value = mod ( i, j );

  if ( value < 0 )
    value = value + abs ( j );
  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = T2quaternion(T)
%   Returns the quaternion corresponding to an homogeneous transformation
%   matrix T. Only the 3x3 rotation matrix in T is used.
%
%   See also QPROD, QUATERNION2T.
%
%   Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche. email:
%   arturo.gil@umh.es date:   21/04/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function Q = T2quaternion(T)
% use only the orientation
R = T(1:3, 1:3);

Q = zeros(1,4);
Q(1) = sqrt(trace(R))/2;
Q(1) = real(Q(1));

Lx = R(3,2) - R(2,3);
Ly = R(1,3) - R(3,1);
Lz = R(2,1) - R(1,2);

if (R(1,1) >= R(2,2)) && (R(1,1) >= R(3,3))
    Lx1 = R(1,1) - R(2,2) - R(3,3) + 1;
    Ly1 = R(2,1) + R(1,2);
    Lz1 = R(3,1) + R(1,3);
elseif (R(2,2) >= R(3,3))
    Lx1 = R(2,1) + R(1,2);
    Ly1 = R(2,2) - R(1,1) - R(3,3) + 1;
    Lz1 = R(3,2) + R(2,3);
else
    Lx1 = R(3,1) + R(1,3);
    Ly1 = R(3,2) + R(2,3);
    Lz1 = R(3,3) - R(1,1) - R(2,2) + 1;
end

if (Lx >= 0) || (Ly >= 0) || (Lz >= 0)
    Lx = Lx + Lx1;
    Ly = Ly + Ly1;
    Lz = Lz + Lz1;
else
    Lx = Lx - Lx1;
    Ly = Ly - Ly1;
    Lz = Lz - Lz1;
end

if norm([Lx Ly Lz]) == 0
    Q = [1 0 0 0];
else
    s = sqrt(1-Q(1)^2)/norm([Lx Ly Lz]);
    Q(2:4) = s*[Lx Ly Lz];
end


function [X_poly] = polyFeatures(X, p)
%POLYFEATURES Maps X (1D vector) into the p-th power
%   [X_poly] = POLYFEATURES(X, p) takes a data matrix X (size m x 1) and
%   maps each example into its polynomial features where
%   X_poly(i, :) = [X(i) X(i).^2 X(i).^3 ...  X(i).^p];
%

% You need to return the following variables correctly.
X_poly = zeros(numel(X), p);

% ====================== YOUR CODE HERE ======================
% Instructions: Given a vector X, return a matrix X_poly where the p-th
%               column of X contains the values of X to the p-th power.
%
%

X_poly(:,1) = X;
for i=2:p
  X_poly(:,i) = X.*X_poly(:,i-1);
end

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

end


function integralImage = computeIntegralImage(image, norm)
% This function computes the integralImage given an image in input and
% evntually normalize it if the flag norm is true.

% First we pad the image this is tricky because when we compute the area of
% the a patch using the size we don't stick to the corner coordinates.
%
%
%     000000000000000000000000000000
%     0#############################
%     0#           |               #
%     0#   SUM     |               #
%     0#           |               #
%     0#-----------x               #
%     0#                           #
%     0#############################
%
%

image = padarray(image, [1 1], 0, 'pre');
[rows, columns, dimensions] = size(image);
integralImage = zeros(size(image));
image=double(image);

% For each row we compute the cumulative sum
% so for example if our row is : 1 1 1
% the cumulative sum is going to be: 1 2 3
for i = 1:rows
    cumulative_row_sum = cumsum(double(image(i, :)));
    if i == 1
       integralImage(i, :) = cumulative_row_sum;
    else
        for j = 1:columns
           integralImage(i, j) = integralImage(i-1, j) + cumulative_row_sum(j) ;
        end
    end
end

% We normalize if the flag is true (this is done if we want to display the result)
if norm==true
    integralImage = 255*(integralImage - min(integralImage(:))) / (max(integralImage(:)) - min(integralImage(:)));
    integralImage = uint8(integralImage);
end

end


function [ bin, bin_limit ] = r8vec_bin ( n, x, bin_num, bin_min, bin_max )

%*****************************************************************************80
%
%% R8VEC_BIN computes bins based on a given R8VEC.
%
%  Discussion:
%
%    The user specifies minimum and maximum bin values, BIN_MIN and
%    BIN_MAX, and the number of bins, BIN_NUM.  This determines a
%    "bin width":
%
%      H = ( BIN_MAX - BIN_MIN ) / BIN_NUM
%
%    so that bin I will count all entries X(J) such that
%
%      BIN_LIMIT(I-1) <= X(J) < BIN_LIMIT(I).
%
%    The array X does NOT have to be sorted.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries of X.
%
%    Input, real X(N), an (unsorted) array to be binned.
%
%    Input, integer BIN_NUM, the number of bins.  Two extra bins,
%    #1 and #BIN_NUM+2, count extreme values.
%
%    Input, real BIN_MIN, BIN_MAX, define the range and size
%    of the bins.  BIN_MIN and BIN_MAX must be distinct.
%    Normally, BIN_MIN < BIN_MAX, and the documentation will assume
%    this, but proper results will be computed if BIN_MIN > BIN_MAX.
%
%    Output, integer BIN(1:BIN_NUM+2).
%    BIN(1) counts entries of X less than BIN_MIN.
%    BIN(BIN_NUM+2) counts entries greater than or equal to BIN_MAX.
%    For 2 <= I <= BIN_NUM+1, BIN(I) counts the entries X(J) such that
%      BIN_LIMIT(I-1) <= X(J) < BIN_LIMIT(I).
%    where H is the bin spacing.
%
%    Output, real BIN_LIMIT(1:BIN_NUM+1), the "limits" of the bins.
%    BIN(I) counts the number of entries X(J) such that
%      BIN_LIMIT(I-1) <= X(J) < BIN_LIMIT(I).
%
  if ( bin_max == bin_min )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_BIN - Fatal error!\n' );
    fprintf ( 1, '  BIN_MIN = BIN_MAX.\n' );
    error ( 'R8VEC_BIN - Fatal error!' )
  end

  bin(1:bin_num+2) = 0;

  for i = 1 : n

    t = ( x(i) - bin_min ) / ( bin_max - bin_min );

    if ( t < 0.0 )
      j = 0;
    elseif ( 1.0 <= t )
      j = bin_num + 1;
    else
      j = 1 + floor ( bin_num * t );
    end

    bin(j+1) = bin(j+1) + 1;

  end
%
%  Compute the bin limits.
%
  for i = 0 : bin_num
    bin_limit(i+1) = (   ( bin_num - i ) * bin_min  ...
                       + (           i ) * bin_max ) ...
                       / ( bin_num    );
  end

  return
end


function spline_test001 ( )

%*****************************************************************************80
%
%% TEST001 tests PARABOLA_VAL2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author
%
%    John Burkardt
%
  ndim = 1;
  ndata = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST001\n' );
  fprintf ( 1, '  PARABOLA_VAL2 evaluates parabolas through\n' );
  fprintf ( 1, '    3 points in a table\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Our data tables will actually be parabolas:\n' );
  fprintf ( 1, '    A: 2*x**2 + 3 * x + 1.\n' );
  fprintf ( 1, '    B: 4*x**2 - 2 * x + 5.\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ndata

    xval = 2.0 * i;
    xdata(i) = xval;
    ydata(1,i) = 2.0 * xval * xval + 3.0 * xval + 1.0;
    zdata(i) = 4.0 * xval * xval - 2.0 * xval + 5.0;

    fprintf ( 1, '%6d  %10f  %10f  %10f\n', i, xdata(i), ydata(1,i), zdata(i) );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Interpolated data:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEFT, X, Y1, Y2\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 5

    xval = 2 * i - 1;
    left = i;

    if ( ndata - 2 < left )
      left = ndata - 2;
    end

    if ( left < 1 )
      left = 1;
    end

    yval = parabola_val2 ( ndim, ndata, xdata, ydata, left, xval );

    zval = parabola_val2 ( ndim, ndata, xdata, zdata, left, xval );

    fprintf ( 1, '%6d  %10f  %10f  %10f\n', left, xval, yval(1), zval(1) );

  end

  return
end


function x = equidistant3 ( n )

%*****************************************************************************80
%
%% EQUIDISTANT3 returns the Type 3 Equidistant points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2018
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the points.
%
  x(1:n) = ( 1 - n : 2 : n - 1 ) / n;

  return
end


function divdif_test03 ( )

%*****************************************************************************80
%
%% DIVDIF_TEST03 tests DIF_BASIS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 June 2013
%
%  Author:
%
%    John Burkardt
%
  ntab = 5;
  nstep = 9;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'DIVDIF_TEST03\n' );
  fprintf ( 1, '  DIF_BASIS computes Lagrange basis polynomials\n' );
  fprintf ( 1, '  in difference form.\n' );
  fprintf ( 1, '\n' );
%
%  Set the base points.
%
  xtab = r8vec_indicator ( ntab );

  r8vec_print ( ntab, xtab, '  The base points:' );
%
%  Get the difference tables for the basis polynomials and print them.
%
  diftab = dif_basis ( ntab, xtab );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The table of difference vectors defining the basis\n' );
  fprintf ( 1, '  polynomials.  Each column represents a polynomial.\n' );
  fprintf ( 1, '\n' );
  for i = 1 : ntab
    fprintf ( 1, '  ' );
    for j = 1 : ntab
      fprintf ( 1, '%14f', diftab(i,j) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Evaluate basis polynomial 3 at a set of points.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Evaluate basis polynomial #3 at a set of points.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X        Y\n' );
  fprintf ( 1, '\n' );
  xhi = ntab;
  xlo = 1.0;

  for i = 1 : nstep

    xval = ( ( nstep - i     ) * xlo   ...
           + (         i - 1 ) * xhi ) ...
           / ( nstep     - 1 );

    yval = dif_val ( ntab, xtab, diftab(1:ntab,3), xval );

    fprintf ( 1, '  %14f  %14f\n', xval, yval );

  end

  return
end


%{
    Demonstration of dimensionality reduction using KPCA.
%}
clc
clear all
close all
addpath(genpath(pwd))

load('.\data\helix.mat', 'data')
kernel = Kernel('type', 'gaussian', 'gamma', 2);
parameter = struct('numComponents', 2, ...
                   'kernelFunc', kernel);
% build a KPCA object
kpca = KernelPCA(parameter);
% train KPCA model
kpca.train(data);

%　mapping data
mappingData = kpca.score;

% Visualization
kplot = KernelPCAVisualization();
% visulize the mapping data
kplot.score(kpca)


function par_est
% parameter estimation with derivatives              Holzbecher September 2005
% for exponential fit of exponent lambda for c(0)=c0
global tfit cfit c0

% specify fitting data
tfit =  [0.25 1 2 4 8];
cfit = [0.7716 0.5791 0.4002 0.1860 0.1019];
c0 = .8;

lambda = fzero(@myfun,0.11);
normc = norm(cfit - c0*exp(-lambda*tfit));
display (['Best fit for lambda = ' num2str(lambda)]);
display (['Norm of residuals =' num2str(normc)]);
tmax = tfit(size(tfit,2));
t = [0:0.01*tmax:tmax];
figure; plot (tfit,cfit,'or',t,c0*exp(-lambda*t),'-');
legend ('given','modelled');
text(0.5*tmax,c0*0.7,['\lambda: ' num2str(lambda)]);
text(0.5*tmax,c0*0.6,['norm of residuals: ' num2str(normc)]);
xlabel ('time'); ylabel ('concentration')

function f = myfun(lambda)
global tfit cfit c0

t = tfit;
c = c0*exp(-lambda*t);    % solve linear decay equation for c with c(0)=c0
clambda = -c.*t;          % equation for dc/dlambda
f = (cfit-c)*clambda';    % specify function f to vanish


function mean = burr_mean ( a, b, c, d )

%*****************************************************************************80
%
%% BURR_MEAN returns the mean of the Burr PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, D, the parameters of the PDF.
%    0 < B,
%    0 < C.
%
%    Output, real MEAN, the mean of the PDF.
%
  mean = a + b * gamma ( 1.0 - 1.0 / c ) * gamma ( d + 1.0 / c ) / gamma ( d );

  return
end


function prob_test136 ( )

%*****************************************************************************80
%
%% TEST136 tests SECH_MEAN, SECH_SAMPLE, SECH_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST136\n' );
  fprintf ( 1, '  For the Hyperbolic Secant PDF:\n' );
  fprintf ( 1, '  SECH_MEAN computes the mean;\n' );
  fprintf ( 1, '  SECH_SAMPLE samples;\n' );
  fprintf ( 1, '  SECH_VARIANCE computes the variance.\n' );

  a = 3.0;
  b = 2.0;

  check = sech_check ( a, b );

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

  mean = sech_mean ( a, b );
  variance = sech_variance ( a, b );

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

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

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

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

  return
end


function dist = polygon_point_dist ( n, v, p )

%*****************************************************************************80
%
%% POLYGON_POINT_DIST: distance ( polygon, point ).
%
%  Discussion:
%
%    Thanks to Stefano Zappacosta for pointing out that the arguments
%    passed to SEGMENT_POINT_DIST_2D needed to be transposed,
%    27 June 2006.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 June 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of vertices.
%
%    Input, real V(2,N), the vertices of the polygon.
%
%    Input, real P(2,1), the point to be checked.
%
%    Output, real DIST, the distance from the point to the polygon.
%

%
%  Find the distance to each of the line segments.
%
  dist = inf;

  for j = 1 : n

    jp1 = j + 1;
    if ( n < jp1 ) then
      jp1 = jp1 - n;
    end

    dist2 = segment_point_dist ( v(1:2,j), v(1:2,jp1), p );

    if ( dist2 < dist )
      dist = dist2;
    end

  end

  return
end


function determ = ortega_determinant ( n, u, v, d )

%*****************************************************************************80
%
%% ORTEGA_DETERMINANT returns the determinant of the ORTEGA matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    James Ortega,
%    Generation of Test Matrices by Similarity Transformations,
%    Communications of the ACM,
%    Volume 7, 1964, pages 377-378.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    2 <= N.
%
%    Input, real U(N), V(N), vectors which define the matrix.
%    U'V must not equal -1.0.  If, in fact, U'V = 0, and U, V and D are
%    integers, then the matrix, inverse, eigenvalues, and eigenvectors
%    will be integers.
%
%    Input, real D(N), the desired eigenvalues.
%
%    Output, real DETERM, the determinant.
%
  determ = prod ( d(1:n) );

  return
end


function MS = createMesh1D(varargin)
% MeshStructure = createMesh1D(Nx, Width)
% MeshStructure = createMesh1D(facelocationX)
% creates a uniform 1D mesh:
% Nx is the number of cells in x (horizontal) direction
% Width is the domain length in x direction
%
% SYNOPSIS:
%   MeshStructure = createMesh1D(Nx, Width)
%
% PARAMETERS:
%   Nx: number of cells in the domain
%   Width: domain length
%
% RETURNS:
%   MeshStructure.
%                 dimensions=1 (1D problem)
%                 numbering: shows the indexes of cellsn from left to right
%                 dx: cell size (=Width/Nx)
%                 cellcenters.x: location of each cell in the x direction
%                 facecenters.x: location of interface between cells in the
%                 x direction
%                 numberofcells: Nx
%
%
% EXAMPLE:
%   L = 1.0; % length of the domain
%   Nx = 10; % number of cells in the domain
%   m = createMesh1D(Nx, L);
%   plot(m.cellcenters.x, ones(size(m.cellcenters.x)), 'o', ...
%        m.facecenters.x, ones(size(m.facecenters.x)), '-+');
%   legend('cell centers', 'face centers');
%
% SEE ALSO:
%     createMesh2D, createMesh3D, createMeshCylindrical1D, ...
%     createMeshCylindrical2D, createCellVariable, createFaceVariable

% Written by Ali A. Eftekhari
% See the license file

if nargin==2
  % uniform 1D mesh
  Nx=varargin{1};
  Width=varargin{2};
  % cell size is dx
  dx = Width/Nx;
  CellSize.x= dx*ones(Nx+2,1);
  CellSize.y= [0.0];
  CellSize.z= [0.0];
  CellLocation.x= [1:Nx]'*dx-dx/2;
  CellLocation.y= [0.0];
  CellLocation.z= [0.0];
  FaceLocation.x= [0:Nx]'*dx;
  FaceLocation.y= [0.0];
  FaceLocation.z= [0.0];
elseif nargin==1
  % nonuniform 1D mesh
  facelocationX=varargin{1};
  n=size(facelocationX);
  if n(1)==1
      facelocationX=facelocationX';
  end
  Nx = length(facelocationX)-1;
  CellSize.x= [facelocationX(2)-facelocationX(1); ...
    facelocationX(2:end)-facelocationX(1:end-1); ...
    facelocationX(end)-facelocationX(end-1)];
  CellSize.y= [0.0];
  CellSize.z= [0.0];
  CellLocation.x= 0.5*(facelocationX(2:end)+facelocationX(1:end-1));
  CellLocation.y= [0.0];
  CellLocation.z= [0.0];
  FaceLocation.x= facelocationX;
  FaceLocation.y= [0.0];
  FaceLocation.z= [0.0];
end

MS=MeshStructure(1, ...
  [Nx,1], ...
  CellSize, ...
  CellLocation, ...
  FaceLocation, ...
  [1], ...
  [1]);


function [dists, neighInds] = nndist(points)
%NNDIST Nearest-neighbor distances of each point in a set
%
%   DISTS = nndist(POINTS)
%   Returns the distance to the nearest neighbor of each point in an array
%   of points.
%   POINTS is an array of points, NP-by-ND.
%   DISTS is a NP-by-1 array containing the distances to the nearest
%   neighbor.
%
%   This functions first computes the Delaunay triangulation of the set of
%   points, then search for nearest distance in the set of each vertex
%   neighbors. This reduces the overall complexity, but difference was
%   noticed only for large sets (>10000 points)
%
%   Example
%     % Display Stienen diagram of a set of random points in unit square
%     pts = rand(100, 2);
%     [dists, inds] = nndist(pts);
%     figure; drawPoint(pts, 'k.');
%     hold on; drawCircle([pts dists/2], 'b');
%     axis equal; axis([-.1 1.1 -.1 1.1]);
%     % also display edges
%     drawEdge([pts pts(inds, :)], 'b');
%
%   See also
%     points2d, distancePoints, minDistancePoints, findPoint
%

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

% number of points
n = size(points, 1);

% allocate memory
dists = zeros(n, 1);
neighInds = zeros(n, 1);

% in case of few points, use direct computation
if n < 3
    inds = 1:n;
    for i = 1:n
        % compute minimal distance
        [dists(i), indN] = minDistancePoints(points(i,:), points(inds~=i, :));
        if indN >= i
            neighInds(i) = inds(indN) + 1;
        else
            neighInds(i) = inds(indN);
        end
    end
    return;
end

% use Delaunay Triangulation to facilitate computations
DT = delaunayTriangulation(points);

% compute distance to nearest neighbor of each point in the pattern
for i = 1:n
    % find indices of neighbor vertices in Delaunay Triangulation.
    % this set contains the nearest neighbor
    inds = unique(DT.ConnectivityList(sum(DT.ConnectivityList == i, 2) > 0, :));
    inds = inds(inds~=i);

    % compute minimal distance
    [dists(i), indN] = min(distancePoints(points(i,:), points(inds, :)));
    neighInds(i) = inds(indN);
end


function [varargout]=quiver3Dpatch(x,y,z,ux,uy,uz,c,a)

% function [F,V,C]=quiver3Dpatch(x,y,z,ux,uy,uz,c,a)
% ------------------------------------------------------------------------
%
% This function allows plotting of colored 3D arrows by generating patch
% data (faces F, vertices V and color data C). The patch data which
% allows plotting of 3D quiver arrows with specified (e.g. colormap driven)
% color. To save memory n arrows are created using only n*6 faces and n*7
% vertices. The vector "a" defines arrow length scaling where a(1) is the
% smallest arrow length and a(2) the largest. Use the PATCH command to plot
% the arrows:
%
% [F,V,C]=quiver3Dpatch(x,y,z,ux,uy,uz,a)
% patch('Faces',F,'Vertices',V,'CData',C,'FaceColor','flat');
%
% Below is an example illustrating color specifications for (combined)
% patch data.
%
%%% EXAMPLE
% % Simulating 3D volume and vector data
% n=25;
% [X,Y,Z]=meshgrid(linspace(-4.77,4.77,n));
% phi=(1+sqrt(5))/2;
% M=2 - (cos(X + phi*Y) + cos(X - phi*Y) + cos(Y + phi*Z) + cos(Y - phi*Z) + cos(Z - phi*X) + cos(Z + phi*X));
%
% % Simulating vector data
% %Vector data here based on the gradient of the image
% [u,v,w] = gradient(M);
% G=hypot(hypot(u,v),w); %Vector lenghts
%
% a=[min(G(:)) max(G(:))]; %Arrow length scaling
% L=G>0.7; %Logic indices for arrows to make a selection if required
% [Fv,Vv,Cv]=quiver3Dpatch(X(L),Y(L),Z(L),u(L),v(L),w(L),G(L),a);
%
% figure;
% xlabel('X','FontSize',10);ylabel('Y','FontSize',10);zlabel('Z','FontSize',10);
% title('Colormap driven vectors','FontSize',15);
% patch('Faces',Fv,'Vertices',Vv,'EdgeColor','none', 'CData',Cv,'FaceColor','flat','FaceAlpha',1);
%
% colormap jet; colorbar;
% % caxis([min(Cv(:)) max(Cv(:))]);
% view(3); grid on; axis equal; axis vis3d;
% set(gca,'FontSize',10);
% camlight headlight; lighting flat;
% drawnow;
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2014/01/13 Updated example in help
% 2014/11/11 Updated to allow for RGB color input data (e.g. C is an nx3 array)
% 2016/06/30 Added handling of empty "a" parameter. Reverts to default
% normal length range.
%------------------------------------------------------------------------

%%

%Convert to columns
x=x(:); y=y(:); z=z(:);
ux=ux(:); uy=uy(:); uz=uz(:);

%Spherical coordinates
[THETA_vec,PHI_vec,R_vec] = cart2sph(ux,uy,uz);

%% Setting arrow size properties

if isempty(a)
    a=[min(R_vec(:)) max(R_vec(:))];
end

%Arrow length
if  min(R_vec(:))==max(R_vec(:)) %If all radii are equal, or if just 1 vector is used
    arrow_length=a(2)*ones(size(R_vec)); %All arrow lengths become a(2)
else
    %Scale arrow lengths between a(1) and a(2)
    arrow_length=R_vec-min(R_vec(:));
    arrow_length=a(1)+((arrow_length./max(arrow_length(:))).*(a(2)-a(1)));
end

%Other arrow dimensions as functions of arrow length and phi (golden ratio)
phi=(1+sqrt(5))/2;
head_size=arrow_length./(phi);
head_width=head_size./(2.*phi);
stick_width=head_width./(2.*phi);
if numel(a)==3
    head_width=a(3)*head_width;
    stick_width=a(3)*stick_width;
end
h=sin((2/3).*pi).*stick_width;
ha=sin((2/3).*pi).*head_width;

%% Creating arrow triangle vertices coordinates

X_tri=[zeros(size(x))  zeros(size(x)) zeros(size(x))...
    head_size.*ones(size(x)) head_size.*ones(size(x)) head_size.*ones(size(x))...
    arrow_length];
Y_tri=[-(0.5.*stick_width).*ones(size(x)) (0.5.*stick_width).*ones(size(x))  zeros(size(x))...
    -(0.5.*head_width).*ones(size(x))  (0.5.*head_width).*ones(size(x))  zeros(size(x))...
    zeros(size(x))];
Z_tri=[-(0.5.*stick_width.*tan(pi/6)).*ones(size(x))...
    -(0.5.*stick_width.*tan(pi/6)).*ones(size(x))...
    (h-(0.5.*stick_width.*tan(pi/6))).*ones(size(x))...
    -(0.5.*head_width.*tan(pi/6)).*ones(size(x))...
    -(0.5.*head_width.*tan(pi/6)).*ones(size(x))...
    (ha-(0.5.*head_width.*tan(pi/6))).*ones(size(x))...
    zeros(size(x))];

% Rotating vertices
[THETA_ar,PHI_ar,R_vec_ar] = cart2sph(X_tri,zeros(size(Y_tri)),Z_tri);
PHI_ar=PHI_ar+PHI_vec*ones(1,size(THETA_ar,2));
[X_arg,Y_arg,Z_arg] = sph2cart(THETA_ar,PHI_ar,R_vec_ar);
Y_arg=Y_arg+Y_tri;
[THETA_ar,PHI_ar,R_vec_ar] = cart2sph(X_arg,Y_arg,Z_arg);
THETA_ar=THETA_ar+THETA_vec*ones(1,size(THETA_ar,2));
[X_arg,Y_arg,Z_arg] = sph2cart(THETA_ar,PHI_ar,R_vec_ar);

X_arg=X_arg+x*ones(1,size(THETA_ar,2)); X_arg=X_arg';
Y_arg=Y_arg+y*ones(1,size(THETA_ar,2)); Y_arg=Y_arg';
Z_arg=Z_arg+z*ones(1,size(THETA_ar,2)); Z_arg=Z_arg';

V=[X_arg(:) Y_arg(:) Z_arg(:)];

%% Creating faces matrix

%Standard vertex order for 6 face arrow style
F_order=[1 2 7; 2 3 7; 3 1 7; 4 5 7; 5 6 7; 6 4 7;];
no_nodes=size(X_tri,2);
b=(no_nodes.*((1:1:numel(x))'-1))*ones(1,3);

%Loops size(F_order,1) times
F=zeros(numel(x)*size(F_order,1),3); %Allocating faces matrix
for f=1:1:size(F_order,1)
    Fi=ones(size(x))*F_order(f,:)+b;
    F(1+(f-1)*numel(x):f*numel(x),:)=Fi;
end

%     %Alternative without for loop, not faster for some tested problems
%     F_order=(ones(numel(x),1)*[1 2 7 2 3 7 3 1 7 4 5 7 5 6 7 6 4 7])';
%     F_order1=ones(numel(x),1)*(1:6);
%     F_order2=ones(numel(x),1)*[2 3 1 5 6 4];
%     F_order=[F_order1(:) F_order2(:)];
%     F_order(:,3)=7;
%     b=repmat(((no_nodes.*(0:1:numel(x)-1)')*ones(1,3)),[6,1]);
%     F=F_order+b;

%% Collect face and vertex output
varargout{1}=F;
varargout{2}=V;

%% Add color specification if requested

if nargout==3
    if isempty(c) %If empty specify vector magnitudes as colormap driven color
        C=repmat(R_vec,[size(F_order,1),1]);
    else %If user specified color replicate to match # of added faces for arrow
        %c my be an nx3 array to allow for RGB type color data
        C=repmat(c,[size(F_order,1),1]);
    end
    varargout{3}=C;
end

end

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


function [gd,relres,iter]=gabfirdual(Ldual,g,a,M,varargin)
%GABFIRDUAL  Compute FIR dual window
%   Usage: gd=gabfirdual(Ldual,g,a,M);
%          gd=gabfirdual(Ldual,g,a,M, varagin);
%
%   Input parameters:
%     Ldual  : Length of dual window
%     g      : Window function
%     a      : Time shift
%     M      : Number of Channels
%     alpha1 : Weight of $l^1$-norm in the time domain
%     alpha2 : Weight of $l^1$-norm in the freq. domain
%
%   Output parameters:
%     gd     : Dual window
%
%   `gabfirdual(Ldual,g,a,M)` computes an FIR window *gd* which is an
%   approximate dual window of the Gabor system defined by *g*, *a* and
%   *M*. The FIR dual window will be supported on *Ldual* samples.
%
%   This function solve a convex optimization problem that can be written
%   as:
%
%   .. gd  = argmin_x    || alpha x||_1 +  || beta Fx||_1
%
%   ..                 + || omega (x -g_l) ||_2^2 + delta || x ||_S0
%
%   ..                 + gamma || nabla F x ||_2^2 + mu || nabla x ||_2^2
%
%   ..     such that  x is a dual windows of g
%
%   .. math:: \begin{split}  \text{gd}  = & \text{arg} \min_x    \| \alpha x \|_1 +  \| \beta \mathcal{F}x\|_1  \\ &  + \| \omega (x - g_l) \|_2^2  \\ &  \delta \| x \|_{S0}+ \mu \| \nabla x \|_2^2 +\gamma \| \nabla \mathcal{F} x \|_2^2 \\ &  \text{such that } x \text{ is a dual window of }g \end{split}
%
%   **Note**: This function require the unlocbox. You can download it at
%   `<http://unlocbox.sourceforge.net>`_
%
%   The function uses an iterative algorithm to compute the approximate
%   FIR dual. The algorithm can be controlled by the following flags:
%
%     'alpha',alpha  Weight in time. If it is a scalar, it represent the
%                  weights of the entire L1 function in time. If it is a
%                  vector, it is the associated weight assotiated to each
%                  component of the L1 norm (length: Ldual).
%                  Default value is $\alpha=0$.
%                  **Warning**: this value should not be too big in order to
%                  avoid the the L1 norm proximal operator kill the signal.
%                  No L1-time constraint: $\alpha=0$
%
%     'beta',beta  Weight in frequency. If it is a scalar, it represent the
%                  weights of the entire L1 function in frequency. If it is a
%                  vector, it is the associated weight assotiated to each
%                  component of the L1 norm in frequency. (length: Ldual).
%                  Default value is $\beta=0$.
%                  **Warning**: this value should not be too big in order to
%                  avoid the the L1 norm proximal operator kill the signal.
%                  No L1-frequency constraint: $\beta=0$
%
%     'omega',omega  Weight in time of the L2-norm. If it is a scalar, it represent the
%                  weights of the entire L2 function in time. If it is a
%                  vector, it is the associated weight assotiated to each
%                  component of the L2 norm (length: Ldual).
%                  Default value is $\omega=0$.
%                  No L2-time constraint: $\omega=0$
%
%     'glike',g_l  $g_l$ is a windows in time. The algorithm try to shape
%                  the dual window like $g_l$. Normalization of $g_l$ is done
%                  automatically. To use option omega should be different
%                  from 0. By default $g_d=0$.
%
%     'mu',mu      Weight of the smooth constraint Default value is 1.
%                  No smooth constraint: $\mu=0$
%
%     'gamma',gamma  Weight of the smooth constraint in frequency. Default value is 1.
%                    No smooth constraint: $\gamma=0$
%
%     'delta',delta  Weight of the S0-norm. Default value is 0.
%                    No S0-norm: $\delta=0$
%
%     'dual'       Look for a dual windows (default)
%
%     'painless'   Construct a starting guess using a painless-case
%                  approximation. This is the default
%
%     'zero'       Choose a starting guess of zero.
%
%     'rand'       Choose a random starting phase.
%
%     'tol',t      Stop if relative residual error is less than the
%                  specified tolerance.
%
%     'maxit',n    Do at most n iterations. default 200
%
%     'print'      Display the progress.
%
%     'debug'      Display all the progresses.
%
%     'quiet'      Don't print anything, this is the default.
%
%     'fast'       Fast algorithm, this is the default.
%
%     'slow'       Safer algorithm, you can try this if the fast algorithm
%                  is not working. Before using this, try to iterate more.
%
%     'printstep',p  If 'print' is specified, then print every p'th
%                    iteration. Default value is p=10;
%
%     'hardconstraint' Force the projection at the end (default)
%
%     'softconstaint' Do not force the projection at the end
%
%   See also: gaboptdual, gabdual, gabtight, gabfirtight, gaboptdual

% Author: Nathanael Perraudin
% Date  : 18 Feb 2014

if nargin<4
  error('%s: Too few input parameters.',upper(mfilename));
end;

% l=length(varargin);
% varargin(l+1)=cellstr('support');
% varargin(l+2)=num2cell(Ldual);

[gd,relres,iter]=gabconvexopt(g,a,M,varargin{:}, 'support',Ldual);


function dc = diffusivity_2d_elman ( a, cl, dc0, m_1d, omega, n1, n2, x, y )

%*****************************************************************************80
%
%% DIFFUSIVITY_2D_ELMAN evaluates a 2D stochastic diffusivity function.
%
%  Discussion:
%
%    The 2D diffusion equation has the form
%
%      - Del ( DC(X,Y) Del U(X,Y) ) = F(X,Y)
%
%    where DC(X,Y) is a function called the diffusivity.
%
%    In the stochastic version of the problem, the diffusivity function
%    includes the influence of stochastic parameters:
%
%      - Del ( DC(X,Y;OMEGA) Del U(X,Y;OMEGA) ) = F(X,Y).
%
%    In this function, the domain is assumed to be the square [-A,+A]x[-A,+A].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 December 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Howard Elman, Darran Furnaval,
%    Solving the stochastic steady-state diffusion problem using multigrid,
%    IMA Journal on Numerical Analysis,
%    Volume 27, Number 4, 2007, pages 675-688.
%
%    Roger Ghanem, Pol Spanos,
%    Stochastic Finite Elements: A Spectral Approach,
%    Revised Edition,
%    Dover, 2003,
%    ISBN: 0486428184,
%    LC: TA347.F5.G56.
%
%  Parameters:
%
%    Input, real A, the "radius" of the square region.  The region
%    is assumed to be [-A,+A]x[-A,+A].
%    0 < A.
%
%    Input, real CL, the correlation length.
%    0 < CL.
%
%    Input, real DC0, the constant term in the expansion of the
%    diffusion coefficient.  Take DC0 = 10.
%
%    Input, integr M_1D, the first and second dimensions of the stochastic
%    parameter array.
%
%    Input, real OMEGA(M_1D*M_1D), the stochastic parameters.
%
%    Input, integer N1, N2, the dimensions of the X and Y arrays.
%
%    Input, real X(N1,N2), Y(N1,N2), the points where the diffusion coefficient is to
%    be evaluated.
%
%    Output, real DC(N1,N2), the value of the diffusion coefficient at X.
%

%
%  Compute THETA.
%
  theta_1d = theta_solve ( a, cl, m_1d );
%
% m = m_1d * m_1d;
% theta_1d = theta_solve ( a, cl, m );
%
%  Compute LAMBDA_1D.
%
  lambda_1d(1:m_1d) = 2.0 * cl ./ ( 1.0 + cl^2 * theta_1d(1:m_1d).^2 );
%
%  Compute C_1DX(1:M1D)  and C_1DY(1:M1D) at (X,Y).
%
  c_1dx = zeros ( m_1d, n1, n2 );
  c_1dy = zeros ( m_1d, n1, n2 );

  k = 0;

  while ( 1 )

    if ( m_1d <= k )
      break
    end

    k = k + 1;
    c_1dx(k,1:n1,1:n2) = cos ( theta_1d(k) * a * x(1:n1,1:n2) ) ...
      / sqrt ( a + sin ( 2.0 * theta_1d(k) * a ) / ( 2.0 * theta_1d(k) ) );
    c_1dy(k,1:n1,1:n2) = cos ( theta_1d(k) * a * y(1:n1,1:n2) ) ...
      / sqrt ( a + sin ( 2.0 * theta_1d(k) * a ) / ( 2.0 * theta_1d(k) ) );

    if ( m_1d <= k )
      break
    end

    k = k + 1;
    c_1dx(k,1:n1,1:n2) = sin ( theta_1d(k) * a * x(1:n1,1:n2) ) ...
      / sqrt ( a - sin ( 2.0 * theta_1d(k) * a ) / ( 2.0 * theta_1d(k) ) );
    c_1dy(k,1:n1,1:n2) = sin ( theta_1d(k) * a * y(1:n1,1:n2) ) ...
      / sqrt ( a - sin ( 2.0 * theta_1d(k) * a ) / ( 2.0 * theta_1d(k) ) );

  end
%
%  Evaluate the diffusion coefficient DC at (X,Y).
%  This nonsense of fussy array shapes really frustrates me!
%
  k = 0;
  dc = dc0 * ones ( 1, n1, n2 );
  for j = 1 : m_1d
    for i = 1 : m_1d
      k = k + 1;
      dc(1,1:n1,1:n2) = dc(1,1:n1,1:n2) + sqrt ( lambda_1d(i) * lambda_1d(j) ) ...
        * c_1dx(i,1:n1,1:n2) .* c_1dy(j,1:n1,1:n2) * omega(k);
    end
  end

  dc = reshape ( dc, n1, n2 );

  return
end
function theta = theta_solve ( a, cl, m )

%*****************************************************************************80
%
%% THETA_SOLVE solves a pair of transcendental equations.
%
%  Discussion:
%
%    The vector THETA returned by this function is needed in order to define
%    the terms in a Karhunen-Loeve expansion of a diffusion coefficient.
%
%    The two equations are:
%
%      1/CL - THETA * TAN ( A * THETA ) = 0
%      THETA - 1/CL * TAN ( A * THETA ) = 0
%
%    A and CL are taken to be positive.  Over each open interval
%
%      ( n - 1/2 pi, n + 1/2 pi ) / A, for N = 0, 1, ...
%
%    the function TAN ( A * THETA ) monotonically rises from -oo to +00;
%    therefore, it can be shown that there is one root of each equation
%    in every interval of this form.  Moreover, because of the positivity
%    of A and CL, we can restrict our search to the interval
%
%      [ n pi, n + 1/2 pi ) / A, for N = 0, 1, ...
%
%    This function computes K such roots, starting in the first interval,
%    finding those two roots, moving to the next interval, and so on, until
%    the requested number of roots have been found.  Odd index roots will
%    correspond to the first equation, and even index roots to the second.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 July 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Howard Elman, Darran Furnival,
%    Solving the Stochastic Steady-State Diffusion Problem Using Multigrid,
%    University of Maryland Department of Computer Science,
%    Technical Report TR-4786.
%
%  Parameters:
%
%    Input, real A, the "radius" of the domain, D = (-A,A)x(-A,A).
%    0 < A.
%
%    Input, real CL, the correlation length.
%    0 < CL.
%
%    Input, integer M, the number of values to compute.
%
%    Output, real THETA(M), the values of Theta.
%
  k = 0;
  theta = zeros(m,1);
%
%  [ XA_INIT, XB_INIT] = [ n * pi, n+1/2 pi ] / a, n = 0, 1, 2, ...
%
  xa_init = 0.0;
  xb_init = ( pi / 2 ) / a;

  while ( 1 )
%
%  Seek root of equation 1 in interval.
%
    if ( m <= k )
      break
    end

    k = k + 1;
    xa = xa_init;
    fa = 1.0 / cl - xa * tan ( a * xa );
    ftol = eps * ( abs ( fa ) + 1.0 );
    xb = xb_init;
    fb = - fa;
    fc = fa;
    bmatol = 100.0 * eps * ( abs ( xa ) + abs ( xb ) );

    while ( bmatol < xb - xa )

      xc = ( xa + xb ) / 2;
      fc = 1.0 / cl - xc * tan ( a * xc );

      if ( abs ( fc ) <= ftol )
        break
      elseif ( 0 < fc )
        xa = xc;
      else
        xb = xc;
      end

    end

    theta(k) = xc;
%
%  Seek root of equation 2 in interval.
%
    if ( m <= k )
      break
    end

    k = k + 1;
%
%  In the first interval, we need to skip the zero root of equation 2.
%
    if ( k == 2 )

      k = k - 1;

    else

      xa = xa_init;
      fa = xa - tan ( a * xa ) / cl;
      ftol = eps * ( abs ( fa ) + 1.0 );
      xb = xb_init;
      fb = - fa;

      while ( bmatol < xb - xa )

        xc = ( xa + xb ) / 2;
        fc = xc - tan ( a * xc ) / cl;

        if ( abs ( fc ) <= ftol )
          break
        elseif ( 0 < fc )
          xa = xc;
        else
          xb = xc;
        end

      end

      theta(k) = xc;

    end
%
%  Advance the interval.
%
    xa_init = xa_init + pi / a;
    xb_init = xb_init + pi / a;

  end

  return
end


function x=v_choosenk(n,k)
%V_CHOOSENK All choices of K elements taken from 1:N [X]=(N,K)
% The output X is a matrix of size (N!/(K!*(N-K)!),K) where each row
% contains a choice of K elements taken from 1:N without duplications.
% The rows of X are in lexically sorted order.
%
% To choose from the elements of an arbitrary vector V use
% V(CHOOSENK(LENGTH(V),K)).

% CHOOSENK(N,K) is the same as the MATLAB5 function NCHOOSEK(1:N,K) but is
% much faster for large N and most values of K.

%   Copyright (c) 1998 Mike Brookes,  mike.brookes@ic.ac.uk
%      Version: $Id: v_choosenk.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

kk=min(k,n-k);
if kk<2
   if kk<1
      if k==n
         x=1:n;
      else
         x=[];
      end
   else
      if k==1
         x=(1:n)';
      else
         x=1:n;
         x=reshape(x(ones(n-1,1),:),n,n-1);
      end
   end
else
   n1=n+1;
   m=prod(n1-kk:n)/prod(1:kk);
   x=zeros(m,k);
   f=n1-k;
   x(1:f,k)=(k:n)';
   for a=k-1:-1:1
      d=f;
      h=f;
      x(1:f,a)=a;
      for b=a+1:a+n-k
         d=d*(n1+a-b-k)/(n1-b);
         e=f+1;
         f=e+d-1;
         x(e:f,a)=b;
         x(e:f,a+1:k)=x(h-d+1:h,a+1:k);
      end
   end
end


function [Geometry,Geom_res] =fbkp_grid(theta,N,interp)

% theta are imput as angles in degrees
% Define the x & y axes for the reconstructed image so that the origin
% (center) is in the spot which RADON would choose.

%   N is a scalar that specifies the number of rows and columns in the
%   reconstructed image. N is determined from the length of the projections: !!!!
%
%       N = 2*floor(size(P,1)/(2*sqrt(2)))
%
theta = pi*theta/180;

xax = (1:N)-ceil(N/2);
x = repmat(xax,N,1) ;
% x coordinates, the y coordinates are rot90(x)
y = rot90(x);
costheta = cos(theta);
sintheta = sin(theta);

% Backprojection - vectorized in (x,y), looping over theta
sa=size(x,1); sb=size(x,2); sc=length(theta);

if strcmp(interp, 'nearest neighbor')
        Geometry=repmat(int16(0),[sa,sb,sc]);
        Geom_res=[];
    for i=1:length(theta)
        t=[];
        t = round( x*costheta(i) + y*sintheta(i) );
        Geometry(:,:,i)=t;
    end

elseif strcmp(interp, 'linear')
    Geometry=repmat(int16(0),[sa,sb,sc]); % parte intera
    Geom_res=repmat(uint8(0),[sa,sb,sc]); % parte decimale

    for i=1:length(theta)
        t = x.*costheta(i) + y.*sintheta(i);
        a = floor(t);
        % img = img + (t-a).*proj(a+1+ctrIdx) + (a+1-t).*proj(a+ctrIdx);
        Geometry(:,:,i)=a ; % floor
        Geom_res(:,:,i)=uint8( abs(t-a)*100) ; % decimal i.e. double - floor

    end
end


% Copyright 2018 Marc René Schädler
%
% This file is part of the mobile hearing aid prototype project
% The the mobile hearing aid prototype project is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
%
% The mobile hearing aid prototype project is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License along with the mobile hearing aid prototype project. If not, see http://www.gnu.org/licenses/.

function signal = sinesweepphase(samples, frequency, phase)
%usage: signal = sinesweepphase(samples, frequency, phase)
%
% samples          - number of samples
% frequency        - normalized frequencies [0..1]
% phase            - phase [0..2*pi]

phase_diff_start = 2.*pi.*frequency(1);
phase_diff_stop = 2.*pi.*frequency(2);

phase_diff = logspace(log10(phase_diff_start),log10(phase_diff_stop),samples);

if nargin >= 4
  phase_diff(1) = phase_diff(1) + phase;
end

signal = sin(cumsum(phase_diff)).';
end


function [C, dA] = mcml_grad(x, X, P)
%MCML_GRAD Computes MCML cost function and gradient
%
%   [C, dA] = mcml_grad(x, X, P)
%
% Computes MCML cost function and gradient.
%
%

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

    % Decode solution
    [n, d] = size(X);
    A = reshape(x, [d d]);

    % Compute conditional probabilities for current solution
    D = zeros(n, n);
    for i=1:n
        diffX = bsxfun(@minus, X(i,:), X(i + 1:end,:));
        dist = sum((diffX * A) .* diffX, 2);
        D(i + 1:end, i) = dist;
        D(i, i + 1:end) = dist';
    end
    Q = exp(-D);
    Q(1:n+1:end) = 0;
    Q = bsxfun(@rdivide, Q, sum(Q, 2));
    Q = max(Q, realmin);

    % Compute cost function
    C = sum(P(:) .* log(P(:) ./ Q(:)));

    % Compute gradient with respect to A
    if nargin > 1
        dA = zeros(d, d);
        PQ = P - Q;
        for i=1:n
            diffX = bsxfun(@minus, X(i,:), X);
            dA = dA + bsxfun(@times, PQ(i,:), diffX') * diffX;
        end
    end
    dA = dA(:);


function psnr = PSNR(img_ref, img_dist, comparison_domain, max_value, mask)
%
%
%      psnr = PSNR(img_ref, img_dist, comparison_domain, max_value, mask)
%
%
%       Input:
%           -img_ref: input reference image
%           -img_dist: input distorted image
%           -comparison_domain: the domain where to compare images
%                   'lin': linear
%                   'log2': log base 2
%                   'log': natural logarithm
%                   'log10': log base 10
%                   'pu': perceptual uniform encoding
%           -max_value: maximum value (in the linear domain) of the images domain
%           -mask: a binary image for masked computations.
%
%       Output:
%           -psnr: classic PSNR in dB; i.e., higher values means better quality.
%
%     Copyright (C) 2016-18  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

%check if images have same size and type
[img_ref, img_dist, domain, mxt] = checkDomains(img_ref, img_dist);
checkNegative(img_ref);
checkNegative(img_dist);

%determine the maximum value
if(~exist('max_value', 'var'))
    max_value = -1000;
end

if(~exist('comparison_domain', 'var'))
    comparison_domain = 'lin';
end

if(~exist('mask', 'var'))
    mask = [];
end

bFlag = strcmp(domain, 'uint8') | strcmp(domain, 'uint16');

if(bFlag && strcmp(comparison_domain, 'pu'))
    error('PU encoding does not make sense for uint8 or uint16 values. Physical values are required!');
end

if(bFlag && contains(comparison_domain, 'log'))
    error('logarithmic encoding does not make sense for uint8 or uint16 values. Please use linear domain!');
end

if(max_value < 0.0)
    max_value = mxt;
end

[img_ref, ~] = changeComparisonDomain(img_ref, comparison_domain);
[img_dist, ~] = changeComparisonDomain(img_dist, comparison_domain);
[max_value, bNeg] = changeComparisonDomain(max_value, comparison_domain);
comparison_domain = '';

if(max_value < 0.0)
    max_value = -max_value;
end

%compute MSE
mse = MSE(img_ref, img_dist, bNeg, comparison_domain, mask);

if(mse > 0.0)
    %compute PSNR
    psnr = 20 * log10(max_value / sqrt(mse));
else
    disp('PSNR: the images are the same!');
    psnr = 1000;
end

end


function data=clust_normalize(data,method);

%   method     description
%   'var'      Variance is normalized to one (linear operation).
%   'range'    Values are normalized between [0,1] (linear operation).

data.Xold=data.X;
if strcmp(method,'range')
     data.min=min(data.X);
     data.max=max(data.X);
     data.X=(data.X-repmat(min(data.X),size(data.X,1),1))./(repmat(max(data.X),...
         size(data.X,1),1)-repmat(min(data.X),size(data.X,1),1));
 elseif strcmp(method,'var')
     data.X=(data.X-repmat(mean(data.X),size(data.X,1),1))./(repmat(std(data.X),size(data.X,1),1));
     data.mean=mean(data.X);
     data.std=std(data.X);
 else
     error('Unknown method given')
end


function varargout = chebcoeffs3(f)
%CHEBCOEFFS3   Trivariate Chebyshev coefficients
%   C = CHEBCOEFFS3(F) returns the tensor of trivariate coefficients such
%   that
%   C = sum_{i=0}^{m-1}  sum_{j=0}^{n-1} sum_{k=0}^{p-1}
%                                      C(i+1,j+1,k+1) T_i(x) T_j(y) T_k(z).
%
%   [CORE, C, R, T] = CHEBCOEFFS3(F) returns the same coefficients kept in
%   the Tucker form.
%
% See also CHEBCOEFFS, CHEBCOEFFS2 and COEFFS3

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

if ( isempty(f) )
    varargout = {[]};
    return
end

if ( iszero(f) )
    varargout = {0};
    return
end

% Get low rank representation of f:
[core, cols, rows, tubes] = tucker(f);

% Get the coeffs of the columns, rows and tubes:
cols_coeffs = chebcoeffs(cols);
rows_coeffs = chebcoeffs(rows);
tubes_coeffs = chebcoeffs(tubes);

if ( nargout <= 1 )
    % Return the tensor of coefficients:
    varargout = {chebfun3.txm(chebfun3.txm(chebfun3.txm(core, ...
        cols_coeffs, 1), rows_coeffs, 2), tubes_coeffs, 3)};
elseif ( nargout == 4 )
    varargout = {core, cols_coeffs, rows_coeffs, tubes_coeffs};
else
    % Two or three outputs are not allowed.
    error('CHEBFUN:CHEBFUN3:chebcoeffs3:outputs', ...
        'Incorrect number of outputs.');
end

end


function [I,x,W,F] = sphericalradial(f,m,P,param)
% SPHERICALRADIAL - ND scaled spherical-radial cubature rule
%
% Syntax:
%   [I,x,W,F] = sphericalradial(f,m,P[,param])
%
% In:
%   f - Function f(x,param) as inline, name or reference
%   m - Mean of the d-dimensional Gaussian distribution
%   P - Covariance of the Gaussian distribution
%   param - Parameters for the function (optional)
%
% Out:
%   I - The integral
%   x - Evaluation points
%   W - Weights
%   F - Function values
%
% Description:
%   Apply the spherical-radial cubature rule to integrals of form:
%     int f(x) N(x | m,P) dx

% History:
%   Aug 5, 2010 - Renamed from 'cubature' to 'sphericalradial' (asolin)

% Copyright (c) 2010 Arno Solin
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

%% Spherical-radial cubature rule

  % The dimension of m is
  n = size(m,1);

  % Evaluation points (nx2n)
  x = [eye(n) -eye(n)];

  % Scaling
  x = sqrt(n)*x;
  x = chol(P)'*x + repmat(m,1,2*n);

  % Evaluate the function at the points
    if ischar(f) || strcmp(class(f),'function_handle')
      if nargin < 4
        F = feval(f,x);
      else
        F = feval(f,x,param);
      end
    elseif isnumeric(f)
         F = f*x;
    else
      if nargin < 4
         F = f(x);
      else
         F = f(x,param);
      end
    end

  % The weights are
  W = 1/(2*n);

  % Return integral value
  I = W*sum(F,2);

  % Return weights
  W = W*ones(1,2*n);


function [M_est,U_est,V_est,L1_error] = RobustApproximation_M_UV_TraceNormReg(M,W,r,lambda,rho,maxIterIN,signM)
%% Robust low-rank matrix approximation with missing data and outliers
% min |W.*(M-E)|_1 + lambda*|V|_*
% s.t., E = UV, U'*U = I
%
%Input:
%   M: m*n data matrix
%   W: m*n indicator matrix, with '1' means 'observed', and '0' 'missing'.
%   r: the rank of r
%   lambda: the weighting factor of the trace-norm regularization, 1e-3 in default.
%   rho: increasing ratio of the penalty parameter mu, usually 1.05.
%   maxInterIN: maximum iteration number of inner loop, usually 100.
%   signM: if M>= 0, then signM = 1, otherwise, signM = 0;
%Output:
%   M_est: m*n full matrix, such that M_est = U_est*V_est
%   U_est: m*r matrix
%   V_est: r*n matrix
%   L1_error: the L1-norm error of observed data only.
%% Normalization
scale = max(max(abs(M)));
M = M/scale;

%% In-default parameters
[m n] = size(M); %matrix dimension
if nargin < 6
    maxIterIN = 100;
end
if nargin < 5
    rho = 1.05;
end
if nargin < 4
    lambda = 1e-3;
end
if nargin < 3
    disp('Please input the data matrix M, the indicator W and the rank r, and try again.');
end

maxIterOUT = 5000;
max_mu = 1e20;
mu = 1e-6;
M_norm = norm(M,'fro');
tol = 1e-8*M_norm;

cW = ones(size(W)) - W; %the complement of W.
display = 1; %display progress
%% Initializing optimization variables as zeros
E = zeros(m,n);
U = zeros(m,r);
V = zeros(r,n);
Y = zeros(m,n); %lagrange multiplier
%% Start main outer loop
iter_OUT = 0;
objs=[];
while iter_OUT < maxIterOUT
    iter_OUT = iter_OUT + 1;

    itr_IN = 0;
    obj_pre = 1e20;
    %start inner loop
    while itr_IN < maxIterIN
        %update U
        temp = (E + Y/mu)*V';

        [Us,sigma,Ud] = svd(temp,'econ'); % stable
        %[Us,sigma,Ud] = svdecon(temp); % fastest

        U = Us*Ud';

        %update V
        temp = U'*(E + Y/mu);

        [Vs,sigma,Vd] = svd(temp,'econ'); % stable
        %[Vs,sigma,Vd] = svdecon(temp); % fastest

        sigma = diag(sigma);
        svp = length(find(sigma > lambda/mu));
        if svp >= 1
            sigma = sigma(1:svp) - lambda/mu;
        else
            svp = 1;
            sigma = 0;
        end
        V = Vs(:,1:svp)*diag(sigma)*Vd(:,1:svp)';
        sigma0 = sigma;

        UV = U*V;

        %update E
        temp1 = UV - Y/mu;
        temp = M-temp1;
        E = max(0,temp - 1/mu) + min(0,temp + 1/mu);
        E = (M-E).*W + temp1.*cW;

        if signM > 0
            E(E<0) = 0;
        end

        %evaluate current objective
        obj_cur = sum(sum(abs(W.*(M-E)))) + lambda*sum(sigma0) + sum(sum(Y.*(E-UV))) + mu/2*norm(E-UV,'fro')^2;

        %check convergence of inner loop
        if abs(obj_cur - obj_pre) <= 1e-8*abs(obj_pre)
            break;
        else
            obj_pre = obj_cur;
            itr_IN = itr_IN + 1;
        end
    end

    leq = E - UV;
    stopC = norm(leq,'fro');
    if display
        obj = sum(sum(abs(W.*(M-UV)))) + lambda*sum(sigma0);
        objs = [objs,obj];
    end
    if display && (iter_OUT==1 || mod(iter_OUT,50)==0 || stopC<tol)
        disp(['iter ' num2str(iter_OUT) ',mu=' num2str(mu,'%2.1e') ...
            ',obj=' num2str(obj) ',stopALM=' num2str(stopC,'%2.3e')]);
    end
    if stopC<tol
        break;
    else
        %update lagrage multiplier
        Y = Y + mu*leq;
        %update penalty parameter
        mu = min(max_mu,mu*rho);
    end
end

%% Denormalization
U_est = sqrt(scale)*U; V_est = sqrt(scale)*V;
M_est = U_est*V_est;
L1_error = sum(sum(abs(W.*(scale*M-M_est))));

end


function [ s, seed ] = polygon_sample ( nv, v, n, seed )

%*****************************************************************************80
%
%% POLYGON_SAMPLE uniformly samples a polygon.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NV, the number of vertices.
%
%    Input, real V(2,NV), the vertices of the polygon, listed in
%    counterclockwise order.
%
%    Input, integer N, the number of points to create.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real S(2,N), the points.
%

%
%  Triangulate the polygon.
%
  triangles = polygon_triangulate ( nv, v(1,1:nv), v(2,1:nv) );
%
%  Determine the areas of each triangle.
%
  for i = 1 : nv - 2
    area_triangle(i) = triangle_area ( ...
      v(1,triangles(1,i)), v(2,triangles(1,i)), ...
      v(1,triangles(2,i)), v(2,triangles(2,i)), ...
      v(1,triangles(3,i)), v(2,triangles(3,i)) );
  end
%
%  Normalize the areas.
%
  area_polygon = sum ( area_triangle(1:nv-2) );
  area_relative(1:nv-2) = area_triangle(1:nv-2) / area_polygon;
%
%  Replace each area by the sum of itself and all previous ones.
%
  area_cumulative(1) = area_relative(1);
  for i = 2 : nv - 2
    area_cumulative(i) = area_relative(i) + area_cumulative(i-1);
  end

  for j = 1 : n
%
%  Choose triangle I at random, based on areas.
%
    [ area_percent, seed ] = r8_uniform_01 ( seed );

    for k = 1 : nv - 2

      i = k;

      if ( area_percent <= area_cumulative(k) )
        break
      end

    end
%
%  Now choose a point at random in triangle I.
%
    [ r, seed ] = r8vec_uniform_01 ( 2, seed );

    if ( 1.0 < sum ( r(1:2) ) )
      r(1:2) = 1.0 - r(1:2);
    end

    s(1:2,j) = ( 1.0 - r(1) - r(2) ) * v(1:2,triangles(1,i)) ...
                     + r(1)          * v(1:2,triangles(2,i)) ...
                            + r(2)   * v(1:2,triangles(3,i));
  end

  return
end


function [x, hist] = chordsol(x, f, tola, tolr)
%CHORDSOL  Chord method code for scalar equations.
%
% This code terminates on small relative-absolute errors
%
% function [X, HIST] = CHORDSOL(X, F, TOLA, TOLR)
%
% Inputs: X = initial iterate
%         F = function
%         TOLA = absolute error tolerance
%         TOLR = relative error tolerance
%
% Output: x=approximate result
%      HIST = array of iteration history, useful for tables and plots
%                The two columns are iteration number and residual.
%
%                If you leave this out by calling chordsol as
%                Z = CHORDSOL(X, F, TOLA, TOLR)
%                the storage for hist is not allocated and
%                the iteration history is not stored.
%
% Uses: fordiff.m, Tim's numerical derivative
%
%
maxit=20;    % maximum iterations
alpha=1.d-4;
small=1.d-8;
if nargout == 2
    histmp=zeros(maxit,2);
end
%
% Initialize
%

itc=0;
fc=feval(f,x);
fp=fordiff(f,x,fc);
tol=tolr*abs(fc)+tola;
%
%    Store iteration history?
%
if nargout == 2
    histmp(itc+1,:)=[itc, abs(fc)];
end
%
%    Main chord loop.
%
while(abs(fc) > tol)
%
%    Iteration becoming unbounded?
%
    if(abs(x) > 1.d9)
        disp(' iterate too large');
        if nargout == 2
            histmp(itc+1,:)=[itc,abs(fc)];
            hist=histmp(1:itc,:);
        end
    return
    end
%
%    Check for small relative derivatives.
%
    if(abs(fp) <= small*abs(fc))
        disp(' small derivative error');
    return;
    end
    s=-fc/fp;
    xt=x+s;
    ft=feval(f,xt);
    x=xt;
    itc=itc+1;
%
%    Too many iterations?
%
        if(itc > maxit)
            disp(' maxit reached');
            if nargout == 2
                histmp(itc+1,:)=[itc,abs(fc)];
                hist=histmp(1:itc,:);
            end
            return
        end
%
        fc=feval(f,x);
    if nargout == 2
        histmp(itc+1,:)=[itc,abs(fc)];
        end
    end
        if nargout == 2
        hist=histmp(1:itc+1,:);
        end
%
% A simple forward difference in MATLAB.
%
function fp = fordiff(f,x,fc)
        h=1.d-7;
%
%       Notice how f is evaluated using the feval command.
%
        fright=feval(f,x+h);
        fp=(fright-fc)/h;


function line_integrals_test01 ( )

%*****************************************************************************80
%
%% LINE_INTEGRALS_TEST01 compares exact and estimated monomial integrals.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2014
%
%  Author:
%
%    John Burkardt
%
  m = 1;
  n = 4192;
  test_num = 11;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LINE_INTEGRALS_TEST01\n' );
  fprintf ( 1, '  Use LINE01_SAMPLE to estimate integrals\n' );
  fprintf ( 1, '  along the length of the unit line in 1D.\n' );
%
%  Get sample points.
%
  seed = 123456789;
  [ x, seed ] = line01_sample ( n, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of sample points used is %d\n', n );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   E     MC-Estimate      Exact           Error\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    e = test - 1;

    value = monomial_value_1d ( n, e, x );

    result = line01_length ( ) * sum ( value(1:n) ) / n;
    exact = line01_monomial_integral ( e );
    error = abs ( result - exact );

    fprintf ( 1, '  %2d  %14.6g  %14.6g  %10.2e\n', ...
      e, result, exact, error );

  end

  return
end


function [y] = favar_transx(x,tcode)
%% Transform  x
% Stock & Watson (2016)
%   Return Series with same dimension and corresponding dates
%   Missing values where not calculated
%   -- Tcodes:
%            1 Level
%            2 First Difference
%            3 Second Difference
%            4 Log-Level
%            5 Log-First-Difference
%            6 Log-Second-Difference
% 		     7 First difference of percent change: (x(t)/x(t-1)-1)-(x(t-1)/x(t-2)-1), FRED-MD data (McCracken & Ng)
%

small=1.0e-06;

n=size(x,1);
y=NaN*zeros(n,1);

 if tcode==1
  y=x;

 elseif tcode==2
  y(2:n)=x(2:n)-x(1:n-1);

 elseif tcode==3
  y(3:n)=x(3:n)-2*x(2:n-1)+x(1:n-2);

 elseif tcode==4
  if min(x) < small
   y=NaN;
  else
   x=log(x);
   y=x;
  end

 elseif tcode==5
  if min(x) < small
   y = NaN;
  else
   x=log(x);
   y(2:n)=x(2:n)-x(1:n-1);
  end

 elseif tcode==6
  if min(x) < small
   y = NaN;
  else
   x=log(x);
   y(3:n)=x(3:n)-2*x(2:n-1)+x(1:n-2);
  end

  elseif tcode==7
  if min(x) < small
   y = NaN;
  else
   y1(2:n)=(x(2:n)-x(1:n-1))./x(1:n-1);
   y(3:n)=y1(3:n)-y1(2:n-1);
  end
 else
  y = NaN;
 end


function xhat = kalman(z, A, C, R, Q)
% Simple Kalman Filter (linear) with optimal gain, no control signal
%
% z Measurement signal              m observations X # of observations
% A State transition model          n X n, n = # of state values
% C Observation model               m X n
% R Covariance of observation noise m X m
% Q Covariance of process noise     n X n
%
% Based on http://en.wikipedia.org/wiki/Kalman_filter, but matrices named
% A, C, G instead of F, H, K.
%
% See http://home.wlu.edu/~levys/kalman_tutorial for background
%
% Copyright (C) 2014 Simon D. Levy
%
% This code is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of the
% License, or (at your option) any later version.
%
% This code is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with this code. If not, see <http:#www.gnu.org/licenses/>.

% Initializtion =====================================================

% Number of sensors
m = size(C, 1);

% Number of state values
n = size(C, 2);

% Number of observations
numobs = size(z, 2);

% Use linear least squares to estimate initial state from initial
% observation
xhat = zeros(n, numobs);
xhat(:,1) = C \ z(:,1);

% Initialize P, I
P = ones(size(A));
I = eye(size(A));

% Kalman Filter =====================================================

for k = 2:numobs

    % Predict
    xhat(:,k) = A * xhat(:,k-1);
    P         = A * P * A' + Q;

    % Update
    G         = P  * C' / (C * P * C' + R);
    P         = (I - G * C) * P;
    xhat(:,k) = xhat(:,k) + G * (z(:,k) - C * xhat(:,k));

end


function b = bernsteincop(x,g)
%BERNSTEINCOP Bernstrein Empirical copula based on sample X.
%   B = BERNSTEINCOP(X,G) returns bivariate bernstein empirical copula
%   for GxG points grid. Extension to n dimensional bernstein empirical
%   copula is straightforward.
%
%   Written by Robert Kopocinski, Wroclaw University of Technology,
%   for Master Thesis: "Simulating dependent random variables using copulas.
%   Applications to Finance and Insurance".
%   Date: 2007/05/12
%
%   Reference:
%      [1]  Durrleman, V. and Nikeghbali, A. and Roncalli, T. (2000) Copulas approximation and
%           new families, Groupe de Recherche Operationnelle Credit Lyonnais

c = ecopula(x);

[m n] = size(c);

b=zeros(g);

for i=1:g
    for j=1:g
        b(i,j)=sum(sum( bernstein(i/g,1:m,m)'*(bernstein(j/g,1:m,m)).*c ));
    end
end

%------------------------------------------------------------
function y = bernstein(u,i,n)
% BERNSTEIN calculates values of Bernstein polynomials.

y = factorial(n)./factorial(i)./factorial(n-i).*u.^i.*(1-u).^(n-i);


% Demonstration of the degree elevation algorithm.
%

crv = nrbtestcrv;

% plot the control points
plot(crv.coefs(1,:),crv.coefs(2,:),'bo')
title('Degree elevation of test curve by 1');
hold on;
plot(crv.coefs(1,:),crv.coefs(2,:),'b--');

% draw nurbs curve
nrbplot(crv,48);

% degree elevate the curve by 1
icrv = nrbdegelev(crv, 1);

% insert new knots and plot new control points
plot(icrv.coefs(1,:),icrv.coefs(2,:),'ro')
plot(icrv.coefs(1,:),icrv.coefs(2,:),'r--');

hold off;


%MINENCLOSINGCIRCLE  Finds a circle of the minimum area enclosing a 2D point set
%
%     [center,radius] = cv.minEnclosingCircle(points)
%
% ## Input
% * __points__ Input vector of 2D points, stored in numeric array
%   (Nx2/Nx1x2/1xNx2) or cell array of 2-element vectors (`{[x,y], ...}`).
%
% ## Output
% * __center__ Output center of circle `[x,y]`.
% * __radius__ Output radius of circle `r`.
%
% The function finds the minimal enclosing circle of a 2D point set using an
% iterative algorithm.
%
% See also: cv.minEnclosingTriangle
%


% Jason Joseph Rebello
% Carnegie Mellon University (Jan 2012 - May 2013)
% MS in Electrical & Computer Engineering
% Principal Component Analysis

% This program uses Principal Component Analysis to reduce the number
% of features used in face recognition. This program allows you to set K
% if you know the number of Principal components needed or calculates K
% based on how much variance you would like to preserve in the images.
% Note: K has been commented out since I am calculating K based on
% variance.

clear all;
clc;
close all;

fprintf('Principal Component Analysis used for Face Recognition\n\n');
t = cputime;

%% Initializing Variable

fprintf('Initializing Variables');
%K = 100; % Number of reduced features (principal components)
dispFaces = 12; % How many faces to display
variance = 90; % Enter a number between 1 and 99 both inclusive based on
                % how much variance to preserve.
topeig = 12; % Used to display the top 10 eigen vectors found
fprintf('...done\n\n');

%% Loading and Visualizing data

fprintf('Loading and Visualizing data');
load('faces.mat');
displayData(X(1:dispFaces, :)); % function taken from Machine Learning course
title('Original faces')         % by Prof Andrew Ng.
fprintf('...done\n\n');
pause(1);

%% Normalizing features

fprintf('Normalizing features');
[X mu stddev]  = normalizeFeatures(X);
fprintf('...done\n\n');

%% Perform PCA to get eigen vectors and eigen values

fprintf('Performing PCA & displaying top %d eigen vectors found', topeig);
[U,S] = performPCA(X);
displayData(U(:, 1:topeig)');
title('Top Eigen Vecotrs found');
fprintf('...done\n\n');
pause(2);

%% Calculate K based on variance

fprintf('Find best value of K based on Variance');
K = findK(S, variance);
fprintf('...done\n\n');

%% Get data with reduced features

fprintf('Displaying data with reduced features');
reducedData = reducedFeatures(X, U, K);
fprintf('...done\n\n');

%% Recover Original Data

fprintf('Recovering Original Data from reduced features');
XRecovered = recoverData(reducedData, U, K);
fprintf('...done\n\n');

%% Display Original and Recovered Data

fprintf('Displaying Original and Recovered Data');
show(X, XRecovered, dispFaces);
fprintf('...done\n\n');

fprintf('Program executed in %f seconds or %f minutes\n\n', cputime-t, ...
                                            (cputime-t)/60);
pause(1);


function theVec=vech(H,offDiagScalFact,omitDiagonal)
%%VECH The vector half operator. Given a square 2D matrix H, this returns
%      the entries that are on or below the main diagonal of H, listed
%      column-by-column, unless omitDiagonal=true, in which case the main
%      diagonal is omitted too.
%
%INPUTS: H An nXn matrix. It can be real or complex.
% offDiagScalFact An optional scalar factor by which the off-diagonal
%          elements are scaled. If this parameter is omitted or an empty
%          matrix is passed, then the default value of 1 is used.
% omitDiagonal An optional parameter indicating whether the elements on the
%          main diagonal should be omitted. The default if omitted or an
%          empty matrix is passed is false.
%
%OUTPUTS: theVec A (n*(n+1)/2)X1 matrix holding the lower-triangular part
%                of the matrix, listed column-by-column. If omitDiagonal is
%                true, then this is a (n*(n+1)/2-n)X1 vector, as the
%                elements on the main diagonal were omitted.
%
%The opposite of this function is vech2Mat.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(omitDiagonal))
    omitDiagonal=false;
end

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

n=size(H,1);

if(omitDiagonal)
    numEls=n*(n+1)/2-n;
    theVec=zeros(numEls,1);
    if(numEls==0)
        return;
    end
    curStart=1;
    for curCol=1:n
        %Off-diagonal elements
        span=(curStart):(curStart+n-curCol-1);
        theVec(span)=H((curCol+1):end,curCol)*offDiagScalFact;

        curStart=curStart+n-curCol;
    end
else
    theVec=zeros(n*(n+1)/2,1);

    curStart=1;
    for curCol=1:n
        %The diagonal element
        theVec(curStart)=H(curCol,curCol);

        %Off-diagonal elements
        span=(curStart+1):(curStart+n-curCol);
        theVec(span)=H((curCol+1):end,curCol)*offDiagScalFact;

        curStart=curStart+n-curCol+1;
    end
end
end

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


function f = flops_mul(n,m,c)
% FLOPS_MUL    Flops for real matrix multiplication.
% FLOPS_MUL(a,b) returns the number of flops for a*b.
% FLOPS_MUL(n,m,c) returns the number of flops for ones(n,m)*ones(m,c).
% This is more accurate than the flopcount returned by matlab.

if nargin == 2
  a = n;
  b = m;
  n = rows(a);
  m = cols(a);
  c = cols(b);
end
f = n*c*(2*m-1);


function [mappedX, mapping] = npe(X, no_dims, k, eig_impl)
%NPE Perform the Neighborhood Preserving Embedding algorithm
%
%       [mappedX, mapping] = npe(X, no_dims, k)
%       [mappedX, mapping] = npe(X, no_dims, k, eig_impl)
%
% Runs the Neighborhood Preserving Embedding algorithm on dataset X to
% reduce it to dimensionality no_dims. The number of neighbors that is used
% by LPP is specified by k (default = 12).
%
%

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

    if size(X, 2) > size(X, 1)
        error('Number of samples should be higher than number of dimensions.');
    end
    if ~exist('no_dims', 'var')
        no_dims = 2;
    end
    if ~exist('k', 'var')
        k = 12;
    end
    if ~exist('eig_impl', 'var')
        eig_impl = 'Matlab';
    end

    % Get dimensionality and number of dimensions
    [n, d] = size(X);
    mapping.mean = mean(X, 1);

    % Compute pairwise distances and find nearest neighbours (vectorized implementation)
    disp('Finding nearest neighbors...');
    [distance, neighborhood] = find_nn(X, k);
    max_k = size(neighborhood, 2);
    if nargout > 1
        mapping.nbhd = distance;
    end
    X = X';
    neighborhood = neighborhood';

    % Find reconstruction weights for all points by solving the MSE problem
    % of reconstructing a point from each neighbours. A used constraint is
    % that the sum of the reconstruction weights for a point should be 1.
    disp('Compute reconstruction weights...');
    if k > d
        tol = 1e-5;
    else
        tol = 0;
    end

    % Construct reconstruction weight matrix
    W = zeros(max_k, n);
    for i=1:n
        nbhd = neighborhood(:,i);
        if ischar(k)
           nbhd = nbhd(nbhd ~= 0);
        end
        kt = numel(nbhd);
        z = X(:,nbhd) - repmat(X(:,i), 1, kt);                  % Shift point to origin
        C = z' * z;												% Compute local covariance
        C = C + eye(kt, kt) * tol * trace(C);					% Regularization of covariance (if K > D)
        wi = C \ ones(kt, 1);                                   % Solve linear system
        wi = wi / sum(wi);                                      % Make sure that sum is 1
        W(:,i) = [wi; nan(max_k - kt, 1)];
    end

    % Now that we have the reconstruction weights matrix, we define the
    % sparse cost matrix M = (I-W)'*(I-W).
    M = sparse(1:n, 1:n, ones(1, n), n, n, 4 * max_k * n);
    for i=1:n
       w = W(:,i);
       w(isnan(w)) = 0;
       j = neighborhood(:,i);
       w = w(j ~= 0);
       j = j(j ~= 0);
       M(i, j) = M(i, j) - w';
       M(j, i) = M(j, i) - w;
       M(j, j) = M(j, j) + w * w';
    end

	% For sparse datasets, we might end up with NaNs or Infs in M. We just set them to zero for now...
	M(isnan(M)) = 0;
	M(isinf(M)) = 0;

    % Compute XWX and XX and make sure these are symmetric
    X = X';
    WP = X' * M * X;
    DP = X' * X;
    DP = (DP + DP') / 2;
    WP = (WP + WP') / 2;

    % Solve generalized eigenproblem
    if size(X, 1) > 1500 && no_dims < (size(X, 1) / 10)
        if strcmp(eig_impl, 'JDQR')
            options.Disp = 0;
            options.LSolver = 'bicgstab';
            [eigvector, eigvalue] = jdqz(WP, DP, no_dims, 'SA', options);
        else
            options.disp = 0;
            options.issym = 1;
            options.isreal = 0;
            [eigvector, eigvalue] = eigs(WP, DP, no_dims, 'SA', options);
        end
    else
        [eigvector, eigvalue] = eig(WP, DP);
    end

    % Sort eigenvalues in descending order and get smallest eigenvectors
    [eigvalue, ind] = sort(diag(eigvalue), 'ascend');
    eigvector = eigvector(:,ind(1:no_dims));

    % Compute final linear basis and map data
    mappedX = X * eigvector;
    mapping.M = eigvector;


function [Q,w,S] = quadrature_points(V,F,k)
  % QUADRATURE_POINTS Sample quadrature points on a triangle or tetrahedron.
  %
  % Q = quadrature_points(V,F)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by simplex-size list of indices into V
  %   k  number of quadrature points {1,3,6}
  % Outputs:
  %   Q  #F by dim by k list of quadrature points
  %   w  #k list of weights
  %   S  #F*k by #V sparse matrix so that Q ~= S*V (~= means after
  %     reordering)
  %
  % Example:
  %   Q = quadrature_points(V,F);
  %   QQ = reshape(permute(Q,[1 3 2]),[],3);
  %   QQQ = permute(reshape(QQ,permute(size(Q),[1 3 2])),[1 3 2]);
  %   max(abs(Q-QQQ))
  %
  % See also: barycenter
  %

  % Compute element quadrature points
  % "A SET OF SYMMETRIC QUADRATURE RULES ON TRIANGLES AND TETRAHEDRA", Table 2.2
  dim = size(V,2);
  ss = size(F,2);
  switch ss
  case 3
    switch k
    case 1
      lambda = [1/3 1/3 1/3];
    case 3
      %a = 0.5;
      a = 0.15;
      lambda = unique(perms([a a 1-2*a]),'rows');
    case 6
      a = 0.05;
      b = 0.1;
      lambda = unique(perms([a b 1-a-b]),'rows');
    otherwise
    error(sprintf('Unsupported number of quadratures %d',k));
    end
    % uniform weights
    w = ones(size(lambda,1),1)/size(lambda,1);
    QQ = cat(4,V(F(:,1),:),V(F(:,2),:),V(F(:,3),:));
  otherwise
    error(sprintf('Unsupported simplex-size %d',ss));
  end

  % #F by #lambda matrix of indices of all of the new points
  I = ((1:size(F,1))') + (0:size(lambda,1)-1)*size(F,1);
  S = sparse( ...
      repmat(I,size(F,2),1), ...
      repmat(F(:),1,size(lambda,1)), ...
      reshape(permute(repmat(lambda,[1 1 size(F,1)]),[3 2 1]),numel(F),size(lambda,1)), ...
      size(F,1)*size(lambda,1), ...
      size(V,1));
  % Q2 = S*V;

  lambda = permute(lambda,[3 4 1 2]);
  Q = sum(bsxfun(@times,QQ,lambda),4);

  % The Q ordering is silly. S*V ~= Q after gnarly reordering
  assert(max(max(max(abs(Q-permute(reshape(S*V,[size(Q,1) size(Q,3) size(Q,2)]),[1 3 2])))))<1e-12)

end


function [L_b,lambda_b,h_b,v_eb_n,C_b_n] = ECEF_to_NED(r_eb_e,v_eb_e,C_b_e)
%ECEF_to_NED - Converts Cartesian  to curvilinear position, velocity
%resolving axes from ECEF to NED and attitude from ECEF- to NED-referenced
%
% Software for use with "Principles of GNSS, Inertial, and Multisensor
% Integrated Navigation Systems," Second Edition.
%
% This function created 2/4/2012 by Paul Groves
%
% Inputs:
%   r_eb_e        Cartesian position of body frame w.r.t. ECEF frame, resolved
%                 along ECEF-frame axes (m)
%   v_eb_e        velocity of body frame w.r.t. ECEF frame, resolved along
%                 ECEF-frame axes (m/s)
%   C_b_e         body-to-ECEF-frame coordinate transformation matrix
%
% Outputs:
%   L_b           latitude (rad)
%   lambda_b      longitude (rad)
%   h_b           height (m)
%   v_eb_n        velocity of body frame w.r.t. ECEF frame, resolved along
%                 north, east, and down (m/s)
%   C_b_n         body-to-NED coordinate transformation matrix

% Parameters
R_0 = 6378137; %WGS84 Equatorial radius in meters
% e = 0.0818191908425; %WGS84 eccentricity

ecc2=0.006694380022900;

% Copyright 2012, Paul Groves
% License: BSD; see license.txt for details

% Begins

% Convert position using Borkowski closed-form exact solution
% From (2.113)
lambda_b = atan2(r_eb_e(2),r_eb_e(1));

% From (C.29) and (C.30)
k1 = sqrt(1 - ecc2) * abs (r_eb_e(3));
k2 = ecc2 * R_0;
beta = sqrt(r_eb_e(1)^2 + r_eb_e(2)^2);
E = (k1 - k2) / beta;
F = (k1 + k2) / beta;

% From (C.31)
P = 4/3 * (E*F + 1);

% From (C.32)
Q = 2 * (E^2 - F^2);

% From (C.33)
D = P^3 + Q^2;

% From (C.34)
V = (sqrt(D) - Q)^(1/3) - (sqrt(D) + Q)^(1/3);

% From (C.35)
G = 0.5 * (sqrt(E^2 + V) + E);

% From (C.36)
T = sqrt(G^2 + (F - V * G) / (2 * G - E)) - G;

% From (C.37)
L_b = sign(r_eb_e(3)) * atan((1 - T^2) / (2 * T * sqrt (1 - ecc2)));

% From (C.38)
h_b = (beta - R_0 * T) * cos(L_b) +...
    (r_eb_e(3) - sign(r_eb_e(3)) * R_0 * sqrt(1 - ecc2)) * sin (L_b);

% Calculate ECEF to NED coordinate transformation matrix using (2.150)
cos_lat = cos(L_b);
sin_lat = sin(L_b);
cos_long = cos(lambda_b);
sin_long = sin(lambda_b);
C_e_n = [-sin_lat * cos_long, -sin_lat * sin_long,  cos_lat;...
                   -sin_long,            cos_long,        0;...
         -cos_lat * cos_long, -cos_lat * sin_long, -sin_lat];

% Transform velocity using (2.73)
v_eb_n = C_e_n * v_eb_e;

% Transform attitude using (2.15)
C_b_n = C_e_n * C_b_e;

% Ends


function [cg, psg] = crandn(rgau,m)
% Generate correlated Gaussian sequences by Fourier synthesis.
%
% Input parameters:
% rgau = correlation function - length n/2
% m    = number of realisations
%
% Output:
% cg   = m x n matrix containing m sequences of n correlated variates from
%        a zero mean, unit variance normal distribution
% psg  = input power spectrum (Fourier transform of correlation function)
%
% Note: Since this uses the fast Fourier transform, it will be fastest if
% n is a power of 2.
%
% S Bocquet 12 August 2008  Tested with MATLAB Version 7.6.0.324 (R2008a)
%
if ~isvector(rgau)
  error('crandn:vector','Correlation function must be a vector')
end
if ((rgau(1) ~= 1) || (max(abs(rgau)) > 1))
  error('crandn:cfnorm','Correlation function not properly normalised');
end
if abs(rgau(end)) > 0.01
  warning('crandn:cfdecay','Correlation function does not go to zero')
end
n = 2*length(rgau);
rgau2 = [rgau(1:end),0,rgau(end:-1:2)]; % two sided correlation function
psg = abs(fft(rgau2)); % Fourier transform of correlation function
nrm = mean(psg)/n; % normalisation
h = sqrt(psg/nrm); % normalised filter function
cg = zeros(n,m);
for i=1:m
  if mod(i,2)==1
    g = complex(randn(1,n),randn(1,n));
    cg2 = ifft(g.*h); % correlated Gaussian process
    cg(:,i) = real(cg2);
  else
    cg(:,i) = imag(cg2);
  end
end
end