%==============================================================================
% Copyright (C) 2005, Jan Modersitzki and Nils Papenberg, see copyright.m,
% this file is part of the FLIRT Package, all rights reserved;
% http://www.math.uni-luebeck.de/SAFIR/FLIRT-MATLAB.html
%==============================================================================
% function [B,Bstr] = getDiffusiveMatrix(Omega,m);
% generates the elastic matrix B for the domain Omega with resolution m,
% by default, mu = 1, lambda = 0;
%==============================================================================
function [B,Bstr] = getDiffusiveMatrixStg(Omega,m)

Bstr = 'diffusive-stg';
h = Omega./m;

d11 = spdiags(ones(m(1),1)*[-1,1],[0,1],m(1),m(1)+1)/h(1);
d12 = spdiags(ones(m(2)-1,1)*[-1,1],[0,1],m(2)-1,m(2))/h(2);
d21 = spdiags(ones(m(1)-1,1)*[-1,1],[0,1],m(1)-1,m(1))/h(1);
d22 = spdiags(ones(m(2),1)*[-1,1],[0,1],m(2),m(2)+1)/h(2);

D11 = sparse(kron(speye(m(2)),d11));
D12 = sparse(kron(d12,speye(m(1)+1)));
D21 = sparse(kron(speye(m(2)+1),d21));
D22 = sparse(kron(d22,speye(m(1))));

% build the diffusive operator
%
%      | \nabla 0      |
%  B = |               |
%      | 0      \nabla |
%      |               |
% B[U] = [\partial_1 U1,\partial_2 U1, \partial_1 U2,\partial_2 U2]

n1 = (m(1)+1)*m(2);
n2 = m(1)*(m(2)+1);
j1 = size(D11,1);
j2 = size(D12,1);
j3 = size(D21,1);
j4 = size(D22,1);

B = [  D11,sparse(j1,n2);
       D12,sparse(j2,n2);
       sparse(j3,n1),D21;
       sparse(j4,n1),D22];
return;
%==============================================================================


function bubblesort_complexity ( )

%*****************************************************************************80
%
%% BUBBLESORT_COMPLEXITY measures the complexity of the bubblesort algorithm.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 December 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BUBBLESORT_COMPLEXITY\n' );
  fprintf ( 1, '  How does the time requirement increase with vector length\n' );
  fprintf ( 1, '  for the bubblesort algorithm?\n' );
%
%  Get some data for N = 1 : 200.
%  Do the loop twice to avoid startup anomalies.
%
  for i = 1 : 2

    data_size = zeros ( 200, 1 );
    data_time = zeros ( 200, 1 );

    for n = 1 : 200
      a = rand ( n, 1 );
      tic;
      a_heap = r8vec_sort_bubble_a ( n, a );
      data_size(n) = n;
      data_time(n) = toc;
    end

  end

  figure ( 1 )
  plot ( data_size, data_time, 'r-' )
  grid on
  xlabel ( 'Vector length N' );
  ylabel ( 'Elapsed time T' );
  title ( 'T(N), time to heap sort a vector of length N' );

  filename = 'bubblesort_1to200.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot saved as "%s".\n', filename );
%
%  Get some data for N = 1, 2, 4, ..., 2^12.
%  Do the loop twice to avoid startup anomalies.
%
  for i = 1 : 2

    data_size = zeros ( 13, 1 );
    data_time = zeros ( 13, 1 );

    n = 1;

    for nlog = 0 : 12
      a = rand ( n, 1 );
      tic;
      a_heap = r8vec_sort_bubble_a ( n, a );
      data_size(nlog+1) = n;
      data_time(nlog+1) = toc;
      n = n * 2;
    end

  end

  figure ( 2 )
  loglog ( data_size, data_time, 'r-o' )
  grid on
  xlabel ( 'Vector length N' );
  ylabel ( 'Elapsed time T' );
  title ( 'T(N), time to heap sort a vector of length N' );

  filename = 'bubblesort_powersoftwo.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot saved as "%s".\n', filename );
!
!  Terminate.
!
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BUBBLESORT_COMPLEXITY\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function a = r8vec_sort_bubble_a ( n, a )

%*****************************************************************************80
%
%% R8VEC_SORT_BUBBLE_A ascending sorts an R8VEC using bubble sort.
%
%  Discussion:
%
%    Bubble sort is simple to program, but inefficient.  It should not
%    be used for large arrays.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), an unsorted array.
%
%    Output, real A(N), the array has been sorted.
%
  for i = 1 : n-1
    for j = i + 1 : n
      if ( a(j) < a(i) )
        t    = a(i);
        a(i) = a(j);
        a(j) = t;
      end
    end
  end

  return
end


function [ x, seed ] = triangle01_sample ( n, seed )

%*****************************************************************************80
%
%% TRIANGLE01_SAMPLE samples the interior of the unit triangle in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Reuven Rubinstein,
%    Monte Carlo Optimization, Simulation, and Sensitivity
%    of Queueing Networks,
%    Krieger, 1992,
%    ISBN: 0894647644,
%    LC: QA298.R79.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real X(2,N), the points.
%
  m = 2;

  for j = 1 : n

    [ e, seed ] = r8vec_uniform_01 ( m + 1, seed );

    e(1:m+1) = - log ( e(1:m+1) );

    x(1:m,j) = e(1:m) / sum ( e(1:m+1) );

  end

  return
end


function npartitions = npart_enum ( n, npart )

%*****************************************************************************80
%
%% NPART_ENUM enumerates the number of partitions of N with NPART parts.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the integer to be partitioned.
%    Normally N must be positive, but for this routine any
%    N is allowed.
%
%    Input, integer NPART, the number of parts of the partition.
%    Normally, 1 <= NPART <= N is required,
%    but for this routine any value of NPART is allowed.
%
%    Output, integer NPARTITIONS is the number of partitions of N
%    with NPART parts.
%
  if ( n <= 0 )

    npartitions = 0;

  elseif ( npart <= 0 || n < npart )

    npartitions = 0;

  else

    offset = 1;

    p = npart_table ( n, npart );

    npartitions = p(n+offset,npart+offset);

  end

  return
end


function [x_max y_max A]=crit_interp_g(y,x);
%fits a gaussian to three points: y=[y(x(1)) y(x(2)) y(x(3))]. Returns the
%position (x_max) and value (y_max) of the interpolated critical point
%(peak or trough). Things go sideways (complex) if y is mixed sign and
%rounding errors can cause spurious complex results if y is negative.
%If x is omitted, it is assumed to be [-1 0 1].
%y=A(1)*exp(-A(2)*(x-A(3)).^2)

%    Copyright Travis Wiens 2009 travis.mlfx@nutaksas.com

if nargin<2
    x=[-1 0 1];
end

lny=log(y);

%lny=1/denom*(a*x^2+b*x+c)
a =(x(3) * (lny(2) - lny(1)) + x(2) * (lny(1) - lny(3)) + x(1) * (lny(3) - lny(2)));
b =(x(3)*x(3) * (lny(1) - lny(2)) + x(2)*x(2) * (lny(3) - lny(1)) + x(1)*x(1) * (lny(2) - lny(3)));
c =(x(2) * x(3) * (x(2) - x(3)) * lny(1) + x(3) * x(1) * (x(3) - x(1)) * lny(2) + x(1) * x(2) * (x(1) - x(2)) * lny(3));

%y=A*exp(-B*(x-C)^2);
C=-b/(2*a);
x_max=C;

if nargout>1
    denom = (x(1) - x(2)) * (x(1) - x(3)) * (x(2) - x(3));
    A=exp(c/denom-b*b/(4*a*denom));
    y_max=A;
    if nargout>2
        B=-a/denom;
        A=[A B C];
    end
end


function M = specialeuclideanfactory(n, k)
% Returns a manifold structure to optimize over the special Euclidean group
%
% function M = specialeuclideanfactory(n)
% function M = specialeuclideanfactory(n, k)
%
% The special Euclidean group (the manifold of rigid transformations):
% This is a product manifold of the rotations group SO(n) and the
% translation group R^n, copied k times.
%
% Points on the manifold are represented as structures X with two fields.
% X.R is a 3D array of size nxnxk such that each slice X.R(:, :, i)
% corresponds to a rotation matrix (orthogonal with determinant 1).
% X.t is a matrix of size nxk such that each column X.t(:, i) corresponds
% to a translation vector.
%
% Tangent vectors are represented as structures with the same fields. Note
% that rotational components of the tangent vectors are represented in the
% Lie algebra, i.e., each slice Xdot.R(:, :, i) is a skew-symmetric matrix.
% Use M.tangent2ambient(X, Xdot) to obtain a representation in the ambient
% space.
%
% This is a description of SE(n)^k with the induced metric from the
% embedding space (R^nxn)^k x (R^n)^k, i.e., this manifold is a Riemannian
% submanifold of the embedding Euclidean space with the usual inner
% product.
%
% By default, k = 1.
%
% This is a test geometry: it may not be the "appropriate" geometry to give
% to SE(n).
%
% See rotationsfactory and euclideanfactory for details.
%
% See also: rotationsfactory euclideanfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Sep. 23, 2014.
% Contributors:
% Change log:

    if ~exist('k', 'var') || isempty(k)
        k = 1;
    end

    elements = struct();
    elements.R = rotationsfactory(n, k);
    elements.t = euclideanfactory(n, k);

    M = productmanifold(elements);

end


function quadrule_test25 ( )

%*****************************************************************************80
%
%% TEST25 tests LEGENDRE_SET_SQRTX2_01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2009
%
%  Author:
%
%    John Burkardt
%
  order_max = 20;

  a = 0.0;
  b = 1.0;

  nsub = 1;

  xlo = 0.0;
  xhi = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST25\n' );
  fprintf ( 1, '  LEGENDRE_SET_SQRTX2_01 sets up Gauss-Legendre\n' );
  fprintf ( 1, '  quadrature over [0,1] with weight function 1/SQRT(X);\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The integration interval is [%f, %f]\n', a, b );
  fprintf ( 1, '  Number of subintervals is %d\n', nsub );
  fprintf ( 1, '  Quadrature order will vary.\n' );
  fprintf ( 1, '  Integrand will vary.\n' );

  nfunc = func_set ( 'COUNT', 'DUMMY' );

  for ilo = 1 : 5 : nfunc

    ihi = min ( ilo + 4, nfunc );

    fprintf ( 1, '\n' );
    fprintf ( 1, '    ' );
    for i = ilo : ihi
      fprintf ( '%14s', fname(i) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );

    for iexp = 0 : 3

      norder = 2^iexp;

      fprintf ( 1, '  %2d', norder );

      for i = ilo : ihi

        func_set ( 'SET', i );

        [ xtab, weight ] = legendre_set_sqrtx2_01 ( norder );

        result = 0.0;
        for j = 1 : norder
          result = result + weight(j) * func ( xtab(j) );
        end

        fprintf ( 1, '  %12f', result );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


%% rand_angle
% Below is a demonstration of the features of the |rand_angle| function

%%
clear; close all; clc;

%% Syntax
% |a=rand_angle(siz);|

%% Description
% This function generates a matrix or array of uniformly distributed random
% angles (of the size siz) in the range [0 2*pi]. The function operates by
% first creating angles in the range 0-pi/2. The angles are next converted
% to unit vectors in the unit circle. The X and Y components of these
% vectors are next independantly and randomly negated. This negating or
% flipping operation causes the vectors to uniformly but randomly span the
% entire cirle. Nexts the vectors are converted to an angle in the range [0
% 2*pi].

%%
% Plot settings for examples
markerSize=25;
lineWidth=3;
fontSize=35;

%% Examples
%

siz=[500,1]; %Size of desired output
a=rand_angle(siz); % The random angles

%%

cFigure; hold on;
xlabel('Angles'); ylabel('Count');
histogram(a,linspace(0,2*pi,6));
set(gca,'FontSize',fontSize);
drawnow;

%%
% Visualization

%Create circle coordinates to visualize circle curve
t=linspace(0,2*pi,100)';
vc=[cos(t) sin(t)];
N=[cos(a) sin(a)];

cFigure; hold on;
hp1=plotV(vc,'b-','LineWidth',lineWidth);
hp2=plotV(N,'k.','MarkerSize',markerSize);
axis tight; axis equal; box on; grid on;
view(2);
set(gca,'FontSize',fontSize);
legend([hp1 hp2],{'Circle boundary','Uniformly distributed points on circle'},'Location','NorthOutside');
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>
%%
% _*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 y = convolve2(x, m, shape, tol)
%CONVOLVE2 Two dimensional convolution.
%   Y = CONVOLVE2(X, M) performs the 2-D convolution of matrices X and
%   M. If [mx,nx] = size(X) and [mm,nm] = size(M), then size(Y) =
%   [mx+mm-1,nx+nm-1]. Values near the boundaries of the output array are
%   calculated as if X was surrounded by a border of zero values.
%
%   Y = CONVOLVE2(X, M, SHAPE) where SHAPE is a string returns a
%   subsection of the 2-D convolution with size specified by SHAPE:
%
%       'full'    - (default) returns the full 2-D convolution,
%       'same'    - returns the central part of the convolution
%                   that is the same size as X (using zero padding),
%       'valid'   - returns only those parts of the convolution
%                   that are computed without the zero-padded
%                   edges, size(Y) = [mx-mm+1,nx-nm+1] when
%                   size(X) > size(M),
%       'wrap'    - as for 'same' except that instead of using
%                   zero-padding the input X is taken to wrap round as
%                   on a toroid.
%       'reflect' - as for 'same' except that instead of using
%                   zero-padding the input X is taken to be reflected
%                   at its boundaries.
%
%   CONVOLVE2 is fastest when mx > mm and nx > nm - i.e. the first
%   argument is the input and the second is the mask.
%
%   If the rank of the mask M is low, CONVOLVE2 will decompose it into a
%   sum of outer product masks, each of which is applied efficiently as
%   convolution with a row vector and a column vector, by calling CONV2.
%   The function will often be faster than CONV2 or FILTER2 (in some
%   cases much faster) and will produce the same results as CONV2 to
%   within a small tolerance.
%
%   Y = CONVOLVE2(... , TOL) where TOL is a number in the range 0.0 to
%   1.0 computes the convolution using a reduced-rank approximation to
%   M, provided this will speed up the computation. TOL limits the
%   relative sum-squared error in the effective mask; that is, if the
%   effective mask is E, the error is controlled such that
%
%       sum(sum( (M-E) .* (M-E) ))
%       --------------------------    <=  TOL
%            sum(sum( M .* M ))
%
%   See also CONV2, FILTER2.

% Copyright David Young, Feb 2002, revised Jan 2005, Jan 2009, Apr 2011

% Deal with optional arguments
error(nargchk(2,4,nargin));
if nargin < 3
    shape = 'full';    % shape default as for CONV2
    tol = 0;
elseif nargin < 4
    if isnumeric(shape)
        tol = shape;
        shape = 'full';
    else
        tol = 0;
    end
end;

% Set up to do the wrap & reflect operations, not handled by conv2
if ismember(shape, {'wrap' 'reflect'})
    x = extendarr(x, m, shape);
    shape = 'valid';
end

% do the convolution itself
y = doconv(x, m, shape, tol);
end

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

function y = doconv(x, m, shape, tol)
% Carry out convolution
[mx, nx] = size(x);
[mm, nm] = size(m);

% If the mask is bigger than the input, or it is 1-D already,
% just let CONV2 handle it.
if mm > mx || nm > nx || mm == 1 || nm == 1
    y = conv2(x, m, shape);
else
    % Get svd of mask
    if mm < nm; m = m'; end        % svd(..,0) wants m > n
    [u,s,v] = svd(m, 0);
    s = diag(s);
    rank = trank(m, s, tol);
    if rank*(mm+nm) < mm*nm         % take advantage of low rank
        if mm < nm;  t = u; u = v; v = t; end  % reverse earlier transpose
        vp = v';
        % For some reason, CONV2(H,C,X) is very slow, so use the normal call
        y = conv2(conv2(x, u(:,1)*s(1), shape), vp(1,:), shape);
        for r = 2:rank
            y = y + conv2(conv2(x, u(:,r)*s(r), shape), vp(r,:), shape);
        end
    else
        if mm < nm; m = m'; end     % reverse earlier transpose
        y = conv2(x, m, shape);
    end
end
end

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

function r = trank(m, s, tol)
% Approximate rank function - returns rank of matrix that fits given
% matrix to within given relative rms error. Expects original matrix
% and vector of singular values.
if tol < 0 || tol > 1
    error('Tolerance must be in range 0 to 1');
end
if tol == 0             % return estimate of actual rank
    tol = length(m) * max(s) * eps;
    r = sum(s > tol);
else
    ss = s .* s;
    t = (1 - tol) * sum(ss);
    r = 0;
    sm = 0;
    while sm < t
        r = r + 1;
        sm = sm + ss(r);
    end
end
end

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

function y = extendarr(x, m, shape)
% Extend x so as to wrap around on both axes, sufficient to allow a
% "valid" convolution with m to return the cyclical convolution.
% We assume mask origin near centre of mask for compatibility with
% "same" option.

[mx, nx] = size(x);
[mm, nm] = size(m);

mo = floor((1+mm)/2); no = floor((1+nm)/2);  % reflected mask origin
ml = mo-1;            nl = no-1;             % mask left/above origin
mr = mm-mo;           nr = nm-no;            % mask right/below origin

if strcmp(shape, 'wrap')
    y = exindex(x, 1-ml:mx+mr, 1-nl:nx+nr, 'circular');
else
    y = exindex(x, 1-ml:mx+mr, 1-nl:nx+nr, 'symmetric');
end

end


function pass = test_nonlinSys1Breaks_US(pref)
% Test 2x2 system (sin/cos). This is pecewiseificaion of the test
%       test_nonlinearSystem1
%
% Asgeir Birkisson, April 2014.

if ( nargin == 0 )
    pref = cheboppref;
end

tol = 1e-10;

d = [-pi 0 pi];
x = chebfun('x',d);
f = [ 0*x ; 0*x ];

%% Piecewise (ultraS):
pref.discretization = @ultraS;

A = chebop(@(x,u,v) [u - diff(v,2) + u.^2; diff(u) + sin(v)],d);
A.lbc = @(u,v) u-1;
A.rbc = @(u,v) [v-1/2; diff(v)];

u = mldivide(A, f, pref);
u1 = u{1}; u2 = u{2};

% Want to check BCs as well.
bcFunLeft = A.lbc(u1,u2);
bcFunRight = chebfun(A.rbc(u1,u2));

% And check that we're continuous over breakpoint
u1jump = jump(u1, 0);
u2jump = jump(u2, 0);

pass(1) = norm( chebfun(A(x, u1, u2))) < tol;
pass(2) = norm(bcFunLeft(d(1))) < tol && norm(bcFunRight(d(end))) < tol;
pass(3) = norm(u1jump) < tol && norm(u2jump) < tol;

end


function fx2 = p03_fx2 ( x )

%*****************************************************************************80
%
%% P03_FX2 evaluates the second derivative of the function for problem 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the abscissa.
%
%    Output, real FX2, the second derivative of the function at X.
%
  fx2 = exp ( - x ) * ( x - 2.0 );

  return
end


% This routine do:
% 1. Discretize schrodinger equation over the specified set of points:
%     v(x)=2m*V(x)/hbar^2; ee=2m*E/hbar^2.
%     [    d^2       2m        ]           2m*E
%     [ - -----  + ------ V(x) ] phi(x) = ------  phi(x)
%     [   d x^2    hbar^2      ]          hbar^2
%
% 2. Find eigen values and eigen vectors.
% 3. Plot the found eigen values and eigen vectors.
%
% Usage:
% [ee,ev] = qm1d_fast(NPTS,NSTM,a,b,f_pot_handle)
% NPTS - number of points for discretization of schrodinger equation
% NSTM - number of eigen values and eigen vector to find
% a - the start point of te interval for x
% b - the end point of the interval for y
% f_pot_handle - handle to function which defines the potential
%
% Examples:
%
% 1. Harmonic oscillator - symetric at x=L/2
% NPTS=1000;
% NSTM=5;
% L=10d0;
% f_pot = @(x) {4d0*(x-L/2).^2};
% qm1d_fast(NPTS,NSTM,0,L,f_pot);
%
% 2. Harmonic oscillator - symetric at x=0
% NPTS=1000;
% NSTM=5;
% L=10d0;
% f_pot = @(x) {4d0*x.^2};
% qm1d_fast(NPTS,NSTM,-L/2,L/2,f_pot);
%
% 3. Barrier - defined with heaviside function
% NPTS=1000;
% NSTM=5;
% L=100d0;
% bb=3d0;
% aa=1d0;
% f_pot = @(x) {heaviside(x-aa)-heaviside(x-bb)};
% qm1d_fast(NPTS,NSTM,0,L,f_pot);
%
% If you don't have heaviside defined in your Matlab version define this function as:
% function [y]=heaviside(x)
% y=zeros(1,length(x))
% ipos = find(x>0);
% y(ipos)=1;
%
% 4.Barrier
% NPTS=1000;
% NSTM=5;
% L=100d0;
% bb=3d0;
% aa=1d0;
% pot_dat = [zeros(1,int16(aa/L*NPTS)), ones(1,int16((bb-aa)/L*NPTS)),  zeros(1,int16((L-bb)/L*NPTS))];
% f_pot = @(x) {pot_dat(int16(x/L*(NPTS-1)))};
% qm1d_fast(NPTS,NSTM,0,L,f_pot);
%
% 5. Harmonic oscillator - with potential defined in file
% Define the potential in file:
% NPTS=1000;
% L=10d0;
% dx=L/(NPTS-1);
% pot_dat=[[0:NPTS-1]*dx; 4d0*([0:NPTS-1]-((NPTS-1)/2)).^2*dx^2]';
% save('pot.dat','-ascii','pot_dat');
% clear
%
% pot_dat=load('pot.dat');
% f_pot = @(x) {spline(pot_dat(:,1),pot_dat(:,2),x)};
% %It is not obligatory the NPTS to be the same as the number of points for the potential
% NPTS=10000;
% NSTM=5;
% L=10d0;
% qm1d_fast(NPTS,NSTM,0,L,f_pot);
%
%
% See also: http://iffwww.iff.kfa-juelich.de/~ekoch/DFT/qm1d.html
function [ee,ev] = qm1d_fast(NPTS,NSTM,a,b,f_pot_handle)
%pot_dat=load(pot_filename);
j=1:NPTS; % indexes for main diagonal
L=b-a;
h=L/(NPTS-1); % space step
x=j*h+a;
V=cell2mat(f_pot_handle(x));

main_diag=2/h^2+V(j);
sub_diag=-1/h^2*ones(1,NPTS-1);

[ee,ev] = trideigs(main_diag,sub_diag,'I',1,NSTM);

% average spacing of energy levels (for adjusting scale of ev)
de0=(ee(NSTM)-ee(1))/(NSTM-1);

% plot potential
plot(x,V(j),'r'); hold on;

% plot eign vectors
de=0.15*sqrt(NPTS)*de0;
for n=1:NSTM
    plot(x,ee(n)+de*ev(:,n)); hold on;
    plot(x,ones(length(j),1)*ee(n)); hold on;
end

% set up plotting options
xlim([0 NPTS]*h+a); ylim([min(V) ee(NSTM)+de0]);


%% CUBESTOKES Stokes equations on the unit cube
%
%   SQUARESTOKE computes CR-P0 approximations of the Stokes equations in
%   the unit cube.
%
% Added by Shuhao Cao. Apr, 2020.
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all; clear;

%% Set up
maxIt = 4;
N = zeros(maxIt,1);
h = zeros(maxIt,1);
erruH1 = zeros(maxIt,1);
erruL2 = zeros(maxIt,1);
errp = zeros(maxIt,1);

%% Generate initial mesh
[node,elem] = cubemesh([-0.5,0.5,-0.5,0.5,-0.5,0.5],0.25);
bdFlag = setboundary3(node,elem,'Dirichlet');

%% PDE and options
pde.exactu = @(p)[(1-cos(2*pi*p(:,1))).*sin(2*pi*p(:,2)).*sin(pi*p(:,3)),...
    -(1-cos(2*pi*p(:,2))).*sin(2*pi*p(:,1)).*sin(pi*p(:,3)), 0*p(:,3)];
pde.f = @(p) [-pi^2*(9*cos(2*pi*p(:,1))-5).*sin(2*pi*p(:,2)).*sin(pi*p(:,3))+ p(:,1).^2,...
    pi^2*(9*cos(2*pi*p(:,2))-5).*sin(2*pi*p(:,1)).*sin(pi*p(:,3)), 0*p(:,3)];
pde.g = @(p) zeros(size(p,1),1);
pde.exactp = @(p) p(:,1).^3/3;
pde.g_D = @(p) pde.exactu(p);
% solver
option.solver = 'diag'; % diagonal preconditioner

%% Finite Element Method
for k = 1:maxIt

    [soln,eqn] = Stokes3CRP0(node,elem,bdFlag,pde,option);
    uh = soln.u;
    uhvec = reshape(uh,size(uh,1)/3,3);
    ph = soln.p;
    N(k) = length(uh)+length(ph);
    h(k) = 1./((size(node,1)).^(1/3)-1);
    % compute error;
    uI = pde.exactu((node(eqn.face(:,1),:)+node(eqn.face(:,2),:)+node(eqn.face(:,3),:))/3);
    erruH1(k) = sqrt((uh-uI(:))'*eqn.A*(uh-uI(:)));
    erruL2(k) = getL2error3(node,elem,pde.exactu,uhvec);
    errp(k) = getL2error3(node,elem,pde.exactp,ph);
    fprintf('Number of Dof %d \n', N(k));

    if k < maxIt
        [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    end
end

%% Plot convergence rates and display error
figure(2);
showrateh3(h,erruH1,1,'-*','| u_I-u_h |_1',...
           h,erruL2,1,'-*','|| u-u_h ||',...
           h,errp,1,'-+','|| p-p_h||');

fprintf('\n');
disp('Table: Error')
colname = {'#Dof','h','|u_I-u_h|_1','||u-u_h||','||p-p_h||'};
disptable(colname,N,[],h,'%0.3e',erruH1,'%0.5e',erruL2,'%0.5e',errp,'%0.5e');


function [ p4, flag ] = line_exp_perp_2d ( p1, p2, p3 )

%*****************************************************************************80
%
%% LINE_EXP_PERP_2D computes a line perpendicular to a line and through a point.
%
%  Discussion:
%
%    The explicit form of a line in 2D is:
%
%      ( P1, P2 ) = ( (X1,Y1), (X2,Y2) ).
%
%    The input point P3 should NOT lie on the line (P1,P2).  If it
%    does, then the output value P4 will equal P3.
%
%    P1-----P4-----------P2
%            |
%            |
%           P3
%
%    P4 is also the nearest point on the line (P1,P2) to the point P3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 July 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(2,1), P2(2,1), two points on the line.
%
%    Input, real P3(2,1), a point (presumably not on the
%    line (P1,P2)), through which the perpendicular must pass.
%
%    Output, real P4(2,1), a point on the line (P1,P2),
%    such that the line (P3,P4) is perpendicular to the line (P1,P2).
%
%    Output, logical FLAG, is TRUE if the value could not be computed.
%
  dim_num = 2;

  bot = sum ( ( p2(1:dim_num,1) - p1(1:dim_num,1) ).^2 );

  if ( bot == 0.0 )
    p4(1:2,1) = Inf;
    flag = 1;
  end
%
%  (P3-P1) dot (P2-P1) = Norm(P3-P1) * Norm(P2-P1) * Cos(Theta).
%
%  (P3-P1) dot (P2-P1) / Norm(P3-P1)**2 = normalized coordinate T
%  of the projection of (P3-P1) onto (P2-P1).
%
  t = sum ( ( p1(1:dim_num,1) - p3(1:dim_num,1) ) ...
    .* ( p1(1:dim_num,1) - p2(1:dim_num,1) ) ) / bot;

  p4(1:dim_num,1) = p1(1:dim_num,1) + t * ( p2(1:dim_num,1) - p1(1:dim_num,1) );
  flag = 0;

  return
end


function a = maxij ( m, n )

%*****************************************************************************80
%
%% MAXIJ returns the MAXIJ matrix.
%
%  Discussion:
%
%    This matrix is occasionally known as the "Boothroyd MAX" matrix.
%
%  Formula:
%
%    A(I,J) = max(I,J)
%
%  Example:
%
%    N = 5
%
%    1 2 3 4 5
%    2 2 3 4 5
%    3 3 3 4 5
%    4 4 4 4 5
%    5 5 5 5 5
%
%  Properties:
%
%    A is integral, therefore det ( A ) is integral, and
%    det ( A ) * inverse ( A ) is integral.
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    The inverse of A is tridiagonal.
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory, David Karney,
%    Example 3.13,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, 1969, page 42,
%    LC: QA263.G68.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns
%    of the matrix.
%
%    Output, real A(M,N), the matrix.
%
  for i = 1 : m
    for j = 1 : n
      a(i,j) = max ( i, j );
    end
  end

  return
end


function OUT = MovSum(DATA,window)
% =======================================================================
% Moving sum of the vector (or matrix) DATA (T obs x N variables). If
% DATA is a matrix moving sum is computed down each column.
% =======================================================================
% OUT = MovAvg(DATA,window)
% -----------------------------------------------------------------------
% INPUT
%   DATA: T observations x N variables
%   window: window of the moving sum
%------------------------------------------------------------------------
% OUPUT
%   OUT: T observations x N variables matrix (the first windows-1
%       obseravations are NaN)
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogio.cesabianchi@gmail.com

if nargin<2,                error('Not enough input.'),          end
if window<=0,               error('window must be positive.'),   end
if (window~=floor(window)), error('window must be an integer.'), end

if min(size(DATA))==1,
    DATA = DATA(:); % forces DATA to be a column vector
end

[nobs,nvar] = size(DATA);
if window>nobs,
    error('window must not be greater than the length of DATA.')
end

temp=[];
for row=1:(nobs-window+1),
    temp = [temp; sum(DATA(row:(row+window-1),:))];
end

OUT = temp;
OUT = [nan(window-1,nvar); OUT]; % add nans to make conformable to original


function lattice_rule_test01 ( )

%*****************************************************************************80
%
%% LATTICE_RULE_TEST01 tests FIBONACCI_LATTICE_Q;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ian Sloan, Stephen Joe,
%    Lattice Methods for Multiple Integration,
%    Oxford, 1994, page 78-80, page 145.
%
  dim_num = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LATTICE_RULE_TEST01\n' );
  fprintf ( 1, '  FIBONACCI_LATTICE_Q applies a Fibonacci lattice rule\n' );
  fprintf ( 1, '  to integrate a function over the unit square.\n' );
  fprintf ( 1, '  These Fibonacci rules are only available in 2D.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The spatial dimension DIM_NUM = %d\n', dim_num );

  a(1:dim_num) = 0.0;
  b(1:dim_num) = 1.0;

  exact = e_01_2d ( dim_num, a, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '         K         M    EXACT       ESTIMATE    ERROR\n' );
  fprintf ( 1, '\n' );

  for k = 3 : 18

    quad = fibonacci_lattice_q ( k, @f_01_2d );

    error = abs ( exact - quad );
    m = fibonacci ( k );

    fprintf ( 1, '  %8d  %8d  %10.6f  %10.6f  %10.6e\n', k, m, exact, quad, error );

  end

  return
end


function prob_test082 ( )

%*****************************************************************************80
%
%% TEST082 tests GEOMETRIC_MEAN, *_SAMPLE, *_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2009
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROB_TEST082\n' );
  fprintf ( 1, '  For the Geometric PDF:\n' );
  fprintf ( 1, '  GEOMETRIC_MEAN computes the mean;\n' );
  fprintf ( 1, '  GEOMETRIC_SAMPLE samples;\n' );
  fprintf ( 1, '  GEOMETRIC_VARIANCE computes the variance.\n' );

  a = 0.25;

  check = geometric_check ( a );

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

  mean = geometric_mean ( a );
  variance = geometric_variance ( a );

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

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

  mean = i4vec_mean ( nsample, x );
  variance = i4vec_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

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

  return
end


function p = predictOneVsAll(all_theta, X)
%PREDICT Predict the label for a trained one-vs-all classifier. The labels
%are in the range 1..K, where K = size(all_theta, 1).
%  p = PREDICTONEVSALL(all_theta, X) will return a vector of predictions
%  for each example in the matrix X. Note that X contains the examples in
%  rows. all_theta is a matrix where the i-th row is a trained logistic
%  regression theta vector for the i-th class. You should set p to a vector
%  of values from 1..K (e.g., p = [1; 3; 1; 2] predicts classes 1, 3, 1, 2
%  for 4 examples)

m = size(X, 1);
num_labels = size(all_theta, 1);

% You need to return the following variables correctly
p = zeros(size(X, 1), 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters (one-vs-all).
%               You should set p to a vector of predictions (from 1 to
%               num_labels).
%
% Hint: This code can be done all vectorized using the max function.
%       In particular, the max function can also return the index of the
%       max element, for more information see 'help max'. If your examples
%       are in rows, then, you can use max(A, [], 2) to obtain the max
%       for each row.
%
% temp = (X*all_theta');
% p(:) = max(temp,[],2);

h_theta = sigmoid(X * all_theta');
p = max(h_theta, [], 2);

% temp is of dimension 5000x10

% Dimention of X: 5000x401
% Dimension of all_theta: 10x401
% Dimension of X*all_theta': 5000x10

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

end


% This is small script that demonstrate the computation of extrinsic
% parameters using 3D structures.
% This test was build from data provided by Daniel Small (thank you Daniel!)

%-- Image points (in pixels):

x = [479.5200  236.0800
  608.4100  415.3700
  461.0000   40.0000
  451.4800  308.7000
  373.9900  314.8900
  299.3200  319.1300
  231.5500  321.3700
  443.7300  282.9200
  378.3600  288.3000
  314.6900  292.7400
  255.4700  296.2300]';

% 3D world coordinates:

X = [ 0         0         0
   54.0000         0         0
         0         0   40.5000
   27.0000   -8.4685   -2.3750
   27.0000  -18.4685   -2.3750
   27.0000  -28.4685   -2.3750
   27.0000  -38.4685   -2.3750
   17.0000   -8.4685   -2.3750
   17.0000  -18.4685   -2.3750
   17.0000  -28.4685   -2.3750
   17.0000  -38.4685   -2.3750]';

%------------ Intrinsic parameters:
%--- focal:
fc = [ 395.0669  357.1178 ]';
%--- Principal point:
cc = [ 380.5387  230.5278 ]';
%--- Distortion coefficients:
kc = [-0.2601    0.0702   -0.0019   -0.0003         0]';
%--- Skew coefficient:
alpha_c = 0;

%----- Computation of the pose of the object in space
%----- (or the rigid motion between world reference frame and camera ref. frame)
[om,T,R] = compute_extrinsic(x,X,fc,cc,kc,alpha_c);

%--- Try to reproject the structure to see if the computed pose makes sense:
x2 = project_points2(X_1,omckk,Tckk,fc,cc,kc,alpha_c);

% Graphical output:
figure(2);
plot(x(1,:),x(2,:),'r+');
hold on;
plot(x2(1,:),x2(2,:),'go');
hold off;
axis('equal');
axis('image');
title('red crosses: data, green circles: reprojected structure -- IT WORKS!!!');
xlabel('x');
ylabel('y');


function gauss_seidel_test01 ( )

%*****************************************************************************80
%
%% GAUSS_SEIDEL_TEST01 tests GAUSS_SEIDEL1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 June 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAUSS_SEIDEL_TEST01:\n' );

  it_num = 400;
  n = 20;

  x_exact = ( 1 : n )';
  a = dif2 ( n );
  b = a * x_exact;

  x = zeros ( n, 1 );
  x_plot(1:n,1) = x;

  step = 1 : it_num + 1;
  e = nan ( it_num+1, 1 );
  xm = nan ( it_num+1, 1 );

  e(1,1) = ( norm ( a * x - b ) ).^2;

  for it = 1 : it_num

    x_new = gauss_seidel1 ( n, a, b, x );

    e(it+1,1) = ( norm ( a * x_new - b ) ).^2;
    x_plot(1:n,it+1) = x_new;
%
%  Display the error.
%
    figure ( 1 )
    plot ( step, log ( e ), 'm-*' )
    title ( 'Log (Error^2)' )
    xlabel ( 'Step' )
    ylabel ( 'Error' )
    grid
%
%  Display the motion.
%
    xm(it,1) = sum ( ( x_new(:) - x(:) ).^2 ) / n;

    figure ( 2 )
    plot ( step, log ( xm ), 'm-*' )
    title ( 'Log (Average generator motion)' )
    xlabel ( 'Step' )
    ylabel ( 'Energy' )
    grid
%
%  Update the solution
%
    x = x_new;

  end
%
%  Plot the evolution of the locations of the generators.
%
  figure ( 3 )

  y = ( 0 : it_num );
  for k = 1 : n
    plot ( x_plot(k,1:it_num+1), y )
    hold on;
  end
  grid on
  hold off;

  title ( 'Generator evolution.' );
  xlabel ( 'Generator positions' );
  ylabel ( 'Iterations' );

  return
end


function value = r4_sin_deg ( x )

%*****************************************************************************80
%
%% R4_SIN_DEG evaluates the sine of an R4 argument in degrees.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument in degrees.
%
%    Output, real VALUE, the sine of X.
%
  raddeg = 0.017453292519943296;

  value = sin ( raddeg * x );

  if ( mod ( x, 90.0 ) == 0.0 )

    n = floor ( abs ( x ) / 90.0 + 0.5 );
    n = mod ( n, 2 );

    if ( n == 0 )
      value = 0.0;
    elseif ( value < 0.0 )
      value = - 1.0;
    else
      value = + 1.0;
    end

  end

  return
end


% FDLA and FMMC solutions for an 8-node, 13-edge graph
% S. Boyd, et. al., "Convex Optimization of Graph Laplacian Eigenvalues"
% ICM'06 talk examples (www.stanford.edu/~boyd/cvx_opt_graph_lapl_eigs.html)
% Written for CVX by Almir Mutapcic 08/29/06
% (figures are generated)
%
% In this example we consider a graph described by the incidence matrix A.
% Each edge has a weight W_i, and we optimize various functions of the
% edge weights as described in the referenced paper; in particular,
%
% - the fastest distributed linear averaging (FDLA) problem (fdla.m)
% - the fastest mixing Markov chain (FMMC) problem (fmmc.m)
%
% Then we compare these solutions to the heuristics listed below:
%
% - maximum-degree heuristic (max_deg.m)
% - constant weights that yield fastest averaging (best_const.m)
% - Metropolis-Hastings heuristic (mh.m)

% small example (incidence matrix A)
A = [ 1  0  0  1  0  0  0  0  0  0  0  0  0;
     -1  1  0  0  1  1  0  0  0  0  0  0  1;
      0 -1  1  0  0  0  0  0 -1  0  0  0  0;
      0  0 -1  0  0 -1  0  0  0 -1  0  0  0;
      0  0  0 -1  0  0 -1  1  0  0  0  0  0;
      0  0  0  0  0  0  1  0  0  0  1  0  0;
      0  0  0  0  0  0  0 -1  1  0 -1  1 -1;
      0  0  0  0 -1  0  0  0  0  1  0 -1  0];

% x and y locations of the graph nodes
xy = [ 1 2   3 3 1 1 2   3 ; ...
       3 2.5 3 2 2 1 1.5 1 ]';

% Compute edge weights: some optimal, some based on heuristics
[n,m] = size(A);

[ w_fdla, rho_fdla ] = fdla(A);
[ w_fmmc, rho_fmmc ] = fmmc(A);
[ w_md,   rho_md   ] = max_deg(A);
[ w_bc,   rho_bc   ] = best_const(A);
[ w_mh,   rho_mh   ] = mh(A);

tau_fdla = 1/log(1/rho_fdla);
tau_fmmc = 1/log(1/rho_fmmc);
tau_md   = 1/log(1/rho_md);
tau_bc   = 1/log(1/rho_bc);
tau_mh   = 1/log(1/rho_mh);

fprintf(1,'\nResults:\n');
fprintf(1,'FDLA weights:\t\t rho = %5.4f \t tau = %5.4f\n',rho_fdla,tau_fdla);
fprintf(1,'FMMC weights:\t\t rho = %5.4f \t tau = %5.4f\n',rho_fmmc,tau_fmmc);
fprintf(1,'M-H weights:\t\t rho = %5.4f \t tau = %5.4f\n',rho_mh,tau_mh);
fprintf(1,'MAX_DEG weights:\t rho = %5.4f \t tau = %5.4f\n',rho_md,tau_md);
fprintf(1,'BEST_CONST weights:\t rho = %5.4f \t tau = %5.4f\n',rho_bc,tau_bc);

% Plot results
figure(1), clf
plotgraph(A,xy,w_fdla);
text(0.55,1.05,'FDLA optimal weights')

figure(2), clf
plotgraph(A,xy,w_fmmc);
text(0.55,1.05,'FMMC optimal weights')

figure(3), clf
plotgraph(A,xy,w_md);
text(0.5,1.05,'Max degree optimal weights')

figure(4), clf
plotgraph(A,xy,w_bc);
text(0.5,1.05,'Best constant optimal weights')

figure(5), clf
plotgraph(A,xy,w_mh);
text(0.46,1.05,'Metropolis-Hastings optimal weights')


function value = monomial_value ( dim_num, point_num, x, expon )

%*****************************************************************************80
%
%% MONOMIAL_VALUE evaluates a monomial.
%
%  Discussion:
%
%    This routine evaluates a monomial of the form
%
%      product ( 1 <= dim <= dim_num ) x(dim)^expon(dim)
%
%    where the exponents are nonnegative integers.  Note that
%    if the combination 0^0 is encountered, it should be treated
%    as 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 August 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer POINT_NUM, the number of points at which the
%    monomial is to be evaluated.
%
%    Input, real X(DIM_NUM,POINT_NUM), the point coordinates.
%
%    Input, integer EXPON(DIM_NUM), the exponents.
%
%    Output, real VALUE(POINT_NUM), the value of the monomial.
%
  value(1,1:point_num) = 1.0;

  for dim = 1 : dim_num
    if ( 0 ~= expon(dim) )
      value(1,1:point_num) = value(1,1:point_num) .* ( x(dim,1:point_num).^expon(dim) );
    end
  end

  return
end


function y = sqrt_cordic ( x, n )

%*****************************************************************************80
%
%% SQRT_CORDIC returns the square root of a value using the CORDIC method.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the number whose square root is desired.
%
%    Input, integer N, the number of iterations to take.
%    This is essentially the number of binary digits of accuracy.
%
%    Output, real Y, the approximate square root of X.
%
  if ( x < 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SQRT_CORDIC - Fatal error!\n' );
    fprintf ( 1, '  X < 0.\n' );
    error ( 'SQRT_CORDIC - Fatal error!' )
  end

  if ( x == 0.0 )
    y = 0.0;
    return
  end

  if ( x == 1.0 )
    y = 1.0;
    return
  end

  poweroftwo = 1.0;

  if ( x < 1.0 )

    while ( x <= poweroftwo * poweroftwo )
      poweroftwo = poweroftwo / 2.0;
    end

    y = poweroftwo;

  elseif ( 1.0 < x )

    while ( poweroftwo * poweroftwo <= x )
      poweroftwo = 2.0 * poweroftwo;
    end

    y = poweroftwo / 2.0;

  end

  for i = 1 : n
    poweroftwo = poweroftwo / 2.0;
    if ( ( y + poweroftwo ) * ( y + poweroftwo ) <= x )
      y = y + poweroftwo;
    end
  end

  return
end


function h = compute_histogram_rbf(f, sigma, x, options)

% compute_histogram_rbf - parzen windows density estimation
%
%   h = compute_histogram_rbf(f, sigma, x);
%
%   f is the signal, h is an estimate of the histogram,
%   where h(i) is the density of the estimation around value x(i).
%
%   sigma is the bandwidth used to estimate the histogram (approx. size of
%       the bins).
%
%   If f is (n,2) matrix, then a joint histogram is estimated.
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;

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

nb_samples = Inf;
if isfield(options, 'nb_samples')
    nb_samples = options.nb_samples;
end
nb_samples = min(nb_samples,size(f,1));
sel = randperm(size(f,1)); sel = sel(1:nb_samples);
f = f(sel,:);

if size(f,2)==2
    h = compute_histogram_rbf_2d(f, sigma, x, options);
    return;
end

if isfield(options, 'renormalize')
    renormalize = options.renormalize;
else
    renormalize = 0;
end

n = length(x);
p = length(f);  % number of samples
f = f(:); x = x(:);

% h is of size p x n
h = repmat(f, [1 n]) - repmat( x(:)', [p 1] );
h = exp( -h.^2/(2*sigma^2) );
if renormalize
    d = repmat( sum(h,2), [1 n] );
    d(d<eps) = 1;
end
h = h./d;
h = sum(h,1);
h = h(:)/sum(h(:));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = compute_histogram_rbf_2d(f, sigma, x, options)

options.null = 0;

if isfield(options, 'nb_max')
    nb_max = options.nb_max;
else
    nb_max = 0;
end

if isfield(options, 'renormalize')
    renormalize = options.renormalize;
else
    renormalize = 0;
end

n = size(x,1);
p = size(f,1);

if p>nb_max
    h = zeros(n);
    niter = ceil(p/nb_max);
    for i=1:niter
        progressbar(i,niter);
        sel = (i-1)*nb_max+1:min(i*nb_max,p);
        h = h + compute_histogram_rbf_2d( f(sel,:) , sigma, x, options);
    end
    h = h/sum(h(:));
    return;
end

% hx is of size p x n x n
hx = repmat( f(:,1), [1 n n]) - repmat( reshape(x(:,1),[1 n 1]),[p 1 n] );
hy = repmat( f(:,2), [1 n n]) - repmat( reshape(x(:,2),[1 1 n]),[p n 1] );
h = exp( -(hx.^2/(2*sigma(1)^2)+hy.^2/(2*sigma(2)^2)) );
if renormalize
    d = repmat( sum(sum(h,2),3), [1 n n] );
    d(d<eps) = 1;
    h = h./d;
end
h = sum(h,1);
h = reshape(h,n,n);
h = h./sum(sum(h));


function spline_test205 ( )

%*****************************************************************************80
%
%% TEST205 tests SPLINE_LINEAR_INT, SPLINE_LINEAR_INTSET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author
%
%    John Burkardt
%
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST205\n' );
  fprintf ( 1, '  SPLINE_LINEAR_INTSET is given some interval endpoints,\n' );
  fprintf ( 1, '  and a value associated with each interval.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  It determines a linear spline, with breakpoints\n' );
  fprintf ( 1, '  at the centers of each interval, whose integral\n' );
  fprintf ( 1, '  over each interval is equal to the given value.\n' );

  int_x(1:n+1) = [ 0.0, 1.0, 4.0, 5.0, 10.0 ]';
  int_v(1:n) = [ 10.0, 2.0, 8.0, 27.5 ]';

  r8vec_print ( n+1, int_x, '  The interval end points:' );
  r8vec_print ( n, int_v, '  The desired interval integral values:' );

  [ data_x, data_y ] = spline_linear_intset ( n, int_x, int_v );

  r8vec_print ( n, data_x, '  The spline break points:' );
  r8vec_print ( n, data_y, '  The spline data values: ' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  As a check, call SPLINE_LINEAR_INT to compute\n' );
  fprintf ( 1, '  the integral of the spline over each interval,\n' );
  fprintf ( 1, '  and compare to the desired value.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     A       B    Desired      Computed\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    a = int_x(i);
    b = int_x(i+1);
    value = spline_linear_int ( n, data_x, data_y, a, b );
    fprintf ( 1, '%6f  %6f  %12f  %12f\n', a, b, int_v(i), value );
  end

  return
end


function kel = cel2kel(cel)
%CEL2KEL Convert temperature from celsius to kelvin
%
%  kel = CEL2KEL(cel) converts temperature from celsius to kelvin.
%
%  See also KEL2CEL, CEL2FAR, CEL2RANK.

kel = cel + 273.15;


function R = p2p_optimum(a, n_vals, eps)
% Compute the achievable sum rate (at the symmetrical rate point) based on the point-to-point optimal code
% for the joint source distribution: [a 1/3*(1-a); 1/3*(1-a) 1/3*(1-a)].

% Input:
% a: parameter to define the joint source distribution, should be in range [0.25, 1)
% n_vals: a vector containing the blocklengths
% eps: target error probability
% Output:
% R: the optimal rates for the blocklengths specified in n_vals.

    % Compute log binomial coefficients
    log_b = binomial_coeff(max(n_vals));

    R = zeros(length(n_vals),1);

    for i = 1:length(n_vals)
        n = n_vals(i);
        m = 0:n;  % Number of occurrence of the largest joint probability mass
        % log_Pr stores all the log probabilities in descending order
        log_Pr = m*log(a)+(n-m)*log(1/3*(1-a));

        % Compute the optimal rate
        log_sum_Pr = log_b{n+1}(n+1)+log_Pr(n+1);
        k = n;
        while exp(log_sum_Pr) < 1 - eps
            % Add up the probabilities of the strings until the total probability >= 1 - eps
            k = k - 1;
            log_sum_Pr = my_logsumexp([log_sum_Pr log_b{n+1}(k+1)+(n-k)*log(3)+log_Pr(k+1)]);
        end
        d = 1 - eps - (exp(log_sum_Pr) - exp(log_b{n+1}(k+1)+(n-k)*log(3)+log_Pr(k+1)));
        M1 = my_logsumexp(log_b{n+1}(k+2:n+1)+(n-(k+1:n))*log(3));
        M2 = log(d) - log_Pr(k+1);
        R(i) = 1/n*my_logsumexp([M1 M2]);
    end
end


% Get the freq. and amp. parameters of a spectral peak by quandratic fitting
%
% Octave compatible
%
% Inputs
%  S        : The spectrum containing the peak to fit
%  fs       : [Hz] The sampling frequency
%  index    : The peak position where the parabola has to be fit
%  [zp]     : the zero padding factor used for the DFT computation
%             (used for the Abe & Smith corrections)
%  [wintype]: The window type used for the DFT computation
%             wintype=2 => Hann
%
% Outputs
%  freq     : [Hz] The frequency parameter
%  amp      : The amplitude parameter (linear scale)
%
% Copyright (c) 2012 University of Crete - Computer Science Department
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function [freq amp p] = spec_fit_freq_amp(S, fs, index, zp, wintype)

    if index<=1
        freq = index;
        amp = log(abs(S(1)));
        return
    elseif index>=length(S)
        freq = index;
        amp = log(abs(S(end)));
        return
    end

    % fit a parabola in log amplitudes
    y1 = log(abs(S(index-1)));
    y2 = log(abs(S(index)));
    y3 = log(abs(S(index+1)));
    A = (y3-y2)/2 + (y1-y2)/2;
    B = -( y1 - y2 - A );
    C = y1 - A + B;
    p = [A B C];

    % max abscissa
    di = -p(2)/(2*p(1));

    % max amplitude
    logamp = p(1)*di*di + p(2)*di + p(3);

    if nargin>3 && ~isempty(zp)
        % Use Abe & Smith corrections
        if wintype==2;
            c0=0.247560; c1=0.084372; c2=-0.090608; c3=-0.055781;   % Hann
        else
            error('Unknown correction terms for the given window for Abe&Smith corrections');
        end
        delta = di;

        % Frequency correction
        ksi = c0*zp^(-2) + c1*zp^(-4);                  % (3)
        di = delta + ksi*(delta-0.5)*(delta+0.5)*delta; % (1)

        % Amplitude correction
        ita = c2*zp^(-4) + c3*zp^(-6);                  % (4)
        logamp = logamp + ita*di^2;                     % (2)
    end

    freq = index + di;
    freq = (freq-1)*(fs/length(S));

    amp = exp(logamp);
return


% MAIN.m  --  Lesson 1 -- Simple Plot
%
% This script performs a simulation of the cart as it passively moves from
% some initial state.
%
% The purpose of this lesson is to learn how use ode45 to generate data for
% a simulation, and then plot the state of the system to see what is going
% on.
%

clc; clear;

%%%% Initial State
z0 = [
    0.0;   %horizontal position
    (pi/180)*80;  %pendulum angle (wrt gravity)
    0.3;   %horizontal velocity
    0.5];  %pendulum angular rate

%%%% Physical Parameters
p.m1 = 1.0;  % (kg) Cart mass
p.m2 = 0.3;  % (kg) pole mass
p.g = 9.81;  % (m/s^2) gravity
p.l = 0.5;   % (m) pendulum (pole) length

%%%% Time vector
t = linspace(0,2,250);  %Simulation time stamps

%%%% Function Handle
dynFun = @(t,z)( cartPoleDynamics(z, p) );

%%%% Simulate the system!
options = odeset(...
    'RelTol',1e-8, ...
    'AbsTol',1e-8);
[~, z] = ode45(dynFun, t, z0, options);   %  <-- This is the key line!
z = z';

%%%% Unpack the state:
x = z(1,:);
q = z(2,:);
dx = z(3,:);
dq = z(4,:);

%%%% Plots:
figure(1); clf;

subplot(2,2,1);
plot(t,x)
ylabel('x')
title('Position')

subplot(2,2,2);
plot(t,q)
ylabel('q')
title('Angle')

subplot(2,2,3);
plot(t,dx)
ylabel('dx')
title('Velocity')

subplot(2,2,4);
plot(t,dq)
ylabel('dq')
title('Angle Rate')


clear all; close all;
I=imread('rice.png');
I=im2double(I);
M=2*size(I,1);
N=2*size(I,2);
u=-M/2:(M/2-1);
v=-N/2:(N/2-1);
[U,V]=meshgrid(u, v);
D=sqrt(U.^2+V.^2);
D0=30;
n=6;
H=1./(1+(D0./D).^(2*n));
J=fftshift(fft2(I, size(H, 1), size(H, 2)));
K=J.*H;
L=ifft2(ifftshift(K));
L=L(1:size(I,1), 1:size(I, 2));
figure;
subplot(121);
imshow(I);
subplot(122);
imshow(L);


function determ = chow_determinant ( alpha, beta, n )

%*****************************************************************************80
%
%% CHOW_DETERMINANT returns the determinant of the CHOW matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the ALPHA value.  A typical value is 1.0.
%
%    Input, real BETA, the BETA value.  A typical value is 0.0.
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM, the determinant.
%
  determ = 1.0;

  k = n - floor ( n / 2 );

  for i = 1 : k
    angle = i * pi / ( n + 2 );
    determ = determ * ( beta + 4.0 * alpha * ( cos ( angle ) )^2 );
  end

  determ  = determ * beta^( n - k );

  return
end


%% Simulating Pole Figure data
%
%%
% Simulating pole figure data from a given ODF is useful to investigate
% pole figure to ODF reconstruction routines. Let us start with a model ODF
% given as the superposition of 6 components.

cs = crystalSymmetry('orthorhombic');
mod1 = orientation.byAxisAngle(xvector,45*degree,cs);
mod2 = orientation.byAxisAngle(yvector,65*degree,cs);
model_odf = 0.5*uniformODF(cs) + ...
  0.05*fibreODF(Miller(1,0,0,cs),xvector,'halfwidth',10*degree) + ...
  0.05*fibreODF(Miller(0,1,0,cs),yvector,'halfwidth',10*degree) + ...
  0.05*fibreODF(Miller(0,0,1,cs),zvector,'halfwidth',10*degree) + ...
  0.05*unimodalODF(mod1,'halfwidth',15*degree) + ...
  0.3*unimodalODF(mod2,'halfwidth',25*degree);

%%

plot(model_odf,'sections',6,'silent','sigma')

%%
% In order to simulate pole figure data, the following parameters have to be
% specified
%
% # an arbitrary <SO3Fun.SO3Fun.html ODF>
% # a list of <Miller.Miller.html Miller indece>
% # a grid of <S2Grid.S2Grid.html specimen directions>
% # superposition coefficients (optional)
% # the magnitude of error (optional)
%
%%
% The list of <Miller.Miller.html Miller indece>

h = [Miller(1,1,1,cs),Miller(1,1,0,cs),Miller(1,0,1,cs),Miller(0,1,1,cs),...
  Miller(1,0,0,cs),Miller(0,1,0,cs),Miller(0,0,1,cs)];

%%
% The <S2Grid.S2Grid.html grid> of specimen directions

r = regularS2Grid('resolution',5*degree);

%%
% Now the pole figures can be simulated using the command
% <SO3Fun.calcPoleFigure.html calcPoleFigure>.

pf = calcPoleFigure(model_odf,h,r)

%%
% Add some noise to the data. Here we assume that the mean intensity is 1000.

pf = noisepf(pf,1000);

%%
% Plot the simulated pole figures.

plot(pf)

%% ODF Estimation from Pole Figure Data
%
% From these simulated pole figures we can now estimate an ODF,

odf = calcODF(pf)

%%
% which can be plotted,

plot(odf,'sections',6,'silent','sigma')

%%
% and compared to the original model ODF.

calcError(odf,model_odf,'resolution',5*degree)

%% Exploration of the relationship between estimation error and number of pole figures
%
% For a more systematic analysis of the estimation error, we vary the number
% of pole figures used for ODF estimation from 1 to 7 and calculate for any
% number of pole figures the approximation error. Furthermore, we also
% apply ghost correction and compare the approximation error to the
% previous reconstructions.

e = [];
for i = 1:pf.numPF

  odf = calcODF(pf({1:i}),'silent','NoGhostCorrection');
  e(i,1) = calcError(odf,model_odf,'resolution',2.5*degree);
  odf = calcODF(pf({1:i}),'silent');
  e(i,2) = calcError(odf,model_odf,'resolution',2.5*degree);

end

%%
% Plot the error in dependency of the number of single orientations.

close all;
plot(1:pf.numPF,e,'LineWidth',2)
ylim([0.07 0.32])
xlabel('Number of Pole Figures');
ylabel('Reconstruction Error');
legend({'Without Ghost Correction','With Ghost Correction'});


%% Generate an Airy Pattern and add noise.
N=65;
xrange=[-1 1];
x=linspace(xrange(1),xrange(2),N);
[xx, yy]=meshgrid(x);
[theta, rr]=cart2pol(xx,yy);
AiryPattern=jinc(rr).^2;
AiryPattern=uint16((2^10-1)*AiryPattern);
NoisyPattern=imnoise(AiryPattern,'poisson');

%% Filter and Interpolate.
xs=x(2)-x(1);
fcut=2; % The Fourier transform of the Airy Pattern (chinese hat) cuts-off at 2 in frequency domain.
%gaussorder=500;
AiryFiltI1=opticalLowpassInterpolation(AiryPattern,xs,fcut,1);
AiryFiltI2=opticalLowpassInterpolation(AiryPattern,xs,fcut,2);
AiryFiltI3=opticalLowpassInterpolation(AiryPattern,xs,fcut,3);
NoisyFiltI1=opticalLowpassInterpolation(NoisyPattern,xs,fcut,1);
NoisyFiltI2=opticalLowpassInterpolation(NoisyPattern,xs,fcut,2);
NoisyFiltI3=opticalLowpassInterpolation(NoisyPattern,xs,fcut,3);
figure(1); clf; colormap jet;
set(1,'defaultaxesfontsize',16);
imagecat(x,x,AiryPattern,AiryFiltI1,AiryFiltI2,AiryFiltI3,NoisyPattern,NoisyFiltI1,NoisyFiltI2,NoisyFiltI3,'link','equal');

%% Compare line profiles along X.

% Function handles to extract the midprofile and generate xaxis.
xprof=@(img) img(floor(0.5*size(img,1))+1,:);
xaxis=@(img,xrange) linspace(xrange(1),xrange(2),size(img,2));

% Profiles from airy pattern.
AiryProfiles=cellfun(@(img) xprof(img),...
    {AiryPattern,AiryFiltI1,AiryFiltI2,AiryFiltI3},...
    'UniformOutput',false);
AiryAxis=cellfun(@(img) xaxis(img,xrange),...
    {AiryPattern,AiryFiltI1,AiryFiltI2,AiryFiltI3},...
    'UniformOutput',false);

% Profiles from noisy pattern.
NoisyProfiles=cellfun(@(img) xprof(img),...
    {NoisyPattern,NoisyFiltI1,NoisyFiltI2,NoisyFiltI3},...
    'UniformOutput',false);
NoisyAxis=cellfun(@(img) xaxis(img,xrange),...
    {NoisyPattern,NoisyFiltI1,NoisyFiltI2,NoisyFiltI3},...
    'UniformOutput',false);

figure(2); clf; set(2,'defaultaxesfontsize',16);
plot(AiryAxis{1},AiryProfiles{1},AiryAxis{2},AiryProfiles{2},...
    AiryAxis{3},AiryProfiles{3},AiryAxis{4},AiryProfiles{4},...
    'LineWidth',2);
legend('Airy','AiryFiltInterpolation1','AiryFiltInterpolation2','AiryFiltInterpolation3');

snapnow;

figure(3); clf; set(3,'defaultaxesfontsize',16);
plot(NoisyAxis{1},NoisyProfiles{1},NoisyAxis{2},NoisyProfiles{2},...
    NoisyAxis{3},NoisyProfiles{3},NoisyAxis{4},NoisyProfiles{4},...
    'LineWidth',2);
legend('Noisy','NoisyFiltInterpolation1','NoisyFiltInterpolation2','NoisyFiltInterpolation3');
snapnow;


clearvars;
close all;
% clc;

rng default;

M = 300;
N = 1000;
K = 60;

A = randn(M, N);
x0 = zeros(N,1);

permutation = randperm(N);
indices = permutation(1:K);
% non-zero entries
x0(indices) = randn(K,1);

b0 = A * x0;
% noise variance
sigma = 0.1;
e = sigma * randn(M, 1);
% add noise
b = b0 + e;

% orthonormalize the rows of A
% perform QR decomposition of rows of A
[Q, R] = qr(A', 0);
% keep the orthogonal rows
A = Q';
% change b accordingly to the new coordinates
b = R'\b;

options.verbose = 0;
options.max_iterations = 200;
options.tolerance = 5e-3;
solver = spx.pursuit.single.L1_ADMM_YZ(A, options);
mu = sigma;
x = solver.solve_bpdn_l2(b, mu);

r = x - x0;
max_diff = max(abs(r));
rel_error = norm(x-x0) /norm(x0);
iterations = solver.details.iterations(1);
elapsed_time = solver.details.elapsed_times(1);
fprintf('Iterations: %d, Relative error: %e, max diff: %.4f, time: %.4f seconds\n', ...
    iterations, rel_error, max_diff, elapsed_time);

print = 1;
if print
    subplot(211);
    stem(x0, '.');
    subplot(212);
    stem(x, '.');
    figure;
    iterations = solver.details.iterations(1);
    iters = 1:iterations;
    primal_objectives = solver.details.primal_objectives(iters, 1);
    dual_objectives = solver.details.dual_objectives(iters, 1);
    plot(iters, primal_objectives);
    hold on;
    plot(iters, dual_objectives);
    xlabel('Iterations');
    ylabel('Objective Value');
    legend({'Primal Objective', 'Dual Objective'});
    grid on;
end

cvx = 0;
if cvx
% time to compare with a CVX implementation
cvx_solver = spx.pursuit.single.BasisPursuit(A, b);
tstart = tic;
x = cvx_solver.solve_lasso(1/(2*mu));
elapsed_time = toc(tstart);
r = x - x0;
max_diff = max(abs(r));
rel_error = norm(x-x0) /norm(x0);
fprintf('Relative error: %e, max diff: %.4f, time: %.4f seconds\n', ...
    rel_error, max_diff, elapsed_time);
end


function result=mcdregres(x,y,varargin)

%MCDREGRES is a robust multivariate regression method. It can handle multiple
% response variables. The estimates are based on the robust MCD estimator of
% location and scatter (see mcdcov.m). The explanatory variables should be
% low-dimensional, otherwise robust principal component regression (rpcr.m)
% or robust partial least squares (rsimpls.m) should be applied.
%
% The MCD regression method is described in
%    Rousseeuw, P.J., Van Aelst, S., Van Driessen, K, Agullo, J. (2004),
%    "Robust multivariate regression", Technometrics, 46, pp 293-305.
%
% Required input arguments:
%            x : Data matrix of the explanatory variables
%                (n observations in rows, p variables in columns)
%            y : Data matrix of the response variables
%                (n observations in rows, q variables in columns)
%
% Optional input arguments:
%        alpha : (1-alpha) measures the amount of contamination the algorithm should
%                resist. Any value between 0.5 and 1 may be specified. (default = 0.75)
%            h : The quantile of observations whose covariance determinant will
%                be minimized.  Any value between n/2 and n may be specified.
%                The default value is 0.75*n.
%       ntrial : The number of random trial subsamples that are drawn for
%                large datasets. (default = 500)
%        plots : If equal to one, a menu is shown which allows to draw a regression
%                outlier map. (default)
%                If the input argument 'classic' is equal to one, the classical
%                plot is drawn as well.
%                If 'plots' is equal to zero, all plots are suppressed.
%                See also makeplot.m
%      classic : If equal to one, classical multivariate linear regression
%                is performed as well, see mlr.m. (default = 0)
%
% Input arguments for advanced users:
%     Hsets : Instead of random trial h-subsets (default, Hsets = []), Hsets makes it possible to give certain
%             h-subsets as input. Hsets is a matrix that contains the indices of the observations of one
%             h-subset as a row.
%
% I/O: result=mcdregres(x,y,'alpha',0.75,'ntrial',500,'plots',1,'classic',0);
%  The user should only give the input arguments that have to change their default value.
%  The name of the input arguments needs to be followed by their value.
%  The order of the input arguments is of no importance.
%
% Example: result=mcdregres(x,y,'plots',0,'alpha',0.70)
%
% The output is a structure which contains
%   result.slope     : Robust slope (matrix)
%   result.int       : Robust intercept (vector)
%   result.fitted    : Robust prediction matrix
%   result.res       : Robust residuals
%   result.cov       : Estimated variance-covariance matrix of the residuals
%   result.rsquared  : Robust R-squared value
%   result.h         : The quantile h used throughout the algorithm
%   result.Hsubsets  : A structure that contains Hopt and Hfreq:
%                        Hopt  : The subset of h points whose covariance matrix has minimal determinant,
%                                ordered following increasing robust distances.
%                        Hfreq : The subset of h points which are the most frequently selected during the whole
%                                algorithm.
%   result.rd        : Robust scores distances in x-space
%   result.resd      : Residual distances (when there are several response variables).
%                      If univariate regression is performed, it contains the standardized residuals.
%   result.cutoff    : Cutoff values for the score and residual distances
%   result.weights   : The observations with weight one are used in the reweighting,
%                      the other observations have zero weight.
%   result.flag      : The observations whose residual distance is larger than result.cutoff.resd
%                      (bad leverage points/vertical outliers) can be considered as outliers and receive
%                      a flag equal to zero.
%                      The regular observations, including the good leverage points,
%                      receive a flag 1.
%   result.class     : 'MCDREG'
%   result.classic   : If the input argument 'classic' is equal to one, this structure
%                      contains results of classical multivariate regression (see also mlr.m).
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Original S-PLUS code by Katrien Vandriessen, implemented in MATLAB by Sabine Verboven
% Version date : 12/02/2004
% Last update: 04/08/2006

%%%%%%%%%%%%%%%%
% INITIALIZATION
%
intercept=ones(length(x),1);
geg=[x,y];
[n,m]=size(geg);
alfa=0.75;
hdefault=min(floor(2*floor((n+m+1)/2)-n+2*(n-floor((n+m+1)/2))*alfa),n);

if nargin < 3
    options.alpha=alfa;
    options.h=hdefault;
    options.ntrial=500;
    options.plots=1;
    options.classic=0;
    options.Hsets=[];
else
    default=struct('alpha',alfa,'h',hdefault,'ntrial',500,'plots',1,'classic',0,'Hsets',[]);
    list=fieldnames(default);
    options=default;
    IN=length(list);
    i=1;
    counter=1;
    %
    if nargin>2
        %
        %placing inputfields in array of strings
        %
        for j=1:nargin-2
            if rem(j,2)~=0
                chklist{i}=varargin{j};
                i=i+1;
            end
        end
        dummy=sum(strcmp(chklist,'h')+2*strcmp(chklist,'alpha'));
        switch dummy
            case 0 %no input for alpha or h so take on the default values
                options.alpha=0.75;
                options.h=floor(options.alpha*n);
            case 3
                error('Both input arguments alpha and h are provided. Only one is required.')
        end
        %
        %Checking which default parameters have to be changed
        % and keep them in the structure 'options'.
        %
        while counter<=IN
            index=strmatch(list(counter,:),chklist,'exact');
            if ~isempty(index) %in case of similarity
                for j=1:nargin-2 %searching the index of the accompanying field
                    if rem(j,2)~=0 %fieldnames are placed on odd index
                        if strcmp(chklist{index},varargin{j})
                            I=j;
                        end
                    end
                end
                options=setfield(options,chklist{index},varargin{I+1});
                index=[];
            end
            counter=counter+1;
        end
        if dummy==1
            options.alpha=options.h/n;
        elseif dummy==2
            options.h=floor(options.alpha*n);
        end
        Hsets = options.Hsets;
    end
end
%%%%%%%%
% MAIN %
[mcd_res, mcd_raw]=mcdcov(geg,'h',options.h,'plots',0,'Hsets',options.Hsets);
options.h = mcd_res.h;

%in case of an exact fit, the calculations stop at this point.
if ~isempty(mcd_res.plane)
    disp('Warning (mcdregres): The MCD covariance matrix is singular. See also mcdcov.m.')
    result=mcd_res;
    return
end

mcdreg.Hsubsets.Hopt = mcd_res.Hsubsets.Hopt;
mcdreg.Hsubsets.Hfreq = mcd_res.Hsubsets.Hfreq;

rewcovmcd=mcd_res.cov; %one-step reweighted covariance matrix
rewcenmcd=mcd_res.center; %one-step reweighted location
q=size(y,2);
p=size(x,2);

%initializing reweighted location and reweighted scatter (paragraph 5.1 of paper)
rewtmcdx=rewcenmcd(1:p)'; %column vectors!!!
rewtmcdy=rewcenmcd((p+1):m)';

rewsmcdx=rewcovmcd(1:p,1:p);
rewsmcdxy=rewcovmcd(1:p,(p+1):m);
rewsmcdyx=rewsmcdxy';
rewsmcdy=rewcovmcd((p+1):m,(p+1):m);

covyy=rewsmcdy;
covxx = rewsmcdx;
covxy = rewsmcdxy;
covyx = rewsmcdyx;
mcdreg.Sigma = [covxx, covxy ; covyx, covyy];
mcdreg.Mu = [rewtmcdx;rewtmcdy];

%reweighted beta and alpha (the columnvector [\beta^L ; \alpha^L])
rewbetamcd=[inv(rewsmcdx)*rewsmcdxy; (rewtmcdy-(rewsmcdyx*inv(rewsmcdx)*rewtmcdx))'];

%calculation of the reweighted weights based on
%the residuals calculated with the reweighted beta-coefficients
rewweights=zeros(n,1);
weights=zeros(n,1);
rewfitted=[x,intercept]*rewbetamcd(1:(p+1),:);
rewresid=y-rewfitted; %(r_i^L)
rewE=rewsmcdy-rewbetamcd(1:p,1:q)'*rewsmcdx*rewbetamcd(1:p,1:q); %reweighted scatter (\Sigma_eps^L)
for j=1:n
    if (sqrt(rewresid(j,1:q)*inv(rewE)*rewresid(j,1:q)')) <= sqrt(chi2inv(0.99,q))
        rewweights(j)=1;
    end
end

%regression reweighting part based on the reweighted observations. (paragraph 5.3 of paper)
rewclasscov=cov(geg(rewweights==1,:));
rewclasscenter=mean(geg(rewweights==1,:));

rewtmcdx=rewclasscenter(1:p)';
rewtmcdy=rewclasscenter((p+1):m)';

rewsmcdx=rewclasscov(1:p,1:p);
rewsmcdxy=rewclasscov(1:p,(p+1):m);
rewsmcdyx=rewsmcdxy';
rewsmcdy=rewclasscov((p+1):m,(p+1):m);

%regression reweighted coefficients beta and alpha ([\beta^{RL} \alpha^{RL}])
rewbetamcdrew=[inv(rewsmcdx)*rewsmcdxy; (rewtmcdy-(rewsmcdyx*inv(rewsmcdx)*rewtmcdx))'];
%regression reweighted scatter (\Sigma_eps^{RL}])
rewE2=rewsmcdy-rewbetamcdrew(1:p,1:q)'*rewsmcdx*rewbetamcdrew(1:p,1:q);
rewfittedrew=[x,intercept]*rewbetamcdrew(1:(p+1),:);
rewresidrew=y-rewfittedrew;

%%%%%%%%%%%%%%%%%
%OUTPUT STRUCTURE
%
mcdreg.covyy=rewsmcdy; %regression and location reweighted covariance matrix of responses
mcdreg.covxx = rewsmcdx;
mcdreg.covxy = rewsmcdxy;
mcdreg.covyx = rewsmcdyx;
mcdreg.Sigmarew = [mcdreg.covxx, mcdreg.covxy ; mcdreg.covyx, mcdreg.covyy];
mcdreg.Murew = [rewtmcdx;rewtmcdy];

mcdreg.x=x;
mcdreg.y=y;
mcdreg.coeffs=rewbetamcdrew; %regression and location reweighted coefficients
mcdreg.cov=rewE2; %scatter matrix based on location and regression reweighting
mcdreg.fitted=rewfittedrew; %estimated respons(es);
mcdreg.res=rewresidrew; %regression and location reweighted residuals

if(intercept)
   mcdreg.interc=1;
else
   mcdreg.interc=0;
end

% Robust distances in x-space = x-distances (rd) needed in diagnostic regression plot
if (-log(det(mcd_res.cov))/m) > 50
   mcdreg.rd='singularity';
else
   mcdreg.rd=sqrt(libra_mahalanobis(mcdreg.x,rewcenmcd(1:p),'cov',rewcovmcd(1:p,1:p)))';
end

% Robust residual distances (resd) needed in diagnostic regression plot
if q>1
   if (-log(det(mcd_res.cov))/m)>50
       mcdreg.resd='singularity';
       disp('Warning (mcdregres): A singularity ')
   else
      cen=zeros(q,1)';
      [nn,pp]=size(rewresidrew);
      mcdreg.resd=sqrt(libra_mahalanobis(rewresidrew,cen,'cov',mcdreg.cov))'; %robust distances of residuals
  end
else
    mcdreg.covarRes=sqrt(mcdreg.cov);
    mcdreg.resd=rewresidrew(:,1)/mcdreg.covarRes; %standardized residuals
end

% cutoff values
mcdreg.cutoff.rd=sqrt(chi2inv(0.975,p));
mcdreg.cutoff.resd=sqrt(chi2inv(0.975,q));
mcdreg.flag=(abs(mcdreg.resd)<=mcdreg.cutoff.resd);

% robust multivariate Rsquared
mcdreg.weights=rewweights;
Yw=y(mcdreg.weights==1,:);
cYw=mcenter(Yw);
res=rewresidrew(mcdreg.weights==1,:);
mcdreg.rsquared=1-(det(res'*res)/det(cYw'*cYw));
mcdreg.class='MCDREG';

if options.classic
    mcdreg.classic=mlr(x,y,'plots',0);
else
    mcdreg.classic=0;
end

result=struct('slope',{mcdreg.coeffs(1:p,:)}, 'int',{mcdreg.coeffs(p+1,:)}, ...
    'fitted',{mcdreg.fitted},'res',{mcdreg.res},'cov',{mcdreg.cov},'rsquared',{mcdreg.rsquared},...
    'h',{options.h},'Hsubsets',{mcdreg.Hsubsets},'rd', {mcdreg.rd},'resd',{mcdreg.resd},'cutoff',{mcdreg.cutoff},...
    'weights',{mcdreg.weights},'flag',{mcdreg.flag'},'class',{mcdreg.class},'classic',{mcdreg.classic},...
    'Mu',{mcdreg.Mu},'Sigma',{mcdreg.Sigma},'Murew',{mcdreg.Murew},'Sigmarew',{mcdreg.Sigmarew});

try
    if options.plots & options.classic
        makeplot(result,'classic',1)
    elseif options.plots
        makeplot(result)
    end
catch %output must be given even if plots are interrupted
    %> delete(gcf) to get rid of the menu
end


% Wrapper around Matlab's histc function with some modifications
%
% This function assigns each input value a bin number according to specified
% limits of values and width of each bin.
% Vector of bin edges is created out of specified limits and bin width. Value
% x(i) is assigned to bin k if edges(k) <= x(i) < edges(k+1). So far this is
% exactly what Matlab's function histc does. The difference comes in the last bin.
% Condition of assignment for the last bin is edges(end-1) <= x(i) <= edges(end).
%
%  USAGE
%   [bins, nBins, edges] = helpers.bin(x, limits, binWidth)
%   x           1D vector of values that should be binned.
%   limits      [min_x max_x] limit values that define bins. These need not be equal to
%               min(x) and max(x). Values of x that equal to min_x or max_x are all
%               included in the final histogram.
%   binWidth    width of each individual bin. Used to create edges vector.
%   bins        Vector of assigned bin numbers to x. length(bins) == length(x).
%               Values outside of limits are assigned to NaN.
%   nBins       Number of bins.
%   edges       Edge values of bins.
%
function [bins, nBins, edges] = bin(x, limits, binWidth)
    nBins = ceil((limits(2) - limits(1)) / binWidth);

    edges = limits(1):binWidth:limits(2);
    if length(edges) == nBins
        edges(end+1) = limits(2);
    end

    ind = (x == edges(end)); % find indices of values that according to mathematics should be
                                 % assigned to bin nBins+1
    [~, bins] = histc(x, edges);
    bins(ind) = nBins;
    bins(bins > nBins) = nan;
    bins(bins == 0) = nan;
end


function [v s] = ricefit(mudat, vr)
%RICEFIT Estimate parameters for Rice/Rician distribution from data.
%   Uses simple moment-matching approach, with numerical optimisation.
%   [v s] = ricefit(dat) estimates v and s from samples in dat
%   [v s] = ricefit(mu, vr) estimates v and s from given mean and variance
%
%   R ~ Rice(v, s) if R = sqrt(X^2 + Y^2), where X ~ N(v*cos(a), s^2) and
%   Y ~ N(v*sin(a), s^2) are independent normal distributions (any real a).
%   Note that v and s are *not* the mean and standard deviation of R!
%
%   Reference: http://en.wikipedia.org/wiki/Rice_distribution (!)
%
%   Example:
%     % Sample data, fit model, compare expected, fitted & observed moments
%     V = 5; S = 4; N = 1000;
%     [MN VR] = ricestat(V, S)      % expected mean and variance
%     r = ricernd(V*ones(1, N), S);
%     [v s] = ricefit(dat)          % fitted Rician distribution parameters
%     [mn vr] = ricestat(v, s)      % fitted mean and variance
%     mean(dat), var(dat)           % observed mean and variance
%
%   See also RICEPDF, RICERND, RICESTAT

%   Missing (?) 'See also's RICECDF, RICEINV, RICELIKE

%   Inspired by normfit from the MATLAB statistics toolbox
%   Copyright 2008 Ged Ridgway (Ged at cantab dot net)

if nargin == 1
    dat = mudat;
    mu = mean(dat(:));
    vr = var(dat(:));
else
    mu = mudat;
end

% Optimise cost based on difference of mu and vr from ricestat(v, s) as a
% function of v and s, using mu and sqrt(vr) as initial values for v and s
cost = @(x) moment_cost(x(1), x(2), mu, vr);
x = fminsearch(cost, [mu sqrt(vr)]);
if nargout == 1
    v = x;
else
    v = x(1);
    s = x(2);
end

function cost = moment_cost(v, s, MU, VR)
% Very naive approach...
% Not clear how to wait relative errors in mean and variance...
[mu vr] = ricestat(v, s);
cost = norm([mu - MU; vr - VR;]);


function euler = dcm2euler(DCMbn)
% dcm2euler: converts from body-to-nav DCM to Euler angles.
%
% INPUT
%   DCMbn, 3x3 body-to-nav DCM.
%
% OUTPUT
%   euler, 3x1 Euler angles [roll pitch yaw] (rad, rad, rad).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
% 	Titterton, D.H. and Weston, J.L. (2004). Strapdown
% Inertial Navigation Technology (2nd Ed.). Institution
% of Engineering and Technology, USA. Eq. 11.4. Eq. 3.66, p. 46.
%
%   R. Gonzalez, J. Giribet, and H.D. Patiño,. An approach to
% benchmarking of loosely coupled low-cost navigation systems,
% Mathematical and Computer Modelling of Dynamical Systems, vol. 21,
% issue 2, pp. 272-287. Eq. 15.
%
% Version: 002
% Date:    2016/11/26
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

phi   =  atan( DCMbn(3,2) ./ DCMbn(3,3) );  % roll
theta = -asin( DCMbn(3,1) );                % pitch
psi   =  atan2( DCMbn(2,1), DCMbn(1,1) );   % yaw

euler = [phi theta psi];

end


function [bandwidth,density,X,Y]=kde2d(data,n,MIN_XY,MAX_XY)
% fast and accurate state-of-the-art
% bivariate kernel density estimator
% with diagonal bandwidth matrix.
% The kernel is assumed to be Gaussian.
% The two bandwidth parameters are
% chosen optimally without ever
% using/assuming a parametric model for the data or any "rules of thumb".
% Unlike many other procedures, this one
% is immune to accuracy failures in the estimation of
% multimodal densities with widely separated modes (see examples).
% INPUTS: data - an N by 2 array with continuous data
%            n - size of the n by n grid over which the density is computed
%                n has to be a power of 2, otherwise n=2^ceil(log2(n));
%                the default value is 2^8;
% MIN_XY,MAX_XY- limits of the bounding box over which the density is computed;
%                the format is:
%                MIN_XY=[lower_Xlim,lower_Ylim]
%                MAX_XY=[upper_Xlim,upper_Ylim].
%                The dafault limits are computed as:
%                MAX=max(data,[],1); MIN=min(data,[],1); Range=MAX-MIN;
%                MAX_XY=MAX+Range/4; MIN_XY=MIN-Range/4;
% OUTPUT: bandwidth - a row vector with the two optimal
%                     bandwidths for a bivaroate Gaussian kernel;
%                     the format is:
%                     bandwidth=[bandwidth_X, bandwidth_Y];
%          density  - an n by n matrix containing the density values over the n by n grid;
%                     density is not computed unless the function is asked for such an output;
%              X,Y  - the meshgrid over which the variable "density" has been computed;
%                     the intended usage is as follows:
%                     surf(X,Y,density)
% Example (simple Gaussian mixture)
% clear all
%   % generate a Gaussian mixture with distant modes
%   data=[randn(500,2);
%       randn(500,1)+3.5, randn(500,1);];
%   % call the routine
%     [bandwidth,density,X,Y]=kde2d(data);
%   % plot the data and the density estimate
%     contour3(X,Y,density,50), hold on
%     plot(data(:,1),data(:,2),'r.','MarkerSize',5)
%
% Example (Gaussian mixture with distant modes):
%
% clear all
%  % generate a Gaussian mixture with distant modes
%  data=[randn(100,1), randn(100,1)/4;
%      randn(100,1)+18, randn(100,1);
%      randn(100,1)+15, randn(100,1)/2-18;];
%  % call the routine
%    [bandwidth,density,X,Y]=kde2d(data);
%  % plot the data and the density estimate
%  surf(X,Y,density,'LineStyle','none'), view([0,60])
%  colormap hot, hold on, alpha(.8)
%  set(gca, 'color', 'blue');
%  plot(data(:,1),data(:,2),'w.','MarkerSize',5)
%
% Example (Sinusoidal density):
%
% clear all
%   X=rand(1000,1); Y=sin(X*10*pi)+randn(size(X))/3; data=[X,Y];
%  % apply routine
%  [bandwidth,density,X,Y]=kde2d(data);
%  % plot the data and the density estimate
%  surf(X,Y,density,'LineStyle','none'), view([0,70])
%  colormap hot, hold on, alpha(.8)
%  set(gca, 'color', 'blue');
%  plot(data(:,1),data(:,2),'w.','MarkerSize',5)
%
%  Reference:
% Kernel density estimation via diffusion
% Z. I. Botev, J. F. Grotowski, and D. P. Kroese (2010)
% Annals of Statistics, Volume 38, Number 5, pages 2916-2957.
global N A2 I
if nargin<2
    n=2^8;
end
n=2^ceil(log2(n)); % round up n to the next power of 2;
N=size(data,1);
if nargin<3
    MAX=max(data,[],1); MIN=min(data,[],1); Range=MAX-MIN;
    MAX_XY=MAX+Range/2; MIN_XY=MIN-Range/2;
end
scaling=MAX_XY-MIN_XY;
if N<=size(data,2)
    error('data has to be an N by 2 array where each row represents a two dimensional observation')
end
transformed_data=(data-repmat(MIN_XY,N,1))./repmat(scaling,N,1);
%bin the data uniformly using regular grid;
initial_data=ndhist(transformed_data,n);
% discrete cosine transform of initial data
a= dct2d(initial_data);
% now compute the optimal bandwidth^2
  I=(0:n-1).^2; A2=a.^2;
 t_star=root(@(t)(t-evolve(t)),N);
p_02=func([0,2],t_star);p_20=func([2,0],t_star); p_11=func([1,1],t_star);
t_y=(p_02^(3/4)/(4*pi*N*p_20^(3/4)*(p_11+sqrt(p_20*p_02))))^(1/3);
t_x=(p_20^(3/4)/(4*pi*N*p_02^(3/4)*(p_11+sqrt(p_20*p_02))))^(1/3);
% smooth the discrete cosine transform of initial data using t_star
a_t=exp(-(0:n-1)'.^2*pi^2*t_x/2)*exp(-(0:n-1).^2*pi^2*t_y/2).*a;
% now apply the inverse discrete cosine transform
if nargout>1
    density=idct2d(a_t)*(numel(a_t)/prod(scaling));
	density(density<0)=eps; % remove any negative density values
    [X,Y]=meshgrid(MIN_XY(1):scaling(1)/(n-1):MAX_XY(1),MIN_XY(2):scaling(2)/(n-1):MAX_XY(2));
end
bandwidth=sqrt([t_x,t_y]).*scaling;
end
%#######################################
function  [out,time]=evolve(t)
global N
Sum_func = func([0,2],t) + func([2,0],t) + 2*func([1,1],t);
time=(2*pi*N*Sum_func)^(-1/3);
out=(t-time)/time;
end
%#######################################
function out=func(s,t)
global N
if sum(s)<=4
    Sum_func=func([s(1)+1,s(2)],t)+func([s(1),s(2)+1],t); const=(1+1/2^(sum(s)+1))/3;
    time=(-2*const*K(s(1))*K(s(2))/N/Sum_func)^(1/(2+sum(s)));
    out=psi(s,time);
else
    out=psi(s,t);
end

end
%#######################################
function out=psi(s,Time)
global I A2
% s is a vector
w=exp(-I*pi^2*Time).*[1,.5*ones(1,length(I)-1)];
wx=w.*(I.^s(1));
wy=w.*(I.^s(2));
out=(-1)^sum(s)*(wy*A2*wx')*pi^(2*sum(s));
end
%#######################################
function out=K(s)
out=(-1)^s*prod((1:2:2*s-1))/sqrt(2*pi);
end
%#######################################
function data=dct2d(data)
% computes the 2 dimensional discrete cosine transform of data
% data is an nd cube
[nrows,ncols]= size(data);
if nrows~=ncols
    error('data is not a square array!')
end
% Compute weights to multiply DFT coefficients
w = [1;2*(exp(-i*(1:nrows-1)*pi/(2*nrows))).'];
weight=w(:,ones(1,ncols));
data=dct1d(dct1d(data)')';
    function transform1d=dct1d(x)

        % Re-order the elements of the columns of x
        x = [ x(1:2:end,:); x(end:-2:2,:) ];

        % Multiply FFT by weights:
        transform1d = real(weight.* fft(x));
    end
end
%#######################################
function data = idct2d(data)
% computes the 2 dimensional inverse discrete cosine transform
[nrows,ncols]=size(data);
% Compute wieghts
w = exp(i*(0:nrows-1)*pi/(2*nrows)).';
weights=w(:,ones(1,ncols));
data=idct1d(idct1d(data)');
    function out=idct1d(x)
        y = real(ifft(weights.*x));
        out = zeros(nrows,ncols);
        out(1:2:nrows,:) = y(1:nrows/2,:);
        out(2:2:nrows,:) = y(nrows:-1:nrows/2+1,:);
    end
end
%#######################################
function binned_data=ndhist(data,M)
% this function computes the histogram
% of an n-dimensional data set;
% 'data' is nrows by n columns
% M is the number of bins used in each dimension
% so that 'binned_data' is a hypercube with
% size length equal to M;
[nrows,ncols]=size(data);
bins=zeros(nrows,ncols);
for i=1:ncols
    [dum,bins(:,i)] = histc(data(:,i),[0:1/M:1],1);
    bins(:,i) = min(bins(:,i),M);
end
% Combine the  vectors of 1D bin counts into a grid of nD bin
% counts.
binned_data = accumarray(bins(all(bins>0,2),:),1/nrows,M(ones(1,ncols)));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function t=root(f,N)
% try to find smallest root whenever there is more than one
N=50*(N<=50)+1050*(N>=1050)+N*((N<1050)&(N>50));
tol=10^-12+0.01*(N-50)/1000;
flag=0;
while flag==0
    try
        t=fzero(f,[0,tol]);
        flag=1;
    catch
        tol=min(tol*2,.1); % double search interval
    end
    if tol==.1 % if all else fails
        t=fminbnd(@(x)abs(f(x)),0,.1); flag=1;
    end
end
end


function [weight] = create_gc_weights(xDim, yDim, zDim, xVar, yVar, zVar)
%     NeuroSLAM System Copyright (C) 2018-2019
%     NeuroSLAM: A Brain inspired SLAM System for 3D Environments
%
%     Fangwen Yu (www.yufangwen.com), Jianga Shang, Youjian Hu, Michael Milford(www.michaelmilford.com)
%
%     The NeuroSLAM V1.0 (MATLAB) was developed based on the OpenRatSLAM (David et al. 2013).
%     The RatSLAM V0.3 (MATLAB) developed by David Ball, Michael Milford and Gordon Wyeth in 2008.
%
%     Reference:
%     Ball, David, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, and Michael Milford.
%     "OpenRatSLAM: an open source brain-based SLAM system." Autonomous Robots 34, no. 3 (2013): 149-176.
%
%     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/>.

    % Creates a 3D normalised distributio of size dimension^3 with a variance of var.

    xDimCentre = floor(xDim / 2) + 1;
    yDimCentre = floor(yDim / 2) + 1;
    zDimCentre = floor(zDim / 2) + 1;

    weight = zeros(xDim, yDim, zDim);

    for z = 1 : zDim
        for x = 1 : xDim
            for y = 1 : yDim
               weight(x,y,z) = 1/(xVar*sqrt(2*pi))*exp((-(x - xDimCentre) ^ 2) / (2 * xVar ^ 2)) ...
                   * 1/(yVar*sqrt(2*pi))*exp((-(y - yDimCentre) ^ 2) / (2 * yVar ^ 2)) ...
                   * 1/(zVar*sqrt(2*pi))*exp((-(z - zDimCentre) ^ 2) / (2 * zVar ^ 2));
            end
        end
    end

    % ensure that it is normalised
    total = sum(sum(sum(weight)));
    weight = weight./total;

end


function [normal,normalf] = compute_normal(vertex,face)

% compute_normal - compute the normal of a triangulation
%
%   [normal,normalf] = compute_normal(vertex,face);
%
%   normal(i,:) is the normal at vertex i.
%   normalf(j,:) is the normal at face j.
%
%   Copyright (c) 2004 Gabriel Peyr�

[vertex,face] = check_face_vertex(vertex,face);

nface = size(face,2);
nvert = size(vertex,2);
normal = zeros(3, nvert);

% unit normals to the faces
normalf = crossp( vertex(:,face(2,:))-vertex(:,face(1,:)), ...
                  vertex(:,face(3,:))-vertex(:,face(1,:)) );
d = sqrt( sum(normalf.^2,1) ); d(d<eps)=1;
normalf = normalf ./ repmat( d, 3,1 );

% unit normal to the vertex
normal = ComputeVertexNormal(face, normalf, nvert);
% normal = zeros(3,nvert);
% for i=1:nface
%     f = face(:,i);
%     for j=1:3
%         normal(:,f(j)) = normal(:,f(j)) + normalf(:,i);
%     end
% end
% normalize
d = sqrt( sum(normal.^2,1) ); d(d<eps)=1;
normal = normal ./ repmat( d, 3,1 );

% enforce that the normal are outward
v = vertex - repmat(mean(vertex,1), 3,1);
s = sum( v.*normal, 2 );
if sum(s>0)<sum(s<0)
    % flip
    normal = -normal;
    normalf = -normalf;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function z = crossp(x,y)
% x and y are (m,3) dimensional
z = x;
z(1,:) = x(2,:).*y(3,:) - x(3,:).*y(2,:);
z(2,:) = x(3,:).*y(1,:) - x(1,:).*y(3,:);
z(3,:) = x(1,:).*y(2,:) - x(2,:).*y(1,:);


function f = spm_Bpdf(x,v,w)
% Probability Density Function (PDF) of Beta distribution
% FORMAT f = spm_Bpdf(x,v,w)
%
% x   - Beta variates (Beta has range [0,1])
% v   - Shape parameter (v>0)
% w   - Shape parameter (w>0)
% F   - PDF of Beta distribution with shape parameters [v,w] at points x
%__________________________________________________________________________
%
% spm_Bpdf implements the Probability Density Function for Beta distributions.
%
% Definition:
%--------------------------------------------------------------------------
% The PDF of the Beta distribution shape parameters v & w, defined
% for positive integer degrees of freedom v>0 & w>0, and for x in
% [0,1] is given by: (See Evans et al., Ch5)
%
%            x^(v-1) * (1-x)^(w-1)
%    f(x) = -----------------------
%                 beta(v,w)
%
% Variate relationships:
%--------------------------------------------------------------------------
% Many: See Evans et al., Ch5
%
% Algorithm:
%--------------------------------------------------------------------------
% Direct computation using logs and MATLAB's implementation of the log
% beta function (betaln).
%
% References:
%--------------------------------------------------------------------------
% Evans M, Hastings N, Peacock B (1993)
%       "Statistical Distributions"
%        2nd Ed. Wiley, New York
%
% Abramowitz M, Stegun IA, (1964)
%       "Handbook of Mathematical Functions"
%        US Government Printing Office
%
% Press WH, Teukolsky SA, Vetterling AT, Flannery BP (1992)
%       "Numerical Recipes in C"
%        Cambridge
%__________________________________________________________________________
% Copyright (C) 1999-2011 Wellcome Trust Centre for Neuroimaging

% Andrew Holmes
% $Id: spm_Bpdf.m 4182 2011-02-01 12:29:09Z guillaume $

%-Format arguments, note & check sizes
%--------------------------------------------------------------------------
if nargin<3, error('Insufficient arguments'), end

ad = [ndims(x);ndims(v);ndims(w)];
rd = max(ad);
as = [[size(x),ones(1,rd-ad(1))];...
      [size(v),ones(1,rd-ad(2))];...
      [size(w),ones(1,rd-ad(3))]];
rs = max(as);
xa = prod(as,2)>1;
if sum(xa)>1 && any(any(diff(as(xa,:)),1))
    error('non-scalar args must match in size');
end

%-Computation
%--------------------------------------------------------------------------
%-Initialise result to zeros
f = zeros(rs);

%-Only defined for x in [0,1] & strictly positive v & w.
% Return NaN if undefined.
md = ( x>=0  &  x<=1  &  v>0  &  w>0 );
if any(~md(:))
    f(~md) = NaN;
    warning('Returning NaN for out of range arguments');
end

%-Special cases: x=0 & x=1
f(md & x==0 & v<1) = Inf;
f(md & x==1 & w<1) = Inf;

%-Non-zero where defined & x in (0,1)
Q  = find( md  &  x>0  &  x<1 );
if isempty(Q), return, end
if xa(1), Qx=Q; else Qx=1; end
if xa(2), Qv=Q; else Qv=1; end
if xa(3), Qw=Q; else Qw=1; end

%-Compute
f(Q) = exp((v(Qv)-1).*log(x(Qx)) + (w(Qw)-1).*log(1-x(Qx)) -betaln(v(Qv),w(Qw)));


function value = r8_shi ( x )

%*****************************************************************************80
%
%% R8_SHI evaluates the hyperbolic sine integral Shi of an R8 argument.
%
%  Discussion:
%
%    Shi ( x ) = Integral ( 0 <= t <= x ) sinh ( t ) dt / t
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the hyperbolic sine integral
%    Shi evaluated at X.
%
  persistent nshi
  persistent shics
  persistent xsml

  if ( isempty ( nshi ) )

    shics = [ ...
      0.0078372685688900950695200984317332, ...
      0.0039227664934234563972697574427225, ...
      0.0000041346787887617266746747908275, ...
      0.0000000024707480372882742135145302, ...
      0.0000000000009379295590763630457157, ...
      0.0000000000000002451817019520867353, ...
      0.0000000000000000000467416155257592, ...
      0.0000000000000000000000067803072389, ...
      0.0000000000000000000000000007731289, ...
      0.0000000000000000000000000000000711 ]';

    nshi = r8_inits ( shics, 10, 0.1 * r8_mach ( 3 ) );
    xsml = sqrt ( r8_mach ( 3 ) );

  end

  absx = abs ( x );

  if ( absx <= xsml )
    value = x;
  elseif ( absx <= 0.375 )
    value = x * ( 1.0 + r8_csevl ( 128.0 * x * x / 9.0 - 1.0, shics, nshi ) );
  else
    value = 0.5 * ( r8_ei ( x ) + r8_e1 ( x ) );
  end

  return
end


function [am,em]=v_lpcrr2am(rr);
%V_LPCRR2AM Convert autocorrelation coefs to ar coef matrix [AM,EM]=(RR)
%AM is a 3-dimensional matrix of size (p+1,p+1,nf) where p is the lpc order
%and nf the number of frames.
%The matrix AM(:,:,*) is upper triangular with 1's on the main diagonal
%and contains the lpc coefficients for all orders from p down to 0.
%
%For lpc order p+1-r, AM(r,r:p+1,*), AM(p+1:-1:r,p+1,*) and EM(*,r) contain
%the lpc coefficients, reflection coefficients and the residual energy respectively.
%
%If A=am(:,:,*), R=toeplitz(rr(*,:)) and E=diag(em(*,:)), then
% A*R*A'=E; inv(R)=A'*(1/E)*A; A*R is lower triangular with the same diagonal as E
%
% This routine is equivalent to: c=chol(inv(toeplitz(rr))); d=diag(c).^-1; em=d.^2; am=diag(d)*c

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

[nf,p1]=size(rr);
p=p1-1;
p2=p1+1;
am=zeros(nf,p1,p1);
em=zeros(nf,p1);
am(:,p1,p1)=1;
em(:,p1)=rr(:,1);
ar=ones(nf,p1);
ar(:,2) = -rr(:,2)./rr(:,1);
e = rr(:,1).*(ar(:,2).^2-1);
for n = 2:p
   q=p2-n;
   em(:,q)=-e;
   am(:,q:p1,q)=ar(:,1:n);
   k = (rr(:,n+1)+sum(rr(:,n:-1:2).*ar(:,2:n),2)) ./ e;
   ar(:,2:n) = ar(:,2:n)+k(:,ones(1,n-1)).*ar(:,n:-1:2);
   ar(:,n+1) = k;
   e = e.*(1-k.^2);
end
em(:,1)=-e;
am(:,:,1)=ar;
am=permute(am,[3 2 1]);


function out = proj_hyperplane_box(x,a,b,l,u)
%PROJ_HYPERPLANE_BOX computes the orthogonal projection onto the intersection of a hyperplane and a box {x:<a,x>=b,l<=x<=u}
%
%  Usage:
%  out = PROJ_HYPERPLANE_BOX(x,a,b,[l],[u])
%  ===========================================
%  Input:
%  x - point to be projected (vector/matrix)
%  a - vector/matrix
%  b - scalar
%  l - lower bound (vector/matrix/scalar) [default: -inf]
%  u - upper bound (vector/matrix/scalar) [default: inf]
%  ===========================================
%  Assumptions:
%  The intersection of the hyperplane and the box is nonempty
%  ===========================================
%  Output:
%  out - projection vector

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% 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/>.

if (nargin < 3)
    error ('usage: proj_hyperplane_box(x,a,b,[l],[u])') ;
end

if (nargin < 5)
    %upper bound is not given, setting to defalut value :inf
    u = inf ;
end
if ((nargin < 4) || (isempty( l)))
    %lower bound is not given, setting to defalut value :-inf
    l = -inf;
end

%checking that sum (l) < b < sum (u)

sumlb = trace(a'*((l .* (sign(a)>0)) + (u .* (sign(a)<0)))) ;
sumub = trace(a'*((u .* (sign(a)>0)) + (l .* (sign(a)<0)))) ;

if ((sumlb > b) || (any(any(l > u))) ||  (sumub < b))
    error('Set is infeasible') ;
end

%solve with equality
%defining f on lambda - to be used by the bisetion

eps = 1e-10 ;

f= @(lam)   trace(a'*min(max(x-lam*a,l),u))-b;
lambda_min = -1;
while(f(lambda_min)<0)
    lambda_min = lambda_min *2  ;
end

lambda_max = 1;
while(f(lambda_max)>0)
    lambda_max = lambda_max *2  ;
end

final_lam = bisection(f,lambda_min,lambda_max,eps) ;
out=  min(max(x-final_lam*a,l),u) ;