function [ Gnew] = gsp_graph_sparsify(G,epsilon)
%GSP_GRAPH_SPARSIFY sparsify a graph using Spielman-Srivastava algorithm
%   Usage: Gnew = gsp_graph_sparsify(G,epsilon);
%
%   Input parameters
%       G       : Graph structure or Laplacian matrix
%       epsilon : Sparsification parameter
%
%   Ouput parameters:
%       Gnew    : New sparsified graph or new laplacian
%
%   This function sparsifies a graph using Spielman-Srivastava algorithm.
%   Note that *epsilon* should be between $1/\sqrt(N)$ and $1$.
%
%   Example:::
%
%         epsilon = 0.5;
%         param.distribute = 1;
%         nnparam.k = 20;
%         param.nnparam = nnparam;
%         G = gsp_sensor(256,param);
%         G2 = gsp_graph_sparsify(G,epsilon);
%         figure(1)
%         gsp_plot_graph(G);
%         title('Original graph')
%         figure(2)
%         gsp_plot_graph(G2);
%         title('Sparsified graph')
%
%   References: spielman2011graph rudelson1999random rudelson2007sampling

% Author: David Shuman, Nathanael Perraudin
% Date  : 22 June 2014
% Testing: test_sparsify

% Test the input parameters
if isstruct(G)
    if isfield(G,'lap_type')
        if ~strcmp(G.lap_type,'combinatorial')
            error('Not implemented yet');
        end
    end
    L = G.L;
else
    L = G;
end

N=size(L,1);

if N<3
    error('GSP_GRAPH_SPARSIFY: Cannot sparsify a graph with less than 3 nodes');
end

% Epsilon should be between 1/sqrt(N) and 1, with a larger epsilon leading to a sparser graph
if ( (epsilon <= 1/sqrt(N)) || (epsilon >1) )
    error('GSP_GRAPH_SPARSIFY: Epsilon out of required range');
end

% Compute resistance distances
resistance_distances = gsp_resistance_distances(L);

% Get the weight matrix, and check if the original graph is connected
if isstruct(G)
    W = G.W;
else
    W = diag(diag(L)) - L;
end
W(W<1e-10) = 0;
W = sparse(W);
original_connected=gsp_check_connectivity_undirected(W);
if (original_connected==0)
    warning('Original graph not connected before sparsification');
end

% Initialize the probability distribution that will be used to select edges in the sparsified graph
[start_nodes,end_nodes,weights]=find(tril(W));
weights=max(0,weights);
Re=max(0,resistance_distances(sub2ind(size(resistance_distances),start_nodes,end_nodes)));
Pe=weights.*Re;
Pe=Pe/sum(Pe);

max_tries=10; % maximum number of tries to get a connected graph

for i=1:max_tries

    % Set Q
    C0=1/30; % Rudelson, 1996 Random Vectors in the Isotropic Position (too hard to figure out actual C0)
    C= 4*C0; % Rudelson and Vershynin, 2007, Thm. 3.1
    q=round(9*C^2*N*log(N)/(epsilon^2));

    % Choose random edges in the new graph according the probability
    % distribution initialized above
    results=gendist(Pe',q,1);
    spin_counts=hist(results,1:length(Pe'));
    per_spin_weights=weights./(q*Pe);

    % Tally the new weights and form the new graph
    new_weights=spin_counts'.*per_spin_weights;

    sparserW=sparse(start_nodes,end_nodes,new_weights,N,N);
    sparserW=sparserW+sparserW';
    sparserL=diag(sum(sparserW))-sparserW;
    sparserA=sign(sparserW);

    % Check if new graph is connected. If not, reduce epsilon and try again
    new_graph_connected=gsp_check_connectivity_undirected(sparserA);
    if new_graph_connected
        break;
    elseif i==max_tries
        warning('Despite attempts to reduce epsilon, sparsified graph is disconnected');
    else
        epsilon=epsilon-(epsilon-1/sqrt(N))/2;
    end
end

if isstruct(G)
    if ~G.directed
        sparserW = (sparserW + sparserW')/2;
        sparserL = (sparserL + sparserL')/2;
    end
    Gnew=G;
    Gnew.W = sparserW;
    Gnew.L = sparserL;
    Gnew.A = sparserA;
    Gnew.d = diag(sparserL);
    Gnew.Ne = nnz(Gnew.W)/2;
else
    Gnew = sparse(sparserL);
end

end


function [ x, w ] = square_symq ( degree, n )

%*****************************************************************************80
%
%% SQUARE_SYMQ returns a symmetric quadrature rule for the square.
%
%  Discussion:
%
%    This procedure returns a quadrature rule for smooth functions
%    on the unit square [-1,1]^2.
%
%    All quadratures are accurate to 15 digits
%    All weights are positive and inside the square
%
%    The nodes are symmetrically arranged.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer DEGREE, the degree of the quadrature rule.
%    1 <= DEGREE <= 20.
%
%    Input, integer N, the number of nodes.
%    This can be determined by a call to RULE_FULL_SIZE(DEGREE).
%
%    Output, real X(2,N), the coordinates of the nodes.
%
%    Output, real W(N), the weights.
%
  if ( degree == 1 )
    [ x, w ] = rule01 ( n );
  elseif ( degree == 2 )
    [ x, w ] = rule02 ( n );
  elseif ( degree == 3 )
    [ x, w ] = rule03 ( n );
  elseif ( degree == 4 )
    [ x, w ] = rule04 ( n );
  elseif ( degree == 5 )
    [ x, w ] = rule05 ( n );
  elseif ( degree == 6 )
    [ x, w ] = rule06 ( n );
  elseif ( degree == 7 )
    [ x, w ] = rule07 ( n );
  elseif ( degree == 8 )
    [ x, w ] = rule08 ( n );
  elseif ( degree == 9 )
    [ x, w ] = rule09 ( n );
  elseif ( degree == 10 )
    [ x, w ] = rule10 ( n );
  elseif ( degree == 11 )
    [ x, w ] = rule11 ( n );
  elseif ( degree == 12 )
    [ x, w ] = rule12 ( n );
  elseif ( degree == 13 )
    [ x, w ] = rule13 ( n );
  elseif ( degree == 14 )
    [ x, w ] = rule14 ( n );
  elseif ( degree == 15 )
    [ x, w ] = rule15 ( n );
  elseif ( degree == 16 )
    [ x, w ] = rule16 ( n );
  elseif ( degree == 17 )
    [ x, w ] = rule17 ( n );
  elseif ( degree == 18 )
    [ x, w ] = rule18 ( n );
  elseif ( degree == 19 )
    [ x, w ] = rule19 ( n );
  elseif ( degree == 20 )
    [ x, w ] = rule20 ( n );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SQUARESYMVQ - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of DEGREE.\n' );
    error ( 'SQUARE_SYMQ - Fatal error!' );
  end

  w_sum = sum ( w(1:n) );

  w(1:n) = 4.0 * w(1:n) / w_sum;

  return
end


function [oa ua pa K confu]=confusion(true_label,estim_label)
%
% function confusion(true_label,estim_label)
%
% This function compute the confusion matrix and extract the OA, AA
% and the Kappa coefficient.
%
% INPUT
% easy! just read

l=length(true_label);
nb_c=max(true_label);

confu=zeros(nb_c,nb_c);

for i=1:l
  confu(estim_label(i),true_label(i))= confu(estim_label(i),true_label(i))+1;
end

oa=trace(confu)/sum(confu(:)); %overall accuracy
ua=diag(confu)./sum(confu,2);  %class accuracy
pa=diag(confu)./sum(confu,1)';

Po=oa;
Pe=(sum(confu)*sum(confu,2))/(sum(confu(:))^2);

K=(Po-Pe)/(1-Pe);%kappa coefficient

%http://kappa.chez-alice.fr/kappa_intro.htm


function nScatt = diracnorm_scat(x,cascade)
	% Calculation of a normalized scattering representation
	% nScatt is an array of size 2N which stores a
	% piecewise constant signal giving
	% the 2^J normalized values || SJ [p]f||/||SJ[p] delta||
	% over intervals of lengths proportional to ||SJ[p] delta||^2 .
	% Normalized scattering transforms are defined in the paper
	% "Group Invariant Scattering", S. Mallat, http://arxiv.org/abs/1101.2286

	N=size(x,1); % N must be an integer multiplied by a very high power of 2
	%Compute the scattering transform of the signal x and output it in a table
	Sx=scat(x,cascade);

	%Prepare the dirac signal
	dirac = zeros(N,1);
	dirac(N/2) = 1;
	%Compute its scattering transform and output it in the form of a table

	Sdirac=scat(dirac, cascade);
	%%
	%fprintf('scat time\n');
	[Scatt order2]=reorder_scat(Sx);

	%%
	nSdirac=reorder_scat(Sdirac);

	%%
	%fprintf('reordering time\n');

	lsig=2*N/2^(length(Sx{2}.signal));

	for k=0:lsig:2*N-lsig

		normd=norm(nSdirac(k+1:k+lsig));
		normx=norm(Scatt(k+1:k+lsig))/normd;

		% the order is coded in the smallest digits of normx
		normx = 10^(-6) * floor(normx * 10^(6));
		Scatt(k+1:k+lsig) = normx + order2(k+1)* 10^(-7);

		nSdirac(k+1:k+lsig)=normd^2;

	end

	C=sum(nSdirac)/lsig;

	p=0;
	for k=0:lsig:2*N-lsig
		step=nSdirac(k+1) * 2 * N/C;

		nScatt(floor(p+1):round(p+step))=Scatt(k+1);
		p=p+step;
	end

end


function [rrho, xxvec, yyvec] = density(x, y, dx, dy, xy0);
% [rho, xvec, yvec] = density(x, y, dx, dy, [x0 x1 y0 y1]);
%
%  2-D probability density plot.
%
%  Input:  Vectors x and y of equal length. If dx and/or dy are omitted, the
%          default is 75 bins in each direction.
%
%          If dx or dy is negative, then the variable is taken as the number of
%          bins rather than a grid resolution.
%
%          The vector containing the limits can be padded with NaNs if only
%          certain limits are desired, e g if x0 and y1 are wanted:
%
%            density(x, y, [.5 nan nan 45])
%
%  Output: The density matrix RHO together with vectors XVEC and YVEC.
%          If no output arguments are specified, DENSITY will plot the density
%          function with the prescribed axes using PCOLOR.
%
%  Requires bin.m. Tested under MatLab 4.2, 5.0, and 5.1.
%
%  See also bin.m for further details about dx, x0, etc, and ffgrid.m.
%

% 1.9.97 Oyvind.Breivik@gfi.uib.no.
%
% Oyvind Breivik
% Department of Geophysics
% University of Bergen
% NORWAY

DX = -75;  % Default grid size

x = x(:);

if nargin < 2
 y = x;
end

y = y(:);

xy = NaN*ones(1,4);

if (nargin < 4)
 xy0 = min(x);
end

if (nargin == 3 & length(dx) > 1)
 xy0 = dx;
 dx = DX;
end

if nargin < 3
 dx = DX;
end

if (nargin == 4 & length(dy) > 1)
 xy0 = dy;
 dy = dx;
end
if nargin < 4
 dy = dx;
end

nxy = length(xy0);
xy(1:nxy) = xy0;

if (isnan(xy(4)))
 xy(4) = max(y);
end
if (isnan(xy(3)))
 xy(3) = min(y);
end
if (isnan(xy(2)))
 xy(2) = max(x);
end
if (isnan(xy(1)))
 xy(1) = min(x);
end
x0 = xy(1); x1 = xy(2); y0 = xy(3); y1 = xy(4);

if (dx < 0)
 dx = (x1 - x0)/abs(dx);
end
if (dy < 0)
 dy = (y1 - y0)/abs(dy);
end

ix = bin(x, dx, x0, x1);
iy = bin(y, dy, y0, y1); % bin data in (x,y)-space

xvec = x0:dx:x1;
yvec = y0:dy:y1;

nx = length(xvec);
ny = length(yvec);

inx = (ix >= 1) & (ix <= nx);
iny = (iy >= 1) & (iy <= ny);
in = (inx & iny);
ix = ix(in); iy = iy(in);
N = length(ix); % how many datapoints are left now?

rho = zeros(length(xvec), length(yvec)) + eps;

for i = 1:N
 rho(ix(i), iy(i)) = rho(ix(i), iy(i)) + 1;
end

rho = rho/(N*dx*dy); % Density is n per dx per dy

rho = rho'; % Get in shape

if nargout == 0
 pcolor(xvec, yvec, rho)
 colorbar
 colormap jet
 xlabel(inputname(1))
 ylabel(inputname(2))
 dum = size(rho');
 str = sprintf('%d data points, grid: %dx%d', N, dum(1)-1, dum(2)-1);
 title(str);
end

if nargout > 0
 rrho = rho;
end

if nargout > 1
 xxvec = xvec;
 yyvec = yvec;
end


function spline_test11 ( )

%*****************************************************************************80
%
%% TEST11 tests BEZ_VAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author
%
%    John Burkardt
%
  n = 2;
  a = 0.0E+00;
  b = 1.0E+00;
  nsample = 21;
  y = [ 1.0E+00, 0.0E+00, 1.0E+00 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST11\n' );
  fprintf ( 1, '  BEZ_VAL evaluates a Bezier function.\n' );
%
%  One point on the curve should be (0.75, 20/32).
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  T, X(T), Y(T)\n' );
  fprintf ( 1, '\n' );

  for i = 1 : nsample

    x = ( ( nsample - i     ) * a   ...
        + (           i - 1 ) * b ) ...
        / ( nsample     - 1 );

    fprintf ( 1, '%8d  %14f  %14f\n', i, x, bez_val ( n, x, a, b, y ) );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  When X = 0.75E+00\n' );
  fprintf ( 1, '  BEZ_VAL(X) should be 0.625E+00.\n' );

  return
end


function result = p00_even ( prob, int_num )

%*****************************************************************************80
%
%% P00_EVEN uses evenly spaced points to integrate a function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer PROB, the problem index.
%
%    Input, integer INT_NUM, the number of sample points.
%
%    Output, real RESULT, the approximate integral.
%
  [ a, b ] = p00_lim ( prob );

  if ( int_num == 1 )
    x(1) = ( a + b ) / 2.0;
  else
    x(1:int_num) = ( ( int_num - (1:int_num)     ) * a    ...
                   + (           (1:int_num) - 1 ) * b  ) ...
                   / ( int_num               - 1 );
  end

  result = sum ( p00_fun ( prob, int_num, x(1:int_num) ) );

  result = ( b - a ) * result / int_num;

  return
end


function img = imvoronoi2d(varargin)
%IMVORONOI2D Generate a 2D voronoi image from a set of points
%
%   IMG = imvoronoi2d(DIM, POINTS);
%   where DIM is a 1*2 array containing the size of the image, and POINTS
%   is a N*2 array of double containing germs coordinate, build the voronoi
%   image corresponding to given germs.
%
%   IMG = imvoronoi2d(LX, LY, POINTS);
%   Specifes the coordinates of the pixels with the two row vectors LX and
%   LY.
%
%   imvoronoi2d(DIM, POINTS, BORDER)
%   Also specifies edge condition. BORDER can be one of:
%   'free'      does not modify result image
%   'periodic'  consider germs are repeated in each direction
%   'remove'    remove cells touching borders of image
%
%   IMG = imvoronoi2d(DIM, N);
%   Creates an image of size DIM (1*2 array), using N germs with uniform
%   distribution in the image space.
%
%   imvoronoi2d
%   without argument return a 100*100 image with 10 random points as germs.
%
%
%   Example:
%   img = imvoronoi2d([100 100], [20 50; 80 30;70 90]);
%   imshow(img);
%
%   See also
%     imShapes, imVoronoi3d, imPowerDiagram, dilatedVoronoi
%

%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 28/04/2004.
%

%   HISTORY
%   use faster algorithm for distance function (loop on points and not on
%   pixels).
%   2008/10/10 update doc, code clean up
%   2009/03/04 switch coordinate to comply with (x,y) of germs
%   29/05/2009 use more possibilities for specifying grid

%% extract input arguments

% default values
points      = [];       % points array
N           = 20;       % default number of germs
edgecond    = 'free';   % edge condition
conn        = 4;        % 4-connectivity by default in dimension 2

% compute coordinate of image voxels
[lx, ly, varargin] = parseGridArgs(varargin{:});

% extraction of points, or number of points
if ~isempty(varargin)
    var = varargin{1};

    if length(var)>1    % use an array of germs
        points = var;
    else                % get number of germs
        N = var;
    end
end

% extraction of edge condition
if length(varargin)>1
    edgecond = varargin{2};
end

%% initialisations

% size in each direction
Ny = length(ly);
Nx = length(lx);

% create array of points, if it does not exist
if isempty(points)
    % generation of germs array
    points = zeros(N, 2);
    for i=1:N
        points(i,1) = rand*lx(end) + lx(1);
        points(i,2) = rand*ly(end) + ly(1);
    end
end

% create periodic conditions if needed:
% add the same point in each direction, including diagonals
if strcmp(edgecond, 'periodic')
    points = duplicateGerms(lx, ly, points);
end

% total number of points to consider
N = size(points, 1);

%% Main algorithm
% * first create distance function: each pixel get the distance to the
%   closest point
% * then perform watershed

% fill distance with a default value
dist = Nx * Ny * ones([Ny Nx]);

% update distance map with distance to closest point
for p = 1:N
    dx = repmat(reshape(lx-points(p, 1), [1 Nx]), [Ny 1]);
    dy = repmat(reshape(ly-points(p, 2), [Ny 1]), [1 Nx]);
    dist = min(dist, hypot(dx, dy));
end

% use a watershed to generate cells, with 4-connectivity
img = watershed(dist, conn) ~= 0;

%% post processing
% ---------------------------------------------

% if edge condition is 'remove', kill all cells touching borders
if strcmp(edgecond, 'remove')
    img = removeBorderRegions(img);
end


function centroid = voronoi_centroids ( n, first, list_num, list, xyz, v_num, v_xyz )

%*****************************************************************************80
%
%% VORONOI_CENTROIDS computes the centroids of each polygon in a Voronoi diagram.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of data points and Voronoi polygons.
%
%    Input, integer FIRST(N+1), for each polygon, points to the location
%    in LIST of the index.
%
%    Input, integer LIST_NUM, the number of items in LIST.
%
%    Input, integer LIST(LIST_NUM), the indices of vertices that form
%    each polygon.
%
%    Input, real XYZ(3,N), the coodinates of the data points.
%
%    Input, integer V_NUM, the number of Voronoi vertices.
%
%    Input, real V_XYZ(3,V_NUM), the coordinates of the Voronoi vertices.
%
%    Output, real CENTROID(3,N), the centroid of each Voronoi polygon.
%
  centroid = zeros ( 3, n );

  r = 1.0;

  for poly = 1 : n
%
%  Compute the area of each triangle formed by one side of the polygon
%  and the Delaunay point.
%
    v3 = list(first(poly+1)-1);

    for l = first(poly) : first(poly+1) - 1

      v2 = v3;
      v3 = list(l);

      stri_area = stri_vertices_to_area ( r, ...
        xyz(1:3,poly), v_xyz(1:3,v2), v_xyz(1:3,v3) );

      stri_centroid = stri_vertices_to_centroid ( r, ...
        xyz(1:3,poly), v_xyz(1:3,v2), v_xyz(1:3,v3) );

      centroid(1:3,poly) = centroid(1:3,poly) + stri_area * stri_centroid(1:3,1);

    end

    centroid(1:3,poly) = centroid(1:3,poly) / norm ( centroid(1:3,poly) );

  end

  return
end


function [xd,xn,option] = denoise(x,h,type,option)
%    [xd,xn,option] = denoise(x,h,type,option);
%
%    DENOISE is a generic program for wavelet based denoising.
%    The program will denoise the signal x using the 2-band wavelet
%    system described by the filter h using either the traditional
%    discrete wavelet transform (DWT) or the linear shift invariant
%    discrete wavelet transform (also known as the undecimated DWT
%    (UDWT)).
%
%    Input:
%       x         : 1D or 2D signal to be denoised
%       h         : Scaling filter to be applied
%       type      : Type of transform (Default: type = 0)
%                   0 --> Discrete wavelet transform (DWT)
%                   1 --> Undecimated DWT (UDWT)
%       option    : Default settings is marked with '*':
%                   *type = 0 --> option = [0 3.0 0 0 0 0]
%                   type = 1 --> option = [0 3.6 0 1 0 0]
%       option(1) : Whether to threshold low-pass part
%                   0 --> Don't threshold low pass component
%                   1 --> Threshold low pass component
%       option(2) : Threshold multiplier, c. The threshold is
%                   computed as:
%                     thld = c*MAD(noise_estimate)).
%                   The default values are:
%                     c = 3.0 for the DWT based denoising
%                     c = 3.6 for the UDWT based denoising
%       option(3) : Type of variance estimator
%                   0 --> MAD (mean absolute deviation)
%                   1 --> STD (classical numerical std estimate)
%       option(4) : Type of thresholding
%                   0 --> Soft thresholding
%                   1 --> Hard thresholding
%       option(5) : Number of levels, L, in wavelet decomposition. By
%                   setting this to the default value '0' a maximal
%                   decomposition is used.
%       option(6) : Actual threshold to use (setting this to
%                   anything but 0 will mean that option(3)
%                   is ignored)
%
%    Output:
%       xd     : Estimate of noise free signal
%       xn     : The estimated noise signal (x-xd)
%       option : A vector of actual parameters used by the
%                program. The vector is configured the same way as
%                the input option vector with one added element
%                option(7) = type.
%
%  HERE'S AN EASY WAY TO RUN THE EXAMPLES:
%  Cut-and-paste the example you want to run to a new file
%  called ex.m, for example. Delete out the % at the beginning
%  of each line in ex.m (Can use search-and-replace in your editor
%  to replace it with a space). Type 'ex' in matlab and hit return.
%
%    Example 1:
%       h = daubcqf(6); [s,N] = makesig('Doppler'); n = randn(1,N);
%       x = s + n/10;     % (approximately 10dB SNR)
%       figure;plot(x);hold on;plot(s,'r');
%
%       %Denoise x with the default method based on the DWT
%       [xd,xn,opt1] = denoise(x,h);
%       figure;plot(xd);hold on;plot(s,'r');
%
%       %Denoise x using the undecimated (LSI) wavelet transform
%       [yd,yn,opt2] = denoise(x,h,1);
%       figure;plot(yd);hold on;plot(s,'r');
%
% Example 2: (on an image)
%      h = daubcqf(6);  load lena;
%      noisyLena = lena + 25 * randn(size(lena));
%      figure; colormap(gray); imagesc(lena); title('Original Image');
%       figure; colormap(gray); imagesc(noisyLena); title('Noisy Image');
%       Denoise lena with the default method based on the DWT
%      [denoisedLena,xn,opt1] = denoise(noisyLena,h);
%      figure; colormap(gray); imagesc(denoisedLena); title('denoised Image');
%
%
%    See also: mdwt, midwt, mrdwt, mirdwt, SoftTh, HardTh, setopt
%

%File Name: denoise.m
%Last Modification Date: 04/15/97	10:44:28
%Current Version: denoise.m	2.4
%File Creation Date: Mon Feb 20 08:33:15 1995
%Author: Jan Erik Odegard  <odegard@ece.rice.edu>
%
%Copyright (c) 2000 RICE UNIVERSITY. All rights reserved.
%Created by Jan Erik Odegard, Department of ECE, Rice University.
%
%This software is distributed and licensed to you on a non-exclusive
%basis, free-of-charge. Redistribution and use in source and binary forms,
%with or without modification, are permitted provided that the following
%conditions are met:
%
%1. Redistribution of source code must retain the above copyright notice,
%   this list of conditions and the following disclaimer.
%2. Redistribution in binary form must reproduce the above copyright notice,
%   this list of conditions and the following disclaimer in the
%   documentation and/or other materials provided with the distribution.
%3. All advertising materials mentioning features or use of this software
%   must display the following acknowledgment: This product includes
%   software developed by Rice University, Houston, Texas and its contributors.
%4. Neither the name of the University nor the names of its contributors
%   may be used to endorse or promote products derived from this software
%   without specific prior written permission.
%
%THIS SOFTWARE IS PROVIDED BY WILLIAM MARSH RICE UNIVERSITY, HOUSTON, TEXAS,
%AND CONTRIBUTORS AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
%BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
%FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL RICE UNIVERSITY
%OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
%EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
%PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
%OR BUSINESS INTERRUPTIONS) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
%WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
%OTHERWISE), PRODUCT LIABILITY, OR OTHERWISE ARISING IN ANY WAY OUT OF THE
%USE OF THIS SOFTWARE,  EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%For information on commercial licenses, contact Rice University's Office of
%Technology Transfer at techtran@rice.edu or (713) 348-6173
%
%Change History: Fixed output of function and an error in the computation
%                of the threshold for redundant denoising.
%                <Jan Erik Odegard> <Mon Jul 31, 1995>
%
%                This code is composed of several of our old codes for
%                wavelet based denoising. In an effort to make the mess
%                more manageable we decided to create on code that would
%                handle all the various wavelet based denoising methods.
%                However, only time will show (as we discover new and
%                improved forms of denoising) if we can succeed in our goals.
%                <Jan Erik Odegard> <Thu May 11, 1995>
%

if(nargin < 2)
  error('You need to provide at least 2 inputs: x and h');
end;
if(nargin < 3),
  type = 0;
  option = [];
elseif(nargin < 4)
  option = [];
end;
if(isempty(type)),
  type = 0;
end;
if(type == 0),
  default_opt = [0 3.0 0 0 0 0];
elseif(type == 1),
  default_opt = [0 3.6 0 1 0 0];
else,
  error(['Unknown denoising method',10,...
	  'If it is any good we need to have a serious talk :-)']);
end;
option = setopt(option,default_opt);
[mx,nx] = size(x);
dim = min(mx,nx);
if(dim == 1),
  n = max(mx,nx);
else,
  n = dim;
end;
if(option(5) == 0),
  L = floor(log2(n));
else
  L = option(5);
end;
if(type == 0), 			% Denoising by DWT
  xd = mdwt(x,h,L);
  if (option(6) == 0),
    tmp = xd(floor(mx/2)+1:mx,floor(nx/2)+1:nx);
    if(option(3) == 0),
      thld = option(2)*median(abs(tmp(:)))/.67;
    elseif(option(3) == 1),
      thld = option(2)*std(tmp(:));
    else
      error('Unknown threshold estimator, Use either MAD or STD');
    end;
  else,
    thld = option(6);
  end;
  if(dim == 1)
    ix = 1:n/(2^L);
    ykeep = xd(ix);
  else
    ix = 1:mx/(2^L);
    jx = 1:nx/(2^L);
    ykeep = xd(ix,jx);
  end;
  if(option(4) == 0),
    xd = SoftTh(xd,thld);
  elseif(option(4) == 1),
    xd = HardTh(xd,thld);
  else,
    error('Unknown threshold rule. Use either Soft (0) or Hard (1)');
  end;
  if (option(1) == 0),
    if(dim == 1),
      xd(ix) = ykeep;
    else,
      xd(ix,jx) = ykeep;
    end;
  end;
  xd = midwt(xd,h,L);
elseif(type == 1), 			% Denoising by UDWT
  [xl,xh] = mrdwt(x,h,L);
  if(dim == 1),
    c_offset = 1;
  else,
    c_offset = 2*nx + 1;
  end;
  if (option(6) == 0),
    tmp = xh(:,c_offset:c_offset+nx-1);
    if(option(3) == 0),
      thld = option(2)*median(abs(tmp(:)))/.67;
    elseif(option(3) == 1),
      thld = option(2)*std(tmp(:));
    else
      error('Unknown threshold estimator, Use either MAD or STD');
    end;
  else,
    thld = option(6);
  end;
  if(option(4) == 0),
    xh = SoftTh(xh,thld);
    if(option(1) == 1),
      xl = SoftTh(xl,thld);
    end;
  elseif(option(4) == 1),
    xh = HardTh(xh,thld);
    if(option(1) == 1),
      xl = HardTh(xl,thld);
    end;
  else,
    error('Unknown threshold rule. Use either Soft (0) or Hard (1)');
  end;
  xd = mirdwt(xl,xh,h,L);
else, 					% Denoising by unknown method
  error(['Unknown denoising method',10,...
         'If it is any good we need to have a serious talk :-)']);
end;
option(6) = thld;
option(7) = type;
xn = x - xd;


function C = nancorr( X,Y )
% NANCORR calculates the sample correlation coefficient
%    for the series with NaNs expected.
%    X is the one series, Y is another.
X=X(:);
Y=Y(:);
L1=length(X);
L2=length(Y);

if L1 ~= L2
    error('The samples must be of the same length')
end

for i=1:L1,
    if isnan(X(i)),
        Y(i)=nan;
    end
    if isnan(Y(i)),
        X(i)=nan;
    end
end

Xm=nanmean(X);
Ym=nanmean(Y);
C=nansum((X-Xm).*(Y-Ym))/sqrt((nansum((X-Xm).^2))*(nansum((Y-Ym).^2)));
end


% Test file for bndfun/sum.m

function pass = test_sum(pref)

% Get preferences.
if ( nargin < 1 )
    pref = chebfunpref();
end

% Set a domain
dom = [-2 7];

% Create seed for random number generator
seedRNG(6178);

% Generate a few random points to use as test values.
x = diff(dom) * rand(1000, 1) + dom(1);

%%
% Spot-check integrals for a couple of functions.
f = bndfun(@(x) exp(x) - 1, struct('domain', dom), pref);
pass(1) = (abs(sum(f) -  1.087497823145222e3) < ...
    10*get(f, 'vscale')*eps);

f = bndfun(@(x) 1./(1 + x.^2), struct('domain', dom), pref);
pass(2) = (abs(sum(f) - (atan(-dom(1))+atan(dom(2)))) < ...
    10*get(f, 'vscale')*eps);

f = bndfun(@(x) cos(1e4*x), struct('domain', dom), pref);
exact = (sin(1e4*dom(2))-sin(1e4*dom(1)))/1e4;
pass(3) = (abs(sum(f) - exact) < 100*get(f, 'vscale')*eps);

z = exp(2*pi*1i/6);
f = bndfun(@(t) sinh(t*z), struct('domain', dom), pref);
exact = ((cos(sqrt(3) - 1i) - cos((7*sqrt(3))/2 - 7i/2))*(sqrt(3) + 1i)*1i)/2;
pass(4) = (abs(sum(f)-exact) < 10*get(f, 'vscale')*eps);

%%
% Check a few basic properties.
a = 2;
b = -1i;
f = bndfun(@(x) x.*sin(x.^2) - 1, struct('domain', dom), pref);
df = diff(f);
g = bndfun(@(x) exp(-(x/10).^2), struct('domain', dom), pref);
dg = diff(g);
fg = f.*g;
gdf = g.*df;
fdg = f.*dg;

tol_f = 10*max(get(f, 'vscale')*eps);
tol_g = 10*max(get(g, 'vscale')*eps);
tol_fg = 10*max(get(fg, 'vscale')*eps);
tol_df = 10*max(get(df, 'vscale')*eps);
tol_dg = 10*max(get(dg, 'vscale')*eps);
tol_gdf = 10*max(get(gdf, 'vscale')*eps);
tol_fdg = 10*max(get(fdg, 'vscale')*eps);

% Linearity.
pass(5) = (abs(sum(a*f + b*g) - (a*sum(f) + b*sum(g))) < max(tol_f, tol_g));

% Integration-by-parts.
pass(6) = (abs(sum(fdg) - (feval(fg, dom(2)) - ...
    feval(fg, dom(1)) - sum(gdf))) < max([tol_fdg ; tol_fg ; tol_gdf]));

% Fundamental Theorem of Calculus.
pass(7) = (abs(sum(df) - (feval(f, dom(2)) - feval(f, dom(1)))) < ...
    max(tol_df, tol_f));
pass(8) = (abs(sum(dg) - (feval(g, dom(2)) - feval(g, dom(1)))) < ...
    max(tol_dg, tol_g));

%%
% Check operation for array-valued bndfun objects.
f = bndfun(@(x) [sin(x) x.^2 exp(1i*x)], struct('domain', dom), pref);
I = sum(f);
I_exact = [(cos(dom(1)) - cos(dom(2))) (dom(2)^3 - dom(1)^3)/3 ...
    1i*(exp(1i*dom(1)) - exp(1i*dom(2)))];
pass(9) = (max(abs(I - I_exact)) < 10*max(get(f, 'vscale')*eps));

% DIM option with array-valued input.
g = sum(f, 2);
h = @(x) sin(x) + x.^2 + exp(1i*x);
pass(10) = (norm(feval(g, x) - h(x), inf) < ...
    10*max(get(g, 'vscale')*eps));

% DIM option with non-array-valued input should leave everything as it was.
h = bndfun(@(x) cos(x), struct('domain', dom), pref);
sumh2 = sum(h, 2);
pass(11) = all((feval(h, x) == feval(sumh2, x)));

%% Test on singular function:

pow = -0.5;
op = @(x) (x - dom(1)).^pow.*sin(x);
pref.blowup = true;
data.domain = dom;
data.exponents = [pow 0];
f = bndfun(op, data, pref);
I = sum(f);
I_exact = -1.92205524578386613;
pass(12) = ( abs(I-I_exact) < 200*eps*abs(I_exact) );

end


function [Yi]=interp1_ND(X,Y,Xi,interpDim,interpMethod)

% function [Yi]=interp1_ND(X,Y,Xi,interpDim,interpMethod)
% ------------------------------------------------------------------------
% This function is similar to interp1. However it can perform 1D
% interpolation for multidimensional arrays. E.g. Time interpolation for 3D
% image data varying in time. The direction of interpolation is specified
% by interpDim, and the method by interpMethod
% ('nearest','linear','cubic','pchip').
%
%
% Reference for pchip method:
%   F. N. Fritsch and R. E. Carlson, "Monotone Piecewise Cubic
%   Interpolation", SIAM J. Numerical Analysis 17, 1980, 238-246.
%   David Kahaner, Cleve Moler and Stephen Nash, Numerical Methods
%   and Software, Prentice Hall, 1988.
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2015/03/04
%------------------------------------------------------------------------

%% Check input

%Check number of dimensions
nDimsX = ndims(X);
nDimsXi = ndims(Xi);

if nDimsX~=nDimsXi
    if size(Xi,nDimsX)~=1
        error('Input arrays have different number of dimensions');
    end
end

%Check column/row form
if iscolumn(X)
    X=X';
end

if iscolumn(Xi)
    Xi=Xi';
end

if isvector(X)
    interpDim=2;
end

%% Cast in 2D array format

siz=size(X);
siz_i=size(Xi);

dimInd=1:nDimsX;
logicNotInterpDim=dimInd~=interpDim;
permOrder=[sort(dimInd(logicNotInterpDim)) interpDim];

%Permuting array order so that interpolation dimension is last
X = permute(X,permOrder);
Y = permute(Y,permOrder);
Xi = permute(Xi,permOrder);

%Convert to 2D array with interpolation performed allong column direction
siz1=prod(siz(logicNotInterpDim));
X=reshape(X(:),[siz1, numel(X)/siz1]);
Y=reshape(Y(:),[siz1, numel(Y)/siz1]);
Xi=reshape(Xi(:),[siz1, numel(Xi)/siz1]);

%%

siz_2D=size(X);
siz_i_2D=size(Xi);

switch interpMethod
    case {'cubic','pchip'}
        diff_Y=diff(Y,1,2);
        diff_X=diff(X,1,2);
        K=diff_Y./diff_X;
end

switch interpMethod
    case 'cubic'
        M=zeros(siz_2D);
        M(:,1)=K(:,1);
        M(:,end)=K(:,end);
        M(:,2:end-1)=0.5*(K(:,1:end-1)+K(:,2:end));
    case 'pchip'
        M = pchipSlopes(X,Y,K);
end

%%

I=(1:siz_2D(1))';

%Initialize
Yi=nan(siz_i_2D);
p0=zeros(size(X,1),1); p1=p0;
switch interpMethod
    case 'cubic'
        m0=p0; m1=p0;
    case 'pchip'
        m0=p0; m1=p0;
end

for q=1:1:siz_i_2D(end)

    %Find close point pair to define interval
    D=abs(X-Xi(:,q*ones(1,size(X,2))));

    [~,J1]=min(D,[],2);
    indMin1=sub2ind(siz_2D,I,J1);
    D(indMin1)=inf;
    [~,J2]=min(D,[],2);
    indSort=[J1 J2];
    indSort=sort(indSort(:,1:2),2); %Sorted indices of closest two

    ind1=sub2ind(siz_2D,I,indSort(:,1));
    ind2=sub2ind(siz_2D,I,indSort(:,2));

    w=X(ind2)-X(ind1); %interval width
    t=(Xi(:,q)-X(ind1))./w; %t coordinate in domain
    p0(1:end)=Y(ind1); %First point
    p1(1:end)=Y(ind2); %End point
    switch interpMethod
        case {'cubic','pchip'}
            m0(1:end)=M(ind1); %First derivative
            m1(1:end)=M(ind2); %Final derivative

            h00=2.*t.^3-3.*t.^2+1;
            h10=t.^3-2*t.^2+t;
            h01=-2*t.^3+3*t.^2;
            h11=t.^3-t.^2;
            pt=h00.*p0 + w.*h10.*m0 + h01.*p1 + w.*h11.*m1;
        case 'linear'
            pt=t.*p1+(1-t).*p0;
        case 'nearest'
            logicPrev=t<=0.5;
            pt(logicPrev)=p0(logicPrev);
            pt(~logicPrev)=p1(~logicPrev);
    end

    Yi(:,q)=pt; %Store in array

end

%% Cast back in original form

% Xi=reshape(Xi(:),siz_i(permOrder));
% Xi=ipermute(Xi,permOrder);

Yi=reshape(Yi(:),siz_i(permOrder));
Yi=ipermute(Yi,permOrder);

end
%%
function M = pchipSlopes(X,Y,dYdX)
%PCHIPSLOPES  Derivative values for shape-preserving Piecewise Cubic Hermite
% Interpolation.
% d = pchipslopes(x,y,del) computes the first derivatives, d(k) = P'(x(k)).

n = size(X,2);

%  Slopes at interior points.
%  d(k) = weighted average of del(k-1) and del(k) when they have the same sign.
%  d(k) = 0 when del(k-1) and del(k) have opposites signs or either is zero.

M = zeros(size(Y));

[I,J]= find(sign(dYdX(:,1:n-2)).*sign(dYdX(:,2:n-1)) > 0);
ind=sub2ind(size(X),I,J);
ind1=sub2ind(size(X),I,J+1);

h = diff(X,1,2);
hs = h(ind)+h(ind1);
w1 = (h(ind)+hs)./(3*hs);
w2 = (hs+h(ind1))./(3*hs);
dmax = max(abs(dYdX(ind)), abs(dYdX(ind1)));
dmin = min(abs(dYdX(ind)), abs(dYdX(ind1)));
M(ind1) = dmin./conj(w1.*(dYdX(ind)./dmax) + w2.*(dYdX(ind1)./dmax));

%  Slopes at end points.
%  Set d(1) and d(n) via non-centered, shape-preserving three-point formulae.

M(:,1) = ((2*h(:,1)+h(:,2)).*dYdX(:,1) - h(:,1).*dYdX(:,2))./(h(:,1)+h(:,2));
L = (sign(M(:,1)) ~= sign(dYdX(:,1)));
M(L,1) = 0;

L = ((sign(dYdX(:,1)) ~= sign(dYdX(:,2))) & (abs(M(:,1)) > abs(3*dYdX(:,1))));
M(L,1) = 3*dYdX(L,1);

M(:,n) = ((2*h(:,n-1)+h(:,n-2)).*dYdX(:,n-1) - h(:,n-1).*dYdX(:,n-2))./(h(:,n-1)+h(:,n-2));

L= sign(M(:,n)) ~= sign(dYdX(:,n-1));
M(L,n) = 0;

L= (sign(dYdX(:,n-1)) ~= sign(dYdX(:,n-2))) & (abs(M(:,n)) > abs(3*dYdX(:,n-1)));
M(L,n) = 3*dYdX(L,n-1);

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 cyclic_reduction_test01 ( )

%*****************************************************************************80
%
%% CYCLIC_REDUCTION_TEST01 tests C83_CR_FA, C83_CR_SL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 May 2010
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CYCLIC_REDUCTION_TEST01\n' );
  fprintf ( 1, '  C83_CR_FA factors a complex tridiagonal matrix;\n' );
  fprintf ( 1, '  C83_CR_SL solves a factored system.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
%
%  Set the matrix values.
%
  a(1,1) = 0.0;
  for j = 2 : n
    a(1,j) = -1.0  - ( j - 1 ) * i;
  end

  for j = 1 : n
    a(2,j) = 2.0 + 2.0 * j * i;
  end

  for j = 1 : n - 1
    a(3,j) = -1.0 - ( j + 1 ) * i;
  end
  a(3,n) = 0.0;
%
%  Set the desired solution.
%
  for j = 1 : n
    x(j) = j + 10 * j * i;
  end
%
%  Compute the corresponding right hand side.
%
  b = c83_mxv ( n, a, x );
%
%  Factor the matrix.
%
  a_cr = c83_cr_fa ( n, a );
%
%  Solve the linear system.
%
  x = c83_cr_sl ( n, a_cr, b );

  c8vec_print ( n, x, '  Solution:' );

  return
end


% A file to demonstrate the power_bounded function
% Author: Muhammad F. Kasim (University of Oxford)

% test == 1 for drawing an ordinary voronoi diagram
% test == 2 for power diagram with random small weights
% test == 3 to see how long does it take for larger number of sites with random small weights
test = 2;

if test < 3
    N = 50; % number of sites
    x = rand(N, 1);
    y = rand(N, 1); % x and y coordinate of the sites
    if test == 1
        wts = zeros(N, 1); % weights of each coordinate (all-zeros weights produce voronoi diagram)
    elseif test == 2
        wts = rand(N, 1) * 0.001;
    end
    crs = [0,0; 0,1; 1,1; 1,0]; % the bounding box in clockwise order

    % get the power diagram
    [V,C] = power_bounded(x,y, wts, crs);

    % draw the resulted power diagram
    plot(x, y, 'r.'); % plot the sites' points
    hold on;
    for i = [1:N]
        xpoly = V(C{i},1);
        ypoly = V(C{i},2);
        xpoly = [xpoly; xpoly(1)];
        ypoly = [ypoly; ypoly(1)];
        plot(xpoly, ypoly, 'b-');
    end
    xlim([min(crs(:,1)), max(crs(:,1))]);
    ylim([min(crs(:,2)), max(crs(:,2))]);
    hold off;

elseif test == 3
    Ns = [100, 1000, 10000, 100000, 1000000];
    times = [];
    for N = Ns
        x = rand(N, 1);
        y = rand(N, 1);
        wts = rand(N, 1) * 0.0001;
        crs = [0,0; 0,1; 1,1; 1,0];

        % time the power diagram
        tic;
        power_bounded(x, y, wts, crs);
        times(end+1) = toc;
    end
    disp(times);

    loglog(Ns, times, 'bo', 'LineWidth', 2);
    hold on;
    loglog(Ns, times, 'b-', 'LineWidth', 2);
    set(gca, 'FontSize', 14)
    xlabel('Number of sites');
    ylabel('Time required (s)');
end


function result = ball_unit_f3_nd ( func, n )

%*****************************************************************************80
%
%% BALL_UNIT_F3_ND approximates an integral inside the unit ball in ND.
%
%  Integration region:
%
%    Points X(1:N) such that:
%
%      Sum ( X(1:N)**2 ) <= 1.
%
%  Discussion:
%
%    A 2**(N+1)-1 point 5-th degree formula is used, Stroud number SN:5-4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2004
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F at the N-vector X, of the form
%      function value = func ( n, x )
%
%    Input, integer N, the dimension of the space.
%
%    Output, real RESULT, the approximate integral of the function.
%
  quad = 0.0E+00;
%
%  The first point is the center of the ball.
%
  x(1:n) = 0.0E+00;

  weight = 4.0E+00 / ( n + 2 )^2;
  quad = quad + weight * feval ( func, n, x );

  s = 1.0E+00 / sqrt ( ( n + 4 ) );

  for i = 1 : n

    ri = sqrt ( ( i + 2 ) / ( n + 4 ) );
%
%  Set up the first point, with (I-1) zeroes, RI, and then N-I S's.
%
    for j = 1 : n

      if ( j < i )
        x(j) = 0.0E+00;
      elseif ( j == i )
        x(j) = ri;
      else
        x(j) = s;
      end

    end

    weight = 2.0E+00^( i - n ) * ( n + 4 ) / ( ( i + 1 ) * ( i + 2 ) * ( n + 2 ) );
%
%  Now go through all sign permutations of the basic point.
%
    for j = 1 : 2^(n+1-i)

      jtemp = j - 1;

      for k = i : n

        if ( mod ( jtemp, 2 ) == 1 )
          x(k) = - abs ( x(k) );
        else
          x(k) = abs ( x(k) );
        end

        jtemp = floor ( jtemp / 2 );

      end

      quad = quad + weight * feval ( func, n, x );

    end

  end

  volume = ball_unit_volume_nd ( n );
  result = quad * volume;

  return
end


function f2 = p08_f2 ( x )

%*****************************************************************************80
%
%% P08_F2 evaluates the second derivative for problem 8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the values of the variables.
%
%    Output, real F2, the second derivative.
%
  if ( x == pi )
    f2 = 1.0;
  else
    f2 = 6.0 + ( - 2.0 / ( x - pi ) / ( x - pi ) ) / pi^4;
  end

  return
end


function qmr_test ( )

%*****************************************************************************80
%
%% QMR_TEST tests QMR.
%
%  Modified:
%
%    27 March 2006
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QMR_TEST:\n' );
  fprintf ( 1, '  QMR uses the Quasi-Minimum Residual \n' );
  fprintf ( 1, '  iterative method to approximate the solution \n' );
  fprintf ( 1, '  of a linear system A * x = b.\n' );

  n = 10;

  A = zeros ( n, n );

  for i = 1 : n
    A(i,i) = 2.0;
  end
  for i = 1 : n-1
    A(i,i+1) = -1;
  end
  for i = 2 : n
    A(i-1,i) = -1;
  end
  x = [ 1 : n ]';
  b = A * x;
  x = ones ( n, 1 );

  M = 0;
  max_it = 10;
  tol = 0.0001;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For this example, the order of the system is N = %d\n', n );
  fprintf ( 1, '  The matrix A is the simple tridiagonal -1, 2, -1.\n' );
  fprintf ( 1, '  The correct solution is x = [ 1, 2, ..., n].\n' );
  fprintf ( 1, '  The right hand side b is determined by computing A * x.\n' );
  fprintf ( 1, '  The exact x is then replaced by a vector of all 1''s for\n' );
  fprintf ( 1, '  use as a starting guess.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Other parameters are set as follows:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The maximum number of steps is %d.\n', max_it );
  fprintf ( 1, '  The error tolerance is %f\n', tol );

  [ x, error_norm, iter, flag ] = qmr ( A, x, b, M, max_it, tol );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The QMR routine has returned with FLAG = %d\n', flag );
  if ( flag == 0 )
    fprintf ( 1, '  This indicates that the iteration has converged.\n' );
  elseif ( flag == 1 )
    fprintf ( 1, '  This indicates that the iteration has NOT converged.\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of iterations taken was %d\n', iter );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The L2 norm of the error per iteration:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : iter
    fprintf ( 1, '  %4d  %f\n', i, error_norm(i) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The computed solution vector X:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %f\n', i, x(i) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'QMR_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


function d = duplicated(x)
%DUPLICATED   Find duplicated rows.
% DUPLICATED(x) returns a vector d such that d(i) = 1 if x(i,:) is a
% duplicate of an earlier row.
%
% Examples:
% duplicated([2 7 8 7 1 2 8]') = [0 0 0 1 0 1 1]'
% duplicated([0 0 1 1 0; 0 1 0 1 1]') = [0 0 0 0 1]'
% duplicated(eye(100))
% duplicated(kron((1:3)',ones(3)))
%
% You can simulate unique(x) or unique(x,'rows') by x(~duplicated(x)).
% The difference is that the latter form will not sort the contents of x,
% as unique will.
%
% See also UNIQUE.

% (c) Microsoft Corporation. All rights reserved.

% This function is not well optimized.
% In particular, it is slower than unique.

[nr,nc] = size(x);
if nc == 1
  d = duplicated1(x);
  return;
end
if nr == 1
  d = 0;
  return;
end
hash = x*rand(nc,1);
[dummy,ord] = sort(hash);
xo = x(ord,:);
%d = [0 all(diff(xo)==0,2)'];
d = [0 all(xo(1:end-1,:)==xo(2:end,:),2)'];
dd = diff([d 0]);
dstart = find(dd > 0);
dend = find(dd < 0);
% loop each run of duplicated columns
for i = 1:length(dstart)
  % place the zero at the first element in the original order
  d(dstart(i)) = 1;
  d(dstart(i)-1 + argmin(ord(dstart(i):dend(i)))) = 0;
end
d(ord) = d;
d = d';
%d = duplicated1(hash);

function d = duplicated1(x)
% special case where x is a column vector.

[s,ord] = sort(x);
d = zeros(size(x));
d(ord) = [0; s(1:end-1)==s(2:end)];
%d(ord) = [0; diff(s)==0];


function f = langermannsfcn(x)

if strcmpi(x,'init')
    f.PopInitRange = [0;10] ;
    f.PopulationSize = 200 ;
elseif size(x,2) == 2
    x = reshape(x,1,[]) ;
    a = [3,5,2,1,7; 5,2,1,4,9] ;
    c = [1,2,5,2,3] ;
%     xa = (repmat(x,size(a,2),1)'-a)'*(repmat(x,size(a,2),1)'-a) ;
%     % xa is now 5x2
%     f = exp(-1/pi*xa)*cos(pi*xa);
%     f = 0 ;
    f = sum(c.*exp(-sum((repmat(x',1,size(a,2)) - a).^2,1)/pi).*...
        cos(pi*sum((repmat(x',1,size(a,2)) - a).^2,1)),2) ;
else
    error('Two dimensional input required for Langermann''s Function')
end


function TF = raisedCos (f,Tb,r)
% Raised Cosine Filter
% f = Frequency Matrix
% Tb = Bit Period
% r = Rolloff Factor

W0 = 1/(2*Tb);
W = (r+1)*W0;
x = abs(f)*(1/(W-W0));
x = x + ((W - 2*W0)/(W-W0))*ones(size(x));
TF = cos((pi/4)*x).^2;
xx = find(abs(f) < (2*W0 - W));
for ll = xx, TF(ll) = 1;end
xx = find(abs(f) > W);
for ll = xx, TF(ll) = 0;end


function [Y] = mnbinpdf(X, W, K)
%MNBINPDF       Maszle's negative binomial probability density function
%
%  Y = MNBINPDF(X, W, K)
%
%  returns the probabilities of the values X using a negative binomial
%  distribution with mean W and clumping parameter K.
%
%  This is an alternate form of the negative binomial commonly
%  employed in biology to describe aggregated count data.
%  W and K are related to the traditional parametrization by the
%  relationships:
%
%      P = K/(K + W),  and K = R,
%
%  with the distinction that K is any positive real number, not just
%  positive integers.
%
%  X should only be integers and is rounded if not.  Calculation is
%  optimized by taking the exponential of the GAMMALN function.
%
%  See also MNBINRND

%----------------------------------------------------------------------
% Copyright (c) 1997.  Don R. Maszle.  All rights reserved.
%
%   -- Revisions -----
%      Author:  Don R. Maszle
%      E-mail:  maze@sparky.berkeley.edu
%   -- SCCS  ---------
%----------------------------------------------------------------------

%----------------------------------------------------------------------
% We use the definition from Bradley and May, Trans. R.Soc.Trop.Med.Hyg.
% v72(3), 1978, p262.

% A notational convenience
a = W/(W + K);
X = round(X);

Y = ((1 - a).^K) ...
    .* (exp( gammaln(X + K) + X.*log(a) - gammaln(K) - gammaln(X + 1)));


%dcm (Cba) to rotation vector (r^a)
%Cba: B to A transformation matrix
%r^a=Rotation vector defined in A. (Rotate A around r with mag(r) to obtain B)
%the magnitude of the returning rotation vector will be between [0 and PI]

%This is the implementaion of savage(3-31).
function rvec=dcm2rot(dcm)

sinPHI=0.5*sqrt((dcm(3,2)-dcm(2,3))^2+(dcm(1,3)-dcm(3,1))^2+(dcm(2,1)-dcm(1,2))^2);
cosPHI=0.5*(dcm(1,1)+dcm(2,2)+dcm(3,3)-1);

PHI=atan2(sinPHI,cosPHI);

if (cosPHI>=0)
    if (sinPHI==0)
        F=1;
    else
        F=PHI/sinPHI;
    end
    rvec=0.5*F*[dcm(3,2)-dcm(2,3);dcm(1,3)-dcm(3,1);dcm(2,1)-dcm(1,2)];
else
    sr_a=1-cosPHI;
    mu1=sqrt((dcm(1,1)-1)/sr_a+1);
    mu2=sqrt((dcm(2,2)-1)/sr_a+1);
    mu3=sqrt((dcm(3,3)-1)/sr_a+1);
    if (mu1>=mu2 && mu1>=mu3)
       u1=mu1*sign(dcm(3,2)-dcm(2,3));
       u2=1/2/u1*(dcm(1,2)+dcm(2,1))/sr_a;
       u3=1/2/u1*(dcm(1,3)+dcm(3,1))/sr_a;
    elseif (mu2>=mu3)
       u2=mu2*sign(dcm(1,3)-dcm(3,1));
       u3=1/2/u2*(dcm(2,3)+dcm(3,2))/sr_a;
       u1=1/2/u2*(dcm(1,2)+dcm(2,1))/sr_a;
    else
       u3=mu3*sign(dcm(2,1)-dcm(1,2));
       u1=1/2/u3*(dcm(1,3)+dcm(3,1))/sr_a;
       u2=1/2/u3*(dcm(2,3)+dcm(3,2))/sr_a;
    end
    rvec=PHI*[u1;u2;u3];
end


function [p]=rmEvaluate2Fits(rss1,rss2,df1,df2,n,units,wtest)
% rmEvaluate2Fits - probability that fit1 == fit2
%
% output = rmEvaluate2Fits(rss1,rss2,df1,df2,n,units,wtest);
%
% Performs f-test and general likelihood ratio (Wilks likelihood)
% tests. Outputs the probability of Ho: data1 == data2.
% Assumes df1>=df2.
%
% rss  : residual sum of squares (model 1 and 2)
% df   : degrees of freedom      (model 1 and 2)
% n    : number of data points
% units: 'p', 'log10p' ['p']
% wtest: 'ftest','glr'
%
% 2006/02 SOD: wrote it.

if ieNotDefined('rss1'), error('Need rss1.'); end;
if ieNotDefined('rss2'), error('Need rss2.'); end;
if ieNotDefined('df1'),  error('Need df1.');  end;
if ieNotDefined('df2'),  error('Need df2.');  end;
if ieNotDefined('n'),    error('Need n.');    end;
if ieNotDefined('units'),wtest = 'p';         end;
if ieNotDefined('wtest'),wtest = 'glr';       end;

% some sanity checks
if df1 == df2,
  disp(sprintf('[%s]:error: df1 == df2.',mfilename));
  return;
end;
if df1<df2,
  disp(sprintf('[%s]:warning: df1>df2 flipping datasets!',mfilename));
  dftmp = df1;
  df1   = df2;
  df2   = dftmp;
  rsstmp = rss1;
  rss1   = rss2;
  rss2   = rsstmp;
end;

% now do tests
switch lower(wtest),
 case {'f','ftest'}
  % ftest
  if df1~=df2,
    f = ((rss1-rss2)./rss2) ./ ((df1-df2)./df2);
    p = 1-fcdf(f,df1-df2,df2);
  else
    f = rss1./rss2;
    p = 1-fcdf(f,df2,df1);
  end;

 case {'g','glr','wilks'}
  % From Fan et al (2001) Generalized likelihood ratio statistics and Wilks
  % phenomenon. The Annals of Statistics. 29(1): 153-193.
  % calculate lambda (glr computation)
  lambda = (n./2).*log(rss1./rss2);

  % probability function: Wilks phenomenon lambda approximates
  % chi-squared distribution.
  df = df1-df2;
  p = 1-chi2cdf(lambda,df);
end;

% convert units if requested
switch lower(units),
 case 'log10p',
  sp         = sign(p);
  sp(sp==0)  = 1;
  p          = abs(p);
  p(p<1e-50) = 10^-50; % remove 0
  p          = -log10(p) .* sp;
 case 'p',
  % do nothing
 otherwise,
  error('[%s]:Unknown unit: %s',mfilename,units);
end;

return;


function chebyshev_polynomial_test09 ( )

%*****************************************************************************80
%
%% CHEBYSHEV_POLYNOMIAL_TEST09 compares a function and projection over [-1,+1].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBYSHEV_POLYNOMIAL_TEST09:\n' );
  fprintf ( 1, '  T_PROJECT_COEFFICIENTS computes the Chebyshev interpolant C(F)(N,X)\n' );
  fprintf ( 1, '  of a function F(X) defined over [-1,+1].\n' );
  fprintf ( 1, '  T_PROJECT_VALUE evaluates that projection.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compute projections of order N to exp(x) over [-1,+1],\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   N   Max||F(X)-C(F)(N,X)||\n' );
  fprintf ( 1, '\n' );
  for n = 0 : 10
    c = t_project_coefficients ( n, @exp );
    m = 101;
    x = ( linspace ( -1.0, +1.0, m ) )';
    v = t_project_value ( m, n, x, c );
    r = v - exp ( x );
    fprintf ( 1, '  %2d  %12.4g\n', n, max ( abs ( r ) ) );
  end

  return
end


function u = bsv ( a, b, alpha, beta, nu, n, output )

%*****************************************************************************80
%
%% BSV applies Newton's method to a discretized steady viscous Burgers equation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the left and right endpoints.
%
%    Input, real ALPHA, BETA, the Dirichlet boundary values at the left
%    and right.
%
%    Input, real NU, the viscosity.  Normally, 0 < NU.
%
%    Input, integer N, the number of nodes to use between A and B.
%
%    Input, logical OUTPUT, is TRUE if printout is desired.
%
%    Output, real U(N), the computed discretized solution.
%
  if ( n < 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BSV - Fatal error!\n' );
    fprintf ( 1, '  N < 2.\n' );
    error ( 'BSV - Fatal error!\n' );
  end
%
%  Set some iteration parameters.
%
  newton_step_max = 50;
  newton_resid_tol = sqrt ( eps );
  newton_step_tol = sqrt ( eps );
%
%  Use equally spaced nodes from A to B.
%
  dx = ( b - a ) / ( n - 1 );
%
%  The initial guess will be the linear interpolant to the boundary conditions.
%
  u = ( linspace ( alpha, beta, n ) )';
%
%  Prepare for the Newton iteration.
%
  J = sparse ( [], [], [], n, n, 3 * n );
  newton_step = 0;

  if ( output )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Step  ||F(U)||\n' );
    fprintf ( 1, '\n' );
  end

  while ( newton_step <= newton_step_max )

    f(1,1) = u(1) - alpha;

%   for i = 2 : n - 1
%     f(i,1) = 0.5 * ( u(i+1)^2 - u(i-1)^2 ) / ( 2.0 * dx ) ...
%          - nu * ( u(i+1) - 2.0 * u(i) + u(i-1) ) / ( dx^2 );
%   end

    f(2:n-1,1) = 0.5 * ( u(3:n).^2 - u(1:n-2).^2 ) / ( 2.0 * dx ) ...
           - nu * ( u(3:n) - 2.0 * u(2:n-1) + u(1:n-2) ) / ( dx^2 );

    f(n,1) = u(n) - beta;

    f_norm = norm ( f, inf );

    if ( output )
      fprintf ( 1, '  %4d  %g\n', newton_step, f_norm );
    end

    if ( f_norm < newton_resid_tol )
      break
    end
%
%  Define the Jacobian matrix.
%
    J(1,1) = 1.0;
    for i = 2 : n - 1
      J(i,i-1) = - 2.0 * u(i-1) / ( 4.0 * dx )       - nu / dx^2;
      J(i,i)   =                                 2.0 * nu / dx^2;
      J(i,i+1) =   2.0 * u(i+1) / ( 4.0 * dx )       - nu / dx^2;
    end
    J(n,n) = 1.0;
%
%  Solve the linear system J * du = -f to get the Newton update.
%
    du = J \ f;
    du_norm = norm ( du, inf );

    u_norm = norm ( u, inf );
    if ( du_norm < newton_step_tol * ( u_norm + 1.0 ) )
      break
    end

    u = u - du;
    newton_step = newton_step + 1;

  end

  if ( newton_step_max < newton_step )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BSV - Warning!\n' );
    fprintf ( 1, '  The Newton iteration did not converge.\n' );
  end

  return
end


% Checks whether the graph is directed, using the matrix transpose function.
%
% INPUTS: adjacency matrix, nxn
% OUTPUTS: boolean variable, 0 or 1
%
% Note: one-liner alternative: S=not(isSymmetric(adj));
% GB: last updated, Sep 23, 2012

function S=isDirected(adj)

S = true;
if adj==transpose(adj); S = false; end


function b = center_logistic(u,y);
s = mean(y);
b = log( s / (1 - s ) ) - mean(u);
for i=1:20
	sigma = 1./ ( 1 + exp( - u - b ) );
	phider = sum(sigma - y) ;
	phider2 = sum(sigma .* ( 1- sigma));
	bnew = b - phider / phider2;
	if norm(bnew-b) < 1e-16, break; end
	b=bnew;

end


% EX_STOKES_SQUARE_TH: solve the Stokes problem in the unit square with generalized Taylor-Hood elements.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_square.txt';

% Type of boundary conditions for each side of the domain
problem_data.drchlt_sides = 1:4;
problem_data.nmnn_sides = [];

% Physical parameters
problem_data.viscosity = @(x, y) ones (size (x));

% Force term
fx = @(x, y) (6 * x + y .* cos(x .* y) + 2 * cos(y) .* sin(x));
fy = @(x, y) (x .* cos(x .* y) - 2 * cos(x) .* sin(y));
problem_data.f  = @(x, y) cat(1, ...
                reshape (fx (x,y), [1, size(x)]), ...
                reshape (fy (x,y), [1, size(x)]));

% Boundary terms
uxex = @(x, y) (sin(x) .* cos(y));
uyex = @(x, y) (-sin(y) .* cos(x));

problem_data.h = @(x, y, iside) cat(1, ...
                  reshape (uxex (x,y), [1, size(x)]), ...
                  reshape (uyex (x,y), [1, size(x)]));

% Exact solution, to compute the errors
problem_data.velex = @(x, y) cat(1, ...
                  reshape (uxex (x,y), [1, size(x)]), ...
                  reshape (uyex (x,y), [1, size(x)]));

problem_data.pressex = @(x, y) (3 * x.^2 + sin(x .* y) - 1.239811742000564725943866);

problem_data.gradvelex = @test_stokes_square_bc_graduex;

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.element_name = 'th';   % Element type for discretization
method_data.degree       = [ 3  3];  % Degree of the splines (pressure space)
method_data.regularity   = [ 2  2];  % Regularity of the splines (pressure space)
method_data.nsub         = [10 10];  % Number of subdivisions
method_data.nquad        = [ 5  5];  % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space_v, vel, space_p, press] = ...
                       solve_stokes (problem_data, method_data);

% 4) POST-PROCESSING
% 4.1) COMPARISON WITH EXACT SOLUTION
error_l2_p = sp_l2_error (space_p, msh, press, problem_data.pressex)
[error_h1_v, error_l2_v] = ...
   sp_h1_error (space_v, msh, vel, problem_data.velex, problem_data.gradvelex)

% 4.2) EXPORT TO PARAVIEW
output_file = 'SQUARE_TH_Deg3_Reg2_Sub10';

fprintf ('The result is saved in the files %s \n and %s \n \n', ...
           [output_file '_vel'], [output_file '_press']);
vtk_pts = {linspace(0, 1, 20), linspace(0, 1, 20)};
sp_to_vtk (press, space_p, geometry, vtk_pts, [output_file '_press'], 'press')
sp_to_vtk (vel,   space_v, geometry, vtk_pts, [output_file '_vel'  ], {'velocity', 'divergence'}, {'value', 'divergence'})

% 4.3) PLOT IN MATLAB
[eu, F] = sp_eval (vel, space_v, geometry, vtk_pts);
[X,  Y] = deal (squeeze(F(1,:,:)), squeeze(F(2,:,:)));

figure()
subplot (1,2,2)
eu2 = problem_data.velex (X, Y);
quiver (X, Y, squeeze(eu2(1,:,:)), squeeze(eu2(2,:,:)))
axis equal
title('Exact solution')
subplot (1,2,1)
quiver (X, Y, squeeze(eu(1,:,:)), squeeze(eu(2,:,:)))
axis equal
title('Computed solution')

%!demo
%! ex_stokes_square_th

%!test
%! problem_data.geo_name = 'geo_square.txt';
%! problem_data.drchlt_sides = 1:4;
%! problem_data.nmnn_sides = [];
%! problem_data.viscosity = @(x, y) ones (size (x));
%! fx = @(x, y) (6 * x + y .* cos(x .* y) + 2 * cos(y) .* sin(x));
%! fy = @(x, y) (x .* cos(x .* y) - 2 * cos(x) .* sin(y));
%! problem_data.f  = @(x, y) cat(1, ...
%!                 reshape (fx (x,y), [1, size(x)]), ...
%!                 reshape (fy (x,y), [1, size(x)]));
%! uxex = @(x, y) (sin(x) .* cos(y));
%! uyex = @(x, y) (-sin(y) .* cos(x));
%! problem_data.h = @(x, y, iside) cat(1, ...
%!                   reshape (uxex (x,y), [1, size(x)]), ...
%!                   reshape (uyex (x,y), [1, size(x)]));
%! problem_data.velex = @(x, y) cat(1, ...
%!                   reshape (uxex (x,y), [1, size(x)]), ...
%!                   reshape (uyex (x,y), [1, size(x)]));
%! problem_data.pressex = @(x, y) (3 * x.^2 + sin(x .* y) - 1.239811742000564725943866);
%! problem_data.gradvelex = @test_stokes_square_bc_graduex;
%! method_data.element_name = 'th';   % Element type for discretization
%! method_data.degree       = [ 3  3];  % Degree of the splines (pressure space)
%! method_data.regularity   = [ 2  2];  % Regularity of the splines (pressure space)
%! method_data.nsub         = [10 10];  % Number of subdivisions
%! method_data.nquad        = [ 5  5];  % Points for the Gaussian quadrature rule
%! [geometry, msh, space_v, vel, space_p, press] = ...
%!                        solve_stokes (problem_data, method_data);
%! error_l2_p = sp_l2_error (space_p, msh, press, problem_data.pressex);
%! [error_h1_v, error_l2_v] = ...
%!    sp_h1_error (space_v, msh, vel, problem_data.velex, problem_data.gradvelex);
%! assert (msh.nel, 100)
%! assert (space_p.ndof, 169)
%! assert (space_v.ndof, 1058)
%! assert (error_l2_p, 1.95560715994218e-08, 1e-15)
%! assert (error_h1_v, 8.98425390938543e-08, 1e-15)
%! assert (error_l2_v, 1.37608469955346e-09, 1e-15)


function point = pointOnLine(line, pos)
%POINTONLINE Create a point on a line at a given position on the line
%
%   P = pointOnLine(LINE, POS);
%   Creates the point belonging to the line LINE, and located at the
%   distance D from the line origin.
%   LINE has the form [x0 y0 dx dy].
%   LINE and D should have the same number N of rows. The result will have
%   N rows ans 2 column (x and y positions).
%
%   See also:
%   lines2d, points2d, onLine, onLine, linePosition
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 07/04/2004.
%

angle = lineAngle(line);
point = [line(:,1) + pos .* cos(angle), line(:,2) + pos .* sin(angle)];


%% MULTIGRID OF FOR THE STOKES EQNS IN 2D
%
% This example is to show the convergence of multigrid methods for various
% finite element approximation of the Stokes equation on the unit square:
%
%       -div(mu*grad u) + grad p = f in \Omega,
%                        - div u = 0  in \Omega,
%                              u = g_D  on \Gamma.
%
% with the pure Dirichlet boundary condition.
%

%% Setting
clear all; close all;
[node,elem] = squaremesh([0,1,0,1],0.5);
% [node,elem] = circlemesh(0,0,1,0.25);
[node,elem] = uniformrefine(node,elem);
showmesh(node,elem);
pde = Stokesdata1;
%  pde = StokesZulehnerdata;
bdFlag = setboundary(node,elem,'Dirichlet');
option.L0 = 1;
option.maxIt = 4;
option.printlevel = 1;
option.plotflag = 0;
option.rateflag = 0;

%% MG options
option.solver = 'mg';
option.smoothingstep = 2;
option.smootherbarSp = 'SGS';

%% RT0-P0
display('RT0-P0')
option.elemType = 'RT0-P0';
option.refType = 'bisect';
femStokesHdiv(node,elem,pde,bdFlag,option);

% %% RT0-P0
% display('BDM1B-P0')
% option.elemType = 'BDM1B-P0';
% femStokesHdiv(node,elem,pde,bdFlag,option);

%% CR-P0 element
display('CR-P0')
option.elemType = 'CRP0';
femStokes(node,elem,pde,bdFlag,option);

%% P2-P0 element
display('P2-P0')
option.elemType = 'P2P0';
femStokes(node,elem,pde,bdFlag,option);

%% isoP2-P0 element
display('isoP2-P0')
option.elemType = 'isoP2P0';
femStokes(node,elem,pde,bdFlag,option);

%% isoP2-P1 element
display('isoP2-P1')
option.elemType = 'isoP2P1';
femStokes(node,elem,pde,bdFlag,option);

% %% P1b-P1 element
% display('P1b-P0')
% option.elemType = 'P1bP1';
% femStokes(node,elem,pde,bdFlag,option);

%% P2-P1 element
display('P2-P1')
option.elemType = 'P2P1';
% option.smoothingStep = 3;
% option.smootherbarSp   = 'VCYCLE';
% option.smootherbarSpPara = 0.75;
femStokes(node,elem,pde,bdFlag,option);


% This is like skf_data_assoc_gmux, except the objects don't move.
% We are uncertain of their initial positions, and get more and more observations
% over time. The goal is to test deterministic links (0 covariance).
% This is like robot1, except the robot doesn't move and is always at [0 0],
% so the relative location is simply L(s).

nobj = 2;
N = nobj+2;
Xs = 1:nobj;
S = nobj+1;
Y = nobj+2;

intra = zeros(N,N);
inter = zeros(N,N);
intra([Xs S], Y) =1;
for i=1:nobj
  inter(Xs(i), Xs(i))=1;
end

Xsz = 2; % state space = (x y)
Ysz = 2;
ns = zeros(1,N);
ns(Xs) = Xsz;
ns(Y) = Ysz;
ns(S) = nobj;

bnet = mk_dbn(intra, inter, ns, 'discrete', S, 'observed', [S Y]);

% For each object, we have
% X(t+1) = F X(t) + noise(Q)
% Y(t) = H X(t) + noise(R)
F = eye(2);
H = eye(2);
Q = 0*eye(Xsz); % no noise in dynamics
R = eye(Ysz);

init_state{1} = [10 10]';
init_state{2} = [10 -10]';
init_cov = eye(2);

% Uncertain of initial state (position)
for i=1:nobj
  bnet.CPD{Xs(i)} = gaussian_CPD(bnet, Xs(i), 'mean', init_state{i}, 'cov', init_cov);
end
bnet.CPD{S} = root_CPD(bnet, S); % always observed
bnet.CPD{Y} = gmux_CPD(bnet, Y, 'cov', repmat(R, [1 1 nobj]), 'weights', repmat(H, [1 1 nobj]));
% slice 2
eclass = bnet.equiv_class;
for i=1:nobj
  bnet.CPD{eclass(Xs(i), 2)} = gaussian_CPD(bnet, Xs(i)+N, 'mean', zeros(Xsz,1), 'cov', Q, 'weights', F);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create LDS params

% X(t) = A X(t-1) + B U(t) + noise(Q)

% [L11]  = [1  ]  * [L1]       +   [Q ]
% [L2]     [  1]    [L2]           [ Q]

% Y(t)|S(t)=s  = C(s) X(t) + noise(R)
% Yt|St=1 = [1 0]  * [L1]  + R
%                    [L2]

nlandmarks = nobj;

% Create indices into block structure
bs = 2*ones(1, nobj); % sizes of blocks in state space
for i=1:nlandmarks
  landmark_block(:,i) = block(i, bs)';
end
Xsz = 2*(nlandmarks); % 2 values for each landmark plus robot
Ysz = 2; % observe relative location

% create block-diagonal trans matrix for each switch
A = zeros(Xsz, Xsz);
for i=1:nlandmarks
  bi = landmark_block(:,i);
  A(bi, bi) = eye(2);
end
A = repmat(A, [1 1 nlandmarks]); % same for all switch values

% create block-diagonal system cov
Qbig = zeros(Xsz, Xsz);
Qbig = repmat(Qbig, [1 1 nlandmarks]);

% create observation matrix for each value of the switch node
% C(:,:,i) = (0 ... I ...) where the I is in the i'th posn.
C = zeros(Ysz, Xsz, nlandmarks);
for i=1:nlandmarks
  C(:, landmark_block(:,i), i) = eye(2);
end

% create observation cov for each value of the switch node
Rbig = repmat(R, [1 1 nlandmarks]);

% initial conditions
init_x = [init_state{1}; init_state{2}];
init_V = zeros(Xsz, Xsz);
for i=1:nlandmarks
  bi = landmark_block(:,i);
  init_V(bi,bi) = init_cov;
end

%%%%%%%%%%%%%%%%
% Observe objects at random
T = 10;
evidence = cell(N, T);
data_assoc = sample_discrete(normalise(ones(1,nobj)), 1, T);
evidence(S,:) = num2cell(data_assoc);
evidence = sample_dbn(bnet, 'evidence', evidence);

% Inference
ev = cell(N,T);
ev(bnet.observed,:) = evidence(bnet.observed, :);
y = cell2num(evidence(Y,:));

engine = pearl_unrolled_dbn_inf_engine(bnet);
engine = enter_evidence(engine, ev);

loopy_est_pos = zeros(2, nlandmarks);
loopy_est_pos_cov = zeros(2, 2, nlandmarks);
for i=1:nobj
  m = marginal_nodes(engine, Xs(i), T);
  loopy_est_pos(:,i) = m.mu;
  loopy_est_pos_cov(:,:,i) = m.Sigma;
end

[xsmooth, Vsmooth] = kalman_smoother(y, A, C, Qbig, Rbig, init_x, init_V, 'model', data_assoc);

kf_est_pos = zeros(2, nlandmarks);
kf_est_pos_cov = zeros(2, 2, nlandmarks);
for i=1:nlandmarks
  bi = landmark_block(:,i);
  kf_est_pos(:,i) = xsmooth(bi, T);
  kf_est_pos_cov(:,:,i) = Vsmooth(bi, bi, T);
end

kf_est_pos
loopy_est_pos

kf_est_pos_time = zeros(2, nlandmarks, T);
for t=1:T
  for i=1:nlandmarks
    bi = landmark_block(:,i);
    kf_est_pos_time(:,i,t) = xsmooth(bi, t);
  end
end
kf_est_pos_time % same for all t since smoothed


function [param]=sine_fit(x,y,fixed_params,initial_params,plot_flag)
% Optimization of parameters of the sine function to time series
%
% Syntax:
%       [param]=sine_fit(x,y)
%
%       that is the same that
%       [param]=sine_fit(x,y,[],[],[])     % no fixed_params, automatic initial_params
%
%       [param]=sine_fit(x,y,fixed_params)        % automatic initial_params
%       [param]=sine_fit(x,y,[],initial_params)   % use it when the estimation is poor
%       [param]=sine_fit(x,y,fixed_params,initial_params,plot_flag)
%
% param = [offset, amplitude, phaseshift, frequency]
%
% if fixed_params=[NaN, NaN , NaN , NaN]        % or fixed_params=[]
% optimization of offset, amplitude, phase shift and frequency (default)
%
% if fixed_params=[NaN, 1 , NaN , 1/(2*pi)]
% optimization of offset and phase shift of a sine of amplitude=1 and frequency=1/(2*pi)
%
% if fixed_params=[0, NaN , NaN , NaN]
% optimization of amplitude, phase shift and frequency of a sine of offset=0
%
%
% Example:
% %% generate data vectors (x and y)
% fsine = @(param,timeval) param(1) + param(2) * sin( param(3) + 2*pi*param(4)*timeval );
% param=[0 1 0 1/(2*pi)];  % offset, amplitude, phaseshift, frequency
% timevec=0:0.1:10*pi;
% x=timevec;
% y=fsine(param,timevec) + 0.1*randn(size(x));
%
% %% standard parameter estimation
% [estimated_params]=sine_fit(x,y)
%
% %% parameter estimation with forced 1.5 fixed amplitude
% [estimated_params]=sine_fit(x,y,[NaN 1.5 NaN NaN])
%
% %% parameter estimation without plotting
% [estimated_params]=sine_fit(x,y,[],[],0)
%
%
% Doubts, bugs: rpavao@gmail.com

% warning off

if nargin<=1 %fail
    fprintf('');
    help sine_fit
    return
end

automatic_initial_params=NaN(1,4);
automatic_initial_params(1)=median(y);
automatic_initial_params(2)=mean(minmax(y));
automatic_initial_params(3)=0;
[~,pos]=findpeaks(smooth(y,10));
automatic_initial_params(4)=1/max(diff(x(pos)));

% phas_shift estimator, not working...
% [~,pos]=max(y);
% automatic_initial_params(3)=mod(x(pos),automatic_initial_params(4));

if nargin==2 %simplest valid input
    fixed_params=NaN(1,4);
    initial_params=automatic_initial_params;
    plot_flag=1;
end
if nargin==3
    initial_params=automatic_initial_params;
    plot_flag=1;
end
if nargin==4
    plot_flag=1;
end

if exist('fixed_params','var')
    if isempty(fixed_params)
        fixed_params=NaN(1,4);
    end
end
if exist('initial_params','var')
    if isempty(initial_params)
        initial_params=automatic_initial_params;
    end
end
if exist('plot_flag','var')
    if isempty(plot_flag)
        plot_flag=1;
    end
end

f_str='f = @(param,timeval)';
free_param_count=0;
bool_vec=NaN(1,4);
for i=1:4;
    if isnan(fixed_params(i))
        free_param_count=free_param_count+1;
        f_str=[f_str ' param(' num2str(free_param_count) ')'];
        bool_vec(i)=1;
    else
        f_str=[f_str ' ' num2str(fixed_params(i))];
        bool_vec(i)=0;
    end
    if i==1; f_str=[f_str ' +']; end
    if i==2; f_str=[f_str ' * sin(']; end
    if i==3; f_str=[f_str ' + 2*pi*']; end
    if i==4; f_str=[f_str '*timeval );']; end
end

eval(f_str)

[BETA,RESID,J,COVB,MSE] = nlinfit(x,y,f,initial_params(bool_vec==1));

free_param_count=0;
for i=1:4;
    if isnan(fixed_params(i))
        free_param_count=free_param_count+1;
        param(i)=BETA(free_param_count);
    else
        param(i)=fixed_params(i);
    end
end

if plot_flag==1
    x_vector=min(x):mean(diff(x))/2:max(x);
    plot(x,y,'k.',x_vector,f(BETA,x_vector),'r-')
    xlim(minmax(x_vector))
end


%[1983]-"Optimization by Simulated Annealing"

% (9/12/2020)

function SA = jSimulatedAnnealing(feat,label,opts)
% Parameters
c  = 0.93;  % cooling rate
T0 = 100;   % initial temperature

if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'c'), c = opts.c; end
if isfield(opts,'T0'), T0 = opts.T0; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = jInitialization(1,dim);
% Fitness
fit = fun(feat,label,X,opts);
% Initial best
Xgb  = X;
fitG = fit;
% Pre
curve = zeros(1,max_Iter);
t = 2;
% Iterations
while t <= max_Iter
	% Probabilty of swap, insert, flip & eliminate
  prob = randi([1,4]);
  % Swap operation
  if prob == 1
    Xnew  = X;
    % Find index with bit '0' & '1'
    bit0  = find(X == 0);
    bit1  = find(X == 1);
    len_0 = length(bit0);
    len_1 = length(bit1);
    % Solve issue with missing bit '0' or '1'
    if len_0 ~= 0 && len_1 ~= 0
      % Get one random index from x1 & x2
      ind0 = randi([1,len_0]);
      ind1 = randi([1,len_1]);
      % Swap between two index
      Xnew(bit0(ind0)) = 1;
      Xnew(bit1(ind1)) = 0;
    end

  % Insert operation
  elseif prob == 2
    Xnew  = X;
    % Find index with zero
    bit0  = find(X == 0);
    len_0 = length(bit0);
    % Solve problem when all index are '1'
    if len_0 ~= 0
      ind = randi([1,len_0]);
      % Add one feature
      Xnew(bit0(ind)) = 1;
    end

  % Eliminate operation
  elseif prob == 3
    Xnew  = X;
    % Find index with one
    bit1  = find(X == 1);
    len_1 = length(bit1);
    % Solve problem when all index are '0'
    if len_1 ~= 0
      ind = randi([1,len_1]);
      % Remove one feature
      Xnew(bit1(ind)) = 0;
    end

  % Flip operation
  elseif prob == 4
    Xnew = X;
    % Flip all variables
    for d = 1:dim
      Xnew(d) = 1 - Xnew(d);
    end
  end

  % Fitness
  Fnew = fun(feat,label,Xnew,opts);
  % Global best update
  if Fnew <= fitG
    Xgb  = Xnew;
    fitG = Fnew;
    X    = Xnew;
  % Accept worst solution with probability
  else
    % Delta energy
    delta = Fnew - fitG;
    % Boltzmann Probility
    P     = exp(-delta / T0);
    if rand() <= P
      X = Xnew;
    end
  end
  % Temperature update
  T0 = c * T0;
  % Save
  curve(t) = fitG;
  fprintf('\nIteration %d Best (SA)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos(Xgb == 1);
sFeat = feat(:,Sf);
% Store results
SA.sf = Sf;
SA.ff = sFeat;
SA.nf = length(Sf);
SA.c  = curve;
SA.f  = feat;
SA.l  = label;
end

function X = jInitialization(N,dim)
% Initialize X vectors
X = zeros(N,dim);
for i = 1:N
  for d = 1:dim
    if rand() > 0.5
      X(i,d) = 1;
    end
  end
end
end


function varargout = ddgaussian(varargin)
% VL_DDGAUSSIAN Second derivative of the Gaussian density function
%   Y=VL_DDGAUSSIAN(X) computes the second derivative of the standard
%   Gaussian density.
%
%   To obtain the second derivative of the Gaussian density of
%   standard deviation S, do
%
%     Y = 1/S^3 * VL_DDGAUSSIAN(X/S) .
%
%   See also: VL_GAUSSIAN(), VL_DGAUSSIAN(), VL_HELP().
[varargout{1:nargout}] = vl_ddgaussian(varargin{:});


clear all, close all, clc
nu=0.001;   % Diffusion constant

% Define spatial domain
L = 20;             % Length of domain
N = 1000;           % Number of discretization points
dx = L/N;
x = -L/2:dx:L/2-dx; % Define x domain

% Define discrete wavenumbers
kappa = (2*pi/L)*[-N/2:N/2-1];
kappa = fftshift(kappa');    % Re-order fft wavenumbers

% Initial condition
u0 = sech(x);

% Simulate PDE in spatial domain
dt = 0.025;
t = 0:dt:100*dt;
[t,u] = ode45(@(t,u)rhsBurgers(t,u,kappa,nu),t,u0);

% Plot solution in time
subplot(1,2,1)
h=waterfall(real(u(1:10:end,:)));
set(h,'LineWidth',2,'FaceAlpha',.5);
colormap(jet/1.5)
view(5,55)

subplot(1,2,2)
imagesc(flipud(real(u)));
colormap jet

%% FIGURES (PRODUCTION)
figure
CC = colormap(jet(100));
dt = 0.1;
for k = 1:100
    if(mod(k-1,10)==0)
        plot(x,real(u(k,:)),'k','LineWidth',1.5)
%         plot(x,real(u(k,:)),'Color',CC(k,:),'LineWidth',1.5)
        hold on, grid on, drawnow
    end
end
% xlabel('Spatial variable, x')
% ylabel('Temperature, u(x,t)')
axis([-L/2 L/2 -.1 1.1])
set(gca,'LineWidth',1.2,'FontSize',12);
set(gcf,'Position',[100 100 550 220]);
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', '../../figures/FFTBurgers1');

%
figure
subplot(1,2,1)
h=waterfall(real(u(1:10:end,:)));
set(h,'LineWidth',2,'FaceAlpha',.5);
colormap(jet/1.5)
view(5,55)
set(gca,'LineWidth',1.5)
set(gca,'XTick',[0 500 1000],'XTickLabels',{})
set(gca,'ZTick',[0 .5 1],'ZTickLabels',{})
set(gca,'YTick',[0 5 10],'YTickLabels',{})
set(gca,'YLim',[1 11])
set(gca,'ZLim',[-.1 1.1])

subplot(1,2,2)
imagesc(flipud(real(u)));
set(gca,'LineWidth',1.5)
set(gca,'XTick',[0 500 1000],'XTickLabels',{})
set(gca,'YTick',[0 50 100],'YTickLabels',{})

colormap jet
set(gcf,'Position',[100 100 600 250])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose', '../../figures/FFTBurgers2');


function [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,y,k,numCentralKnots,useSparse)
%%BSPLINEPOLYFITMULTIDIM Given a set of real vector (or scalar) points as
%         well as real or complex scalar function values, this function
%         returns the knots and coefficients for a set of b-splines that
%         can be used to interpolate to a desired polynomial degree within
%         the sampled region of the function. B-splines can have continuous
%         derivatives across the entire sampled region and can be easily
%         differentiated. This function determines the knots to use by
%         taking the tensor product of the approximate knots from
%         Equation 10 of Chapter 13 of [1]. The knots are not optimal. The
%         first and last knots in each dimensions are repeated. This
%         function can provide interpolation coefficients for multiple sets
%         of values at once.
%
%INPUTS: tau A numPointsXnumDims matrix of the coordinates of the points in
%            each dimension in ascending order. The full grid of points
%            in all dimensions could be found using
%            [tau1,tau2,etc.]=ndgrid(tau(1:tauLengths(1),1),tau(1:tauLengths(2),2),...)
%            and the dimensions can be put together into the full set of
%            numDimsXtotalNumPoints points as
%            tauTotal=[tau1(:).';tau2(:).';...]. Note that using meshgrid
%            in 2D/3D will put the points in the wrong order; one has to
%            use the ordering of ndgrid. These values must be real.
% tauLengths A numDimsX1 or a 1XnumDims vector this lists the number of
%            points in each dimension of tau. The total number of sample
%            points is totalNumPoints=prod(tauLengths).
%          y This is the totalNumPointsXnumSets full sets of function
%            values taken at all of the points implied by the vectors in
%            tau for numSets different interpolation problems. The ordering
%            of the points is the same as the ordering of tauTotal, which
%            is described above. These values can be real or complex.
%          k A numDimsX1 or 1XnumDims set of the order of the b-splines in
%            each dimension. The value k-1 is the polynomial order of the
%            approximation being performed. If the order is the same in all
%            dimensions, then a single scalar can be passed.
% numCentralKnots This optional parameter is used if one passes a higher
%            density of points in tau than is needed, because one wants to
%            perform least squares interpolation. This is the numDimsX1 or
%            1XnumDims list of the number of knots in the central region of
%            knots in each dimension. It varies from 0 to n-k. The higher
%            the number, the more knots are used total. A total of
%            numCentralKnots+2*k will be returned by this function, though,
%            as explained below, the first and last knots in each dimension
%            are not used.
%  useSparse The linear system being solved for the coefficients can be
%            quite large, but is not necessarily dense. If useSparse is
%            true, then the matrix for the system will be allocated as a
%            sparse matrix. This can slow down the algorithm, but it can
%            also keep Matlab from running out of memory on large systems.
%            The default if this parameter is omitted or an empty matrix is
%            passed is true.
%
%OUTPUTS: a A hypermatrix with numDim+1 indices containing the set of
%           coefficients for the b-splines covering all of the
%           interpolation intervals with the final index selecting the set
%           if numSets>1. If numSets=1, then the final index is unitary,
%           meaning that there are effectively only numDim dimensions. This
%           and the following outputs can be passed to
%           BSplineInterpValMultiDim to perform interpolation over the
%           region spanned by the tau values.
%         t The maxNumKnotsXnumDims set of knots for the interpolation
%           function. The first and last k(curDim)-1 knots in each
%           dimension are outside of the ends of the region with data or
%           mark the ends of the region with data. The number of rows is
%           the maximum number needed for all of the dimensions. tLength
%           says how many items in each column are actually used. The knots
%           are the same for each set.
%   tLength A numDimX1 vector where tLength(i) says the number of elements
%           in t(:,i). The lengths are the same for each set.
%
%The comments ot the function BSplinePolyFit describe how Equation 7 in
%Chapter IX is used in one dimension. The issue of multiple dimensions is
%addressed in Chapter 17 of [1] with a focus on solving the problem in
%2D. The basic equation to be solved in 2D is given by Equation 11 in
%Chapter 17 of [1] and it can be easily extended to an arbitrary number
%of dimensions. Chapter 17 gives a solution that is more efficient in 2D
%than solving Equation 11. However, we choose to solve the equation to make
%the problem valid for an arbitrary number of dimensions.
%
%The ability to do a least-squares fit involves making fewer inner knots
%than suggested in the text. Rather than making n-k knots, we make
%numCentralKnots. However, this invalidates the approximation in Equation
%10 of Chapter 13 of [1] for selecting knots. The correction applied is to
%make the average taken in the equation to involve more points.
%Additionally, the knots in Chapter 13 are univariate. We make them
%multivariate by taking a tensor product.
%
%EXAMPLE 1:
%Here, we fit a curve to 2D data on an irregular grid. We choose a
%sufficiently high order that it recreates the polynomial exactly and we
%verify this by looking at the maximum error on a fine grid and plotting
%the results.
% f=@(x,y)(x.^4-2*x.^2+x).*(y.^4-2*y.^2+y);
% numDims=2;
% numPointsX=10;
% numPointsY=11;
% tauLengths=[numPointsX;numPointsY];
% tau=zeros(max(tauLengths),numDims);
% tau(1:tauLengths(1),1)=linspace(-1.5,1.5,numPointsX);
% tau(1:tauLengths(2),2)=linspace(-1.5,1.5,numPointsY);
% %Note that using meshgrid would have put the elements in the wong order.
% %ndgrid must be used.
% [tau1,tau2]=ndgrid(tau(1:tauLengths(1),1),tau(1:tauLengths(2),2));
% y=f(tau1,tau2);
%
% k=[5;5];
% [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,y(:),k);
%
% numPointsX=100;
% numPointsY=101;
% pointsX=linspace(-1.5,1.5,numPointsX);
% pointsY=linspace(-1.5,1.5,numPointsY);
% [X,Y]=ndgrid(pointsX,pointsY);
% x=[X(:).';Y(:).'];
%
% zTrue=f(X,Y);
%
% z=BSplineInterpValMultiDim(x,t,tLength,a,k);
% z=reshape(z,[numPointsX,numPointsY]);
%
% figure(1)
% clf
% surface(X,Y,z,'EdgeColor','none')
% title('Interpolated Surface')
%
% figure(2)
% clf
% surface(X,Y,zTrue,'EdgeColor','none')
% title('True Surface')
% max(abs(zTrue(:)-z(:)))
%One sees that the curves are the same and that the difference at any point
%is on the order of finite precision error. This is consistent with the
%fact that the order of the spline fit was chosen to be the same as the
%order of the true polynomial.
%
%EXAMPLE 2:
%Here, we fit a curve to 3D data on an irregular grid. We choose a
%sufficiently high order that it recreates the polynomial exactly and we
%verify this by looking at the maximum error on a fine grid.
% f=@(x,y,z)(x.^4-2*x.^2+x).*(y.^4-2*y.^2+y).*(z.^4-3*z.^2+z);
% numDims=3;
% numPointsX=10;
% numPointsY=11;
% numPointsZ=9;
% tauLengths=[numPointsX;numPointsY;numPointsZ];
% tau=NaN(max(tauLengths),numDims);
% tau(1:tauLengths(1),1)=linspace(-1.5,1.5,numPointsX);
% tau(1:tauLengths(2),2)=linspace(-1.5,1.5,numPointsY);
% tau(1:tauLengths(3),3)=linspace(-1.5,1.5,numPointsZ);
% %Note that using meshgrid would have put the elements in the wong order.
% %ndgrid must be used.
% [tau1,tau2,tau3]=ndgrid(tau(1:tauLengths(1),1),tau(1:tauLengths(2),2),tau(1:tauLengths(3),3));
% y=f(tau1,tau2,tau3);
%
% k=[5;5;5];
% [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,y(:),k);
%
% numPointsX=50;
% numPointsY=51;
% numPointsZ=49;
% pointsX=linspace(-1.5,1.5,numPointsX);
% pointsY=linspace(-1.5,1.5,numPointsY);
% pointsZ=linspace(-1.5,1.5,numPointsZ);
% [X,Y,Z]=ndgrid(pointsX,pointsY,pointsZ);
% x=[X(:).';Y(:).';Z(:).'];
%
% wTrue=f(X,Y,Z);
% w=BSplineInterpValMultiDim(x,t,tLength,a,k);
% max(abs(wTrue(:)-w(:)))
%As was the case with Example 1, the maximum error is on the order of what
%one would expect due to finite precision limitations.
%
%EXAMPLE 3:
%In this case, a complex 2D function is fit and interpolated.
% numPoints=50;
% xMin=0;
% xMax=3;
% yMin=0;
% yMax=3;
% x=linspace(xMin,xMax,numPoints);
% y=linspace(yMin,yMax,numPoints);
% [X,Y]=ndgrid(x,y);
% f=@(x,y)((sin(5*y)+1j*cos(10*x)).*exp(1j*(x.^2-2*y.^2)));
%
% fxy=f(X(:),Y(:));
% k=[5;5];
% tau=[x(:),y(:)];
% tauLengths=[numPoints,numPoints];
% [a,t,tLength]=BSplinePolyFitMultiDim(tau,tauLengths,fxy,k);
%
% %Interpolate on a finer grid.
% numPoints=100;
% x=linspace(xMin,xMax,numPoints);
% y=linspace(xMin,xMax,numPoints);
% [X,Y]=ndgrid(x,y);
%
% fxy=f(X(:),Y(:));
% pts=[X(:).';Y(:).'];
% fxyInterp=reshape(BSplineInterpValMultiDim(pts,t,tLength,a,k),[numPoints,numPoints]);
%
% fxy=reshape(fxy,[numPoints,numPoints]);
%
% figure(1)
% clf
% hold on
% surface(X,Y,real(fxy),'EdgeColor','none')
% title('Real Function Values')
% colorbar()
%
% figure(2)
% clf
% hold on
% surface(X,Y,imag(fxy),'EdgeColor','none')
% title('Imaginary Function Values')
% colorbar()
%
% figure(3)
% clf
% hold on
% surface(X,Y,abs(real(fxyInterp)-real(fxy)),'EdgeColor','none')
% title('Real Interpolation Error')
% colorbar()
%
% figure(4)
% clf
% hold on
% surface(X,Y,abs(imag(fxyInterp)-imag(fxy)),'EdgeColor','none')
% title('Imaginary Interpolation Error')
% colorbar()
%
%REFERENCES:
%[1] C. de Boor, A Practical Guide to Splines. New York: Springer-Verlag,
%    1978.
%
%April 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDims=size(tau,2);
n=tauLengths(:);
numSets=size(y,2);

if(nargin<6)
    useSparse=false;
end

%The total number of points.
NTotal=prod(n);
if(NTotal~=size(y,1))
    error('The dimensionalities of the inputs are not consistent.')
end

if(isscalar(k))
    k=repmat(k,[numDims,1]);
end
k=k(:);

if(nargin<5||isempty(numCentralKnots))
    numCentralKnots=n-k;
elseif(any(numCentralKnots<0)||any(numCentralKnots(:)>n-k))
    error('The value of numCentralKnots is invalid.')
end
numCentralKnots=numCentralKnots(:);

deltaVal=n-k-numCentralKnots;

tLength=numCentralKnots+2*k;
maxTLength=max(tLength);

t=zeros(maxTLength,numDims);
for curDim=1:numDims
    taLengthCur=tauLengths(curDim);
    tauCur=tau(1:taLengthCur,curDim);
    kCur=k(curDim);
    deltaValCur=deltaVal(curDim);
    numCentKnotsCur=numCentralKnots(curDim);

    t(1:kCur,curDim)=tauCur(1);
    t((numCentKnotsCur+kCur+1):(numCentKnotsCur+2*kCur),curDim)=tauCur(n(curDim));

    %The ad-hoc point choice from 10 of Chapter 13, modified to take more
    %points if a least-squares solution is being performed.
    for i=1:numCentKnotsCur
        t(k+i,curDim)=sum(tauCur((i+1):(i+kCur-1+deltaValCur)))/(kCur-1+deltaValCur);
    end
end

%Given the locations of the knots, we build the equations. The a values are
%of the form a(d1,d2,..dnumDims). To access them, we stack them one after
%another in the same order as a(:) would in Matlab.
totalNumA=prod(numCentralKnots+k);

if(useSparse)
    %The amount of memory used will grow as B is filled in.
    B=sparse(NTotal,totalNumA);
else
    B=zeros(NTotal,totalNumA);
end

spanMin=ones(numDims,1);
tIdxVals=k;

%tauIdxVals holds the index of the value of tau used in each dimension
tauIdxVals=ones(numDims,1);
tauVal=zeros(numDims,1);
%Fill in tauVal with the current value.
for curDim=1:numDims
    tauVal(curDim)=tau(1,curDim);
end

%The number of B values produced per call to evalBSplinePolysMultiDim.
numBCurEls=prod(k);

%We have to go through all of the indices.
for curEq=1:NTotal
    BCur=evalBSplinePolysMultiDim(t,tLength,tauVal,k,tIdxVals);
    %We have to put the elements in BCur into the proper spots in B. These
    %spots start at spanMin in each dimension.
    for curEl=1:numBCurEls
        dimIdx=index2NDim(k,curEl);
        %The multidimensional index of the a value by which the current B
        %value is multiplied.
        aDimIdx=spanMin+dimIdx-1;
        aIdx=nDim2Index(n-deltaVal,aDimIdx);

        B(curEq,aIdx)=BCur(curEl);
    end

    if(curEq==NTotal)
        break;
    end

    %Now, we increment tauIdxVals to move onto a new value of tau in each
    %dimension. the values are kept in the tauVal array. Each time a value
    %is incremented, we have to check whether the corresponding entry in
    %tIdxVals and spanMin should be incremented.
    curIdx=1;
    tauIdxVals(curIdx)=tauIdxVals(curIdx)+1;
    while(tauIdxVals(curIdx)>tauLengths(curIdx))
        %If it exceeds k, then go back to 1.
        tauIdxVals(curIdx)=1;
        tauVal(curIdx)=tau(1,curIdx);

        tIdxVals(curIdx)=k(curIdx);
        spanMin(curIdx)=1;

        %Increment tIdxVals and spanMin as necessary to reach the valid
        %region.
        while(tauVal(curIdx)>t(tIdxVals(curIdx)+1,curIdx))
            tIdxVals(curIdx)=tIdxVals(curIdx)+1;
            spanMin(curIdx)=spanMin(curIdx)+1;
        end

        curIdx=curIdx+1;
        tauIdxVals(curIdx)=tauIdxVals(curIdx)+1;
    end

    %Increment tIdxVals and spanMin as necessary to reach the valid
    %region.
    tauVal(curIdx)=tau(tauIdxVals(curIdx),curIdx);
    while(tauVal(curIdx)>t(tIdxVals(curIdx)+1,curIdx))
        tIdxVals(curIdx)=tIdxVals(curIdx)+1;
        spanMin(curIdx)=spanMin(curIdx)+1;
    end
end

a=B\y;
if(numDims>1)
    a=reshape(a,[(numCentralKnots+k)',numSets]);
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 ret = logistic(input)
    ret = 1 ./ (1 + exp(-input));
end


function [d] = spm_kl_eig_normal (m_q,c_q,c_p)
% KL divergence between normal densities using eigendecomposition
% function [d] = spm_kl_eig_normal (m_q,c_q,c_p)
%
% Calculate the KL distance
%
% KL (Q||P) = <log Q/P> where avg is wrt Q
%
% between two Normal densities Q and P where P is
% zero mean and has a diagonal covariance.
%
% m_q, c_q    Mean and covariance of first Normal density
% c_p         Covariance of second (zero-mean) Normal density
%___________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_kl_eig_normal.m 1143 2008-02-07 19:33:33Z spm $

d=length(m_q);
m_q=m_q(:);

ldcp=0.5*sum(log(diag(c_p)));
[v,eigvals]=eig(c_q);
ldcq=-0.5*sum(log(diag(eigvals)));
Term1=ldcp+ldcq;

inv_c_p=diag(1./diag(c_p));
Term2=0.5*trace(inv_c_p*c_q)+0.5*m_q'*inv_c_p*m_q;
d=Term1+Term2-0.5*d;


function jdate = julian (month, day, year)

% Julian date

% Input

%  month = calendar month [1 - 12]
%  day   = calendar day [1 - 31]
%  year  = calendar year [yyyy]

% Output

%  jdate = Julian date

% special notes

%  (1) calendar year must include all digits

%  (2) will report October 5, 1582 to October 14, 1582
%      as invalid calendar dates and stop

% Orbital Mechanics with Matlab

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

y = year;
m = month;
b = 0;
c = 0;

if (m <= 2)
   y = y - 1;
   m = m + 12;
end

if (y < 0)
   c = -.75;
end

% check for valid calendar date

if (year < 1582)
   % null
elseif (year > 1582)
   a = fix(y / 100);
   b = 2 - a + floor(a / 4);
elseif (month < 10)
   % null
elseif (month > 10)
   a = fix(y / 100);
   b = 2 - a + floor(a / 4);
elseif (day <= 4)
   % null
elseif (day > 14)
   a = fix(y / 100);
   b = 2 - a + floor(a / 4);
else
   clc; home;

   fprintf('\n\n  this is an invalid calendar date!!\n');

   keycheck;

   return;
end

jd = fix(365.25 * y + c) + fix(30.6001 * (m + 1));

jdate = jd + day + b + 1720994.5;


function value = fedn ( dim_num, x )

%*****************************************************************************80
%
%% FEDN(X(1:DIM_NUM)) = exp ( sum ( X(1:DIM_NUM) ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real X(DIM_NUM), the argument.
%
%    Output, real VALUE, the value of the function at X.
%
  value = exp ( sum ( x(1:dim_num) ) );

  return
end


function i4_rise_values_test ( )

%*****************************************************************************80
%
%% I4_RISE_VALUES_TEST demonstrates the use of I4_RISE_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 December 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4_RISE_VALUES_TEST:\n' );
  fprintf ( 1, '  I4_RISE_VALUES returns some exact values\n' );
  fprintf ( 1, '  of the integer rising factorial function:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     M     N      I4_RISE(M,N)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, m, n, fmn ] = i4_rise_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %4d  %4d  %10d\n', m, n, fmn );

  end

  return
end


function [t,t1] = geyer_icse(x,maxlag)
% GEYER_ICSE - Compute autocorrelation time tau using Geyer's
%              initial convex sequence estimator
%
%   C = GEYER_ICSE(X) returns autocorrelation time tau.
%   C = GEYER_ICSE(X,MAXLAG) returns autocorrelation time tau with
%       MAXLAG . Default MAXLAG = M-1.
%
%   References:
%      [1] C. J. Geyer, (1992). "Practical Markov Chain Monte Carlo",
%          Statistical Science, 7(4):473-511
%
% Copyright (C) 2002 Aki Vehtari

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

% compute autocorrelation
if nargin > 1
  cc=acorr(x,maxlag);
else
  cc=acorr(x);
end
[n,m]=size(cc);

% acorr returns values starting from lag 1, so add lag 0 here
cc=[ones(1,m);cc];
n=n+1;

% now make n even
if mod(n,2)
  n=n-1;
  cc(end,:)=[];
end

% loop through variables
t=zeros(1,m);
t1=zeros(1,m);
opt=optimset('LargeScale','off','display','off');
for i1=1:m
  c=cc(:,i1);
  c=sum(reshape(c,2,n/2),1);
  ci=find(c<0);
  if isempty(ci)
    warning(sprintf('Inital positive could not be found for variable %d, using maxlag value',i1));
    ci=n/2;
  else
    ci=ci(1)-1; % initial positive
  end
  c=[c(1:ci) 0];    % initial positive sequence
  t1(i1)=-1+2*sum(c); % initial positive sequence estimator
  if ci>2
    ca=fmincon(@se,c,[],[],[],[],0*c,c,@sc,opt,c); % monotone convex sequence
  else
    ca=c;
  end
  t(i1)=-1+2*sum(ca); % monotone convex sequence estimator
end

function e = se(x,xx)
% SE - Error in monotone convex sequene estimator
e=sum((xx-x).^2);

function [c,ceq] = sc(x,xx)
% SE - Constraint in monotone convex sequene estimator
ceq=0*x;
c=ceq;
d=diff(x);
dd=-diff(d);
d(d<0)=0;d=d.^2;
dd(dd<0)=0;dd=dd.^2;
c(1:end-1)=d;c(2:end)=c(2:end)+d;
c(1:end-2)=dd;c(2:end-1)=c(2:end-1)+dd;c(3:end)=c(3:end)+dd;


% Cobb-Douglas production function
% Y=F(K,N)=K^{alpha}*N^{1-alpha}
% In per capita form y=k^{alpha}

function y=cobb(k)
global alpha
if alpha>0 & alpha<1
   y=k.^alpha;
else
   y=-inf*abs(k);
   disp('invalid capital share')
end


% Flush out the MATLAB.
close all;
clc;
clear all;

% Read the desired image file.
ImageData=imread('building.pnm');

% Add noise to oriiginal image.
ImageData = imnoise(ImageData,'Salt & Pepper', 0.04);

% Display the original image.
figure,imshow(ImageData);
title(' Original Image with noise: ');

% Define constant.
Constant=0.3;

% Create the matrix.
[X,Y]=size(ImageData);

        for a = 1:X

            for b = 1:Y
                m=double(ImageData(a,b));

                OutputImage(a,b)=Constant.*log10(1+m);
            end

        end

% Display the output image.
figure, imshow(OutputImage);
title(' Final Image: ');


function prob_test0965 ( )

%*****************************************************************************80
%
%% TEST0965 tests LEVY_CDF, LEVY_CDF_INV, LEVY_PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0965\n' );
  fprintf ( 1, '  For the Levy PDF:\n' );
  fprintf ( 1, '  LEVY_CDF evaluates the CDF;\n' );
  fprintf ( 1, '  LEVY_CDF_INV inverts the CDF.\n' );
  fprintf ( 1, '  LEVY_PDF evaluates the PDF;\n' );

  a = 1.0;
  b = 2.0;

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

  seed = 123456789;

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

  for i = 1 : 10

    [ x, seed ] = levy_sample ( a, b, seed );

    pdf = levy_pdf ( x, a, b );

    cdf = levy_cdf ( x, a, b );

    x2 = levy_cdf_inv ( cdf, a, b );

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

  end

  return
end